From 7b4ffb51428da624762397a6d5328a521b37b110 Mon Sep 17 00:00:00 2001 From: Ian McMahon Date: Mon, 16 Nov 2015 23:43:51 -0500 Subject: [PATCH 1/4] 1.2.0 version bump --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index bc5d9fa..8944f13 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ baxter_interface - 1.1.1 + 1.2.0 Convenient python interface classes for control of the Baxter Research Robot from Rethink Robotics. From 84d72ab4f1cd4adc80c87e6330c1cba40709fa20 Mon Sep 17 00:00:00 2001 From: Ian McMahon Date: Mon, 7 Dec 2015 16:38:01 -0500 Subject: [PATCH 2/4] Re #65 Prevent JTAS execution when Re-Enabled This fix ensures that the Trajectory callback of the JTAS exits whenever the robot has been disabled. This results in the robot holding its current pose when it is re-enabled, rather than attempting to return to the originally commanded location. --- .../joint_trajectory_action.py | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/joint_trajectory_action/joint_trajectory_action.py b/src/joint_trajectory_action/joint_trajectory_action.py index 04f101e..99b2171 100644 --- a/src/joint_trajectory_action/joint_trajectory_action.py +++ b/src/joint_trajectory_action/joint_trajectory_action.py @@ -70,6 +70,7 @@ def __init__(self, limb, reconfig_server, rate=100.0, auto_start=False) self._action_name = rospy.get_name() self._limb = baxter_interface.Limb(limb) + self._enable = baxter_interface.RobotEnable() self._name = limb self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb,)) self._cuff.state_changed.connect(self._cuff_cb) @@ -120,6 +121,9 @@ def __init__(self, limb, reconfig_server, rate=100.0, tcp_nodelay=True, queue_size=1) + def robot_is_enabled(self): + return self._enable.state().enabled + def clean_shutdown(self): self._alive = False self._limb.exit_control_mode() @@ -225,7 +229,8 @@ def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): if self._mode == 'velocity': velocities = [0.0] * len(joint_names) cmd = dict(zip(joint_names, velocities)) - while (not self._server.is_new_goal_available() and self._alive): + while (not self._server.is_new_goal_available() and self._alive + and self.robot_is_enabled()): self._limb.set_joint_velocities(cmd) if self._cuff_state: self._limb.exit_control_mode() @@ -240,7 +245,8 @@ def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): pnt.velocities = [0.0] * len(joint_names) if dimensions_dict['accelerations']: pnt.accelerations = [0.0] * len(joint_names) - while (not self._server.is_new_goal_available() and self._alive): + while (not self._server.is_new_goal_available() and self._alive + and self.robot_is_enabled()): self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode) # zero inverse dynamics feedforward command if self._mode == 'position_w_id': @@ -253,7 +259,7 @@ def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict): rospy.sleep(1.0 / self._control_rate) def _command_joints(self, joint_names, point, start_time, dimensions_dict): - if self._server.is_preempt_requested(): + if self._server.is_preempt_requested() or not self.robot_is_enabled(): rospy.loginfo("%s: Trajectory Preempted" % (self._action_name,)) self._server.set_preempted() self._command_stop(joint_names, self._limb.joint_angles(), start_time, dimensions_dict) @@ -261,8 +267,8 @@ def _command_joints(self, joint_names, point, start_time, dimensions_dict): velocities = [] deltas = self._get_current_error(joint_names, point.positions) for delta in deltas: - if (math.fabs(delta[1]) >= self._path_thresh[delta[0]] - and self._path_thresh[delta[0]] >= 0.0): + if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]] + and self._path_thresh[delta[0]] >= 0.0)) or not self.robot_is_enabled(): rospy.logerr("%s: Exceeded Error Threshold on %s: %s" % (self._action_name, delta[0], str(delta[1]),)) self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED @@ -406,7 +412,8 @@ def _on_trajectory_action(self, goal): # Keep track of current indices for spline segment generation now_from_start = rospy.get_time() - start_time end_time = trajectory_points[-1].time_from_start.to_sec() - while (now_from_start < end_time and not rospy.is_shutdown()): + while (now_from_start < end_time and not rospy.is_shutdown() and + self.robot_is_enabled()): #Acquire Mutex now = rospy.get_time() now_from_start = now - start_time @@ -451,7 +458,7 @@ def check_goal_state(): return True while (now_from_start < (last_time + self._goal_time) - and not rospy.is_shutdown()): + and not rospy.is_shutdown() and self.robot_is_enabled()): if not self._command_joints(joint_names, last, start_time, dimensions_dict): return now_from_start = rospy.get_time() - start_time From 7f4f28f2b46b371fd68d6d8facab58f6cefdcdb8 Mon Sep 17 00:00:00 2001 From: Ian McMahon Date: Wed, 16 Dec 2015 16:42:47 -0500 Subject: [PATCH 3/4] Fix Limb interf to ignore extra left/right states Prior to this commit, the on_joint_states callback would look for any "left" or "right" string and populate its joint states with that joint. This commit now limits the search to only look for joints that are previously know to exist in the Baxter arm. --- src/baxter_interface/limb.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/baxter_interface/limb.py b/src/baxter_interface/limb.py index 0834a6e..0201c3d 100644 --- a/src/baxter_interface/limb.py +++ b/src/baxter_interface/limb.py @@ -126,7 +126,7 @@ def __init__(self, limb): def _on_joint_states(self, msg): for idx, name in enumerate(msg.name): - if self.name in name: + if name in self._joint_names[self.name]: self._joint_angle[name] = msg.position[idx] self._joint_velocity[name] = msg.velocity[idx] self._joint_effort[name] = msg.effort[idx] From fb84083471f4dbcf9c146ff0731e29f300cf5246 Mon Sep 17 00:00:00 2001 From: Ian McMahon Date: Mon, 21 Dec 2015 13:28:47 -0500 Subject: [PATCH 4/4] Updated CHANGELOG.rst for Release 1.2.0 --- CHANGELOG.rst | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 258c456..f3f8606 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,3 +1,16 @@ +1.2.0 (2015-12-21) +--------------------------------- +- Added an optional parameter to Limb interface's move_to_joint_positions() to allow it to be aborted by test function + Thanks to Yoan Mollard (ymollard) for submitting this pull request! +- Added a wait for the endpoint_state messages to be received before exiting Limb init +- Fixed a bug in the JTAS that would cause the robot to jump back into the last commanded pose when the + robot is Disabled/Re-enabled +- Fixed a bug that would cause the Limb's on_joint_states method to fail if extra joints are added to the + robot's /robot/joint_states +- Due to baxter_core_msgs change, updated EndEffectorProperties CUSTOM_GRIPPER to PASSIVE_GRIPPER +- Due to baxter_core_msgs change, updated head interface's speed range from [0, 100] to [0, 1.0] +- Due to baxter_core_msgs change, updated Navigator interface to use update Nav topics and lights msg field + 1.1.1 (2015-5-15) --------------------------------- - Fixed a bug that caused the JTAS to error with a path of one or two points is supplied as a trajectory