Copying current pose to joint trajectory doesn't always work

[shaun-edwards]

When executing a joint trajectory, the first pose is copied from the current robot state here. If the robot state has not been updated via the joint_states message since the last IK call, then the wrong joint pose is captured.

[shaun-edwards]

@geoffreychiou, could you take a look at this one next week?

[geoffreychiou]

@shaun-edwards

I'm trying to write the unit-test for this.

Lets say i'm running this:

  robot.addTrajPoint(TRAJECTORY_NAME, "home",      0.5);
  robot.addTrajPoint(TRAJECTORY_NAME, "waypoint1", 1.0);
  robot.addTrajPoint(TRAJECTORY_NAME, "tf_pub1",   2.0);
  robot.addTrajPoint(TRAJECTORY_NAME, "waypoint2", 3.0);
  robot.addTrajPoint(TRAJECTORY_NAME, "waypoint3", 4.0);

  robot.execute(TRAJECTORY_NAME);
  robot.execute(TRAJECTORY_NAME);

The expected behavior is:

// Robot Joints @ [0 0 0 0 0 0] (home)
  robot.execute(TRAJECTORY_NAME);
  {
    // Robot Joints Start @ [0 0 0 0 0 0] (home)
    // Robot runs through trajectory
    // Robot Joints End @ [waypoint3]
  }
// Robot Joints @ [waypoint3]
  robot.execute(TRAJECTORY_NAME);
  {
    // Robot Joints Start @ [waypoint3]
    // Robot runs through trajectory
    // Robot Joints End @ [waypoint3]
  }

Instead, the robot state is not updated all the time so the second execution might start from home instead of waypoint3?

[geoffreychiou]

Here is debugging output without any fix:

I ran execute 4 times and published the joint values before and after each execution. On the lines where there are two joints in a row, I just published the joint values twice and you will notice that it is the correct value on the second one.

My fix was to add the first two lines to the code below:

  ros::spinOnce();
  ros::Duration(0.100).sleep();

  robot_state_->copyJointGroupPositions(joint_group_->getName(), current_joint_position);
  points.push_back(toJointTrajPtMsg(current_joint_position, 0.0));

Here are the results, I ran the test a couple of times and they stay the same.

[geoffreychiou]

Found a bug where it still fails like 1/10 times, gonna try and fix this later.

[shaun-edwards]

The proposed fix:

  ros::spinOnce();
  ros::Duration(0.100).sleep();

  robot_state_->copyJointGroupPositions(joint_group_->getName(), current_joint_position);
  points.push_back(toJointTrajPtMsg(current_joint_position, 0.0));

simply uses a delay to avoid the problem. However, this is not a good fix since it requires joint updates to happen faster than the sleep delay. This can't be guaranteed.

The root cause of this bug has to do with multiple threads accessing the same memory (and being blocked from updating the Robot state with the current pose). The more robust fix is to address this logical issue by using multiple variables, one to hold the robot state for IK solutions and one to hold the current robot position. This way, each can be updated by their respective threads, with not chance of being blocked, except in the rare case where their values need be synced.

[shaun-edwards]

@nsnitish, please address this issue next. Read through the discussion of @geoffreychiou work.

Pay attention to may last comment:

The root cause of this bug has to do with multiple threads accessing the same memory (and being blocked from updating the Robot state with the current pose). The more robust fix is to address this logical issue by using multiple variables, one to hold the robot state for IK solutions and one to hold the current robot position. This way, each can be updated by their respective threads, with not chance of being blocked, except in the rare case where their values need be synced.