Change first point in trajectory to commanded position

[pazeshun]

MERGE AFTER #74 #79

What is problem

Please see jsk-ros-pkg/jsk_robot#738 (comment)

What changed

• Subscribe /robot/limb/<left|right>/gravity_compensation_torques to get current commanded joint position. This position is changed even if Baxter's arm is moved manually.
• If time_from_start of first trajectory point isn't zero, add current commanded position as the first point instead of current joint position in case of commanded position and current position are too different.

cc @k-okada

[k-okada]

Nice!, can you show the output of gravity_compensation_torques ?

[pazeshun]

$ rostopic echo -n 1 /robot/limb/left/gravity_compensation_torques 
header: 
  seq: 333628316
  stamp: 
    secs: 1481007370
    nsecs: 769355443
  frame_id: base
name: ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
commanded_position: [0.34906585039886573, -0.4363323129985821, -0.6981317007977315, 1.0471975511965974, -0.34906585039886573, 1.396263401595463, 0.0]
commanded_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
commanded_acceleration: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
commanded_effort: [2.1215947928763472, -4.612676301081012, -4.572281475900699, -2.1592677553024644, -2.4708107181131593, 2.154565669341879, -0.1935289545702207]
actual_position: [0.3497476196378743, -0.43641753415340023, -0.6979612584878633, 1.0461748973378522, -0.34898062924393164, 1.3959225169757266, 0.0003834951969713534]
actual_velocity: [-0.008246680714126275, -0.007461282550876154, -0.0019634954081253035, -0.006675884387626033, -0.029845130203504617, 0.003141592653000486, 0.007853981632501214]
actual_effort: [2.056, -19.4, -15.212, -10.612, -2.984, 1.78, -0.184]
gravity_model_effort: [0.020563893770772886, -14.972127630236647, -10.552536250672896, -9.20733996340377, -1.4790414281200428, 0.6206779708197883, -0.02314250185353865]
gravity_only: [0.020563893770772886, -44.02642020093964, -10.552536250672896, -9.20733996340377, -1.4790414281200428, 0.6206779708197883, -0.02314250185353865]
hysteresis_model_effort: [0.0, -29.87281036376953, 0.0, 0.0, 0.0, 0.0, 0.0]
crosstalk_model_effort: [0.0, -0.8185177930665389, 0.0, 0.0, 0.0, 0.0, 0.0]
hystState: -0.015
---
$ rostopic hz /robot/limb/left/gravity_compensation_torques 
subscribed to [/robot/limb/left/gravity_compensation_torques]
average rate: 94.400
	min: 0.010s max: 0.011s std dev: 0.00048s window: 94
^Caverage rate: 94.450
	min: 0.000s max: 0.022s std dev: 0.00373s window: 144
$ rostopic info /robot/limb/left/gravity_compensation_torques 
Type: baxter_core_msgs/SEAJointState

Publishers: 
 * /realtime_loop

Subscribers: 
 * /collision/left/joint_collision_estimator_left
 * /rcloader_left
 * /rcloader_right

[pazeshun]

It seems that /robot/joint_states and actual_position of gravity_compensation_torques is the same.

[pazeshun]

gravity_compensation_torques is published even if gravity compensation is off.