diff --git a/hello_helpers/README.md b/hello_helpers/README.md index 71bcbb7..c403cc2 100644 --- a/hello_helpers/README.md +++ b/hello_helpers/README.md @@ -14,14 +14,144 @@ ## Typical Usage -``` +```python import hello_helpers.fit_plane as fp ``` +```python +import hello_helpers.hello_misc as hm +``` +```python +import hello_helpers.hello_ros_viz as hr ``` + +# API + +## Classes + +### [HelloNode](./src/hello_helpers/hello_misc.py) + +This class is a convenience class for creating a ROS node for Stretch. The most common way to use this class is to extend it. In your extending class, the main funcion would call `HelloNode`'s main function. This would look like: + +```python +import hello_helpers.hello_misc as hm + +class MyNode(hm.HelloNode): + def __init__(self): + hm.HelloNode.__init__(self) + + def main(self): + hm.HelloNode.main(self, 'my_node', 'my_node', wait_for_first_pointcloud=False) + # my_node's main logic goes here + +node = MyNode() +node.main() +``` + +There is also a one-liner class method for instantiating a `HelloNode` for easy prototyping. One example where this is handy is sending pose commands from iPython: + +```python +# roslaunch the stretch launch file beforehand + import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp') +temp.move_to_pose({'joint_lift': 0.4}) ``` + +#### Methods + +##### `move_to_pose(pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False)` + +This method takes in a dictionary that describes a desired pose for the robot and communicates with [stretch_driver](../stretch_core/README.md#stretchdrivernodesstretchdriver) to execute it. The basic format of this dictionary is string/number key/value pairs, where the keys are joint names and the values are desired position goals. For example, `{'joint_lift': 0.5}` would put the lift at 0.5m in its joint range. A full list of command-able joints is published to the `/stretch/joint_states` topic. Used within a node extending `HelloNode`, calling this method would look like: + +```python +self.move_to_pose({'joint_lift': 0.5}) ``` -import hello_helpers.hello_ros_viz as hr + +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, `{'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({'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({'joint_arm': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True) +``` + +##### `get_robot_floor_pose_xya(floor_frame='odom')` + +Returns the current estimated x, y position and angle of the robot on the floor. This is typically called with respect to the odom frame or the map frame. x and y are in meters and the angle is in radians. + +##### `main(node_name, node_topic_namespace, wait_for_first_pointcloud=True)` + +When extending the `HelloNode` class, call this method at the very beginning of your `main()` method. This method handles setting up a few ROS components, including registering the node with the ROS server, creating a TF listener, creating a [FollowJointTrajectory](http://docs.ros.org/en/noetic/api/control_msgs/html/action/FollowJointTrajectory.html) client for the [`move_to_pose()`](#movetoposepose-returnbeforedonefalse-customcontactthresholdsfalse-customfullgoalfalse) method, subscribing to depth camera point cloud topic, and connecting to the quick-stop service. + +Since it takes up to 30 seconds for the head camera to start streaming data, the `wait_for_first_pointcloud` argument will get the node to wait until it has seen camera data, which is helpful if your node is processing camera data. + +##### `quick_create(name, wait_for_first_pointcloud=False)` + +A class level method for quick testing. This allows you to avoid having to extend `HelloNode` to use it. + +```python +# roslaunch the stretch launch file beforehand + +import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp') +temp.move_to_pose({'joint_lift': 0.4}) +``` + +#### Subscribed Topics + +##### /camera/depth/color/points ([sensor_msgs/PointCloud2](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/PointCloud2.html)) + +Provides a point cloud as currently seen by the Realsense depth camera in Stretch's head. Accessible from the `self.point_cloud` attribute. + +```python +# roslaunch the stretch launch file beforehand + +import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp', wait_for_first_pointcloud=True) +print(temp.point_cloud) +``` + +#### Subscribed Services + +##### /stop_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html)) + +Provides a service to quickly stop any motion currently executing on the robot. + +```python +# roslaunch the stretch launch file beforehand + +from std_srvs.srv import TriggerRequest +import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp') +temp.stop_the_robot_service(TriggerRequest()) +``` + +#### Other + +##### TF Listener + +Provides a [tf2](http://wiki.ros.org/tf2) listener that buffers [transforms](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html) under the `self.tf2_buffer` attribute. For example, we can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using: + +```python +# roslaunch the stretch launch file beforehand + +import rospy +import hello_helpers.hello_misc as hm +temp = hm.HelloNode.quick_create('temp') +rate = rospy.Rate(10.0) +while True: + try: + t = temp.tf2_buffer.lookup_transform('link_grasp_center', 'base_link', rospy.Time()) + print(t.transform.translation, t.transform.rotation) + except: + continue + rate.sleep() ``` ## License diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index cbe67fb..3def40e 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -5,6 +5,7 @@ import os import sys import glob import math +import numbers import rospy import tf2_ros @@ -69,40 +70,56 @@ class HelloNode: self.joint_state = None self.point_cloud = None + @classmethod + def quick_create(cls, name, wait_for_first_pointcloud=False): + i = cls() + i.main(name, name, wait_for_first_pointcloud) + return i + def joint_states_callback(self, joint_state): self.joint_state = joint_state def point_cloud_callback(self, point_cloud): self.point_cloud = point_cloud - def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False): - joint_names = [key for key in pose] + def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False): point = JointTrajectoryPoint() point.time_from_start = rospy.Duration(0.0) - trajectory_goal = FollowJointTrajectoryGoal() trajectory_goal.goal_time_tolerance = rospy.Time(1.0) - trajectory_goal.trajectory.joint_names = joint_names - if not custom_contact_thresholds: - joint_positions = [pose[key] for key in joint_names] - point.positions = joint_positions - trajectory_goal.trajectory.points = [point] + trajectory_goal.trajectory.header.stamp = rospy.Time.now() + trajectory_goal.trajectory.joint_names = list(pose.keys()) + trajectory_goal.trajectory.points = [point] + + # populate goal + if custom_full_goal: + pose_correct = all([len(joint_goal)==4 for joint_goal in pose.values()]) + if not pose_correct: + rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The 'custom_full_goal' option requires tuple with 4 values (pose_target, velocity, acceleration, contact_threshold_effort) for each joint name, but pose = {pose}") + return + point.positions = [joint_goal[0] for joint_goal in pose.values()] + point.velocities = [joint_goal[1] for joint_goal in pose.values()] + point.accelerations = [joint_goal[2] for joint_goal in pose.values()] + point.effort = [joint_goal[3] for joint_goal in pose.values()] + elif custom_contact_thresholds: + pose_correct = all([len(joint_goal)==2 for joint_goal in pose.values()]) + if not pose_correct: + rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The 'custom_contact_thresholds' option requires tuple with 2 values (pose_target, contact_threshold_effort) for each joint name, but pose = {pose}") + return + point.positions = [joint_goal[0] for joint_goal in pose.values()] + point.effort = [joint_goal[1] for joint_goal in pose.values()] else: - pose_correct = all([len(pose[key])==2 for key in joint_names]) + pose_correct = all([isinstance(joint_position, numbers.Real) for joint_position in pose.values()]) if not pose_correct: - rospy.logerr("HelloNode.move_to_pose: Not sending trajectory due to improper pose. custom_contact_thresholds requires 2 values (pose_target, contact_threshold_effort) for each joint name, but pose = {0}".format(pose)) + rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The default option requires 1 value, pose_target as integer, for each joint name, but pose = {pose}") return - joint_positions = [pose[key][0] for key in joint_names] - joint_efforts = [pose[key][1] for key in joint_names] - point.positions = joint_positions - point.effort = joint_efforts - trajectory_goal.trajectory.points = [point] - trajectory_goal.trajectory.header.stamp = rospy.Time.now() + point.positions = [joint_position for joint_position in pose.values()] + + # send goal self.trajectory_client.send_goal(trajectory_goal) - if not return_before_done: + if not return_before_done: self.trajectory_client.wait_for_result() - #print('Received the following result:') - #print(self.trajectory_client.get_result()) + rospy.logdebug(f"{self.node_name}'s HelloNode.move_to_pose: got result {self.trajectory_client.get_result()}") def get_robot_floor_pose_xya(self, floor_frame='odom'): # Returns the current estimated x, y position and angle of the diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index d949c85..4a86d00 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -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 diff --git a/stretch_core/nodes/detect_aruco_markers b/stretch_core/nodes/detect_aruco_markers index fe56de1..d7cc399 100755 --- a/stretch_core/nodes/detect_aruco_markers +++ b/stretch_core/nodes/detect_aruco_markers @@ -511,8 +511,8 @@ class ArucoMarkerCollection: self.show_debug_images = show_debug_images self.marker_info = marker_info - self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) - self.aruco_detection_parameters = aruco.DetectorParameters_create() + self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250) + self.aruco_detection_parameters = aruco.DetectorParameters() # Apparently available in OpenCV 3.4.1, but not OpenCV 3.2.0. self.aruco_detection_parameters.cornerRefinementMethod = aruco.CORNER_REFINE_SUBPIX self.aruco_detection_parameters.cornerRefinementWinSize = 2 diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 8273290..5a9a99e 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -77,7 +77,7 @@ class JointTrajectoryAction: num_valid_points = sum([c.get_num_valid_commands() for c in self.command_groups]) if num_valid_points <= 0: - err_str = ("Received a command without any valid joint names." + err_str = ("Received a command without any valid joint names. " "Received joint names = {0}").format(commanded_joint_names) self.invalid_joints_callback(err_str) self.node.robot_mode_rwlock.release_read() diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 145093a..65897ba 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -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) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index d8146e5..8c8ea3f 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -119,103 +119,106 @@ class GraspObjectNode(hm.HelloNode): self.look_at_surface(scan_time_s = 3.0) grasp_target = self.manipulation_view.get_grasp_target(self.tf2_buffer) - - if grasp_target is not None: - pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer) - - if (self.lift_position is None): - return TriggerResponse( - success=False, - message='lift position unavailable' - ) - - if actually_move: - rospy.loginfo('Raise tool to pregrasp height.') - lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) - lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m) - pose = {'joint_lift': lift_to_pregrasp_m} - self.move_to_pose(pose) + if grasp_target is None: + return TriggerResponse( + success=False, + message='Failed to find grasp target' + ) - pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer) - print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) - print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) + pregrasp_lift_m = self.manipulation_view.get_pregrasp_lift(grasp_target, self.tf2_buffer) - if actually_move: - rospy.loginfo('Rotate the gripper for grasping.') - pose = {'joint_wrist_yaw': pregrasp_yaw} - self.move_to_pose(pose) - - rospy.loginfo('Open the gripper.') - pose = {'gripper_aperture': 0.125} - self.move_to_pose(pose) + if (self.lift_position is None): + return TriggerResponse( + success=False, + message='lift position unavailable' + ) - pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer) + if actually_move: + rospy.loginfo('Raise tool to pregrasp height.') + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m) + pose = {'joint_lift': lift_to_pregrasp_m} + self.move_to_pose(pose) + + pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer) + print('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw)) + print('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi))) + + if actually_move: + rospy.loginfo('Rotate the gripper for grasping.') + pose = {'joint_wrist_yaw': pregrasp_yaw} + self.move_to_pose(pose) - print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) - print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m)) - - if actually_move: - rospy.loginfo('Drive to pregrasp location.') - self.drive(pregrasp_mobile_base_m) - - if pregrasp_wrist_extension_m > 0.0: - extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m) - extension_m = min(extension_m, max_extension_m) - rospy.loginfo('Extend tool above surface.') - pose = {'wrist_extension': extension_m} - self.move_to_pose(pose) - else: - print('negative wrist extension for pregrasp, so not extending or retracting.') - - grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) - print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) - - if actually_move: - rospy.loginfo('Move the grasp pose from the pregrasp pose.') - - lift_m = max(self.lift_position + grasp_lift_m, 0.1) - lift_m = min(lift_m, max_lift_m) - - extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m) + rospy.loginfo('Open the gripper.') + pose = {'gripper_aperture': 0.125} + self.move_to_pose(pose) + + pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer) + + print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) + print('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m)) + + if actually_move: + rospy.loginfo('Drive to pregrasp location.') + self.drive(pregrasp_mobile_base_m) + + if pregrasp_wrist_extension_m > 0.0: + extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m) extension_m = min(extension_m, max_extension_m) - - pose = {'translate_mobile_base': grasp_mobile_base_m, - 'joint_lift': lift_m, - 'wrist_extension': extension_m} + rospy.loginfo('Extend tool above surface.') + pose = {'wrist_extension': extension_m} self.move_to_pose(pose) + else: + print('negative wrist extension for pregrasp, so not extending or retracting.') - rospy.loginfo('Attempt to close the gripper on the object.') - gripper_aperture_m = grasp_target['width_m'] - 0.18 - pose = {'gripper_aperture': gripper_aperture_m} - self.move_to_pose(pose) - - # Lifting appears to happen before the gripper has - # finished unless there is this sleep. Need to look - # into this issue. - rospy.sleep(3.0) - - rospy.loginfo('Attempt to lift the object.') - object_lift_height_m = 0.1 - - lift_m = max(self.lift_position + object_lift_height_m, 0.2) - lift_m = min(lift_m, max_lift_m) - - pose = {'joint_lift': lift_m} - self.move_to_pose(pose) + grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) + print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) - rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.') - pose = {'gripper_aperture': gripper_aperture_m + 0.005} - self.move_to_pose(pose) + if actually_move: + rospy.loginfo('Move the grasp pose from the pregrasp pose.') + lift_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_m = min(lift_m, max_lift_m) - if actually_move: - rospy.loginfo('Retract the tool.') - pose = {'wrist_extension': 0.01} - self.move_to_pose(pose) + extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m) + extension_m = min(extension_m, max_extension_m) - rospy.loginfo('Reorient the wrist.') - pose = {'joint_wrist_yaw': 0.0} - self.move_to_pose(pose) + pose = {'translate_mobile_base': grasp_mobile_base_m, + 'joint_lift': lift_m, + 'wrist_extension': extension_m} + self.move_to_pose(pose) + + rospy.loginfo('Attempt to close the gripper on the object.') + gripper_aperture_m = grasp_target['width_m'] - 0.18 + pose = {'gripper_aperture': gripper_aperture_m} + self.move_to_pose(pose) + + # Lifting appears to happen before the gripper has + # finished unless there is this sleep. Need to look + # into this issue. + rospy.sleep(3.0) + + rospy.loginfo('Attempt to lift the object.') + object_lift_height_m = 0.1 + + lift_m = max(self.lift_position + object_lift_height_m, 0.2) + lift_m = min(lift_m, max_lift_m) + + pose = {'joint_lift': lift_m} + self.move_to_pose(pose) + + rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.') + pose = {'gripper_aperture': gripper_aperture_m + 0.005} + self.move_to_pose(pose) + + if actually_move: + rospy.loginfo('Retract the tool.') + pose = {'wrist_extension': 0.01} + self.move_to_pose(pose) + + rospy.loginfo('Reorient the wrist.') + pose = {'joint_wrist_yaw': 0.0} + self.move_to_pose(pose) return TriggerResponse( success=True, diff --git a/stretch_demos/nodes/handover_object b/stretch_demos/nodes/handover_object index c681b98..070965e 100755 --- a/stretch_demos/nodes/handover_object +++ b/stretch_demos/nodes/handover_object @@ -163,12 +163,12 @@ class HandoverObjectNode(hm.HelloNode): def trigger_handover_object_callback(self, request): - with self.move_lock: + with self.move_lock: # First, retract the wrist in preparation for handing out an object. pose = {'wrist_extension': 0.005} self.move_to_pose(pose) - if self.handover_goal_ready: + if self.handover_goal_ready: pose = {'joint_lift': self.lift_goal_m} self.move_to_pose(pose) tolerance_distance_m = 0.01 @@ -177,9 +177,14 @@ class HandoverObjectNode(hm.HelloNode): self.move_to_pose(pose) self.handover_goal_ready = False - return TriggerResponse( - success=True, - message='Completed object handover!' + return TriggerResponse( + success=True, + message='Completed object handover!' + ) + else: + return TriggerResponse( + success=False, + message='Failed to find handover goal' ) diff --git a/stretch_description/README.md b/stretch_description/README.md index bf9180e..c2c25b2 100644 --- a/stretch_description/README.md +++ b/stretch_description/README.md @@ -2,7 +2,13 @@ ## Overview -*stretch_description* provides materials for a [URDF](http://wiki.ros.org/urdf) kinematic model of the Stretch RE1 mobile manipulator from Hello Robot Inc. +*stretch_description* provides materials for a [URDF](http://wiki.ros.org/urdf) kinematic model of the Stretch mobile manipulator from Hello Robot Inc. + +## Quick View + +``` +roslaunch stretch_description display.launch +``` ## Details @@ -10,7 +16,7 @@ The *meshes directory* contains [STL mesh files](https://en.wikipedia.org/wiki/S The *urdf directory* contains [xacro files](http://wiki.ros.org/xacro) representing various parts of the robot that are used to generate the robot's URDF. -stretch_ros expects a URDF with the name stretch.urdf to reside within the urdf directory. The file stretch.urdf serves as the URDF for the robot and must be generated. Typically, it is a calibrated urdf file for the particular Stretch RE1 robot being used. To generate this file, please read the documentation within stretch_ros/stretch_calibration. +stretch_ros expects a URDF with the name stretch.urdf to reside within the urdf directory. The file stretch.urdf serves as the URDF for the robot and must be generated. Typically, it is a calibrated urdf file unique to the particular Stretch robot being used. To generate this file, please read the documentation within stretch_ros/stretch_calibration. The xacro_to_urdf.sh will usually only be indirectly run as part of various scripts and launch files within stretch_ros/stretch_calibration. @@ -20,7 +26,7 @@ Sometimes a stretch_uncalibrated.urdf file will reside with the urdf directory. Sometimes a URDF is useful outside of ROS, such as for simulations and analysis. Running the *export_urdf.sh* script in the urdf directory will export a full URDF model of the robot based on stretch.urdf. -The exported URDF will be found within an exported_urdf directory. It is also copied to a directory for your specific robot found under ~/stretch_user. The exported URDF includes meshes and controller calibration YAML files. The exported URDF can be visualized using stretch_urdf_show.py, which is part of the stretch_body Python code. +The exported URDF will be found within an exported_urdf directory. It is also copied to a directory for your specific robot found under ~/stretch_user. The exported URDF includes meshes and controller calibration YAML files. The exported URDF can be visualized using `stretch_robot_urdf_visualizer.py`, which is part of the stretch_body Python code. ## Changing the Tool @@ -30,7 +36,7 @@ If you wish to remove the default gripper and add a different tool, you will typ As an example we provide the xacro `stretch_dry_erase_marker.xacro` and its dependent mesh files with stretch_ros. -Some of the tools found in the [Stretch Body Tool Share](https://github.com/hello-robot/stretch_tool_share/) include URDF data. To integrate these tools into the URDF for your Stretch +Some of the tools found in the [Stretch Tool Share](https://github.com/hello-robot/stretch_tool_share/) include URDF data. To integrate these tools into the URDF for your Stretch ```bash >>$ cd ~/repos @@ -54,8 +60,6 @@ Now visualize the new tool >>$ roslaunch stretch_calibration simple_test_head_calibration.launch ``` - - ## License and Patents Patents are pending that cover aspects of the Stretch RE1 mobile manipulator. diff --git a/stretch_description/batch/guthrie/meshes/base_imu.STL b/stretch_description/batch/guthrie/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/guthrie/meshes/base_imu.STL differ diff --git a/stretch_description/batch/guthrie/urdf/stretch_base_imu.xacro b/stretch_description/batch/guthrie/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/guthrie/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/guthrie/urdf/stretch_description.xacro b/stretch_description/batch/guthrie/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/batch/guthrie/urdf/stretch_description.xacro +++ b/stretch_description/batch/guthrie/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/batch/hank/meshes/base_imu.STL b/stretch_description/batch/hank/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/hank/meshes/base_imu.STL differ diff --git a/stretch_description/batch/hank/urdf/stretch_base_imu.xacro b/stretch_description/batch/hank/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/hank/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/hank/urdf/stretch_description.xacro b/stretch_description/batch/hank/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/batch/hank/urdf/stretch_description.xacro +++ b/stretch_description/batch/hank/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/batch/irma/meshes/base_imu.STL b/stretch_description/batch/irma/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/irma/meshes/base_imu.STL differ diff --git a/stretch_description/batch/irma/urdf/stretch_base_imu.xacro b/stretch_description/batch/irma/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/irma/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/irma/urdf/stretch_description.xacro b/stretch_description/batch/irma/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/batch/irma/urdf/stretch_description.xacro +++ b/stretch_description/batch/irma/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/batch/joplin/meshes/base_imu.STL b/stretch_description/batch/joplin/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/joplin/meshes/base_imu.STL differ diff --git a/stretch_description/batch/joplin/urdf/stretch_base_imu.xacro b/stretch_description/batch/joplin/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/joplin/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/joplin/urdf/stretch_description.xacro b/stretch_description/batch/joplin/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/batch/joplin/urdf/stretch_description.xacro +++ b/stretch_description/batch/joplin/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/batch/kendrick/meshes/base_imu.STL b/stretch_description/batch/kendrick/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/kendrick/meshes/base_imu.STL differ diff --git a/stretch_description/batch/kendrick/urdf/stretch_base_imu.xacro b/stretch_description/batch/kendrick/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/kendrick/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/kendrick/urdf/stretch_description.xacro b/stretch_description/batch/kendrick/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/batch/kendrick/urdf/stretch_description.xacro +++ b/stretch_description/batch/kendrick/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/batch/louis/meshes/base_imu.STL b/stretch_description/batch/louis/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/louis/meshes/base_imu.STL differ diff --git a/stretch_description/batch/louis/urdf/stretch_base_imu.xacro b/stretch_description/batch/louis/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/louis/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/louis/urdf/stretch_description.xacro b/stretch_description/batch/louis/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/batch/louis/urdf/stretch_description.xacro +++ b/stretch_description/batch/louis/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/batch/mitski/meshes/base_imu.STL b/stretch_description/batch/mitski/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/mitski/meshes/base_imu.STL differ diff --git a/stretch_description/batch/mitski/urdf/stretch_base_imu.xacro b/stretch_description/batch/mitski/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/mitski/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/mitski/urdf/stretch_description.xacro b/stretch_description/batch/mitski/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/batch/mitski/urdf/stretch_description.xacro +++ b/stretch_description/batch/mitski/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/batch/mitski/urdf/stretch_main.xacro b/stretch_description/batch/mitski/urdf/stretch_main.xacro index 97cd61f..93f1597 100644 --- a/stretch_description/batch/mitski/urdf/stretch_main.xacro +++ b/stretch_description/batch/mitski/urdf/stretch_main.xacro @@ -99,8 +99,8 @@ - + xyz="0 0 1" /> + - + xyz="0 0 1" /> + diff --git a/stretch_description/batch/nina/meshes/base_imu.STL b/stretch_description/batch/nina/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/batch/nina/meshes/base_imu.STL differ diff --git a/stretch_description/batch/nina/meshes/base_link.STL b/stretch_description/batch/nina/meshes/base_link.STL new file mode 100644 index 0000000..8a33531 Binary files /dev/null and b/stretch_description/batch/nina/meshes/base_link.STL differ diff --git a/stretch_description/batch/nina/meshes/laser.STL b/stretch_description/batch/nina/meshes/laser.STL new file mode 100644 index 0000000..5646371 Binary files /dev/null and b/stretch_description/batch/nina/meshes/laser.STL differ diff --git a/stretch_description/batch/nina/meshes/link_arm_l0.STL b/stretch_description/batch/nina/meshes/link_arm_l0.STL new file mode 100644 index 0000000..c09a843 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_arm_l0.STL differ diff --git a/stretch_description/batch/nina/meshes/link_arm_l1.STL b/stretch_description/batch/nina/meshes/link_arm_l1.STL new file mode 100644 index 0000000..68d6383 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_arm_l1.STL differ diff --git a/stretch_description/batch/nina/meshes/link_arm_l2.STL b/stretch_description/batch/nina/meshes/link_arm_l2.STL new file mode 100644 index 0000000..9cef8b0 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_arm_l2.STL differ diff --git a/stretch_description/batch/nina/meshes/link_arm_l3.STL b/stretch_description/batch/nina/meshes/link_arm_l3.STL new file mode 100644 index 0000000..e45e5d3 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_arm_l3.STL differ diff --git a/stretch_description/batch/nina/meshes/link_arm_l4.STL b/stretch_description/batch/nina/meshes/link_arm_l4.STL new file mode 100644 index 0000000..1a547ec Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_arm_l4.STL differ diff --git a/stretch_description/batch/nina/meshes/link_aruco_inner_wrist.STL b/stretch_description/batch/nina/meshes/link_aruco_inner_wrist.STL new file mode 100644 index 0000000..613a5d3 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_aruco_inner_wrist.STL differ diff --git a/stretch_description/batch/nina/meshes/link_aruco_left_base.STL b/stretch_description/batch/nina/meshes/link_aruco_left_base.STL new file mode 100644 index 0000000..109749c Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_aruco_left_base.STL differ diff --git a/stretch_description/batch/nina/meshes/link_aruco_right_base.STL b/stretch_description/batch/nina/meshes/link_aruco_right_base.STL new file mode 100644 index 0000000..0894866 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_aruco_right_base.STL differ diff --git a/stretch_description/batch/nina/meshes/link_aruco_shoulder.STL b/stretch_description/batch/nina/meshes/link_aruco_shoulder.STL new file mode 100644 index 0000000..59134c7 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_aruco_shoulder.STL differ diff --git a/stretch_description/batch/nina/meshes/link_aruco_top_wrist.STL b/stretch_description/batch/nina/meshes/link_aruco_top_wrist.STL new file mode 100644 index 0000000..5748893 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_aruco_top_wrist.STL differ diff --git a/stretch_description/batch/nina/meshes/link_camera.STL b/stretch_description/batch/nina/meshes/link_camera.STL new file mode 100644 index 0000000..e257ea9 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_camera.STL differ diff --git a/stretch_description/batch/nina/meshes/link_dry_erase_holder.STL b/stretch_description/batch/nina/meshes/link_dry_erase_holder.STL new file mode 100644 index 0000000..a123087 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_dry_erase_holder.STL differ diff --git a/stretch_description/batch/nina/meshes/link_dry_erase_marker.STL b/stretch_description/batch/nina/meshes/link_dry_erase_marker.STL new file mode 100644 index 0000000..5929498 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_dry_erase_marker.STL differ diff --git a/stretch_description/batch/nina/meshes/link_gripper.STL b/stretch_description/batch/nina/meshes/link_gripper.STL new file mode 100644 index 0000000..5b64977 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_gripper.STL differ diff --git a/stretch_description/batch/nina/meshes/link_gripper_finger_left.STL b/stretch_description/batch/nina/meshes/link_gripper_finger_left.STL new file mode 100644 index 0000000..3da0e91 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_gripper_finger_left.STL differ diff --git a/stretch_description/batch/nina/meshes/link_gripper_finger_right.STL b/stretch_description/batch/nina/meshes/link_gripper_finger_right.STL new file mode 100644 index 0000000..7a6ac10 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_gripper_finger_right.STL differ diff --git a/stretch_description/batch/nina/meshes/link_gripper_fingertip_left.STL b/stretch_description/batch/nina/meshes/link_gripper_fingertip_left.STL new file mode 100644 index 0000000..58b5e22 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_gripper_fingertip_left.STL differ diff --git a/stretch_description/batch/nina/meshes/link_gripper_fingertip_right.STL b/stretch_description/batch/nina/meshes/link_gripper_fingertip_right.STL new file mode 100644 index 0000000..022eff8 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_gripper_fingertip_right.STL differ diff --git a/stretch_description/batch/nina/meshes/link_head.STL b/stretch_description/batch/nina/meshes/link_head.STL new file mode 100644 index 0000000..8373521 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_head.STL differ diff --git a/stretch_description/batch/nina/meshes/link_head_pan.STL b/stretch_description/batch/nina/meshes/link_head_pan.STL new file mode 100644 index 0000000..04ea7f9 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_head_pan.STL differ diff --git a/stretch_description/batch/nina/meshes/link_head_tilt.STL b/stretch_description/batch/nina/meshes/link_head_tilt.STL new file mode 100644 index 0000000..4ba7edc Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_head_tilt.STL differ diff --git a/stretch_description/batch/nina/meshes/link_left_wheel.STL b/stretch_description/batch/nina/meshes/link_left_wheel.STL new file mode 100644 index 0000000..d0412b9 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_left_wheel.STL differ diff --git a/stretch_description/batch/nina/meshes/link_lift.STL b/stretch_description/batch/nina/meshes/link_lift.STL new file mode 100644 index 0000000..2b19885 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_lift.STL differ diff --git a/stretch_description/batch/nina/meshes/link_mast.STL b/stretch_description/batch/nina/meshes/link_mast.STL new file mode 100644 index 0000000..87cbc61 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_mast.STL differ diff --git a/stretch_description/batch/nina/meshes/link_puller.STL b/stretch_description/batch/nina/meshes/link_puller.STL new file mode 100644 index 0000000..33dd044 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_puller.STL differ diff --git a/stretch_description/batch/nina/meshes/link_respeaker.STL b/stretch_description/batch/nina/meshes/link_respeaker.STL new file mode 100644 index 0000000..34ba67f Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_respeaker.STL differ diff --git a/stretch_description/batch/nina/meshes/link_right_wheel.STL b/stretch_description/batch/nina/meshes/link_right_wheel.STL new file mode 100644 index 0000000..676298a Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_right_wheel.STL differ diff --git a/stretch_description/batch/nina/meshes/link_wrist_yaw.STL b/stretch_description/batch/nina/meshes/link_wrist_yaw.STL new file mode 100644 index 0000000..50cf4b9 Binary files /dev/null and b/stretch_description/batch/nina/meshes/link_wrist_yaw.STL differ diff --git a/stretch_description/batch/nina/meshes/omni_wheel_m.STL b/stretch_description/batch/nina/meshes/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch_description/batch/nina/meshes/omni_wheel_m.STL differ diff --git a/stretch_description/batch/nina/meshes/respeaker_base.STL b/stretch_description/batch/nina/meshes/respeaker_base.STL new file mode 100644 index 0000000..f681199 Binary files /dev/null and b/stretch_description/batch/nina/meshes/respeaker_base.STL differ diff --git a/stretch_description/batch/nina/urdf/stretch_aruco.xacro b/stretch_description/batch/nina/urdf/stretch_aruco.xacro new file mode 100644 index 0000000..48bb565 --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_aruco.xacro @@ -0,0 +1,283 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_base_imu.xacro b/stretch_description/batch/nina/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_d435i.xacro b/stretch_description/batch/nina/urdf/stretch_d435i.xacro new file mode 100644 index 0000000..167383d --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_d435i.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_description.xacro b/stretch_description/batch/nina/urdf/stretch_description.xacro new file mode 100644 index 0000000..d2f9359 --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_description.xacro @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_dry_erase_marker.xacro b/stretch_description/batch/nina/urdf/stretch_dry_erase_marker.xacro new file mode 100644 index 0000000..de1e234 --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_dry_erase_marker.xacro @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_gripper.xacro b/stretch_description/batch/nina/urdf/stretch_gripper.xacro new file mode 100644 index 0000000..03acc68 --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_gripper.xacro @@ -0,0 +1,302 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_gripper_with_puller.xacro b/stretch_description/batch/nina/urdf/stretch_gripper_with_puller.xacro new file mode 100644 index 0000000..c3e9ecf --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_gripper_with_puller.xacro @@ -0,0 +1,359 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_laser_range_finder.xacro b/stretch_description/batch/nina/urdf/stretch_laser_range_finder.xacro new file mode 100644 index 0000000..bc89ffd --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_laser_range_finder.xacro @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/batch/nina/urdf/stretch_main.xacro b/stretch_description/batch/nina/urdf/stretch_main.xacro new file mode 100644 index 0000000..93f1597 --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_main.xacro @@ -0,0 +1,841 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + 1.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/stretch_description/batch/nina/urdf/stretch_respeaker.xacro b/stretch_description/batch/nina/urdf/stretch_respeaker.xacro new file mode 100644 index 0000000..120d9ee --- /dev/null +++ b/stretch_description/batch/nina/urdf/stretch_respeaker.xacro @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/launch/display.launch b/stretch_description/launch/display.launch new file mode 100644 index 0000000..9ce9bde --- /dev/null +++ b/stretch_description/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + + zeros: + joint_lift: 0.2 + joint_wrist_yaw: 3.4 + + + + + + + + + + diff --git a/stretch_description/meshes/base_imu.STL b/stretch_description/meshes/base_imu.STL new file mode 100644 index 0000000..1422db0 Binary files /dev/null and b/stretch_description/meshes/base_imu.STL differ diff --git a/stretch_description/rviz/stretch.rviz b/stretch_description/rviz/stretch.rviz new file mode 100644 index 0000000..a1a23aa --- /dev/null +++ b/stretch_description/rviz/stretch.rviz @@ -0,0 +1,330 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 719 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_accel_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_accel_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_gyro_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_arm_l4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_inner_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_left_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_right_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_aruco_top_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_grasp_center: + Alpha: 1 + Show Axes: false + Show Trail: false + link_gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_finger_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_finger_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_fingertip_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_fingertip_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head_pan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_head_tilt: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_lift: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_mast: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_wrist_yaw: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + respeaker_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.0607848167419434 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.17513221502304077 + Y: 0.0775941014289856 + Z: 0.4477757513523102 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979804515838623 + Target Frame: + Yaw: 0.4481166899204254 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001b50000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000057d0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/stretch_description/urdf/stretch_base_imu.xacro b/stretch_description/urdf/stretch_base_imu.xacro new file mode 100644 index 0000000..5616676 --- /dev/null +++ b/stretch_description/urdf/stretch_base_imu.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_description/urdf/stretch_description.xacro b/stretch_description/urdf/stretch_description.xacro index 800a7a1..d2f9359 100644 --- a/stretch_description/urdf/stretch_description.xacro +++ b/stretch_description/urdf/stretch_description.xacro @@ -9,6 +9,7 @@ + diff --git a/stretch_description/urdf/stretch_main.xacro b/stretch_description/urdf/stretch_main.xacro index 97cd61f..93f1597 100644 --- a/stretch_description/urdf/stretch_main.xacro +++ b/stretch_description/urdf/stretch_main.xacro @@ -99,8 +99,8 @@ - + xyz="0 0 1" /> + - + xyz="0 0 1" /> + diff --git a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py index 84005b4..519736c 100755 --- a/stretch_funmap/src/stretch_funmap/segment_max_height_image.py +++ b/stretch_funmap/src/stretch_funmap/segment_max_height_image.py @@ -15,7 +15,7 @@ from stretch_funmap.numba_height_image import numba_create_segment_image_uint8 import hello_helpers.fit_plane as fp -def find_object_to_grasp(height_image, display_on=False): +def find_object_to_grasp(height_image, display_on=False): h_image = height_image.image m_per_unit = height_image.m_per_height_unit m_per_pix = height_image.m_per_pix @@ -251,9 +251,11 @@ def find_closest_flat_surface(height_image, robot_xy_pix, display_on=False): segments_image, segment_info, height_to_segment_id = segment(image, m_per_unit, zero_height, segmentation_scale, verbose=False) if segment_info is None: return None, None - + floor_id, floor_mask = find_floor(segment_info, segments_image, verbose=False) - + if floor_mask is None: + return None, None + remove_floor = True if remove_floor: segments_image[floor_mask > 0] = 0 @@ -1030,15 +1032,9 @@ def find_floor(segment_info, segments_image, verbose=False): height_m = segment_info[i]['height_m'] bin_value = segment_info[i]['bin_value'] - # Old values changed on June 3, 2021 due to no floor segment - # being within the bounds, which resulted in an error. - - #min_floor_m = -0.02 - #max_floor_m = 0.1 - - # New values set on June 3, 2021. - min_floor_m = -0.05 - max_floor_m = 0.05 + # New values set on June 14, 2022. + min_floor_m = -0.1 + max_floor_m = 0.1 if verbose: print('segment = {0}, histogram bin_value = {1}, height_m = {2}, min_floor_m = {3}, max_floor_m = {4}'.format(i, bin_value, height_m, min_floor_m, max_floor_m)) @@ -1061,8 +1057,7 @@ def find_floor(segment_info, segments_image, verbose=False): print('floor_id =', floor_id) print('max_bin_value =', max_bin_value) else: - if verbose: - print('segment_max_height_image.py : no floor segment found...') + print('segment_max_height_image.py : no floor segment found...') return floor_id, floor_mask diff --git a/stretch_gazebo/README.md b/stretch_gazebo/README.md index da54fa0..500cccb 100644 --- a/stretch_gazebo/README.md +++ b/stretch_gazebo/README.md @@ -53,6 +53,16 @@ This will launch an Rviz instance that visualizes the sensors and an empty world ![](../images/gazebo.png) +#### Running Demo with Keyboard Teleop node +*keyboard_teleop_gazebo* : node that provides a keyboard interface to control the robot's joints within the gazebo simulation. + +```bash +# Terminal 1: +roslaunch stretch_gazebo gazebo.launch rviz:=true +# Terminal 2: +rosrun stretch_gazebo keyboard_teleop_gazebo +``` + ## Running Gazebo with MoveIt! and Stretch ```bash diff --git a/stretch_gazebo/nodes/keyboard.py b/stretch_gazebo/nodes/keyboard.py new file mode 100644 index 0000000..ed823e6 --- /dev/null +++ b/stretch_gazebo/nodes/keyboard.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 + +import sys, tty, termios + +# This allows Ctrl-C and related keyboard commands to still function. +# It detects escape codes in order to recognize arrow keys. It also +# has buffer flushing to avoid repeated commands. This is +# blocking code. + +def getch(): + stdin_fd = 0 + # "Return a list containing the tty attributes for file descriptor + # fd, as follows: [iflag, oflag, cflag, lflag, ispeed, ospeed, cc]" + # from https://docs.python.org/2/library/termios.html + original_tty_attributes = termios.tcgetattr(stdin_fd) + new_tty_attributes = termios.tcgetattr(stdin_fd) + # Change the lflag (local modes) to turn off canonical mode + new_tty_attributes[3] &= ~termios.ICANON + # Set VMIN = 0 and VTIME > 0 for a timed read, as explained in: + # http://unixwiz.net/techtips/termios-vmin-vtime.html + new_tty_attributes[6][termios.VMIN] = b'\x00' + new_tty_attributes[6][termios.VTIME] = b'\x01' + try: + termios.tcsetattr(stdin_fd, termios.TCSAFLUSH, new_tty_attributes) + ch1 = sys.stdin.read(1) + if ch1 == '\x1b': + # special key pressed + ch2 = sys.stdin.read(1) + ch3 = sys.stdin.read(1) + ch = ch1 + ch2 + ch3 + else: + # not a special key + ch = ch1 + finally: + termios.tcsetattr(stdin_fd, termios.TCSAFLUSH, original_tty_attributes) + return ch + + +if __name__ == '__main__': + + while True: + c = getch() + print('c') + diff --git a/stretch_gazebo/nodes/keyboard_teleop_gazebo b/stretch_gazebo/nodes/keyboard_teleop_gazebo new file mode 100755 index 0000000..fe74528 --- /dev/null +++ b/stretch_gazebo/nodes/keyboard_teleop_gazebo @@ -0,0 +1,288 @@ +#!/usr/bin/env python +from __future__ import print_function + +import math +import keyboard as kb + +import rospy +from sensor_msgs.msg import JointState +from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction +from trajectory_msgs.msg import JointTrajectoryPoint +from geometry_msgs.msg import Twist +import actionlib + + +# NOTE: FUNMAP services and capabilities won't work with Stretch Gazebo simulation + +class GetKeyboardCommands: + def __init__(self): + self.step_size = 'medium' + self.rad_per_deg = math.pi / 180.0 + self.small_deg = 3.0 + self.small_rad = self.rad_per_deg * self.small_deg + self.small_translate = 0.005 # 0.02 + self.medium_deg = 6.0 + self.medium_rad = self.rad_per_deg * self.medium_deg + self.medium_translate = 0.04 + self.big_deg = 12.0 + self.big_rad = self.rad_per_deg * self.big_deg + self.big_translate = 0.06 + + def get_deltas(self): + if self.step_size == 'small': + deltas = {'rad': self.small_rad, 'translate': self.small_translate} + if self.step_size == 'medium': + deltas = {'rad': self.medium_rad, 'translate': self.medium_translate} + if self.step_size == 'big': + deltas = {'rad': self.big_rad, 'translate': self.big_translate} + return deltas + + def print_commands(self, joint_state, command): + if command is None: + return + + joints = joint_state.name + + def in_joints(i): + return len(list(set(i) & set(joints))) > 0 + + print('---------- KEYBOARD TELEOP MENU -----------') + print(' ') + if in_joints(['joint_head_tilt']): + print(' i HEAD UP ') + if in_joints(['joint_head_pan']): + print(' j HEAD LEFT l HEAD RIGHT ') + if in_joints(['joint_head_tilt']): + print(' , HEAD DOWN ') + print(' ') + print(' ') + print(' 7 BASE ROTATE LEFT 9 BASE ROTATE RIGHT') + print(' home page-up ') + print(' ') + print(' ') + if in_joints(['joint_lift']): + print(' 8 LIFT UP ') + print(' up-arrow ') + print(' 4 BASE FORWARD 6 BASE BACK ') + print(' left-arrow right-arrow ') + if in_joints(['joint_lift']): + print(' 2 LIFT DOWN ') + print(' down-arrow ') + print(' ') + print(' ') + if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']): + print(' w ARM OUT ') + if in_joints(['joint_wrist_yaw']): + print(' a WRIST FORWARD d WRIST BACK ') + if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']): + print(' x ARM IN ') + if in_joints(['joint_wrist_pitch', 'joint_wrist_roll']): + print(' ') + print(' ') + if in_joints(['joint_wrist_pitch']): + print(' c PITCH FORWARD v PITCH BACK ') + if in_joints(['joint_wrist_roll']): + print(' o ROLL FORWARD p ROLL BACK ') + print(' ') + print(' ') + if in_joints(['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']): + print(' 5 GRIPPER CLOSE ') + print(' 0 GRIPPER OPEN ') + print(' ') + print(' step size: b BIG, m MEDIUM, s SMALL ') + print(' ') + print(' q QUIT ') + print(' ') + print('-------------------------------------------') + + def get_command(self, node): + command = None + + c = kb.getch() + + #################################################### + ## BASIC KEYBOARD TELEOPERATION COMMANDS + #################################################### + + # 8 or up arrow + if c == '8' or c == '\x1b[A': + command = {'joint': 'joint_lift', 'delta': self.get_deltas()['translate']} + # 2 or down arrow + if c == '2' or c == '\x1b[B': + command = {'joint': 'joint_lift', 'delta': -self.get_deltas()['translate']} + + # 4 or left arrow + if c == '4' or c == '\x1b[D': + command = {'joint': 'translate_mobile_base', 'inc': self.get_deltas()['translate']} + # 6 or right arrow + if c == '6' or c == '\x1b[C': + command = {'joint': 'translate_mobile_base', 'inc': -self.get_deltas()['translate']} + # 1 or end key + if c == '7' or c == '\x1b[H': + command = {'joint': 'rotate_mobile_base', 'inc': self.get_deltas()['rad']} + # 3 or pg down 5~ + if c == '9' or c == '\x1b[5': + command = {'joint': 'rotate_mobile_base', 'inc': -self.get_deltas()['rad']} + + if c == 'w' or c == 'W': + command = {'joint': 'wrist_extension', 'delta': self.get_deltas()['translate']} + if c == 'x' or c == 'X': + command = {'joint': 'wrist_extension', 'delta': -self.get_deltas()['translate']} + if c == 'd' or c == 'D': + command = {'joint': 'joint_wrist_yaw', 'delta': -self.get_deltas()['rad']} + if c == 'a' or c == 'A': + command = {'joint': 'joint_wrist_yaw', 'delta': self.get_deltas()['rad']} + if c == 'v' or c == 'V': + command = {'joint': 'joint_wrist_pitch', 'delta': -self.get_deltas()['rad']} + if c == 'c' or c == 'C': + command = {'joint': 'joint_wrist_pitch', 'delta': self.get_deltas()['rad']} + if c == 'p' or c == 'P': + command = {'joint': 'joint_wrist_roll', 'delta': -self.get_deltas()['rad']} + if c == 'o' or c == 'O': + command = {'joint': 'joint_wrist_roll', 'delta': self.get_deltas()['rad']} + if c == '5' or c == '\x1b[E' or c == 'g' or c == 'G': + # grasp + command = {'joint': 'joint_gripper_finger_left', 'delta': -self.get_deltas()['rad']} + if c == '0' or c == '\x1b[2' or c == 'r' or c == 'R': + # release + command = {'joint': 'joint_gripper_finger_left', 'delta': self.get_deltas()['rad']} + if c == 'i' or c == 'I': + command = {'joint': 'joint_head_tilt', 'delta': (2.0 * self.get_deltas()['rad'])} + if c == ',' or c == '<': + command = {'joint': 'joint_head_tilt', 'delta': -(2.0 * self.get_deltas()['rad'])} + if c == 'j' or c == 'J': + command = {'joint': 'joint_head_pan', 'delta': (2.0 * self.get_deltas()['rad'])} + if c == 'l' or c == 'L': + command = {'joint': 'joint_head_pan', 'delta': -(2.0 * self.get_deltas()['rad'])} + if c == 'b' or c == 'B': + rospy.loginfo('process_keyboard.py: changing to BIG step size') + self.step_size = 'big' + if c == 'm' or c == 'M': + rospy.loginfo('process_keyboard.py: changing to MEDIUM step size') + self.step_size = 'medium' + if c == 's' or c == 'S': + rospy.loginfo('process_keyboard.py: changing to SMALL step size') + self.step_size = 'small' + if c == 'q' or c == 'Q': + rospy.loginfo('keyboard_teleop exiting...') + rospy.signal_shutdown('Received quit character (q), so exiting') + + #################################################### + + return command + + +class KeyboardTeleopNode: + + def __init__(self): + + self.keys = GetKeyboardCommands() + self.rate = 10.0 + self.joint_state = None + self.twist = Twist() + + def joint_states_callback(self, joint_state): + self.joint_state = joint_state + + def send_command(self, command): + joint_state = self.joint_state + self.trajectory_client_selector(command) + if (joint_state is not None) and (command is not None): + if 'translate_mobile_base' == command['joint'] or 'rotate_mobile_base' == command['joint']: + self.cmd_vel_pub.publish(self.twist) + return 0 + point = JointTrajectoryPoint() + point.time_from_start = rospy.Duration(0.2) + trajectory_goal = FollowJointTrajectoryGoal() + trajectory_goal.goal_time_tolerance = rospy.Time(1.0) + + joint_name = command['joint'] + + if joint_name in ['joint_lift', 'joint_wrist_yaw', 'joint_head_pan', 'joint_head_tilt']: + trajectory_goal.trajectory.joint_names = [joint_name] + joint_index = joint_state.name.index(joint_name) + joint_value = joint_state.position[joint_index] + delta = command['delta'] + new_value = joint_value + delta + point.positions = [new_value] + elif joint_name in ["joint_gripper_finger_left", "wrist_extension"]: + if joint_name == "joint_gripper_finger_left": + trajectory_goal.trajectory.joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right'] + else: + trajectory_goal.trajectory.joint_names = ['joint_arm_l0','joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3'] + positions = [] + for j_name in trajectory_goal.trajectory.joint_names: + joint_index = joint_state.name.index(j_name) + joint_value = joint_state.position[joint_index] + delta = command['delta'] + new_value = joint_value + delta/len(trajectory_goal.trajectory.joint_names) + positions.append(new_value) + point.positions = positions + + trajectory_goal.trajectory.points = [point] + trajectory_goal.trajectory.header.stamp = rospy.Time.now() + self.trajectory_client.send_goal(trajectory_goal) + self.trajectory_client.wait_for_result() + + def trajectory_client_selector(self, command): + self.trajectory_client = None + self.twist.linear.x = 0 + self.twist.angular.z = 0 + try: + joint = command['joint'] + if joint == 'joint_lift' or joint == 'joint_wrist_yaw' or joint == 'wrist_extension': + self.trajectory_client = self.trajectory_arm_client + if joint == 'joint_head_pan' or joint == 'joint_head_tilt': + self.trajectory_client = self.trajectory_head_client + if joint == 'joint_gripper_finger_right' or joint == 'joint_gripper_finger_left': + self.trajectory_client = self.trajectory_gripper_client + if joint == 'translate_mobile_base' or joint == 'rotate_mobile_base': + if joint == 'translate_mobile_base': + if 'inc' in command: + self.twist.linear.x = command['inc'] + else: + self.twist.linear.x = command['delta'] + else: + if 'inc' in command: + self.twist.angular.z = command['inc'] + else: + self.twist.angular.z = command['delta'] + except TypeError: + 0 + + def main(self): + rospy.init_node('keyboard_teleop_gazebo') + + self.trajectory_gripper_client = actionlib.SimpleActionClient( + '/stretch_gripper_controller/follow_joint_trajectory', FollowJointTrajectoryAction) + server_reached = self.trajectory_gripper_client.wait_for_server(timeout=rospy.Duration(60.0)) + + self.trajectory_head_client = actionlib.SimpleActionClient('/stretch_head_controller/follow_joint_trajectory', + FollowJointTrajectoryAction) + server_reached = self.trajectory_head_client.wait_for_server(timeout=rospy.Duration(60.0)) + + self.trajectory_arm_client = actionlib.SimpleActionClient('/stretch_arm_controller/follow_joint_trajectory', + FollowJointTrajectoryAction) + server_reached = self.trajectory_arm_client.wait_for_server(timeout=rospy.Duration(60.0)) + + self.cmd_vel_pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=10) + + rospy.Subscriber('/joint_states', JointState, self.joint_states_callback) + + rate = rospy.Rate(self.rate) + + command = 1 # set equal to not None, so menu is printed out on first loop + while not rospy.is_shutdown(): + if self.joint_state is not None: + self.keys.print_commands(self.joint_state, command) + command = self.keys.get_command(self) + self.send_command(command) + rate.sleep() + + +if __name__ == '__main__': + try: + node = KeyboardTeleopNode() + node.main() + except KeyboardInterrupt: + rospy.loginfo('interrupt received, so shutting down')