From 2684437f4367f72b62d6601393f07913305a8ecc Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 26 Sep 2016 20:48:39 +0900 Subject: [PATCH 1/3] it already retuned when num_points == 0 --- src/joint_trajectory_action/joint_trajectory_action.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/joint_trajectory_action/joint_trajectory_action.py b/src/joint_trajectory_action/joint_trajectory_action.py index 4b0366a..4d03eee 100644 --- a/src/joint_trajectory_action/joint_trajectory_action.py +++ b/src/joint_trajectory_action/joint_trajectory_action.py @@ -361,7 +361,7 @@ 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) From edefa2c63a4a4a4382de2711ae0101ecf93e9757 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 6 Dec 2016 13:32:35 +0900 Subject: [PATCH 2/3] Subscribe gravity_compensation_torques topic and get commanded joint angle --- src/baxter_interface/limb.py | 38 ++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/src/baxter_interface/limb.py b/src/baxter_interface/limb.py index 0201c3d..2464b28 100644 --- a/src/baxter_interface/limb.py +++ b/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 From 5a8767dbe49b3ce0d52c848139efb9f9729234ea Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 6 Dec 2016 13:37:18 +0900 Subject: [PATCH 3/3] Change first point in trajectory to commanded position --- src/joint_trajectory_action/joint_trajectory_action.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/joint_trajectory_action/joint_trajectory_action.py b/src/joint_trajectory_action/joint_trajectory_action.py index 4d03eee..4f064f2 100644 --- a/src/joint_trajectory_action/joint_trajectory_action.py +++ b/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 @@ -364,7 +367,7 @@ def _on_trajectory_action(self, goal): 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']: