From 5101146b8164b387a5efc910e3305b2412e6ae75 Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Mon, 5 Jun 2023 17:35:22 -0700 Subject: [PATCH 1/3] Added DexWrist keyboard teleop and unified launch command for DexWrist config --- .../urdf/stretch_description.xacro | 33 +- stretch_gazebo/config/dex_wrist.yaml | 15 + stretch_gazebo/launch/gazebo.launch | 21 +- stretch_gazebo/nodes/keyboard_teleop_gazebo | 22 +- .../urdf/stretch_gazebo_dex_wrist.urdf.xacro | 634 ++++++++++++++++++ ...tretch_gazebo_standard_gripper.urdf.xacro} | 4 +- 6 files changed, 704 insertions(+), 25 deletions(-) create mode 100644 stretch_gazebo/config/dex_wrist.yaml create mode 100644 stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro rename stretch_gazebo/urdf/{stretch_gazebo.urdf.xacro => stretch_gazebo_standard_gripper.urdf.xacro} (99%) diff --git a/stretch_description/urdf/stretch_description.xacro b/stretch_description/urdf/stretch_description.xacro index b6a5fc9..f41f40b 100644 --- a/stretch_description/urdf/stretch_description.xacro +++ b/stretch_description/urdf/stretch_description.xacro @@ -1,15 +1,18 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + diff --git a/stretch_gazebo/config/dex_wrist.yaml b/stretch_gazebo/config/dex_wrist.yaml new file mode 100644 index 0000000..ca5239c --- /dev/null +++ b/stretch_gazebo/config/dex_wrist.yaml @@ -0,0 +1,15 @@ +stretch_dex_wrist_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint_wrist_pitch + - joint_wrist_roll + allow_partial_joints_goal: true + + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + joint_gripper_finger_right: {trajectory: 0.1, goal: 0.1} + joint_gripper_finger_left: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 25 + action_monitor_rate: 10 \ No newline at end of file diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch index 6b75bcd..466fb7f 100644 --- a/stretch_gazebo/launch/gazebo.launch +++ b/stretch_gazebo/launch/gazebo.launch @@ -6,10 +6,14 @@ - + + + + + @@ -21,11 +25,12 @@ - + + + args=" -urdf -model robot -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3" respawn="false" output="screen" /> @@ -50,8 +55,16 @@ + + + + + args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller stretch_dex_wrist_controller" + if="$(arg dex_wrist)"/> diff --git a/stretch_gazebo/nodes/keyboard_teleop_gazebo b/stretch_gazebo/nodes/keyboard_teleop_gazebo index fe74528..d68b6da 100755 --- a/stretch_gazebo/nodes/keyboard_teleop_gazebo +++ b/stretch_gazebo/nodes/keyboard_teleop_gazebo @@ -180,6 +180,7 @@ class KeyboardTeleopNode: self.rate = 10.0 self.joint_state = None self.twist = Twist() + self.dex_wrist_control = False def joint_states_callback(self, joint_state): self.joint_state = joint_state @@ -197,8 +198,7 @@ class KeyboardTeleopNode: 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']: + if joint_name in ['joint_lift', 'joint_wrist_yaw', 'joint_wrist_roll', 'joint_wrist_pitch', '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] @@ -221,8 +221,9 @@ class KeyboardTeleopNode: 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() + if self.trajectory_client: + self.trajectory_client.send_goal(trajectory_goal) + self.trajectory_client.wait_for_result() def trajectory_client_selector(self, command): self.trajectory_client = None @@ -236,6 +237,9 @@ class KeyboardTeleopNode: 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 self.dex_wrist_control: + if joint == 'joint_wrist_roll' or joint == 'joint_wrist_pitch': + self.trajectory_client = self.trajectory_dex_wrist_client if joint == 'translate_mobile_base' or joint == 'rotate_mobile_base': if joint == 'translate_mobile_base': if 'inc' in command: @@ -252,6 +256,10 @@ class KeyboardTeleopNode: def main(self): rospy.init_node('keyboard_teleop_gazebo') + try: + self.dex_wrist_control = rospy.get_param('stretch_gazebo/dex_wrist', default=False) + except KeyError: + self.dex_wrist_control = False self.trajectory_gripper_client = actionlib.SimpleActionClient( '/stretch_gripper_controller/follow_joint_trajectory', FollowJointTrajectoryAction) @@ -265,6 +273,12 @@ class KeyboardTeleopNode: FollowJointTrajectoryAction) server_reached = self.trajectory_arm_client.wait_for_server(timeout=rospy.Duration(60.0)) + if self.dex_wrist_control: + rospy.loginfo("Dex Wrist Control Activated") + self.trajectory_dex_wrist_client = actionlib.SimpleActionClient('/stretch_dex_wrist_controller/follow_joint_trajectory', + FollowJointTrajectoryAction) + server_reached = self.trajectory_dex_wrist_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) diff --git a/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro new file mode 100644 index 0000000..56a9512 --- /dev/null +++ b/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro @@ -0,0 +1,634 @@ + + + + + + + + + + + + + + + / + + + + + + Gazebo/Gray + + + + + + + + Gazebo/Blue + 0.001 + + + + + + + + Gazebo/Blue + 0.001 + + + + false + 0.001 + Gazebo/Blue + 0.0 + 0.0 + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/VelocityJointInterface + + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/VelocityJointInterface + + + + + + + + + 0 0 0 0 0 0 + + 1.5009831567151233 + + 640 + 480 + RGB_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 0 + + + + + 0 0 0 0 0 0 + + 1.5009831567151233 + + 640 + 480 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 0 + + + + + 0 0 0 0 0 0 + + 1.5009831567151233 + + 640 + 480 + L_INT8 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 0 + + + + + 0 0 0 0 0 0 + + 1.5009831567151233 + + 640 + 480 + + + 0.1 + 100 + + + gaussian + 0.0 + 0.007 + + + 1 + 30 + 0 + + + + + 30 + 30 + 30 + depth/image_raw + depth/camera_info + color/image_raw + color/camera_info + infrared/image_raw + infrared/camera_info + infrared2/image_raw + infrared2/camera_info + camera_color_optical_frame + camera_depth_optical_frame + camera_left_ir_optical_frame + camera_right_ir_optical_frame + 0.1 + 10 + 1 + depth/color/points + 0.15 + 10 + + + + + true + + true + false + + camera/imu/data + camera_gyro_frame + 50.0 + 0.001 + 0 0 0 + 0 0 0 + camera_gyro_frame + false + + 0 0 0 0 0 0 + + + + + + Gazebo/Black + + + + Gazebo/Black + + + + Gazebo/Black + + + + Gazebo/Black + + + + Gazebo/Black + + + + + Gazebo/Green + + + + + + Gazebo/Black + + 0 0 0 0 0 0 + $(arg visualize_lidar) + 5.5 + + + + 2000 + 1 + -0.9 + 5.0 + + + + 0.15 + 12.0 + 0.01 + + + gaussian + 0.0 + 0.001 + + + + scan + laser + + + + + + + + + Gazebo/Black + + 0 0 0 0 0 0 + $(arg visualize_lidar) + 5.5 + + + + 2000 + 1 + -0.9 + 5.0 + + + + 0.15 + 12.0 + 0.01 + + + gaussian + 0.0 + 0.001 + + + + scan + laser + + + + + + + + true + + true + false + + imu/data + base_link + 25.0 + 0.001 + 0 0 0 + 0 0 0 + base_link + false + + 0 0 0 0 0 0 + + + + + + true + + true + false + + wrist_imu/data + link_wrist_yaw + 25.0 + 0.001 + 0 0 0 + 0 0 0 + link_wrist_yaw + false + + 0 0 0 0 0 0 + + + + + + + + + + Gazebo/Black + 0.001 + + + + Gazebo/Gray + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + + + + + + + Gazebo/Gray + 0.001 + + + + + + + + Gazebo/Gray + 0.001 + + + + + + + + Gazebo/Gray + 0.001 + + + + + + + + Gazebo/Gray + 0.001 + + + + + + + + Gazebo/Gray + 0.001 + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + + + + + + + Gazebo/Black + 0.001 + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + + + + + Gazebo/Gray + + + + + + + + Gazebo/Gray + 0.001 + + + + + + + + Gazebo/Gray + 0.001 + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + + + + + + + Gazebo/Gray + 0.001 + + + + + + + Gazebo/Gray + 0.001 + + + + + + + Gazebo/Gray + 0.001 + + + + + + + Gazebo/Gray + 0.001 + + + + + + + Gazebo/Gray + 0.001 + + + + + + + Gazebo/Black + 0.001 + + + + + + + Gazebo/Gray + 0.001 + + + + + + + Gazebo/Black + 0.001 + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro similarity index 99% rename from stretch_gazebo/urdf/stretch_gazebo.urdf.xacro rename to stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro index 6a676a7..a1fc7aa 100644 --- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro +++ b/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro @@ -569,7 +569,7 @@ Gazebo/Black 0.001 - + @@ -598,4 +598,4 @@ - + \ No newline at end of file From 2bb518a65a1223a17ed124e170874807129c85db Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Mon, 5 Jun 2023 17:49:46 -0700 Subject: [PATCH 2/3] Switch default description to std gripper --- stretch_description/urdf/stretch_description.xacro | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/stretch_description/urdf/stretch_description.xacro b/stretch_description/urdf/stretch_description.xacro index f41f40b..5f9a44d 100644 --- a/stretch_description/urdf/stretch_description.xacro +++ b/stretch_description/urdf/stretch_description.xacro @@ -1,18 +1,15 @@ - + - - + - + + - - - - + \ No newline at end of file From 60c6eb2aabd6b5428ba9acaa040cd7cf3947c050 Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Mon, 5 Jun 2023 17:52:10 -0700 Subject: [PATCH 3/3] Launch Bug fix --- stretch_gazebo/launch/gazebo.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch index 466fb7f..f479e09 100644 --- a/stretch_gazebo/launch/gazebo.launch +++ b/stretch_gazebo/launch/gazebo.launch @@ -30,7 +30,7 @@ + args=" -urdf -model robot -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14" respawn="false" output="screen" />