Browse Source

New joint_arm in stretch_driver action server

pull/89/head
Binit Shah 1 year ago
parent
commit
fe4b4a6e62
3 changed files with 39 additions and 18 deletions
  1. +3
    -3
      hello_helpers/README.md
  2. +31
    -12
      stretch_core/nodes/command_groups.py
  3. +5
    -3
      stretch_core/nodes/stretch_driver

+ 3
- 3
hello_helpers/README.md View File

@ -69,16 +69,16 @@ self.move_to_pose({'joint_lift': 0.5})
Internally, this dictionary is converted into a [FollowJointTrajectoryGoal](http://docs.ros.org/en/diamondback/api/control_msgs/html/msg/FollowJointTrajectoryGoal.html) that is sent to a [FollowJointTrajectory action](http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html) server in stretch_driver. This method waits by default for the server to report that the goal has completed executing. However, you can return before the goal has completed by setting the `return_before_done` argument to True. This can be useful for preempting goals.
There are two additional arguments that enable you to customize how the pose is executed. If you set `custom_contact_thresholds` to True, this method expects a different format dictionary: string/tuple key/value pairs, where the keys are still joint names, but the values are `(position_goal, effort_threshold)`. The addition of a effort threshold enables you to detect when a joint has made contact with something in the environment, which is useful for manipulation or safe movements. For example, `{'wrist_extension': (0.5, 20)}` commands the telescoping arm fully out (the arm is nearly fully extended at 0.5 meters) but with a low enough effort threshold (20% of the arm motor's max effort) that the motor will stop when the end of arm has made contact with something. Again, in a node, this would look like:
There are two additional arguments that enable you to customize how the pose is executed. If you set `custom_contact_thresholds` to True, this method expects a different format dictionary: string/tuple key/value pairs, where the keys are still joint names, but the values are `(position_goal, effort_threshold)`. The addition of a effort threshold enables you to detect when a joint has made contact with something in the environment, which is useful for manipulation or safe movements. For example, `{'joint_arm': (0.5, 20)}` commands the telescoping arm fully out (the arm is nearly fully extended at 0.5 meters) but with a low enough effort threshold (20% of the arm motor's max effort) that the motor will stop when the end of arm has made contact with something. Again, in a node, this would look like:
```python
self.move_to_pose({'wrist_extension': (0.5, 40)}, custom_contact_thresholds=True)
self.move_to_pose({'joint_arm': (0.5, 40)}, custom_contact_thresholds=True)
```
If you set `custom_full_goal` to True, the dictionary format is string/tuple key/value pairs, where keys are joint names again, but values are `(position_goal, velocity, acceleration, effort_threshold)`. The velocity and acceleration components allow you to customize the trajectory profile the joint follows while moving to the goal position. In the following example, the arm telescopes out slowly until contact is detected:
```python
self.move_to_pose({'wrist_extension': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True)
self.move_to_pose({'joint_arm': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True)
```
##### `get_robot_floor_pose_xya(floor_frame='odom')`

+ 31
- 12
stretch_core/nodes/command_groups.py View File

@ -247,9 +247,11 @@ class GripperCommandGroup(SimpleCommandGroup):
class ArmCommandGroup(SimpleCommandGroup):
def __init__(self, range_m=None, calibrated_retracted_offset_m=None, node=None):
SimpleCommandGroup.__init__(self, 'wrist_extension', range_m, acceptable_joint_error=0.008, node=node)
SimpleCommandGroup.__init__(self, 'joint_arm', range_m, acceptable_joint_error=0.008, node=node)
self.telescoping_joints = ['joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
self.is_telescoping = False
self.wrist_extension_name = 'wrist_extension'
self.is_named_wrist_extension = False
if calibrated_retracted_offset_m is None:
calibrated_retracted_offset_m = node.controller_parameters['arm_retracted_offset']
self.retracted_offset_m = calibrated_retracted_offset_m
@ -278,16 +280,26 @@ class ArmCommandGroup(SimpleCommandGroup):
self.is_telescoping = False
self.index = None
active_telescoping_joint_names = list(set(commanded_joint_names) & set(self.telescoping_joints))
if self.name in commanded_joint_names:
if len(active_telescoping_joint_names) == 0:
self.index = commanded_joint_names.index(self.name)
self.active = True
else:
err_str = ("Received a command for the wrist_extension joint and one or more telescoping_joints. "
if self.name in commanded_joint_names or self.wrist_extension_name in commanded_joint_names:
if self.name in commanded_joint_names and self.wrist_extension_name in commanded_joint_names:
err_str = ("Received a command for the joint_arm and wrist_extension joints. "
"These are mutually exclusive options. The joint names in the received command = "
"{0}").format(commanded_joint_names)
invalid_joints_callback(err_str)
return False
if len(active_telescoping_joint_names) != 0:
err_str = ("Received a command for the joint_arm/wrist_extension joint and one or more telescoping_joints. "
"These are mutually exclusive options. The joint names in the received command = "
"{0}").format(commanded_joint_names)
invalid_joints_callback(err_str)
return False
if self.name in commanded_joint_names:
self.index = commanded_joint_names.index(self.name)
self.active = True
else:
self.index = commanded_joint_names.index(self.wrist_extension_name)
self.is_named_wrist_extension = True
self.active = True
elif len(active_telescoping_joint_names) != 0:
if len(active_telescoping_joint_names) == len(self.telescoping_joints):
self.active = True
@ -328,14 +340,14 @@ class ArmCommandGroup(SimpleCommandGroup):
if goal_pos is None:
err_str = ("Received goal point with positions array length={0}. "
"This joint ({1})'s index is {2}. Length of array must cover all joints listed "
"in commanded_joint_names.").format(len(point.positions), self.name, self.index)
"in commanded_joint_names.").format(len(point.positions), self.name if not self.is_named_wrist_extension else self.wrist_extension_name, self.index)
invalid_goal_callback(err_str)
return False
self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
if self.goal['position'] is None:
err_str = ("Received {0} goal point that is out of bounds. "
"Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
"Range = {1}, but goal point = {2}.").format(self.name if not self.is_named_wrist_extension else self.wrist_extension_name, self.range, goal_pos)
invalid_goal_callback(err_str)
return False
@ -343,7 +355,7 @@ class ArmCommandGroup(SimpleCommandGroup):
def init_execution(self, robot, robot_status, **kwargs):
if self.active:
_, extension_error_m = self.update_execution(robot_status)
_, extension_error_m = self.update_execution(robot_status, force_single=True)
robot.arm.move_by(extension_error_m,
v_m=self.goal['velocity'],
a_m=self.goal['acceleration'],
@ -354,15 +366,22 @@ class ArmCommandGroup(SimpleCommandGroup):
def update_execution(self, robot_status, **kwargs):
success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None
force_single = kwargs['force_single'] if 'force_single' in kwargs.keys() else False
self.error = None
if self.active:
if success_callback and robot_status['arm']['motor']['in_guarded_event']:
success_callback("{0} contact detected.".format(self.name))
success_callback("{0} contact detected.".format(self.name if not self.is_named_wrist_extension else self.wrist_extension_name))
return True
arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0
extension_current = robot_status['arm']['pos'] + arm_backlash_correction
self.error = self.goal['position'] - extension_current
return (self.telescoping_joints, self.error) if self.is_telescoping else (self.name, self.error)
if force_single:
return self.name, self.error
else:
if self.is_telescoping:
return [(telescoping_joint, self.error / len(self.telescoping_joints)) for telescoping_joint in self.telescoping_joints]
else:
return [(self.name, self.error), (self.wrist_extension_name, self.error)]
return None

+ 5
- 3
stretch_core/nodes/stretch_driver View File

@ -189,7 +189,7 @@ class StretchDriverNode:
joint_state.velocity.append(vel)
joint_state.effort.append(effort)
# add telescoping joints to joint state
# add telescoping joints and wrist_extension to joint state
arm_cg = self.joint_trajectory_action.arm_cg
joint_state.name.extend(arm_cg.telescoping_joints)
pos, vel, effort = arm_cg.joint_state(robot_status)
@ -197,6 +197,10 @@ class StretchDriverNode:
joint_state.position.append(pos / len(arm_cg.telescoping_joints))
joint_state.velocity.append(vel / len(arm_cg.telescoping_joints))
joint_state.effort.append(effort)
joint_state.name.append(arm_cg.wrist_extension_name)
joint_state.position.append(pos)
joint_state.velocity.append(vel)
joint_state.effort.append(effort)
# add gripper joints to joint state
gripper_cg = self.joint_trajectory_action.gripper_cg
@ -453,8 +457,6 @@ class StretchDriverNode:
self.linear_velocity_mps = 0.0 # m/s ROS SI standard for cmd_vel (REP 103)
self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)
self.max_arm_height = 1.1
self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self.power_pub = rospy.Publisher('battery', BatteryState, queue_size=1)

Loading…
Cancel
Save