Change first point in trajectory to commanded position

src/baxter_interface/limb.py

@@ -43,6 +43,7 @@
 from baxter_core_msgs.msg import (
     JointCommand,
     EndpointState,
+    SEAJointState,
 )
 from baxter_interface import settings
 
@@ -67,6 +68,7 @@ def __init__(self, limb):
         self._joint_angle = dict()
         self._joint_velocity = dict()
         self._joint_effort = dict()
+        self._joint_commanded_angle = dict()
         self._cartesian_pose = dict()
         self._cartesian_velocity = dict()
         self._cartesian_effort = dict()
@@ -115,6 +117,13 @@ def __init__(self, limb):
             queue_size=1,
             tcp_nodelay=True)
 
+        _sea_joint_state_sub = rospy.Subscriber(
+            ns + 'gravity_compensation_torques',
+            SEAJointState,
+            self._on_sea_joint_state,
+            queue_size=1,
+            tcp_nodelay=True)
+
         err_msg = ("%s limb init failed to get current joint_states "
                    "from %s") % (self.name.capitalize(), joint_state_topic)
         baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
@@ -123,6 +132,10 @@ def __init__(self, limb):
                    "from %s") % (self.name.capitalize(), ns + 'endpoint_state')
         baxter_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
                                  timeout_msg=err_msg)
+        err_msg = ("%s limb init failed to get current SEA joint state"
+                   "from %s") % (self.name.capitalize(), ns + 'gravity_compensation_torques')
+        baxter_dataflow.wait_for(lambda: len(self._joint_commanded_angle.keys()) > 0,
+                                 timeout_msg=err_msg)
 
     def _on_joint_states(self, msg):
         for idx, name in enumerate(msg.name):
@@ -174,6 +187,11 @@ def _on_endpoint_states(self, msg):
             ),
         }
 
+    def _on_sea_joint_state(self, msg):
+        for idx, name in enumerate(msg.name):
+            if name in self._joint_names[self.name]:
+                self._joint_commanded_angle[name] = msg.commanded_position[idx]
+
     def joint_names(self):
         """
         Return the names of the joints for the specified limb.
@@ -292,6 +310,26 @@ def endpoint_effort(self):
         """
         return deepcopy(self._cartesian_effort)
 
+    def joint_commanded_angle(self, joint):
+        """
+        Return the requested joint commanded angle.
+
+        @type joint: str
+        @param joint: name of a joint
+        @rtype: float
+        @return: commanded angle in radians of individual joint
+        """
+        return self._joint_commanded_angle[joint]
+
+    def joint_commanded_angles(self):
+        """
+        Return all joint angles.
+
+        @rtype: dict({str:float})
+        @return: unordered dict of joint name Keys to commanded angle (rad) Values
+        """
+        return deepcopy(self._joint_commanded_angle)
+
     def set_command_timeout(self, timeout):
         """
         Set the timeout in seconds for the joint controller

src/joint_trajectory_action/joint_trajectory_action.py

@@ -194,6 +194,9 @@ def _get_current_error(self, joint_names, set_point):
         error = map(operator.sub, set_point, current)
         return zip(joint_names, error)
 
+    def _get_commanded_position(self, joint_names):
+        return [self._limb.joint_commanded_angle(joint) for joint in joint_names]
+
     def _update_feedback(self, cmd_point, jnt_names, cur_time):
         self._fdbk.header.stamp = rospy.Duration.from_sec(rospy.get_time())
         self._fdbk.joint_names = jnt_names
@@ -361,10 +364,10 @@ def _on_trajectory_action(self, goal):
 
         dimensions_dict = self._determine_dimensions(trajectory_points)
 
-        if num_points >= 1 and trajectory_points[0].time_from_start.to_sec() > 0:
+        if trajectory_points[0].time_from_start.to_sec() > 0: # it already retuned when num_points == 0 (see 10 lines above)
             # Add current position as trajectory point
             first_trajectory_point = JointTrajectoryPoint()
-            first_trajectory_point.positions = self._get_current_position(joint_names)
+            first_trajectory_point.positions = self._get_commanded_position(joint_names)
             # To preserve desired velocities and accelerations, copy them to the first
             # trajectory point if the trajectory is only 1 point.
             if dimensions_dict['velocities']: