diff --git a/images/gazebo.png b/images/gazebo.png new file mode 100644 index 0000000..24aa810 Binary files /dev/null and b/images/gazebo.png differ diff --git a/images/gazebo_moveit.gif b/images/gazebo_moveit.gif new file mode 100644 index 0000000..472ba55 Binary files /dev/null and b/images/gazebo_moveit.gif differ diff --git a/stretch_description/meshes/base_link.STL b/stretch_description/meshes/base_link.STL index eea6a77..4d5884c 100644 Binary files a/stretch_description/meshes/base_link.STL and b/stretch_description/meshes/base_link.STL differ diff --git a/stretch_description/meshes/omni_wheel_m.STL b/stretch_description/meshes/omni_wheel_m.STL new file mode 100644 index 0000000..19d2bb8 Binary files /dev/null and b/stretch_description/meshes/omni_wheel_m.STL differ diff --git a/stretch_description/urdf/stretch_aruco.xacro b/stretch_description/urdf/stretch_aruco.xacro index 602f60c..8902d2a 100644 --- a/stretch_description/urdf/stretch_aruco.xacro +++ b/stretch_description/urdf/stretch_aruco.xacro @@ -119,7 +119,7 @@ xyz="-2.77555756156289E-17 2.56739074444567E-16 -0.000125000000000042" rpy="0 0 0" /> + value="0" /> + value="0" /> + value="0" /> - + - + - + diff --git a/stretch_description/urdf/stretch_gripper.xacro b/stretch_description/urdf/stretch_gripper.xacro index 14615d8..03acc68 100644 --- a/stretch_description/urdf/stretch_gripper.xacro +++ b/stretch_description/urdf/stretch_gripper.xacro @@ -12,12 +12,12 @@ + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> + izz="0.001" /> + izz="0.001" /> + izz="0.001" /> + izz="0.001" /> - - - diff --git a/stretch_description/urdf/stretch_laser_range_finder.xacro b/stretch_description/urdf/stretch_laser_range_finder.xacro index af62fb6..a770215 100644 --- a/stretch_description/urdf/stretch_laser_range_finder.xacro +++ b/stretch_description/urdf/stretch_laser_range_finder.xacro @@ -58,5 +58,4 @@ - diff --git a/stretch_description/urdf/stretch_main.xacro b/stretch_description/urdf/stretch_main.xacro index 7d878f2..7113379 100644 --- a/stretch_description/urdf/stretch_main.xacro +++ b/stretch_description/urdf/stretch_main.xacro @@ -1,6 +1,11 @@ - + + + + + + @@ -49,14 +54,14 @@ xyz="1.25554620866719E-07 3.54748938447003E-07 0.0239581106165018" rpy="0 0 0" /> + value="0.01" /> + izz="0.001" /> @@ -95,6 +100,7 @@ link="link_right_wheel" /> + + value="0.01" /> + izz="0.001" /> @@ -150,8 +156,60 @@ link="link_left_wheel" /> + - + + + + + + + + + + + 0 + 0 + 1.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -252,7 +310,7 @@ name="joint_lift" type="prismatic"> @@ -260,10 +318,7 @@ link="link_lift" /> - - - - + + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> - @@ -387,12 +441,12 @@ + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> - @@ -444,12 +497,12 @@ + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> - @@ -501,12 +553,12 @@ + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> - @@ -558,12 +609,12 @@ + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> + rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" /> @@ -602,12 +653,6 @@ link="link_wrist_yaw" /> - @@ -620,12 +665,12 @@ + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> - @@ -732,12 +776,12 @@ + ixx="0.001" + ixy="0" + ixz="0" + iyy="0.001" + iyz="0" + izz="0.001" /> - - - - diff --git a/stretch_gazebo/CMakeLists.txt b/stretch_gazebo/CMakeLists.txt new file mode 100644 index 0000000..16be715 --- /dev/null +++ b/stretch_gazebo/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.0.2) +project(stretch_gazebo) + +find_package(catkin REQUIRED) + +catkin_package() + +install(FILES config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +install(DIRECTORY urdf +DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/stretch_gazebo/README.md b/stretch_gazebo/README.md new file mode 100644 index 0000000..3f1fd85 --- /dev/null +++ b/stretch_gazebo/README.md @@ -0,0 +1,63 @@ +![](../images/banner.png) + +## Overview + +*stretch_gazebo* is an implementation of simulating a Stretch robot with [Gazebo](http://gazebosim.org/) simulator. + +## Details + +The *urdf directory* contains [a xacro file](http://wiki.ros.org/xacro) that extends the capabilities of the original xacro files living in *stretch_description* package to include Gazebo functionality. + +The *config* directory contains rviz files and [ros_control](http://wiki.ros.org/ros_control) controller configuration files for various parts of the robot including: + +* Base: [diff_drive_controller/DiffDriveController](http://wiki.ros.org/diff_drive_controller) +* Arm: [position_controllers/JointTrajectoryController](http://wiki.ros.org/joint_trajectory_controller) +* Gripper: [position_controllers/JointTrajectoryController](http://wiki.ros.org/joint_trajectory_controller) +* Head: [position_controllers/JointTrajectoryController](http://wiki.ros.org/joint_trajectory_controller) +* Joints: [joint_state_controller/JointStateController](http://wiki.ros.org/joint_state_controller) + +The *launch* directory includes two files: + +* gazebo.launch: Opens up an empty Gazebo world and spawns the robot loading all the controllers, including all the sensors except Cliff sensors and respeaker. +* teleop_joy.launch: Spawns a joy and teleop_twist_joy instance and remaps *cmd_vel* topics to */stretch_diff_drive_controller/cmd_vel*, which the robot is taking drive commands from. Note that the *teleop_twist_joy* package has a deadman switch by default which disables the drive commands to be published unless it is being pressed. For an Logitech F310 joystick this button is A. + +The *script* directory contains a single python file that publishes ground truth odometry of the robot from Gazebo. +## Setup + +Use `rosdep` to install the required packages. + +## Running Demo + +```bash + # Terminal 1: + roslaunch stretch_gazebo gazebo.launch rviz:=true + # Terminal 2: + roslaunch stretch_gazebo teleop_joy.launch +``` + +This will launch an Rviz instance that visualizes the sensors and an empty world in Gazebo with Stretch and load all the controllers. Although, the base will be able to move with the joystick comamnds, there joystick won't give joint commands to arm, head or gripper. To move these joints see the next section about *Running Gazebo with MoveIt! and Stretch*. + +![](../images/gazebo.png) + +## Running Gazebo with MoveIt! and Stretch + +```bash + # Terminal 1: + roslaunch stretch_gazebo gazebo.launch + # Terminal 2: + roslaunch stretch_gazebo teleop_joy.launch + # Terminal 3 + roslaunch stretch_moveit_config demo_gazebo.launch +``` + +This will launch an Rviz instance that visualizes the joints with markers and an empty world in Gazebo with Stretch and load all the controllers. There are pre-defined positions for each joint group for demonstration purposes. There are three joint groups that can be controlled individually via Motion Planning Rviz plugin. Start and goal positions for joints can be selected similar to [this moveit tutorial](https://ros-planning.github.io/moveit_tutorials/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.html#choosing-specific-start-goal-states). A few notes to be kept in mind: + +* Planning group can be changed via *Planning Group* drop down in Planning tab of Motion Planning Rviz plugin. +* Pre-defined start and goal states can be speciified in *Start State* and *Goal State* drop downs in Planning tab of Motion Planning Rviz plugin. +* *stretch_gripper* group does not show markers, and is intended to be controlled via the joints tab that is located in the very right of Motion Planning Rviz plugin. +* When planning with *stretch_head* group make sure you select *Approx IK Solutions* in Planning tab of Motion Planning Rviz plugin. + +![](../images/gazebo_moveit.gif) +## License + +For license information, please see the LICENSE files. diff --git a/stretch_gazebo/config/arm.yaml b/stretch_gazebo/config/arm.yaml new file mode 100644 index 0000000..1e179fc --- /dev/null +++ b/stretch_gazebo/config/arm.yaml @@ -0,0 +1,23 @@ +stretch_arm_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint_lift + - joint_arm_l3 + - joint_arm_l2 + - joint_arm_l1 + - joint_arm_l0 + - joint_wrist_yaw + allow_partial_joints_goal: true + + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + joint_lift: {trajectory: 0.1, goal: 0.1} + joint_arm_l3: {trajectory: 0.1, goal: 0.1} + joint_arm_l2: {trajectory: 0.1, goal: 0.1} + joint_arm_l1: {trajectory: 0.1, goal: 0.1} + joint_arm_l0: {trajectory: 0.1, goal: 0.1} + joint_wrist_yaw: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 25 + action_monitor_rate: 10 diff --git a/stretch_gazebo/config/drive_config.yaml b/stretch_gazebo/config/drive_config.yaml new file mode 100644 index 0000000..3f4281d --- /dev/null +++ b/stretch_gazebo/config/drive_config.yaml @@ -0,0 +1,31 @@ +type: "diff_drive_controller/DiffDriveController" +publish_rate: 50 + +left_wheel: [joint_left_wheel] +right_wheel: [joint_right_wheel] + +wheel_separation: 0.315 +wheel_radius: 0.05 + +# Odometry covariances for the encoder output of the robot. These values should +# be tuned to your robot's sample odometry data, but these values are a good place +# to start +pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] +twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] + +# Top level frame (link) of the robot description +base_frame_id: base_link + +# Velocity and acceleration limits for the robot +linear: + x: + has_velocity_limits : true + max_velocity : 0.5 # m/s + has_acceleration_limits: true + max_acceleration : 1.0 # m/s^2 +angular: + z: + has_velocity_limits : true + max_velocity : 2.0 # rad/s + has_acceleration_limits: true + max_acceleration : 6.0 # rad/s^2 diff --git a/stretch_gazebo/config/gripper.yaml b/stretch_gazebo/config/gripper.yaml new file mode 100644 index 0000000..9e2442c --- /dev/null +++ b/stretch_gazebo/config/gripper.yaml @@ -0,0 +1,15 @@ +stretch_gripper_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint_gripper_finger_right + - joint_gripper_finger_left + 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 diff --git a/stretch_gazebo/config/head.yaml b/stretch_gazebo/config/head.yaml new file mode 100644 index 0000000..e0ef3e7 --- /dev/null +++ b/stretch_gazebo/config/head.yaml @@ -0,0 +1,15 @@ +stretch_head_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint_head_pan + - joint_head_tilt + allow_partial_joints_goal: true + + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + joint_head_pan: {trajectory: 0.1, goal: 0.1} + joint_head_tilt: {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/config/joints.yaml b/stretch_gazebo/config/joints.yaml new file mode 100644 index 0000000..b5cae6a --- /dev/null +++ b/stretch_gazebo/config/joints.yaml @@ -0,0 +1,2 @@ +type: "joint_state_controller/JointStateController" +publish_rate: 50 diff --git a/stretch_gazebo/config/sim.rviz b/stretch_gazebo/config/sim.rviz new file mode 100644 index 0000000..e23aa3d --- /dev/null +++ b/stretch_gazebo/config/sim.rviz @@ -0,0 +1,391 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 755 + - 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: LaserScan +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 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /realsense/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + 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.7518186569213867 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.35893547534942627 + Y: -0.690294623374939 + Z: 0.8657690286636353 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5203983187675476 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.6135666370391846 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1052 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000037efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000026100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000037e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 0 diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch new file mode 100644 index 0000000..87530b0 --- /dev/null +++ b/stretch_gazebo/launch/gazebo.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_gazebo/launch/teleop_joy.launch b/stretch_gazebo/launch/teleop_joy.launch new file mode 100644 index 0000000..887ca6f --- /dev/null +++ b/stretch_gazebo/launch/teleop_joy.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/stretch_gazebo/package.xml b/stretch_gazebo/package.xml new file mode 100644 index 0000000..c40d3b8 --- /dev/null +++ b/stretch_gazebo/package.xml @@ -0,0 +1,29 @@ + + + stretch_gazebo + 0.0.0 + The stretch_gazebo package + + David V. Lu!! + Vatan Aksoy Tezer + Hello Robot Inc. + + Apache License 2.0 + + catkin + + controller_manager + gazebo_msgs + gazebo_plugins + gazebo_ros + nav_msgs + realsense_gazebo_plugin + realsense2_camera + realsense2_description + robot_state_publisher + rospy + rviz + std_msgs + xacro + + diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom.py b/stretch_gazebo/scripts/publish_ground_truth_odom.py new file mode 100755 index 0000000..43bfd4a --- /dev/null +++ b/stretch_gazebo/scripts/publish_ground_truth_odom.py @@ -0,0 +1,29 @@ +#! /usr/bin/env python + +from gazebo_msgs.srv import GetModelState, GetModelStateRequest +from nav_msgs.msg import Odometry +from std_msgs.msg import Header +import rospy + +rospy.init_node('ground_truth_odometry_publisher') +odom_pub=rospy.Publisher('ground_truth', Odometry, queue_size=10) + +rospy.wait_for_service ('/gazebo/get_model_state') +get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) + +odom=Odometry() +header = Header() +header.frame_id='/ground_truth' +model = GetModelStateRequest() +model.model_name='robot' + +r = rospy.Rate(20) + +while not rospy.is_shutdown(): + result = get_model_srv(model) + odom.pose.pose = result.pose + odom.twist.twist = result.twist + header.stamp = rospy.Time.now() + odom.header = header + odom_pub.publish(odom) + r.sleep() diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro new file mode 100644 index 0000000..a939a88 --- /dev/null +++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro @@ -0,0 +1,560 @@ + + + + + + + + + + + + / + + + + + + 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 + + realsense/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 + false + 5.5 + + + + 2000 + 1 + ${-M_PI} + ${M_PI} + + + + 0.08 + 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/Black + 0.001 + + + + + + + + Gazebo/Black + 0.001 + + + + + + + + Gazebo/Gray + 0.001 + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + 1 + + + hardware_interface/PositionJointInterface + + + + diff --git a/stretch_moveit_config/.setup_assistant b/stretch_moveit_config/.setup_assistant new file mode 100644 index 0000000..7bf96f7 --- /dev/null +++ b/stretch_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: stretch_description + relative_path: urdf/stretch_description.xacro + xacro_args: "" + SRDF: + relative_path: config/stretch_description.srdf + CONFIG: + author_name: David V. Lu!! + author_email: davidvlu@gmail.com + generated_timestamp: 1602791794 \ No newline at end of file diff --git a/stretch_moveit_config/CMakeLists.txt b/stretch_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..d981bb3 --- /dev/null +++ b/stretch_moveit_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(stretch_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/stretch_moveit_config/README.md b/stretch_moveit_config/README.md new file mode 100644 index 0000000..c53abfd --- /dev/null +++ b/stretch_moveit_config/README.md @@ -0,0 +1,23 @@ +![](../images/HelloRobotLogoBar.png) + +## Stretch & MoveIt! +MoveIt is the standard ROS manipulation platform, and this package is the configuration for working with Stretch with the MoveIt framework. + + +### Offline Demo + +To experiment with the planning capabilities of MoveIt on Stretch, you can run a demo _without_ Stretch hardware. + + roslaunch stretch_moveit_config demo.launch + +This will allow you to move the robot around using interactive markers and create plans between poses. + +### Hardware Integration + +To run MoveIt with the actual hardware, (assuming `stretch_driver` is already running) simply run + + roslaunch stretch_moveit_config move_group.launch + +This will runs all of the planning capabilities, but without the setup, simulation and interface that the above demo provides. In order to create plans for the robot with the same interface as the offline demo, you can run + + roslaunch stretch_moveit_config moveit_rviz.launch diff --git a/stretch_moveit_config/config/chomp_planning.yaml b/stretch_moveit_config/config/chomp_planning.yaml new file mode 100644 index 0000000..75258e5 --- /dev/null +++ b/stretch_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/stretch_moveit_config/config/fake_controllers.yaml b/stretch_moveit_config/config/fake_controllers.yaml new file mode 100644 index 0000000..e16ec64 --- /dev/null +++ b/stretch_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,16 @@ +controller_list: + - name: fake_stretch_arm_controller + joints: + - joint_lift + - joint_arm_l3 + - joint_arm_l2 + - joint_arm_l1 + - joint_arm_l0 + - joint_wrist_yaw + - name: fake_gripper_controller + joints: + - joint_gripper_finger_left + - joint_gripper_finger_right +initial: # Define initial robot poses. + - group: stretch_arm + pose: home \ No newline at end of file diff --git a/stretch_moveit_config/config/joint_limits.yaml b/stretch_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..c666ced --- /dev/null +++ b/stretch_moveit_config/config/joint_limits.yaml @@ -0,0 +1,50 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint_arm_l0: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_arm_l1: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_arm_l2: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_arm_l3: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_gripper_finger_left: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_gripper_finger_right: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_lift: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 + joint_wrist_yaw: + has_velocity_limits: true + max_velocity: 1 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/stretch_moveit_config/config/kinematics.yaml b/stretch_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..5928c35 --- /dev/null +++ b/stretch_moveit_config/config/kinematics.yaml @@ -0,0 +1,8 @@ +stretch_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 +stretch_head: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.1 diff --git a/stretch_moveit_config/config/ompl_planning.yaml b/stretch_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..d2b037a --- /dev/null +++ b/stretch_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,210 @@ +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +stretch_arm: + default_planner_config: None + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +stretch_gripper: + default_planner_config: "" + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +stretch_head: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo \ No newline at end of file diff --git a/stretch_moveit_config/config/ros_controllers.yaml b/stretch_moveit_config/config/ros_controllers.yaml new file mode 100644 index 0000000..03882df --- /dev/null +++ b/stretch_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,31 @@ +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: stretch_arm_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_lift + - joint_arm_l3 + - joint_arm_l2 + - joint_arm_l1 + - joint_arm_l0 + - joint_wrist_yaw + - name: stretch_head_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_head_pan + - joint_head_tilt + - name: stretch_gripper_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_gripper_finger_left + - joint_gripper_finger_right diff --git a/stretch_moveit_config/config/sensors_3d.yaml b/stretch_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..a4bb13e --- /dev/null +++ b/stretch_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,10 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater \ No newline at end of file diff --git a/stretch_moveit_config/config/stretch_description.srdf b/stretch_moveit_config/config/stretch_description.srdf new file mode 100644 index 0000000..3bb1624 --- /dev/null +++ b/stretch_moveit_config/config/stretch_description.srdf @@ -0,0 +1,408 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/chomp_planning_pipeline.launch.xml b/stretch_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..90e0b96 --- /dev/null +++ b/stretch_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/default_warehouse_db.launch b/stretch_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..d21274e --- /dev/null +++ b/stretch_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/demo.launch b/stretch_moveit_config/launch/demo.launch new file mode 100644 index 0000000..5c485b3 --- /dev/null +++ b/stretch_moveit_config/launch/demo.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/demo_gazebo.launch b/stretch_moveit_config/launch/demo_gazebo.launch new file mode 100644 index 0000000..0c9f8c7 --- /dev/null +++ b/stretch_moveit_config/launch/demo_gazebo.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/stretch_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..d422edf --- /dev/null +++ b/stretch_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/stretch_moveit_config/launch/gazebo.launch b/stretch_moveit_config/launch/gazebo.launch new file mode 100644 index 0000000..d7368f0 --- /dev/null +++ b/stretch_moveit_config/launch/gazebo.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/joystick_control.launch b/stretch_moveit_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/stretch_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/move_group.launch b/stretch_moveit_config/launch/move_group.launch new file mode 100644 index 0000000..56eeb57 --- /dev/null +++ b/stretch_moveit_config/launch/move_group.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/moveit.rviz b/stretch_moveit_config/launch/moveit.rviz new file mode 100644 index 0000000..e9b1294 --- /dev/null +++ b/stretch_moveit_config/launch/moveit.rviz @@ -0,0 +1,570 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.7425600290298462 + Tree Height: 351 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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 + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + 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 + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: stretch_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + 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 + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + 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 + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.5615234375 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.11356700211763382 + Y: 0.10592000186443329 + Z: 2.2351800055275817e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5697969794273376 + Target Frame: base_link + Value: XYOrbit (rviz) + Yaw: 5.364950656890869 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1052 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000003c2fc0200000007fb000000100044006900730070006c006100790073010000003d000001f0000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000233000001cc0000017d00ffffff000004d8000003c200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 0 diff --git a/stretch_moveit_config/launch/moveit_rviz.launch b/stretch_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..fa149ad --- /dev/null +++ b/stretch_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/ompl_planning_pipeline.launch.xml b/stretch_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..230e9d9 --- /dev/null +++ b/stretch_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/planning_context.launch b/stretch_moveit_config/launch/planning_context.launch new file mode 100644 index 0000000..97dd61c --- /dev/null +++ b/stretch_moveit_config/launch/planning_context.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/planning_pipeline.launch.xml b/stretch_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..87cbbfe --- /dev/null +++ b/stretch_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/stretch_moveit_config/launch/ros_controllers.launch b/stretch_moveit_config/launch/ros_controllers.launch new file mode 100644 index 0000000..7a60150 --- /dev/null +++ b/stretch_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/stretch_moveit_config/launch/run_benchmark_ompl.launch b/stretch_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..6b2e57c --- /dev/null +++ b/stretch_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/sensor_manager.launch.xml b/stretch_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..2345471 --- /dev/null +++ b/stretch_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/setup_assistant.launch b/stretch_moveit_config/launch/setup_assistant.launch new file mode 100644 index 0000000..8b7ca6a --- /dev/null +++ b/stretch_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/stretch_description_moveit_controller_manager.launch.xml b/stretch_moveit_config/launch/stretch_description_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..3931bd4 --- /dev/null +++ b/stretch_moveit_config/launch/stretch_description_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/stretch_moveit_config/launch/stretch_description_moveit_sensor_manager.launch.xml b/stretch_moveit_config/launch/stretch_description_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/stretch_moveit_config/launch/stretch_description_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/stretch_moveit_config/launch/trajectory_execution.launch.xml b/stretch_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..5851abb --- /dev/null +++ b/stretch_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/warehouse.launch b/stretch_moveit_config/launch/warehouse.launch new file mode 100644 index 0000000..d4ea8ac --- /dev/null +++ b/stretch_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/launch/warehouse_settings.launch.xml b/stretch_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/stretch_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/stretch_moveit_config/package.xml b/stretch_moveit_config/package.xml new file mode 100644 index 0000000..6e3e179 --- /dev/null +++ b/stretch_moveit_config/package.xml @@ -0,0 +1,33 @@ + + + stretch_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the stretch_description with the MoveIt Motion Planning Framework + + David V. Lu!! + David V. Lu!! + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + joint_state_publisher + joint_state_publisher_gui + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_move_group + moveit_ros_visualization + moveit_setup_assistant + robot_state_publisher + stretch_description + tf2_ros + xacro + + +