diff --git a/stretch_description/urdf/stretch_description.xacro b/stretch_description/urdf/stretch_description.xacro
index b6a5fc9..5f9a44d 100644
--- a/stretch_description/urdf/stretch_description.xacro
+++ b/stretch_description/urdf/stretch_description.xacro
@@ -1,15 +1,15 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
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..f479e09 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -6,10 +6,14 @@
-
+
+
+
+
+
@@ -21,7 +25,8 @@
-
+
+
+
+
+
+
+ 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