Change first point in trajectory to commanded position

[pazeshun]

Same as RethinkRobotics#77

[pazeshun]

Ping @wkentaro

[pazeshun]

Ping @wkentaro

[wkentaro]

Is this tested with real robot?

[pazeshun]

Yes. With master branch of jsk_apc.

[k-okada]

With this PRj, gazebo simulation is not working, joint_trajectory_action do not generate command because of errors,

$ rosrun baxter_interface joint_trajectory_action_server.py --interpolation minjerk
[ERROR] [WallTime: 1501462702.406211] [2142.279000] /rsdk_position_w_id_joint_trajectory_action_server: Exceeded Error Threshold on left_s1: 0.999433532707
[ERROR] [WallTime: 1501462702.407546] [2142.279000] /rsdk_position_w_id_joint_trajectory_action_server: Exceeded Error Threshold on right_s0: 0.653352615824

If we revert following lines, we can solve the problem.

-            first_trajectory_point.positions = self._get_current_position(joint_names)
+            first_trajectory_point.positions = self._get_commanded_position(joint_names)

Why you have to use comanded_position the first trajectory point??? @pazeshun

[pazeshun]

current_position comes from /robot/joint_states which may be different from the commanded_position.
Please see jsk-ros-pkg/jsk_robot#738 (comment) and RethinkRobotics#77 (comment) for detailed information.

[pazeshun]

I've also heard simulator problem like start-jsk/jsk_apc#2106 (comment).
Perhaps we have to stop using commanded_position...

[k-okada]

thank you for information, Yes I just fixed this problem by writing as same code as start-jsk/jsk_apc#2106 (comment) ...

[k-okada]

A robot in gazebo shaking, but the data is clean, need to check on real robot

rqt_plot -b pyqt /robot/limb/left/joint_command/command[0],/robot/limb/left/joint_command/command[1],/robot/limb/left/joint_command/command[2],/robot/limb/left/joint_command/command[3],/robot/limb/left/joint_command/command[4],/robot/limb/left/joint_command/command[5],/robot/limb/left/joint_command/command[6]  /robot/limb/left/follow_joint_trajectory/goal/goal/trajectory/points[0]/positions[0]  /robot/limb/left/follow_joint_trajectory/goal/goal/trajectory/points[0]/positions[1]  /robot/limb/left/follow_joint_trajectory/goal/goal/trajectory/points[0]/positions[2]  /robot/limb/left/follow_joint_trajectory/goal/goal/trajectory/points[0]/positions[3]  /robot/limb/left/follow_joint_trajectory/goal/goal/trajectory/points[0]/positions[4]  /robot/limb/left/follow_joint_trajectory/goal/goal/trajectory/points[0]/positions[5]  /robot/limb/left/follow_joint_trajectory/goal/goal/trajectory/points[0]/positions[6]