Trajectory start position doesnt match current robot position on CSDA10F dual arm robot

[Danfoa]

Good day,

We have been operating the CSDA10F robot with Moveit GUI interface, and it works properly. Nevertheless when we use the MoveGroupInterface on a c++ Node, we are getting the error:

[ERROR] [1527599320.788737189]: Aborting Trajectory.  Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

It seems that on-the-fly point streaming (#88 and #215 )is a known solution to this problem (synchronization problem as far as I understand) but as mention in PR #88 this solution does not work for dual arm systems.

What seems odd to me is that, while performing planning and motion with Moveit and Rviz GUI, this problem seems not present. Cloud it be possible that we are generating malformed motion request? The file with which we are testing it is this (which by the way works perfectly on a simulated robot, with the demo.launch of our robot's Moveit's package) and basically follows the guidelines from Moveit MoveGroupInterface tutorial.

So the question will be Is it a drivers problem, or syncronization issue ? or are we doing something wrong wuile using the MoveGroupInterface class ?, in order to help clarify the second I am attaching how move below.

On our test Node we defined the move groups:

// Set up move group objects 
  moveit::planning_interface::MoveGroupInterface arm_left_move_group("arm_left");
  arm_left_move_group.allowReplanning(true);    
  moveit::planning_interface::MoveGroupInterface csda10f_move_group("csda10f");
  csda10f_move_group.allowReplanning(true);     

  const robot_state::JointModelGroup *csda10f_joint_model_group = csda10f_move_group.getCurrentState()->getJointModelGroup("csda10f");
  const robot_state::JointModelGroup *arm_left_joint_model_group = arm_left_move_group.getCurrentState()->getJointModelGroup("arm_left");

Set a given pose as target and execute

arm_left_move_group.setPoseTarget(approach_pose);
bool success = (arm_left_move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
arm_left_move_group.execute(my_plan);

Shall we somehow hardcode into the Motion Plan the current robot state? robot_state::RobotState start_state(*arm_left_move_group.getCurrentState());

[gavanderhoorn]

It seems that on-the-fly point streaming (#88 and #215 )is a known solution to this problem (synchronization problem as far as I understand) but as mention in PR #88 this solution does not work for dual arm systems.

I wouldn't say that is a 'known solution' actually. It's just an alternative approach to the trajectory-based interface.

Shall we somehow hardcode into the Motion Plan the current robot state?

You'll indeed need to make sure to update MoveIt's start state before planning so that it coincides with the current state of the robot.

I don't see that anywhere in the example you link, so that is probably why you get the error.

[andreaskoepf]

@Danfoa Dose this happen only for a consecutive move or do you see the error also if you are moving for the first time after the robot was at standstill for some seconds? One problem that we at Xamla identified is that the joint-feedback of the robot is about 100-150ms behind the joint-command position (e.g. there might be a moving average filter inside the motor controller). A problem could be that when you read out the position of the robot directly after a trajectory execution was completed to plan the next segment the read-out position might be not the final position (especially when executing fast moves).

[Danfoa]

Good day @gavanderhoorn

You'll indeed need to make sure to update MoveIt's start state before planning so that it coincides with the current state of the robot.
I don't see that anywhere in the example you link, so that is probably why you get the error.

I though the current joint state was taken as the start pose as default, nevertheless now I hardcoded it before every planning, and also waited after each motion as @andreaskoepf suggested. Thanks by the way.

But the problem is still present and it seems that it only occurs when we try to perform motion of the two arms or the 15DoF of the robot, It seems that planning with this motion groups through the GUI also fails with the same error... has anyone experienced this before?.

PS: Sorry for the late and uncompleted feedback, although we are developing the tests are being performed by other engineer.

[andreaskoepf]

@Danfoa Maybe in order to narrow the problem down a bit it you could enable the telnet debug-connection of your robot and view the MotoPlus driver output which is printed in the error case here which prints the start and the current position. This should give you some hints about which joints are causing the deviation.

To enable telnet debugging set the robot parameter S2C1119 to 2 (to disable the telnet connection set this parametr to 0). The robot must be rebooted after setting this parameter.

Just guessing I could imagine that the shared Base-Joint of the torso could cause a problem.

[Danfoa]

@andreaskoepf Thank you very much

Just guessing I could imagine that the shared Base-Joint of the torso could cause a problem.

We have successfully planned and move the torso move group of the robot, will that discard the this as the source problem ? or it could still be a problem.

I will follow your instructions thank you again.

PS: In one of our early test we checked the joint_state topic to understand if robot state information was coming "correctly", and we were getting recursively joint state of torso_b2, then torso _b1 and then the arms state, and both torso joints seem to follow the "mirror" restriction.

header:
seq: 651
stamp:
secs: 1525344386
nsecs: 756181186
frame_id: ''
name: [torso_joint_b2]
position: [0.5762314200401306]
velocity: []
effort: []
---
header:
seq: 652
stamp:
secs: 1525344386
nsecs: 756250741
frame_id: ''
name: [torso_joint_b1]
position: [0.5762314200401306]
velocity: []
effort: []

[Danfoa]

See #251.