ros-industrial / motoman

We are having some troubles sending new waypoints while the robot is in motion using our own library: https://github.com/ra-mtp-ntnu/moto.

We have some questions:

How can we send a whole trajectory to the robot?
How can we send new trajectory points (stream) to the robot while the robot is in motion without getting resulttype busy in return? I think this line is responible:

motoman/motoman_driver/MotoPlus/MotionServer.c

Line 1122 in 2cdbe79

Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_BUSY, 0, replyMsg, receiveMsg->body.jointTrajData.groupNo);

How can we send a whole trajectory to the robot?

you can't. Or at least, not in a single message.

MotoROS uses a trajectory streaming approach, meaning motoman_driver takes a JointTrajectory, decomposes it into its constituent JoinTrajectoryPoints, converts those to their Simple Message equivalent and starts streaming those to MotoROS. MotoROS has an internal buffer which gets 'fed' by this incoming stream of traj pts. The consuming side is a 'motion thread' which further converts the Simple Message traj pts into motion increments.

So there is no way to send "a whole trajectory", it's always individual points.

You could say that a trajectory smaller than the depth of the buffer is transferred "whole", but even that would not be technically correct.

How can we send new trajectory points (stream) to the robot while the robot is in motion without getting resulttype busy in return?

As the size / depth of the mentioned buffer is finite and trajectory execution is typically slower than the time needed for streaming points, at some point the buffer is full. At that point MotoROS starts sending BUSY replies. This signals to motoman_driver it should try again at a later time. It waits a little and then retries. If trajectory execution has progressed sufficiently, the buffer will not be full any more and new traj pts can be queued.

Management of the buffer is independent of the control flow of the task executing the actual motion, so the robot is typically "in motion" while MotoROS is still receiving incoming trajectory points (see my explanation above).

Important: whether or not you receive BUSY depends on the state of the buffer, not on whether there is any motion being executed.

While it's not completely up-to-date any more, you may be interested in Motoplus-ROS Incremental Motion interface - Engineering Design Specifications, which describes the design and implementation of MotoROS.

As I believe this isn't (going to turn out to be) a problem with any of the packages here in this repository, I'm closing the issue.

Feel free to keep commenting on it of course.

If it turns out there is indeed something actionable, we can re-open.

Has this clarified things @tingelst ?

Has this clarified things @tingelst ?

Yes , a bit.

I haven't had the time to test this in the lab, but can our problem then be that we send trajectory points to fast?

I see that there is a sleep in the ros driver:

motoman/motoman_driver/src/joint_trajectory_streamer.cpp

Line 408 in 9a7411f

ros::Duration(0.005).sleep();

can our problem then be that we send trajectory points to fast?

I wouldn't know, as I don't believe you've described your problem.

Seeing BUSY is not a problem. It's by design.

There is no way for a buffer to be "large enough" for all possible trajectories, hence some way to tell the sending side to "back off" for a bit is always needed. That's what a reply with BUSY does: it tells the client (in this case your library) to stop sending more data and try again at a later time.

I see that there is a sleep in the ros driver

that sleep is there to avoid a busy wait. It isn't strictly required, as long as BUSY replies are correctly handled.

Just re-read your #447 (comment) and noticed:

can our problem then be that we send trajectory points to fast?

if by "too fast" you mean "are we trying to send too many points in too short a time?", then yes, that could be the cause of whatever problem you are having.

The BUSY replies are part of a handshake mechanism, with which the producer (ie: the client, your library, motoman_driver) together with the consumer (ie: MotoROS) manage the space in the buffer.

The consumer tells the producer to wait sending more information until there is space again in the queue. In this case, space is created by the motion thread having processed a trajectory segment.

In other words: space in the queue is created by the robot physically moving through your trajectory.

Then we agree, thanks @gavanderhoorn!

I was just in the lab and managed to send several points resulting in a smooth motion by waiting for a not busy reply. I implemented the same waiting mechanism as in the ros driver.

Send points while moving without BUSY in return