Executing motion without starting path at current robot position

[demorise]

I have a couple of lines which I have discretized into waypoints for descartes. For my application, I need to generate a "joint solution" for each line, which I then execute, rather than combining all the lines into a single path. I have also ensured that the end-point of each line coincides with the start-point of the next line since descartes requires that every path begins at the robot's current position. However, I still keep getting errors that my path does not begin at the robot's current position when I try to execute the second line. How can I rectify this? Preferably, is there a way to execute paths without necessarily specifying the robot's current position as the beginning of the path.
Since the JointTrajectory Message specifies where each joint needs to be at each point in time, why can't the robot's joints just move to those positions irrespective of the start-point?

[gavanderhoorn]

Could you please copy-paste the exact error message you see? You may actually be receiving an error message from MoveIt. That was 'recently' (about 5 months ago) extended with a start-state-is-current-state check. Descartes does not have that afaik (it's only a planner, not an executing entity).

Since the JointTrajectory Message specifies where each joint needs to be at each point in time, why can't the robot's joints just move to those positions irrespective of the start-point?

A JointTrajectory only contains the points for the actual trajectory, unless you've added some approach and depart segments.

The reason for this check in MoveIt is that it can be very dangerous for a robot (really: a driver) to be given a trajectory that starts not where it is right now as that would mean it has to (very quickly) move to pt_0 (which will probably be done using joint or linear interpolation, without regards for the environment).

Personally I prefer the driver (or even MoveIt) to refuse to command such potentially dangerous motions, but you can disable the check if you wish.

[gavanderhoorn]

Could I please ask you to not cross-post support requests to ROS Answers? I just came across Executing motion in Descartes without starting path at current robot position there.

At the very least, cross-posting leads to split discussions. Worst, to duplication of effort.

[demorise]

@gavanderhoorn : Apologies for the cross-posting. Thanks for your response.
Here is the error I get:
"Validation failed: Trajectory does not start at current position"
How do I disable this safety check? The distance between the robot's current position and the trajectory start position in my case is probably in microns and should not present any safety issues.
Thanks.

[gavanderhoorn]

Is that the exact error message?

What robot and driver are you using? A motoman by any chance?

I can only find partial matches with that error string, and the motoman_driver package is one candidate (here).

If you are using a Motoman, then this check cannot be disabled and you'll have to make sure the trajectories start at the current pose of the robot (motoros uses incremental/relative motion, so deviation from the start state can lead to a static offset that is not resolved).

---

Edit: there is a situation in which this check is incorrect though, but I would need more information about your setup: physical robot configuration, urdf (which one, content, etc) and MoveIt config your using. See ros-industrial/motoman#111 for some context. If you are using a Motoman, you could see whether ros-industrial/motoman#179 helps. This PR is lagging behind kinetic-devel a bit though.

[demorise]

Hi @gavanderhoorn : You are right. It's "doesn't" and not "does not". I didn't have access to the robot yesterday, so I copied it off my search history where I had unconsciously changed it to "does not". Also, you are right about the robot. It's a Motoman robot. I really appreciate your effort in trying to help with this. Perhaps I should give a more detailed explanation of the issue I'm facing below:

Let's say I have a rectangle I want to trace with the tip of the robot. Rather than discretizing the four lines into points at once solving for a joint solution, I discretize each line at a time, compute a joint solution for each line, then execute the four joint solutions consecutively. The first line always runs successfully because I get the current Cartesian pose of the robot and use it as the start point of the path. For the subsequent lines, I have adjusted my code to ensure that the end point of one line coincides with the start point of the next line. However, I still keep getting the "Validation failed: Trajectory doesn't start at current position" error.

[Jmeyer1292]

@demorise We ran into this on a project with a motoman recently. The robot expects that the start position is within 10 pulse counts (very small value) of the current position. When told to move through a path, the robot fails to stop within 10 pulse counts of the final position. This may be a problem with the driver, a problem with bad time parameterization, or whatever but the end result is that to use the motoman driver practically one must manually adjust your own paths as you execute. Do your own checks that the current /joint_state vector is within some tolerance and then update just the first point of the new trajectory.

[demorise]

@Jmeyer1292 Thanks for your contribution. I have tried solving this by subscribing to the joint_state topic as follows:

ros::Subscriber joint_state_sub = nh.subscribe("/joint_states", 100, &jointStateCallback);

I then replace the positions of the first point of the joint solution i.e. joint_solution.points[0].positions[i] (where i is the index of each joint) with the robot's current joint positions. However, when I do this, I get a "trajectory splicing not supported" error. My guess is that the system is having issues reconciling this new information with the "time_from_start" portion of the joint_solution(JointTrajectory msg).Could you suggest a better way to replace the trajectory start position in the joint space?

[Jmeyer1292]

@demorise That error sounds like the robot driver is getting a new motion while the previous one is still active. I don't think that error has anything to do with the position of a given trajectory.

Make sure that the action has completed before you send a new trajectory and maybe even put a small sleep in between the motions if you can. This might be needed because the standard action server kinda sucks and returns when the robot is within some epsilon of the final position even if the driver is still going.

If you can't dwell because of your process then we'll need to figure out how to time parameterize your path so that the robot stops at corners but is still executing "one" trajectory.

[gavanderhoorn]

(note that the rest of this discussion is off-topic for this issue tracker I believe)

An immediate work-around would be to generate a single trajectory for the four sides of your square and execute that in one go. Descartes supports this scenario afaik.

If that is undesirable, you would probably have to make sure that you generate the next trajectory with a start-state that you retrieved from the appropriate JointState topic after the robot has come to a complete stop. You should be able to use the in_motion field of the RobotStatus message to determine that.

As I wrote in my earlier comment above, motoros uses relative motions, so this check is actually quite important, or the robot won't do what you expect it to.

[gavanderhoorn]

@Jmeyer1292 wrote:

Make sure that the action has completed before you send a new trajectory and maybe even put a small sleep in between the motions if you can. This might be needed because the standard action server kinda sucks and returns when the robot is within some epsilon of the final position even if the driver is still going.

the controller side code actually sends out RobotStatus messages that contain the in_motion field. I believe the OP should be able to use that together with tracking whether he is waiting for a goal to complete to have a reliable estimate of when the robot has completed the trajectory.

[Jmeyer1292]

@gavanderhoorn I don't have any concrete evidence to give you right now, but I've definitely seen that it's sometimes necessary to dwell a moment between when the joint move action returns and when I send the next one. The action listens to that robot status in_motion field.

[gavanderhoorn]

Just remembered that there is actually a ROS node that can check whether a set of joints has 'settled': joint_states_settler. That could possibly help here.

[gavanderhoorn]

@Jmeyer1292 wrote:

I've definitely seen that it's sometimes necessary to dwell a moment between when the joint move action returns and when I send the next one. The action listens to that robot status in_motion field.

I'm not doubting you, but looking at the code it's essentially an &&: within goal tolerances and controller reports it stopped moving (here).

Checking for in-goal-constraints is thresholding, but the controller should be really accurate with the in_motion field, so if that is not working correctly, that would not be good.

What could be happening is that there is still some 'sagging' or similar motion between end-of-motion and where it settles. That could result in the deviation error. But that is speculation.

[demorise]

@Jmeyer1292 Thanks a lot. Now I see.... I also just came across this: ros-industrial/motoman#34 ,which buttresses the point you just made. I might be able to accommodate a sleep of about half a second between the paths. I'll try this and see what happens.

If you can't dwell because of your process then we'll need to figure out how to time parameterize your path so that the robot stops at corners but is still executing "one" trajectory.

I will however like to know more about this if it's not much of a trouble to you.

[demorise]

An immediate work-around would be to generate a single trajectory for the four sides of your square and execute that in one go. Descartes supports this scenario afaik.

@gavanderhoorn You are right. I tried this method and it worked perfectly. It is however less desirable for me because I lose some "non-path" related information associated with the line.