[Question] Cannot solve "Goal start doesnt match current pose"

[JHeinke]

Hi,

i am facing a new problem since i updatet my Robot and have to use this new driver.
My program works without any problems using the old version of the driver. I just changed the ur5_bringup to use the new one, like you wrote in the Wiki.

The problem in Detail:
I am sending new joint_values to the robot by using the FollowJointTrajectory controller with an goal to reach. With the old version everything was fine. I need a smooth and not stopping movement because i change the TCP position with an frequence of 60Hz with small steps. This worked very well.

When i am starting the robot and my programm, the driver reports "on goal" like with the old driver. But then, when i am starting the movement the robot does not move and the Error "Goal start doesnt match current pose" comes up. Sometime it moves, but only for some steps.
When I use MoveIt and RViz everything works fine, so i think it is something in my code but i cant see where the error could be.

Do i have to use the tool0_controller? I am using the Moveit-Services GetPositionFK and GetPositionIK for actual position and joint values with the chains from base_link to ee_link.

Because this doesnt come up when i used the old driver, i thought you can tell me what changes in my program i have to do or what action has the effect, that this Error comes up. I hope, starting an Issue was okay and that you can give me a hint to solve this problem.

Thank you very much

Julius

[ThomasTimm]

Hi Julius.

The problem is, as stated, that the starting pose in your trajectory is different from the robots current position. Thus, the robot discarts the trajectory as a safety meassure, as it is not able to execute the ordered trajectory.
To avoid this, make sure that the first point in your trajectory has a time_from_start value of 0 and the position is equal to the current position of the robot.

The last thing can be difficult to ensure, especially if the robot is moving fast, so I'm currently considering different alternatives. Please see other thread ( #36 ) for this.

[JHeinke]

Thanks for your answer. I have set the time_from_start value to 0.
The first thing my program does is to load the actual values from /joint_states and transform it into the first goal. This goal is sent to the robot with an constant rate.
I changed my program a little bit so that now two trajectory.points are loaded into the goal variable. The first one does have a time_from_start of 0 (the current joint values), the second one of 0.5 (new values, getting from ik).
As long as i do not give any input to the program the robot stands still and there are no error/warning messages at the beginning and the driver says "on goal". So on start-up there are two points in the trajectory with the same value.

My thought was, when the robot is not on the same position like the driver thinks it is, the error has to be there on start-up even when i am not changing the robots position because there is a goal too, even when this goal is not changing and has the same values as the step before. Am i right? I hope i understand you right.

Is it possible that this error belongs more to #36? Because of the fact that there is sometimes a little movement for the first changes (just one or two steps).

[ThomasTimm]

My thought was, when the robot is not on the same position like the driver thinks it is, the error has to be there on start-up even when i am not changing the robots position because there is a goal too, even when this goal is not changing and has the same values as the step before. Am i right?

The robot is always at the same position as the driver thinks it is. Or almost, the robot send its position to the driver at 125 Hz, so the drivers perception of where the robot is may be up to 8 ms delayed, depending on when you ask the driver for that information. When the driver recieves this information from the robot, it publishes that information to /joint_states

But this is not so relevant, as the driver checks that the starting position you have specified is identical to the position that the driver was told by the controller that the robot is at and thus, that the starting pose is identical to the information last published to /joint_states

From your description I am uncertain on what you are doing exactly. It sounds like you want to continously stream new target positions to the robot, since you're saying that a goal is sent to the robot with a constant rate. If this is the case, I would strongly recommend that you calcualte the jacobian and use the joint_speed interface. This will give you much smoother performance, and the possibility to send new commands at up to 125 Hz.

It also sounds like you just get the actual values from /joint_states once when the program starts, and then reuse these values. You need to update the values each time before sending a new trajectory, otherwise the values will be wrong once the robot starts moving.

So whenever you compute a trajectory, do the following step:

1. Compute IK of your target.
2. build your trajectory
3. wait for a message on /joint_states
4. update starting position based on the new value from /joint_state
5. send your trajectory to the driver

The error you get can also occour when it takes more than 8 ms to get from the transition between step 3 and 4, and to the trajectory is recieved by the driver.
Unfortunately there is no way to gaurentee this wont happen, which is the reason I've started the discussion in #36

[JHeinke]

Thank you very much for your help. I will write a new interface with your tips.

[ghost]

As an fyi, the merge that added this check (#31) looks for a difference
greater than 0.01 radians ~= 0.5 degrees to generate this error. I picked
this value empiracally based on observations of the returned values'
variation. If it is too restrictive in general we can have a discussion
regarding changing this to be a bit more forgiving.

—
Reply to this email directly or view it on GitHub
#35 (comment)
.