diff --git a/.gitignore b/.gitignore index 3281968f..38cdc071 100644 --- a/.gitignore +++ b/.gitignore @@ -21,6 +21,7 @@ venv # Catkin stuff CATKIN_IGNORE +compile_commands.json # Files that will be auto-generated on compile moma_demos/grasp_demo/config/yumi/gpd.cfg diff --git a/moma_bringup/README.md b/moma_bringup/README.md index 98760d86..587b6891 100644 --- a/moma_bringup/README.md +++ b/moma_bringup/README.md @@ -2,3 +2,14 @@ This package contains launch files to start interfaces with the real robots. See the [wiki](https://github.com/ethz-asl/moma/wiki/Robots) for more information about our platforms. +### Running handeye calibration (currently only in sim) + +Spawn the panda robot which contains an april tag: +```console +roslaunch moma_gazebo panda_moma_corner.launch +``` + +Then launch the interactive calibration planner: +```console +roslaunch moma_bringup move_group_sphere.py +``` diff --git a/moma_bringup/config/heron.rviz b/moma_bringup/config/heron.rviz new file mode 100644 index 00000000..182c9f96 --- /dev/null +++ b/moma_bringup/config/heron.rviz @@ -0,0 +1,1321 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + Splitter Ratio: 0.7425600290298462 + Tree Height: 314 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +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 + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /rviz_visual_tools + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + 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 + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + 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 + Use Sim Time: false + Value: false + - Class: moveit_rviz_plugin/PlanningScene + Enabled: false + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + 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 + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: false + Value: false + - Acceleration_Scaling_Factor: 0.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: false + 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 + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + body_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + franka_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + z_axis_link: + 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: /panda/move_group/display_planned_path + Use Sim Time: false + 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: panda_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: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + 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 + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + body_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + franka_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + z_axis_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: false + Show Visual Aids: false + Update Topic: /mobile_base/twist_marker_server/update + 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 + back_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + body_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + franka_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7_sc: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + steering_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra1_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra1_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra2_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_infra2_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + z_axis_link: + 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 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 50 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /front/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Queue Size: 10 + Topic: /mobile_base/move_base/global_costmap/footprint + Unreliable: false + Value: false + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + back_laser_link: + Value: true + base_footprint: + Value: true + base_link: + Value: true + body_link: + Value: true + franka_mount_link: + Value: false + front_laser_link: + Value: true + panda_hand: + Value: false + panda_hand_sc: + Value: true + panda_hand_tcp: + Value: true + panda_leftfinger: + Value: false + panda_link0: + Value: true + panda_link0_sc: + Value: true + panda_link1: + Value: false + panda_link1_sc: + Value: true + panda_link2: + Value: false + panda_link2_sc: + Value: true + panda_link3: + Value: false + panda_link3_sc: + Value: true + panda_link4: + Value: false + panda_link4_sc: + Value: true + panda_link5: + Value: false + panda_link5_sc: + Value: true + panda_link6: + Value: false + panda_link6_sc: + Value: true + panda_link7: + Value: false + panda_link7_sc: + Value: true + panda_link8: + Value: false + panda_rightfinger: + Value: false + steering_back_left_link: + Value: true + steering_back_right_link: + Value: true + steering_front_left_link: + Value: true + steering_front_right_link: + Value: true + wheel_back_left_link: + Value: true + wheel_back_right_link: + Value: true + wheel_front_left_link: + Value: true + wheel_front_right_link: + Value: true + wrist_camera_bottom_screw_frame: + Value: false + wrist_camera_color_frame: + Value: false + wrist_camera_color_optical_frame: + Value: false + wrist_camera_depth_frame: + Value: false + wrist_camera_depth_optical_frame: + Value: false + wrist_camera_infra1_frame: + Value: false + wrist_camera_infra1_optical_frame: + Value: false + wrist_camera_infra2_frame: + Value: false + wrist_camera_infra2_optical_frame: + Value: false + wrist_camera_link: + Value: false + z_axis_link: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: false + Tree: + base_footprint: + base_link: + body_link: + back_laser_link: + {} + front_laser_link: + {} + steering_back_left_link: + wheel_back_left_link: + {} + steering_back_right_link: + wheel_back_right_link: + {} + steering_front_left_link: + wheel_front_left_link: + {} + steering_front_right_link: + wheel_front_right_link: + {} + z_axis_link: + franka_mount_link: + panda_link0: + panda_link0_sc: + {} + panda_link1: + panda_link1_sc: + {} + panda_link2: + panda_link2_sc: + {} + panda_link3: + panda_link3_sc: + {} + panda_link4: + panda_link4_sc: + {} + panda_link5: + panda_link5_sc: + {} + panda_link6: + panda_link6_sc: + {} + panda_link7: + panda_link7_sc: + {} + panda_link8: + panda_hand: + panda_hand_sc: + {} + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} + wrist_camera_bottom_screw_frame: + wrist_camera_link: + wrist_camera_color_frame: + wrist_camera_color_optical_frame: + {} + wrist_camera_depth_frame: + wrist_camera_depth_optical_frame: + {} + wrist_camera_infra1_frame: + wrist_camera_infra1_optical_frame: + {} + wrist_camera_infra2_frame: + wrist_camera_infra2_optical_frame: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /mobile_base/map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: MoveGoalPose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /mobile_base/move_base/current_goal + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: GlobalCostMap + Topic: /mobile_base/move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: LocalCostMap + Topic: /mobile_base/move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: GlobalPlanningPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /mobile_base/move_base/TrajectoryPlannerROS/global_plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: LocalPlanningPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /mobile_base/move_base/TrajectoryPlannerROS/local_plan + Unreliable: false + Value: true + - Class: rviz/Camera + Enabled: false + Image Rendering: background and overlay + Image Topic: /wrist_camera/color/image_raw + Name: WristCamera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Visibility: + GlobalCostMap: true + GlobalPlanningPath: true + GraspPoseCandidates: true + Grid: true + InteractiveMarkers: true + LaserScan: true + LocalCostMap: true + LocalPlanningPath: true + Map: true + MarkerArray: true + MotionPlanning: true + MoveGoalPose: true + PlanningScene: true + Polygon: true + RobotModel: true + SceneCloud: true + SelectedGraspPose: true + TF: true + Trajectory: true + Value: true + Zoom Factor: 1 + - Alpha: 1 + Arrow Length: 0.009999999776482582 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: GraspPoseCandidates + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /grasp_candidates + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: SelectedGraspPose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /selected_grasp + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /visualization_marker_array + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + 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: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: SceneCloud + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /scene_cloud + 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: base_footprint + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz_visual_tools/KeyTool + - Class: rviz/SetGoal + Topic: goal + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 3.588529586791992 + 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.08675336837768555 + Y: 0.08422327041625977 + Z: -4.0090208131005056e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.174796000123024 + Target Frame: panda_link0 + Yaw: 0.6417703628540039 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 863 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + RvizVisualToolsGui: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1440 + WristCamera: + collapsed: false + X: 2264 + Y: 162 diff --git a/moma_bringup/launch/move_group_sphere.launch b/moma_bringup/launch/move_group_sphere.launch new file mode 100644 index 00000000..8de4c836 --- /dev/null +++ b/moma_bringup/launch/move_group_sphere.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/moma_bringup/scripts/easy_handeye_2_xacro.py b/moma_bringup/scripts/easy_handeye_2_xacro.py new file mode 100644 index 00000000..13a05cee --- /dev/null +++ b/moma_bringup/scripts/easy_handeye_2_xacro.py @@ -0,0 +1,199 @@ +import re +import os +import argparse + +import scipy as sp +import numpy as np +import yaml +from pathlib import Path + +""" +Input: xacro file & easy handeye result into .txt or .yaml file +File converts quaternions to euler transforms, and puts results in xacro file + +To replace manual copy/paste procedure. + +1. cp panda_arm.xacro to this location. +2. create easy_handeye_file.txt with the easy handeye result after calibration +""" + +class CommandLineArgs: + """ + Take arguments from command line + Parameters: + --------------------------------------------------------- + easy_handeye_file (-e) : text file or yaml file from easy handeye result + xacro_file (-x) : xacro file for arm that needs updating + """ + + def __init__(self): + parser = argparse.ArgumentParser( + description="Less manual calibration", + fromfile_prefix_chars="@", + allow_abbrev=False, + ) + parser.add_argument( + "-e", + "--easy_handeye_file", + action="store", + type=self._file_path, + help="set file name of txt file from easy handeye calibration result", + ) + + parser.add_argument( + "-x", + "--xacro_file", + action="store", + type=self._file_path, + help="set file name of xacro file to change", + ) + + self.args = parser.parse_args() + print("Command Line Arguments: ", vars(self.args)) + + def _file_path(self, string): + if os.path.isfile(string): + return string + else: + print("-------------------------------------------") + print("String: %s is not a file" % (string)) + print("-------------------------------------------") + print("Please input file paths correctly") + print("-------------------------------------------") + raise FileNotFoundError + +def from_easy_handeye(easy_handeye_file): + """ + Opens file of result from easy handeye, and converts to dict. + in future prehaps take input from easy_handeye rviz GUI. + + File should look like this: + + translation: + x: 0.01185524852091389 + y: -0.02569462330148611 + z: 0.07128557290494696 + rotation: + x: 0.6468698017615132 + y: -0.2542335707423363 + z: 0.6650909062427098 + w: 0.27309126223640573 + """ + try: + with open(Path(easy_handeye_file)) as _file: + try: + conf = yaml.safe_load(_file) + except yaml.YAMLError as err: + print(err) + + return conf + + except FileNotFoundError: + print(f"Error: file {easy_handeye_file} not found") + except Exception as err: + print(f"an error occured: {str(err)}") + +def convert_quat_euler(quat, typ="xyz"): + """ + Converts from quaternion to Euler representation. + + quat (list) : 4 x 1 list with [x, y, z, w] format + typ (str) : type of Euler rotation + + ZYX -> intrinsic + xyz -> extrinsic equivalent + + returns (list) : Euler representation [x, y, z] + """ + r = sp.spatial.transform.Rotation + return r.from_quat(quat).as_euler(typ, degrees=False) + + +def replace_wrist_calibration(file_name, translation, rotation): + """ + regex patterns to search for and replace inside the xacro file + finds wrist calibration and replaces everything inside ' ' + """ + patterns = [ + ( + r"wrist_calibration_rpy:=\'(.*?)\'", + f"wrist_calibration_rpy:='{rotation[0]} {rotation[1]} {rotation[2]}'", + ), + ( + r"wrist_calibration_xyz:=\'(.*?)\'", + f"wrist_calibration_xyz:='{translation[0]} {translation[1]} {translation[2]}'", + ), + ] + + try: + # read xacro file + with open(file_name, "r") as xacro_file: + xacro_content = xacro_file.read() + + for pattern, replacement in patterns: + xacro_content = re.sub(pattern, replacement, xacro_content) + + # write new translations to xacro file + with open(file_name, "w") as xacro_file: + xacro_file.write(xacro_content) + + print(f"Sucessfully updated {file_name}") + + except FileNotFoundError: + print(f"Error: file {file_name} not found") + except Exception as err: + print(f"an error occured: {str(err)}") + + +def main(): + + cli = CommandLineArgs() + if cli.args.easy_handeye_file is None: + easy_handeye_file = "easy_handeye_file.txt" + else: + easy_handeye_file = cli.args.easy_handeye_file + + if cli.args.xacro_file is None: + xacro_file = "panda_arm.xacro" + else: + xacro_file = cli.args.xacro_file + + result = from_easy_handeye(easy_handeye_file) + + translation = [val for val in result["translation"].values()] + + quat = [val for val in result["rotation"].values()] + rotation = convert_quat_euler(quat) + + print( + "Translation x: %.3f y: %.3f z: %.3f" + % (translation[0], translation[1], translation[2]) + ) + print( + "Quaternion x: %.3f y: %.3f z: %.3f, w: %.3f" + % (quat[0], quat[1], quat[2], quat[3]) + ) + print( + "Euler Rotation x: %.3f y: %.3f z: %.3f" + % (rotation[0], rotation[1], rotation[2]) + ) + print("Translation x: %.3f y: %.3f z: %.3f" % (translation[0], translation[1], translation[2])) + print("Quaternion x: %.3f y: %.3f z: %.3f, w: %.3f" % (quat[0], quat[1], quat[2], quat[3])) + print("Euler Rotation x: %.3f y: %.3f z: %.3f" % (rotation[0], rotation[1], rotation[2])) + + confirm = ( + input( + f"Are you sure you want to edit the wrist calibration in {xacro_file}? (y/n) " + ) + .strip() + .lower() + ) + + if confirm != "y": + print("Not editing file.") + return + else: + replace_wrist_calibration(xacro_file, translation, rotation) + +if __name__ == "__main__": + main() diff --git a/moma_bringup/scripts/easy_handeye_file.txt b/moma_bringup/scripts/easy_handeye_file.txt new file mode 100644 index 00000000..30d5315b --- /dev/null +++ b/moma_bringup/scripts/easy_handeye_file.txt @@ -0,0 +1,9 @@ +translation: + x: 0.01185524852091389 + y: -0.02569462330148611 + z: 0.07128557290494696 +rotation: + x: 0.6468698017615132 + y: -0.2542335707423363 + z: 0.6650909062427098 + w: 0.27309126223640573 diff --git a/moma_bringup/scripts/move_group_sphere.py b/moma_bringup/scripts/move_group_sphere.py new file mode 100755 index 00000000..71d4e066 --- /dev/null +++ b/moma_bringup/scripts/move_group_sphere.py @@ -0,0 +1,550 @@ +#!/usr/bin/env python + +# Python 2/3 compatibility imports +from __future__ import print_function +from six.moves import input + +import sys +import copy +import rospy +import moveit_commander +import moveit_msgs.msg +import geometry_msgs.msg +import numpy as np +import scipy as sc +from scipy.spatial.transform import Rotation + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import axes3d + +try: + from math import pi, tau, dist, fabs, cos +except: # For Python 2 compatibility + from math import pi, fabs, cos, sqrt + + tau = 2.0 * pi + + def dist(p, q): + return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q))) + + +from std_msgs.msg import String +from moveit_commander.conversions import pose_to_list + + +def all_close(goal, actual, tolerance): + """ + Convenience method for testing if the values in two lists are within a tolerance of each other. + For Pose and PoseStamped inputs, the angle between the two quaternions is compared (the angle + between the identical orientations q and -q is calculated correctly). + @param: goal A list of floats, a Pose or a PoseStamped + @param: actual A list of floats, a Pose or a PoseStamped + @param: tolerance A float + @returns: bool + """ + if type(goal) is list: + for index in range(len(goal)): + if abs(actual[index] - goal[index]) > tolerance: + return False + + elif type(goal) is geometry_msgs.msg.PoseStamped: + return all_close(goal.pose, actual.pose, tolerance) + + elif type(goal) is geometry_msgs.msg.Pose: + x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual) + x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal) + # Euclidean distance + d = dist((x1, y1, z1), (x0, y0, z0)) + # phi = angle between orientations + cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1) + return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0) + + return True + + +class SampleSphericalPoses: + """ """ + + def __init__(self): + super(SampleSphericalPoses, self).__init__() + + # Set random generator + self.rng = np.random.default_rng() + + # Set radius Z-distance from EE to tag + self.radius = 0.5 + + # Set target (TODO get this from world//real tag) + self.target_centre = np.array([0.5, 0.0, 0.2]) + + # Set sphere centre as offset from target + self.sphere_centre = self.target_centre + np.array([0.0, 0.0, 0.1]) + + # Set camera offset + self.cam_offset = np.array([0.0, -0.05, 0.0]) + + # Set phi & theta limits for sphere + self.theta_limits = [0.0, np.pi / 2] + self.phi_limits = [-5 * (np.pi/8), 5* (np.pi/8)] + + def get_pose(self): + """ + Get randomely generated samples of spherical coordinate, + and get pose to target. + + #TODO -> rearange the functions + + @returns: tuple (pos, ori) + pos: A list of cartesian positions [x, y, z] + ori: A list of quaternion orientations [x, y, z, w] + """ + + # Get a randomely sampled phi & theta + phi = self.get_phi(self.rng.random()) + theta = self.get_theta(self.rng.random()) + print(f"(r, theta, phi) = ({self.radius}, {theta}, {phi})") + + # The positional part of the pose in cartesian + pos = self.spherical2cartesian(self.radius, theta, -phi) + pos = self.add_offset(pos, self.sphere_centre) + print(f"(x, y, z) = ({pos[0]}, {pos[1]}, {pos[2]})") + + rot_matrix = self.get_rotation_matrix(pos, self.target_centre) + if not self.is_rotation_matrix(rot_matrix): + raise ValueError("Rotation matrix is invalid") + else: + ori = self.rotation2quat(rot_matrix) + print(f"(x, y, z, w) = ({ori[0]}, {ori[1]}, {ori[2]}. {ori[3]})") + + # return pos, ori + # adding cam offset + cam_point = self.apply_quat_rotation(ori, self.cam_offset) + pos_cam_centre = self.add_offset(pos, cam_point) + rot_matrix = self.get_rotation_matrix(pos_cam_centre, self.target_centre) + if not self.is_rotation_matrix(rot_matrix): + raise ValueError("Rotation matrix is invalid") + else: + ori = self.rotation2quat(rot_matrix) + print(f"Cam: (x, y, z, w) = ({ori[0]}, {ori[1]}, {ori[2]}. {ori[3]})") + return pos, ori + + def get_phi(self, rand): + """ + Longitude (EW) + """ + return (self.phi_limits[1] - self.phi_limits[0]) * rand + self.phi_limits[0] + + def get_theta(self, rand): + """ + Latitude (NS) from equator + """ + return (self.theta_limits[1] - self.theta_limits[0]) * rand + self.theta_limits[ + 0 + ] + + def spherical2cartesian(self, radius, theta, phi): + """ """ + # Taking equatorial theta, so need to caluate azimuth + azi = (np.pi / 2.0) - theta + + x = radius * np.sin(azi) * np.cos(phi) + y = radius * np.sin(azi) * np.sin(phi) + z = radius * np.cos(azi) + + return np.array([x, y, z]) + + def add_offset(self, position, offset): + """ """ + return position + offset + + def get_rotation_matrix(self, position, target): + """ + NEED TO GET TARGET POSITION + + MAYBE PUT IN EXAMPLE FROM SIM + """ + world_z = np.array([0, 0, 1]) + # world X (1,0,0) + # world Y (0,1,0) + + # target is 2 dimensions from world + # find R3 (Z) as normalised difference to target + # target is down so negative needed TODO add in function + R3 = position - target + R3 /= -np.linalg.norm(R3) + + R1 = np.cross(R3, world_z) + R1 /= np.linalg.norm(R1) + + R2 = np.cross(R3, R1) + R2 /= np.linalg.norm(R2) + + R = np.c_[R1, R2, R3] + print("R : ", R) + + return R + + def is_rotation_matrix(self, R): + # square matrix test + if R.ndim != 2 or R.shape[0] != R.shape[1]: + return False + + should_be_identity = np.allclose( + R.dot(R.T), np.identity(R.shape[0], np.float64) + ) + should_be_one = np.allclose(np.linalg.det(R), 1) + return should_be_identity and should_be_one + + def rotation2quat(self, rotation): + """ + Pass in rotation + """ + quat = Rotation.from_matrix(rotation).as_quat() + return quat + + def apply_quat_rotation(self, quat, point): + """ """ + rot = Rotation.from_quat(quat) + return rot.apply(point) + + def pointZAxisAt(self, point): + """! Given a point, this function orients the object such that + its z-axis points at the given point + @param point point in world coordinates + """ + z_axis_target = Vector(point) - self.getWorldPos() + self.alignZAxisTo(z_axis_target) + + def pointZAxisAway(self, point): + """! Given a point, this function orients the object such that + its z-axis points in the opposite direction of the given point + @param point point in world coordinates + """ + z_axis_target = Vector(point) - self.getWorldPos() + self.alignZAxisTo(-z_axis_target) + + def alignZAxisTo(self, direction): + """! Given a direction vector, this function computes the + rotation quaternion, such that the new x-axis is horizontal + and the z-axis points in the specified direction + @param direction list/array/Vector of 3 values + @returns mathutils.Quaternion representation the rotation + that is needed to rotate the object + """ + world_z = np.array([0, 0, 1], dtype=np.float) + new_z = direction + new_x = np.cross(world_z, new_z) + new_x /= np.linalg.norm(new_x) + new_y = np.cross(new_z, new_x) + new_y /= np.linalg.norm(new_y) + rotmat = Matrix(np.array([new_x, new_y, new_z]).T) + self.setOrientation(rotmat.to_quaternion()) + + +class MoveGroupSphereSamples(object): + """MoveGroupSphereSamples""" + + def __init__(self): + super(MoveGroupSphereSamples, self).__init__() + + ## First initialize `moveit_commander`_ and a `rospy`_ node: + moveit_commander.roscpp_initialize(sys.argv) + rospy.init_node("move_group_test", anonymous=True) + + ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's + ## kinematic model and the robot's current joint states + robot = moveit_commander.RobotCommander() + + ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface + ## for getting, setting, and updating the robot's internal understanding of the + ## surrounding world: + scene = moveit_commander.PlanningSceneInterface() + + ## Instantiate a `MoveGroupCommander`_ object. This object is an interface + ## to a planning group (group of joints). In this tutorial the group is the primary + ## arm joints in the Panda robot, so we set the group's name to "panda_arm". + ## If you are using a different robot, change this value to the name of your robot + ## arm planning group. + ## This interface can be used to plan and execute motions: + group_name = "panda_arm" + move_group = moveit_commander.MoveGroupCommander(group_name) + + ## Create a `DisplayTrajectory`_ ROS publisher which is used to display + ## trajectories in Rviz: + display_trajectory_publisher = rospy.Publisher( + "/move_group/display_planned_path", + moveit_msgs.msg.DisplayTrajectory, + queue_size=20, + ) + + ## Getting Basic Information + ## ^^^^^^^^^^^^^^^^^^^^^^^^^ + # We can get the name of the reference frame for this robot: + planning_frame = move_group.get_planning_frame() + print("============ Planning frame: %s" % planning_frame) + + # We can also print the name of the end-effector link for this group: + eef_link = move_group.get_end_effector_link() + print("============ End effector link: %s" % eef_link) + + # We can get a list of all the groups in the robot: + group_names = robot.get_group_names() + print("============ Available Planning Groups:", robot.get_group_names()) + + # Sometimes for debugging it is useful to print the entire state of the + # robot: + print("============ Printing robot state") + print(robot.get_current_state()) + print("") + + # Misc variables + self.robot = robot + self.scene = scene + self.move_group = move_group + self.display_trajectory_publisher = display_trajectory_publisher + self.planning_frame = planning_frame + self.eef_link = eef_link + self.group_names = group_names + + def go_to_home_state(self): + """ + Planning to a Joint Goal + The Panda's zero configuration is at a `singularity `_, so the first + thing we want to do is move it to a slightly better configuration. + We use the constant `tau = 2*pi `_ for convenience: + """ + # We get the joint values from the group and change some of the values: + joint_goal = self.move_group.get_current_joint_values() + joint_goal[0] = 0 + joint_goal[1] = -tau / 8 + joint_goal[2] = 0 + joint_goal[3] = -tau / 4 + joint_goal[4] = 0 + joint_goal[5] = tau / 6 # 1/6 of a turn + joint_goal[6] = 0 + + # The go command can be called with joint values, poses, or without any + # parameters if you have already set the pose or joint target for the group + self.move_group.go(joint_goal, wait=True) + + # Calling ``stop()`` ensures that there is no residual movement + self.move_group.stop() + + # For testing: + current_joints = self.move_group.get_current_joint_values() + return all_close(joint_goal, current_joints, 0.01) + + def go_to_pose_goal(self, position, orientation): + """ + Planning to a Pose Goal + Plan a motion for this group to a desired pose for the + end-effector: + """ + + pose_goal = geometry_msgs.msg.Pose() + pose_goal.orientation.x = orientation[0] + pose_goal.orientation.y = orientation[1] + pose_goal.orientation.z = orientation[2] + pose_goal.orientation.w = orientation[3] + + pose_goal.position.x = position[0] + pose_goal.position.y = position[1] + pose_goal.position.z = position[2] + + self.move_group.set_pose_target(pose_goal) + + success = self.move_group.go(wait=True) + self.move_group.stop() + self.move_group.clear_pose_targets() + + current_pose = self.move_group.get_current_pose().pose + return all_close(pose_goal, current_pose, 0.01) + + def plan_cartesian_path(self, scale=1): + ## BEGIN_SUB_TUTORIAL plan_cartesian_path + ## + ## Cartesian Paths + ## ^^^^^^^^^^^^^^^ + ## You can plan a Cartesian path directly by specifying a list of waypoints + ## for the end-effector to go through. If executing interactively in a + ## Python shell, set scale = 1.0. + ## + waypoints = [] + + wpose = self.move_group.get_current_pose().pose + wpose.position.z -= scale * 0.1 # First move up (z) + wpose.position.y += scale * 0.2 # and sideways (y) + waypoints.append(copy.deepcopy(wpose)) + + wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) + waypoints.append(copy.deepcopy(wpose)) + + wpose.position.y -= scale * 0.1 # Third move sideways (y) + waypoints.append(copy.deepcopy(wpose)) + + # We want the Cartesian path to be interpolated at a resolution of 1 cm + # which is why we will specify 0.01 as the eef_step in Cartesian + # translation. We will disable the jump threshold by setting it to 0.0, + # ignoring the check for infeasible jumps in joint space, which is sufficient + # for this tutorial. + (plan, fraction) = self.move_group.compute_cartesian_path( + waypoints, 0.01, 0.0 # waypoints to follow # eef_step + ) # jump_threshold + + # Note: We are just planning, not asking move_group to actually move the robot yet: + return plan, fraction + + def display_trajectory(self, plan): + ## Displaying a Trajectory + ## ^^^^^^^^^^^^^^^^^^^^^^^ + ## You can ask RViz to visualize a plan (aka trajectory) for you. But the + ## group.plan() method does this automatically so this is not that useful + ## here (it just displays the same trajectory again): + ## + ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. + ## We populate the trajectory_start with our current robot state to copy over + ## any AttachedCollisionObjects and add our plan to the trajectory. + self.display_trajectory = moveit_msgs.msg.DisplayTrajectory() + self.display_trajectory.trajectory_start = self.robot.get_current_state() + self.display_trajectory.trajectory.append(plan) + # Publish + display_trajectory_publisher.publish(self.display_trajectory) + + def execute_plan(self, plan): + ## + ## Executing a Plan + ## ^^^^^^^^^^^^^^^^ + ## Use execute if you would like the robot to follow + ## the plan that has already been computed: + self.move_group.execute(plan, wait=True) + + ## **Note:** The robot's current joint state must be within some tolerance of the + ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail + +def plot_3D_no_truth(position_list): + # Extract x, y, and z coordinates + x = [pos[0] for pos in position_list] + y = [pos[1] for pos in position_list] + z = [pos[2] for pos in position_list] + + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + ax.plot_trisurf(x, y, z, color="white", edgecolors="grey", alpha=0.5) + + ax.scatter(x, y, z, c="cyan", alpha=0.5) + # add here different colours for made position and didn't make position + + # Set labels for the axes + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_zlabel("Z") + + # Show the plot + plt.legend() + plt.show() + + +def plot_3D(position_list, truth_list): + # Extract x, y, and z coordinates + x = [pos[0] for pos in position_list] + y = [pos[1] for pos in position_list] + z = [pos[2] for pos in position_list] + + # Separate points into two lists based on the truth values + true_points = [position_list[i] for i in range(len(position_list)) if truth_list[i]] + false_points = [ + position_list[i] for i in range(len(position_list)) if not truth_list[i] + ] + + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + ax.plot_trisurf(x, y, z, color="white", edgecolors="grey", alpha=0.5) + + # add here different colours for made position and didn't make position + # Scatter plot for True points (in one color) + ax.scatter( + [pos[0] for pos in true_points], + [pos[1] for pos in true_points], + [pos[2] for pos in true_points], + c="cyan", + alpha=0.5, + ) + + # Scatter plot for False points (in another color) + ax.scatter( + [pos[0] for pos in false_points], + [pos[1] for pos in false_points], + [pos[2] for pos in false_points], + c="magenta", + label="did not reach", + alpha=0.5, + ) + # Set labels for the axes + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_zlabel("Z") + + # Show the plot + plt.legend() + plt.show() + + +def main(): + move = MoveGroupSphereSamples() + move.go_to_home_state() + + pos_list = [] + truth_list = [] + + while True: + try: + print("") + print("----------------------------------------------------------") + print("Sampling sphere coordinates for wrist calibration") + print("----------------------------------------------------------") + print("Press 'h' to go to a home state") + print("----------------------------------------------------------") + print("Press 'enter' to go to the next sample point") + print("----------------------------------------------------------") + print("Press 'g' to plot a graph") + print("----------------------------------------------------------") + print("Press 'q' to stop sampling exit") + + print("") + + user_input = input("============ Choose an option: ") + print("") + + if user_input == "h": + move.go_to_home_state() + elif user_input == "g": + plot_3D(pos_list, truth_list) + #plot_3D_no_truth(pos_list) + elif user_input == "q": + print("============ Sampling complete!") + print("") + break + else: + sample = SampleSphericalPoses() + print("getting random pose on sphere") + print("") + pos, ori = sample.get_pose() + print("pos :", pos) + print("ori :", ori) + print("") + pos_list.append(pos) + if not move.go_to_pose_goal(pos, ori): + print("Trajectory not executed") + print("") + truth_list.append(False) + else: + truth_list.append(True) + + except rospy.ROSInterruptException: + return + except KeyboardInterrupt: + return + + +if __name__ == "__main__": + main() diff --git a/moma_description/urdf/giraffe.gazebo.xacro b/moma_description/urdf/giraffe.gazebo.xacro new file mode 100644 index 00000000..bf01633e --- /dev/null +++ b/moma_description/urdf/giraffe.gazebo.xacro @@ -0,0 +1,1655 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + / + + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Blue + + + + + + + Gazebo/Purple + + 0 0 0 0 0 0 + true + 10 + + + + 1600 + -3.14 + 3.14 + + + + 0.5 + 20 + 0.01 + + + + front_laser_link + 101 + front/scan + + + + + Gazebo/Purple + + 0 0 0 0 0 0 + true + 10 + + + + 1600 + -3.14 + 3.14 + + + + 0.5 + 20 + 0.01 + + + + back_laser_link + 101 + back/scan + + + + + + / + gazebo_ros_control/DefaultRobotHWSim + false + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + + hardware_interface/EffortJointInterface + + + + + true + 50.0 + 0.01 + base_footprint + odom + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 30 + + 0.976899239 + + R8G8B8 + 640 + 480 + + + 0.1 + 10 + + + + true + + 0.0 + wrist_camera + /wrist_camera/color/image_raw + /wrist_camera/color/camera_info + /wrist_camera/depth/image_rect_raw + /wrist_camera/depth/camera_info + /wrist_camera/depth/color/points + wrist_camera_depth_optical_frame + 0.1 + 10 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + franka_hw/FrankaStateInterface + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + + franka_hw/FrankaModelInterface + + root + franka_hw/FrankaModelInterface + + + tip + franka_hw/FrankaModelInterface + + + franka_hw/FrankaModelInterface + + + franka_hw/FrankaModelInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + 10 + + + + + + + 0 + 0.001 + + + + + 1.13 + 1.13 + + + + + + + + + 10 + + + + + + + 0 + 0.001 + + + + + 1.13 + 1.13 + + + + + + + + + 0.001 + franka_gazebo/FrankaHWSim + + true + + diff --git a/moma_description/urdf/giraffe.urdf.xacro b/moma_description/urdf/giraffe.urdf.xacro new file mode 100644 index 00000000..dc556ab1 --- /dev/null +++ b/moma_description/urdf/giraffe.urdf.xacro @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moma_description/urdf/panda_arm.xacro b/moma_description/urdf/panda_arm.xacro index db618bf8..1f813864 100644 --- a/moma_description/urdf/panda_arm.xacro +++ b/moma_description/urdf/panda_arm.xacro @@ -4,25 +4,26 @@ + arm_id:='panda' + parent:='world' + xyz:='0 0 0' + rpy:='0 0 0' + tcp_xyz:='0 0 0.1034' + tcp_rpy='0 0 0' + hand:=true + gazebo:=false + use_wrist_camera:=false + wrist_calibration_rpy:='-0.0109077 -1.5469874 2.3727493' + wrist_calibration_xyz:='0.02144228129647578 -0.028987519327164604 0.04877543953719046' + wrist_camera_parent_link:='panda_link8' + use_fixed_camera:=false + fixed_calibration_rpy:='-0.1566281 1.3870703 2.9725147' + fixed_calibration_xyz:='0.890 -0.162 1.293' + fixed_camera_parent_link:='panda_link0' + use_bota:=false + multiple_robots:=false + include_control_plugin:=true, + publish_realsense_extrinsics:=true"> @@ -50,8 +51,7 @@ tcp_rpy="${tcp_rpy}" connected_to="${arm_id}_link8" safety_distance="0.03" - gazebo="${gazebo}" - /> + gazebo="${gazebo}" /> @@ -141,15 +141,14 @@ + tip="${arm_id}_joint8" /> - + @@ -159,11 +158,25 @@ 0.001 franka_gazebo/FrankaHWSim + + /panda + robot_description_arm + robot_description_arm + true + + + + + + + + + - \ No newline at end of file + diff --git a/moma_gazebo/launch/giraffe_moveit_gazebo.launch b/moma_gazebo/launch/giraffe_moveit_gazebo.launch new file mode 100644 index 00000000..f563da0d --- /dev/null +++ b/moma_gazebo/launch/giraffe_moveit_gazebo.launch @@ -0,0 +1,132 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [panda/franka_state_controller/joint_states, + panda/franka_gripper/joint_states, + mobile_base/joint_states] + + + + + + + + + + + + + + + + + + + + diff --git a/moma_gazebo/worlds/cone.world b/moma_gazebo/worlds/cone.world new file mode 100644 index 00000000..dc9fa2e7 --- /dev/null +++ b/moma_gazebo/worlds/cone.world @@ -0,0 +1,935 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + 0 + 0 + 0 + + -0.756237 1.39226 0 0 -0 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + 0 + 0 + 0 + + 1.80022 0.377178 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -2.19745 -2.25046 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -1.40494 -2.55135 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -0.501986 -2.9745 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + 2.09724 -2.18232 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -3.69879 0.345038 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -0.406438 -4.23374 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + 2.38815 -3.2576 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + 2.39316 -4.49605 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -3.00495 -2.78053 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + 0 + + 0 0 0 0 -0 0 + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 0 + + -4.64786 -0.646522 0 0 -0 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + 0 + 0 + 0 + + -5.3108 -2.23708 0 0 -0 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + 0 + 0 + 0 + + -2.07692 -4.26198 0 0 -0 0 + + + 457 444000000 + 459 829915274 + 1646216792 728719362 + 457444 + + -0.756319 1.39223 0 0 0 -2.4e-05 + 1 1 1 + + -0.756319 1.39223 0 0 0 -2.4e-05 + 0 0 0 0 -0 0 + 3.45865 5.7443 4.40608 1.34644 0.778762 3.14097 + 1729.32 2872.15 2203.04 0 -0 0 + + + + 1.80014 0.377148 0 0 0 -2.2e-05 + 1 1 1 + + 1.80014 0.377148 0 0 0 -2.2e-05 + 0 0 0 0 -0 0 + 3.45864 5.7443 4.40608 1.34642 0.778778 3.14097 + 1729.32 2872.15 2203.04 0 -0 0 + + + + -5.31082 -2.23709 -0 0 -0 -7e-06 + 1 1 1 + + -5.31082 -2.23709 -0 0 -0 -7e-06 + 0 0 0 0 -0 0 + -3.87743 -2.8936 0.727843 -2.17783 0.268134 -3.13538 + -1938.71 -1446.8 363.922 0 -0 0 + + + + -2.07693 -4.26199 -0 0 0 -4e-06 + 1 1 1 + + -2.07693 -4.26199 -0 0 0 -4e-06 + 0 0 0 0 -0 0 + 7.60795 0.965639 1.20183 -2.41278 0.170207 0.001195 + 3803.98 482.819 600.916 0 -0 0 + + + + -2.19745 -2.25046 -0 0 -0 0 + 1 1 1 + + -2.19745 -2.25046 -0 0 -0 0 + 0 0 0 0 -0 0 + 0 -0 -0.008512 -2.58585 1.13682 3.14159 + 0 -0 -0.008512 0 -0 0 + + + + -1.40494 -2.55135 -0 0 -0 0 + 1 1 1 + + -1.40494 -2.55135 -0 0 -0 0 + 0 0 0 0 -0 0 + 0 -0 -0.008512 -2.58585 1.13682 3.14159 + 0 -0 -0.008512 0 -0 0 + + + + -0.501986 -2.9745 -0 0 -0 0 + 1 1 1 + + -0.501986 -2.9745 -0 0 -0 0 + 0 0 0 0 -0 0 + 0 -0 -0.008512 -2.58585 1.13682 3.14159 + 0 -0 -0.008512 0 -0 0 + + + + 2.09724 -2.18232 -0 1e-06 -0 0 + 1 1 1 + + 2.09724 -2.18232 -0 1e-06 -0 0 + 0 0 0 0 -0 0 + -0 -0 -9.79988 3e-05 2e-05 -0 + -0 -0 -9.79988 0 -0 0 + + + + -3.69879 0.345038 -1e-05 0 -0 0 + 1 1 1 + + -3.69879 0.345038 -1e-05 0 -0 0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + -0.406438 -4.23374 -0 0 -1e-06 0 + 1 1 1 + + -0.406438 -4.23374 -0 0 -1e-06 0 + 0 0 0 0 -0 0 + -0 0 0.008512 2.58585 -1.13682 -3.14159 + -0 0 0.008512 0 -0 0 + + + + 2.38815 -3.2576 -0 0 -0 0 + 1 1 1 + + 2.38815 -3.2576 -0 0 -0 0 + 0 0 0 0 -0 0 + 0 -0 -0.008512 -2.58585 1.13682 3.14159 + 0 -0 -0.008512 0 -0 0 + + + + 2.39316 -4.49605 -0 0 -1e-06 0 + 1 1 1 + + 2.39316 -4.49605 -0 0 -1e-06 0 + 0 0 0 0 -0 0 + -0 0 0.008512 2.58585 -1.13682 -3.14159 + -0 0 0.008512 0 -0 0 + + + + -3.00495 -2.78053 -0 0 -1e-06 0 + 1 1 1 + + -3.00495 -2.78053 -0 0 -1e-06 0 + 0 0 0 0 -0 0 + -0 0 0.008512 2.58585 -1.13682 -3.14159 + -0 0 0.008512 0 -0 0 + + + + -4.64786 -0.646522 -0 0 -1e-06 0 + 1 1 1 + + -4.64786 -0.646522 -0 0 -1e-06 0 + 0 0 0 0 -0 0 + -0 0 0.008512 2.58585 -1.13682 -3.14159 + -0 0 0.008512 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + -15.5892 0.392985 21.6628 0 0.851642 0.028194 + orbit + perspective + + + + diff --git a/moma_gazebo/worlds/moma_corner.world b/moma_gazebo/worlds/moma_corner.world index df12ff8a..a8aa99c9 100644 --- a/moma_gazebo/worlds/moma_corner.world +++ b/moma_gazebo/worlds/moma_corner.world @@ -61,7 +61,7 @@ - 0.20 0.20 0.001 + 0.23 0.23 0.001 @@ -77,7 +77,7 @@ 0 1 - 0.3 -0.6 0.2 0 -0 0 + 0.5 -0.0 0.2 0 -0 0 diff --git a/move_group.cfg b/move_group.cfg new file mode 100644 index 00000000..195c0223 --- /dev/null +++ b/move_group.cfg @@ -0,0 +1 @@ +use panda_arm diff --git a/moveit_configs/giraffe_moveit_config/.setup_assistant b/moveit_configs/giraffe_moveit_config/.setup_assistant new file mode 100644 index 00000000..8331b1f8 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: moma_description + relative_path: urdf/giraffe.urdf.xacro + xacro_args: "" + SRDF: + relative_path: config/giraffe.srdf + CONFIG: + author_name: Lucy Harris + author_email: harrisl@ethz.ch + generated_timestamp: 1702659392 \ No newline at end of file diff --git a/moveit_configs/giraffe_moveit_config/CMakeLists.txt b/moveit_configs/giraffe_moveit_config/CMakeLists.txt new file mode 100644 index 00000000..2c29c175 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(giraffe_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/moveit_configs/giraffe_moveit_config/config/cartesian_limits.yaml b/moveit_configs/giraffe_moveit_config/config/cartesian_limits.yaml new file mode 100644 index 00000000..7df72f69 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/moveit_configs/giraffe_moveit_config/config/chomp_planning.yaml b/moveit_configs/giraffe_moveit_config/config/chomp_planning.yaml new file mode 100644 index 00000000..eb9c9122 --- /dev/null +++ b/moveit_configs/giraffe_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.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/moveit_configs/giraffe_moveit_config/config/fake_controllers.yaml b/moveit_configs/giraffe_moveit_config/config/fake_controllers.yaml new file mode 100644 index 00000000..384802c9 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,14 @@ +controller_list: + - name: fake_panda_arm_controller + type: $(arg fake_execution_type) + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 +initial: # Define initial robot poses per group + - group: panda_arm + pose: home \ No newline at end of file diff --git a/moveit_configs/giraffe_moveit_config/config/gazebo_controllers.yaml b/moveit_configs/giraffe_moveit_config/config/gazebo_controllers.yaml new file mode 100644 index 00000000..e4d2eb00 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/moveit_configs/giraffe_moveit_config/config/gazebo_giraffe.urdf b/moveit_configs/giraffe_moveit_config/config/gazebo_giraffe.urdf new file mode 100644 index 00000000..d921e9ef --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/gazebo_giraffe.urdf @@ -0,0 +1,1592 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + 1.0 + 1.0 + 0 0 1 + 0.0 + 0.0 + + + + + 0.005 + 1e8 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + / + + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Green + 1.0 + 1.0 + 1000000.0 + 100.0 + + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Orange + 1.0 + 1.0 + 1000000.0 + 100.0 + + + Gazebo/Blue + + + Gazebo/Purple + + 0 0 0 0 0 0 + false + 10 + + + + 1600 + -3.14 + 3.14 + + + + 0.5 + 20 + 0.01 + + + + front_laser_link + 101 + front/scan + + + + + + /mobile_base + gazebo_ros_control/DefaultRobotHWSim + false + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + 1 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + hardware_interface/EffortJointInterface + + + + + true + 50.0 + 0.01 + base_footprint + odom + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 30 + + 0.976899239 + + R8G8B8 + 640 + 480 + + + 0.1 + 10 + + + + true + + 0.0 + wrist_camera + /wrist_camera/color/image_raw + /wrist_camera/color/camera_info + /wrist_camera/depth/image_rect_raw + /wrist_camera/depth/camera_info + /wrist_camera/depth/color/points + wrist_camera_depth_optical_frame + 0.1 + 10 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + franka_hw/FrankaStateInterface + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + franka_hw/FrankaStateInterface + + + + franka_hw/FrankaModelInterface + + root + franka_hw/FrankaModelInterface + + + tip + franka_hw/FrankaModelInterface + + + franka_hw/FrankaModelInterface + + + franka_hw/FrankaModelInterface + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + true + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + 10 + + + + + + + 0 + 0.001 + + + + + 1.13 + 1.13 + + + + + + + + + 10 + + + + + + + 0 + 0.001 + + + + + 1.13 + 1.13 + + + + + + + + + 0.001 + franka_gazebo/FrankaHWSim + /panda + robot_description_arm + robot_description_arm + + true + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/config/giraffe.srdf b/moveit_configs/giraffe_moveit_config/config/giraffe.srdf new file mode 100644 index 00000000..1b5fefd3 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/giraffe.srdf @@ -0,0 +1,252 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/config/joint_limits.yaml b/moveit_configs/giraffe_moveit_config/config/joint_limits.yaml new file mode 100644 index 00000000..174d7aff --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/joint_limits.yaml @@ -0,0 +1,45 @@ +# 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: + panda_joint1: + has_velocity_limits: true + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint2: + has_velocity_limits: true + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint3: + has_velocity_limits: true + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint4: + has_velocity_limits: true + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint5: + has_velocity_limits: true + max_velocity: 2.61 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint6: + has_velocity_limits: true + max_velocity: 2.61 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint7: + has_velocity_limits: true + max_velocity: 2.61 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/moveit_configs/giraffe_moveit_config/config/kinematics.yaml b/moveit_configs/giraffe_moveit_config/config/kinematics.yaml new file mode 100644 index 00000000..fac6862b --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/kinematics.yaml @@ -0,0 +1,7 @@ +panda_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.0001 + goal_position_tolerance: 0.0001 + goal_orientation_tolerance: 0.001 \ No newline at end of file diff --git a/moveit_configs/giraffe_moveit_config/config/ompl_planning.yaml b/moveit_configs/giraffe_moveit_config/config/ompl_planning.yaml new file mode 100644 index 00000000..b375665c --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,198 @@ +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 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 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 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_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 + AITstar: + type: geometric::AITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + set_max_num_goals: 1 # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1 + ABITstar: + type: geometric::ABITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + initial_inflation_factor: 1000000 # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000 + inflation_scaling_parameter: 10 # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + truncation_scaling_parameter: 5.0 # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + BITstar: + type: geometric::BITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 +panda_arm: + 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 + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(panda_joint1,panda_joint2) + longest_valid_segment_fraction: 0.005 diff --git a/moveit_configs/giraffe_moveit_config/config/ros_controllers.yaml b/moveit_configs/giraffe_moveit_config/config/ros_controllers.yaml new file mode 100644 index 00000000..756d57b3 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,55 @@ +panda_arm_controller: + type: position_controllers/JointTrajectoryController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + constraints: + goal_time: 0.5 + panda_joint1: { goal: 0.05} + panda_joint2: { goal: 0.05} + panda_joint3: { goal: 0.05} + panda_joint4: { goal: 0.05} + panda_joint5: { goal: 0.05} + panda_joint6: { goal: 0.05} + panda_joint7: { goal: 0.05} + gains: + panda_joint1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint2: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint7: + p: 100 + d: 1 + i: 1 + i_clamp: 1 diff --git a/moveit_configs/giraffe_moveit_config/config/sensors_3d.yaml b/moveit_configs/giraffe_moveit_config/config/sensors_3d.yaml new file mode 100644 index 00000000..51010a36 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + [] \ No newline at end of file diff --git a/moveit_configs/giraffe_moveit_config/config/simple_moveit_controllers.yaml b/moveit_configs/giraffe_moveit_config/config/simple_moveit_controllers.yaml new file mode 100644 index 00000000..2cf5a9a8 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/simple_moveit_controllers.yaml @@ -0,0 +1,13 @@ +controller_list: + - name: panda/effort_joint_trajectory_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 \ No newline at end of file diff --git a/moveit_configs/giraffe_moveit_config/config/stomp_planning.yaml b/moveit_configs/giraffe_moveit_config/config/stomp_planning.yaml new file mode 100644 index 00000000..0cd2c8a0 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/config/stomp_planning.yaml @@ -0,0 +1,39 @@ +stomp/panda_arm: + group_name: panda_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/moveit_configs/giraffe_moveit_config/launch/chomp_planning_pipeline.launch.xml b/moveit_configs/giraffe_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 00000000..2b18e3dc --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/default_warehouse_db.launch b/moveit_configs/giraffe_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 00000000..67aa7ca9 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/demo.launch b/moveit_configs/giraffe_moveit_config/launch/demo.launch new file mode 100644 index 00000000..fbde9449 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/demo.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/demo_gazebo.launch b/moveit_configs/giraffe_moveit_config/launch/demo_gazebo.launch new file mode 100644 index 00000000..0ef8f954 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/moveit_configs/giraffe_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..7adf5ab0 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/gazebo.launch b/moveit_configs/giraffe_moveit_config/launch/gazebo.launch new file mode 100644 index 00000000..d1a88859 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/giraffe_moveit_sensor_manager.launch.xml b/moveit_configs/giraffe_moveit_config/launch/giraffe_moveit_sensor_manager.launch.xml new file mode 100644 index 00000000..5d02698d --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/giraffe_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/joystick_control.launch b/moveit_configs/giraffe_moveit_config/launch/joystick_control.launch new file mode 100644 index 00000000..9411f6e6 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/move_group.launch b/moveit_configs/giraffe_moveit_config/launch/move_group.launch new file mode 100644 index 00000000..ca69db24 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/move_group.launch @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/moveit.rviz b/moveit_configs/giraffe_moveit_config/launch/moveit.rviz new file mode 100644 index 00000000..7410dea8 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/moveit.rviz @@ -0,0 +1,131 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 226 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + 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 + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Name: MotionPlanning + Planned Path: + Links: ~ + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + 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 + 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 + 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 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: ~ + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_footprint + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.0 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.5 + Target Frame: base_footprint + Yaw: -0.6232355833053589 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 848 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1291 + X: 454 + Y: 25 diff --git a/moveit_configs/giraffe_moveit_config/launch/moveit_rviz.launch b/moveit_configs/giraffe_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 00000000..a4605c03 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml b/moveit_configs/giraffe_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 00000000..346d861d --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/ompl_planning_pipeline.launch.xml b/moveit_configs/giraffe_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 00000000..a80892b3 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/moveit_configs/giraffe_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 00000000..c7c4cf50 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/planning_context.launch b/moveit_configs/giraffe_moveit_config/launch/planning_context.launch new file mode 100644 index 00000000..262b9ff2 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/planning_context.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/planning_pipeline.launch.xml b/moveit_configs/giraffe_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 00000000..4b4d0d66 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml b/moveit_configs/giraffe_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..9ebc91c1 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/ros_controllers.launch b/moveit_configs/giraffe_moveit_config/launch/ros_controllers.launch new file mode 100644 index 00000000..159ae7a1 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/run_benchmark_ompl.launch b/moveit_configs/giraffe_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 00000000..05ca4b22 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/sensor_manager.launch.xml b/moveit_configs/giraffe_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 00000000..fe02eae8 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/setup_assistant.launch b/moveit_configs/giraffe_moveit_config/launch/setup_assistant.launch new file mode 100644 index 00000000..2a789dd9 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/simple_moveit_controller_manager.launch.xml b/moveit_configs/giraffe_moveit_config/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 00000000..e77fa51a --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/stomp_planning_pipeline.launch.xml b/moveit_configs/giraffe_moveit_config/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 00000000..c4999f5b --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/trajectory_execution.launch.xml b/moveit_configs/giraffe_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 00000000..20c3dfc4 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/warehouse.launch b/moveit_configs/giraffe_moveit_config/launch/warehouse.launch new file mode 100644 index 00000000..0712e670 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/launch/warehouse_settings.launch.xml b/moveit_configs/giraffe_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 00000000..e473b083 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/moveit_configs/giraffe_moveit_config/package.xml b/moveit_configs/giraffe_moveit_config/package.xml new file mode 100644 index 00000000..9140ee81 --- /dev/null +++ b/moveit_configs/giraffe_moveit_config/package.xml @@ -0,0 +1,41 @@ + + + giraffe_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the giraffe with the MoveIt Motion Planning Framework + + Lucy Harris + Lucy Harris + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + moma_description + + + diff --git a/moveit_configs/panda_moveit_config/launch/gazebo.launch b/moveit_configs/panda_moveit_config/launch/gazebo.launch index 353bd123..1dfaf3cd 100644 --- a/moveit_configs/panda_moveit_config/launch/gazebo.launch +++ b/moveit_configs/panda_moveit_config/launch/gazebo.launch @@ -93,4 +93,4 @@ - \ No newline at end of file +