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

[ros-industrial]

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

[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.

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)
.