diff --git a/_static/videos/rviz_joints_nullspace.webm b/_static/videos/rviz_joints_nullspace.webm index d626dd1650..78ad2cfb79 100644 Binary files a/_static/videos/rviz_joints_nullspace.webm and b/_static/videos/rviz_joints_nullspace.webm differ diff --git a/doc/tutorials/planning_around_objects/hello_moveit_kinova.cpp b/doc/tutorials/planning_around_objects/hello_moveit_kinova.cpp new file mode 100644 index 0000000000..ad2ccfed14 --- /dev/null +++ b/doc/tutorials/planning_around_objects/hello_moveit_kinova.cpp @@ -0,0 +1,125 @@ +#include +#include +#include + +#include +#include +#include + +int main(int argc, char* argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("hello_moveit"); + + // We spin up a SingleThreadedExecutor for the current state monitor to get + // information about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto spinner = std::thread([&executor]() { executor.spin(); }); + + // Create the MoveIt MoveGroup Interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "manipulator"); + + // Construct and initialize MoveItVisualTools + auto moveit_visual_tools = + moveit_visual_tools::MoveItVisualTools{ node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC, + move_group_interface.getRobotModel() }; + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.loadRemoteControl(); + + // Create a closure for updating the text in rviz + auto const draw_title = [&moveit_visual_tools](auto text) { + auto const text_pose = [] { + auto msg = Eigen::Isometry3d::Identity(); + msg.translation().z() = 1.0; + return msg; + }(); + moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); + }; + auto const prompt = [&moveit_visual_tools](auto text) { moveit_visual_tools.prompt(text); }; + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("manipulator")]( + auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); }; + + // Set a target Pose with updated values !!! + auto const target_pose = [] { + geometry_msgs::msg::Pose msg; + msg.orientation.y = 0.8; + msg.orientation.w = 0.6; + msg.position.x = 0.1; + msg.position.y = 0.4; + msg.position.z = 0.4; + return msg; + }(); + move_group_interface.setPoseTarget(target_pose); + + // Create collision object for the robot to avoid + auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] { + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = frame_id; + collision_object.id = "box1"; + shape_msgs::msg::SolidPrimitive primitive; + + // Define the size of the box in meters + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[primitive.BOX_X] = 0.5; + primitive.dimensions[primitive.BOX_Y] = 0.1; + primitive.dimensions[primitive.BOX_Z] = 0.5; + + // Define the pose of the box (relative to the frame_id) + geometry_msgs::msg::Pose box_pose; + box_pose.orientation.w = 1.0; + box_pose.position.x = 0.2; + box_pose.position.y = 0.2; + box_pose.position.z = 0.25; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(box_pose); + collision_object.operation = collision_object.ADD; + + return collision_object; + }(); + + // Add the collision object to the scene + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + planning_scene_interface.applyCollisionObject(collision_object); + + // Create a plan to that target pose + prompt("Press 'next' in the RvizVisualToolsGui window to plan"); + draw_title("Planning"); + moveit_visual_tools.trigger(); + auto const [success, plan] = [&move_group_interface] { + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if (success) + { + draw_trajectory_tool_path(plan.trajectory); + moveit_visual_tools.trigger(); + prompt("Press 'next' in the RvizVisualToolsGui window to execute"); + draw_title("Executing"); + moveit_visual_tools.trigger(); + move_group_interface.execute(plan); + } + else + { + draw_title("Planning Failed!"); + moveit_visual_tools.trigger(); + RCLCPP_ERROR(logger, "Planning failed!"); + } + + // Shutdown ROS + rclcpp::shutdown(); + spinner.join(); + return 0; +} diff --git a/doc/tutorials/planning_around_objects/hello_moveit.cpp b/doc/tutorials/planning_around_objects/hello_moveit_panda.cpp similarity index 100% rename from doc/tutorials/planning_around_objects/hello_moveit.cpp rename to doc/tutorials/planning_around_objects/hello_moveit_panda.cpp diff --git a/doc/tutorials/planning_around_objects/planning_around_object.png b/doc/tutorials/planning_around_objects/planning_around_object.png index 7715e578b7..bf20a93e0a 100644 Binary files a/doc/tutorials/planning_around_objects/planning_around_object.png and b/doc/tutorials/planning_around_objects/planning_around_object.png differ diff --git a/doc/tutorials/planning_around_objects/planning_around_objects.rst b/doc/tutorials/planning_around_objects/planning_around_objects.rst index e3dc6ecade..5c073592bf 100644 --- a/doc/tutorials/planning_around_objects/planning_around_objects.rst +++ b/doc/tutorials/planning_around_objects/planning_around_objects.rst @@ -100,7 +100,7 @@ This code block should directly follow the code block that creates the collision Just as we did in the last tutorial, start RViz using the ``demo.launch.py`` script and run our program. If you're using one of the Docker tutorial containers, you can specify a different RViz configuration that already has the RvizVisualToolsGui panel added using: :: - ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_hello_moveit.rviz + ros2 launch moveit2_tutorials demo.launch.py rviz_config:=kinova_hello_moveit.rviz .. image:: planning_around_object.png @@ -108,7 +108,7 @@ Summary ------- - You extended the program you wrote with MoveIt to plan around an object in the scene. -- :codedir:`Here is a copy of the full hello_moveit.cpp source`. +- :codedir:`Here is a copy of the full hello_moveit.cpp source`. Further Reading --------------- diff --git a/doc/tutorials/quickstart_in_rviz/launch/demo.launch.py b/doc/tutorials/quickstart_in_rviz/launch/demo.launch.py index cf7db5566f..54228e94f5 100644 --- a/doc/tutorials/quickstart_in_rviz/launch/demo.launch.py +++ b/doc/tutorials/quickstart_in_rviz/launch/demo.launch.py @@ -16,7 +16,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "rviz_config", - default_value="panda_moveit_config_demo.rviz", + default_value="kinova_moveit_config_demo.rviz", description="RViz configuration file", ) ) @@ -28,15 +28,24 @@ def generate_launch_description(): def launch_setup(context, *args, **kwargs): + launch_arguments = { + "robot_ip": "xxx.yyy.zzz.www", + "use_fake_hardware": "true", + "gripper": "robotiq_2f_85", + "dof": "7", + } + moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + MoveItConfigsBuilder( + "gen3", package_name="kinova_gen3_7dof_robotiq_2f_85_moveit_config" + ) + .robot_description(mappings=launch_arguments) + .trajectory_execution(file_path="config/moveit_controllers.yaml") .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True ) .planning_pipelines( - pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"] + pipelines=["ompl", "stomp", "pilz_industrial_motion_planner"] ) .to_moveit_configs() ) @@ -76,7 +85,7 @@ def launch_setup(context, *args, **kwargs): executable="static_transform_publisher", name="static_transform_publisher", output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], ) # Publish TF @@ -90,7 +99,7 @@ def launch_setup(context, *args, **kwargs): # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), + get_package_share_directory("kinova_gen3_7dof_robotiq_2f_85_moveit_config"), "config", "ros2_controllers.yaml", ) @@ -106,8 +115,6 @@ def launch_setup(context, *args, **kwargs): executable="spawner", arguments=[ "joint_state_broadcaster", - "--controller-manager-timeout", - "300", "--controller-manager", "/controller_manager", ], @@ -116,13 +123,13 @@ def launch_setup(context, *args, **kwargs): arm_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["panda_arm_controller", "-c", "/controller_manager"], + arguments=["joint_trajectory_controller", "-c", "/controller_manager"], ) hand_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["panda_hand_controller", "-c", "/controller_manager"], + arguments=["robotiq_gripper_controller", "-c", "/controller_manager"], ) nodes_to_start = [ rviz_node, diff --git a/doc/tutorials/quickstart_in_rviz/launch/kinova_hello_moveit.rviz b/doc/tutorials/quickstart_in_rviz/launch/kinova_hello_moveit.rviz new file mode 100644 index 0000000000..d45296f1bb --- /dev/null +++ b/doc/tutorials/quickstart_in_rviz/launch/kinova_hello_moveit.rviz @@ -0,0 +1,654 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + - /MarkerArray1 + Splitter Ratio: 0.5 + Tree Height: 114 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/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: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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: /display_planned_path + Use Sim Time: false + Value: true + - 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: true + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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: /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: manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Path: true + Sphere: true + Text: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: rviz_visual_tools + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.8008623123168945 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5753980278968811 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.4903981685638428 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000257000002d6fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000fb000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000013c000001900000016900fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006901000002d20000003f0000003f00ffffff000000010000010f000002d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000028b000002d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 1994 diff --git a/doc/tutorials/quickstart_in_rviz/launch/kinova_moveit_config_demo.rviz b/doc/tutorials/quickstart_in_rviz/launch/kinova_moveit_config_demo.rviz new file mode 100644 index 0000000000..5192daa733 --- /dev/null +++ b/doc/tutorials/quickstart_in_rviz/launch/kinova_moveit_config_demo.rviz @@ -0,0 +1,630 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 137 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/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: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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: /display_planned_path + Value: true + - 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: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + 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: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: 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: /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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_left_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_inner_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_85_right_knuckle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.8008623123168945 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5753980278968811 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.4903981685638428 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 812 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000257000002d2fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000114000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000157000001b80000017d00ffffff000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000028b000002d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1533 + X: 1992 + Y: 231 diff --git a/doc/tutorials/quickstart_in_rviz/launch/kinova_moveit_config_demo_empty.rviz b/doc/tutorials/quickstart_in_rviz/launch/kinova_moveit_config_demo_empty.rviz new file mode 100644 index 0000000000..bfd8f12767 --- /dev/null +++ b/doc/tutorials/quickstart_in_rviz/launch/kinova_moveit_config_demo_empty.rviz @@ -0,0 +1,123 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 865 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/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 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.8008623123168945 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5753980278968811 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.4903981685638428 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 1992 + Y: 27 diff --git a/doc/tutorials/quickstart_in_rviz/launch/panda_demo.launch.py b/doc/tutorials/quickstart_in_rviz/launch/panda_demo.launch.py new file mode 100644 index 0000000000..cf7db5566f --- /dev/null +++ b/doc/tutorials/quickstart_in_rviz/launch/panda_demo.launch.py @@ -0,0 +1,138 @@ +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "rviz_config", + default_value="panda_moveit_config_demo.rviz", + description="RViz configuration file", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) + + +def launch_setup(context, *args, **kwargs): + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines( + pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"] + ) + .to_moveit_configs() + ) + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict()], + ) + + rviz_base = LaunchConfiguration("rviz_config") + rviz_config = PathJoinSubstitution( + [FindPackageShare("moveit2_tutorials"), "launch", rviz_base] + ) + + # RViz + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="both", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + nodes_to_start = [ + rviz_node, + static_tf, + robot_state_publisher, + run_move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + arm_controller_spawner, + hand_controller_spawner, + ] + + return nodes_to_start diff --git a/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst b/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst index 743b97d067..fe3336e28a 100644 --- a/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst +++ b/doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst @@ -30,7 +30,7 @@ Step 1: Launch the Demo and Configure the Plugin |C| - * You should now see the Panda robot in RViz: + * You should now see the Kinova robot in RViz: |D| @@ -46,9 +46,9 @@ Step 1: Launch the Demo and Configure the Plugin .. |D| image:: rviz_start.png :width: 700px -* Once you have the Motion Planning Plugin loaded, we can configure it. In the "Global Options" tab of the "Displays" subwindow, set the **Fixed Frame** field to ``/panda_link0`` +* Once you have the Motion Planning Plugin loaded, we can configure it. In the "Global Options" tab of the "Displays" subwindow, set the **Fixed Frame** field to ``/base_link`` -* Now, you can start configuring the Plugin for your robot (the Panda in this case). Click on "MotionPlanning" within "Displays". +* Now, you can start configuring the Plugin for your robot (the Kinova Gen 3 in this case). Click on "MotionPlanning" within "Displays". * Make sure the **Robot Description** field is set to ``robot_description``. @@ -57,7 +57,7 @@ Step 1: Launch the Demo and Configure the Plugin * Make sure the **Trajectory Topic** under **Planned Path** is set to ``/display_planned_path``. - * In **Planning Request**, change the **Planning Group** to ``panda_arm``. You can also see this in the MotionPlanning panel in the bottom left. + * In **Planning Request**, change the **Planning Group** to ``manipulator``. You can also see this in the MotionPlanning panel in the bottom left. .. image:: rviz_plugin_start.png @@ -91,8 +91,8 @@ The display states for each of these visualizations can be toggled on and off us .. image:: rviz_plugin_visualize_robots.png :width: 700px -Step 3: Interact with the Panda -------------------------------- +Step 3: Interact with the Kinova Gen 3 +-------------------------------------- For the next steps we will want only the scene robot, start state and goal state: @@ -154,10 +154,10 @@ You can use the **Joints** tab to move single joints and the redundant joints of The joints moving while the end effector stays still -Step 4: Use Motion Planning with the Panda -------------------------------------------- +Step 4: Use Motion Planning with the Kinova Gen 3 +------------------------------------------------- -* Now, you can start motion planning with the Panda in the MoveIt RViz Plugin. +* Now, you can start motion planning with the Kinova Gen 3 in the MoveIt RViz Plugin. * Move the Start State to a desired location. @@ -188,7 +188,7 @@ You can visually introspect trajectories point by point in RViz. Note: Once you placed your end-effector to a new goal, be sure to run *Plan* before running *Play* -- otherwise you'll see the waypoints for the previous goal if available. -.. image:: rviz_plugin_slider.png +.. image:: rviz_plugin_plan_slider.png :width: 700px Plan Cartesian motions @@ -196,17 +196,17 @@ Plan Cartesian motions If the "Use Cartesian Path" checkbox is activated, the robot will attempt to move the end effector linearly in cartesian space. -.. image:: rviz_plan_free.png +.. image:: rviz_plugin_plan_free.png :width: 700px -.. image:: rviz_plan_cartesian.png +.. image:: rviz_plugin_plan_cartesian.png :width: 700px Executing Trajectories, Adjusting Speed +++++++++++++++++++++++++++++++++++++++ -Clicking "Plan & Execute" or "Execute" after a successful plan will send the trajectory to the robot - in this tutorial, since you used ``demo.launch``, the robot is only simulated. +Clicking "Plan & Execute" or "Execute" after a successful plan will send the trajectory to the robot - in this tutorial, since you used ``kinova_demo.launch``, the robot is only simulated. Initially, the default velocity and acceleration are scaled to 10% (``0.1``) of the robot's maximum. You can change these scaling factors in the Planning tab shown below, or change these default values in the ``moveit_config`` of your robot (in ``joint_limits.yaml``). diff --git a/doc/tutorials/quickstart_in_rviz/rviz_motion_planning.png b/doc/tutorials/quickstart_in_rviz/rviz_motion_planning.png deleted file mode 100644 index 67457354ab..0000000000 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_motion_planning.png and /dev/null differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_panels.png b/doc/tutorials/quickstart_in_rviz/rviz_panels.png index f12ebc8ad7..7fdc790535 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_panels.png and b/doc/tutorials/quickstart_in_rviz/rviz_panels.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plan_free.png b/doc/tutorials/quickstart_in_rviz/rviz_plan_free.png deleted file mode 100644 index f8cd16341c..0000000000 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plan_free.png and /dev/null differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision.png index 7ee3413986..d6bca14283 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png index 22263811ac..66f1727b29 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_head.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_head.png index a7939d4ab9..5de947381a 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_head.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_head.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_interact.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_interact.png index f8f58e8a91..f2d4e025f7 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_interact.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_interact.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_invalid.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_invalid.png index bf2e365735..a21ef04009 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_invalid.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_invalid.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_motion_planning.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_motion_planning.png new file mode 100644 index 0000000000..28bdd6b6e4 Binary files /dev/null and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_motion_planning.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_cartesian.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_cartesian.png new file mode 100644 index 0000000000..3f5d5a07b2 Binary files /dev/null and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_cartesian.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_free.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_free.png new file mode 100644 index 0000000000..5d2d73d683 Binary files /dev/null and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_free.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_slider.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_slider.png new file mode 100644 index 0000000000..f56a20ac7d Binary files /dev/null and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_plan_slider.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_planned_path.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_planned_path.png index 2c89ade1da..57b372d2dd 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_planned_path.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_planned_path.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_slider.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_slider.png deleted file mode 100644 index 805626986a..0000000000 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_slider.png and /dev/null differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_start.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_start.png index 49595ddd26..d994269856 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_start.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_start.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_plugin_visualize_robots.png b/doc/tutorials/quickstart_in_rviz/rviz_plugin_visualize_robots.png index 82be09a0c1..77352e181f 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_plugin_visualize_robots.png and b/doc/tutorials/quickstart_in_rviz/rviz_plugin_visualize_robots.png differ diff --git a/doc/tutorials/quickstart_in_rviz/rviz_start.png b/doc/tutorials/quickstart_in_rviz/rviz_start.png index c959545112..3d27750e7e 100644 Binary files a/doc/tutorials/quickstart_in_rviz/rviz_start.png and b/doc/tutorials/quickstart_in_rviz/rviz_start.png differ diff --git a/doc/tutorials/quickstart_in_rviz/test/bringup_test.cpp b/doc/tutorials/quickstart_in_rviz/test/bringup_test.cpp index 4554604fac..2fe80a5f20 100644 --- a/doc/tutorials/quickstart_in_rviz/test/bringup_test.cpp +++ b/doc/tutorials/quickstart_in_rviz/test/bringup_test.cpp @@ -50,17 +50,16 @@ TEST_F(BringupTestFixture, BasicBringupTest) { // Check for several expected action servers auto control_client = rclcpp_action::create_client( - node_, "/panda_arm_controller/follow_joint_trajectory"); + node_, "/joint_trajectory_controller/follow_joint_trajectory"); EXPECT_TRUE(control_client->wait_for_action_server()); auto move_group_client = rclcpp_action::create_client(node_, "/move_action"); EXPECT_TRUE(move_group_client->wait_for_action_server()); // Send a trajectory request trajectory_msgs::msg::JointTrajectory traj_msg; - traj_msg.joint_names = { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", - "panda_joint5", "panda_joint6", "panda_joint7" }; + traj_msg.joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7" }; trajectory_msgs::msg::JointTrajectoryPoint point_msg; - point_msg.positions = { 0, -0.785, 0, -2.356, 0, 1.571, 0.785 }; + point_msg.positions = { 0, 0.26, 3.14, -2.27, 0, 0.96, 1.57 }; point_msg.time_from_start.sec = 1; traj_msg.points.push_back(point_msg); control_msgs::action::FollowJointTrajectory::Goal joint_traj_request; diff --git a/doc/tutorials/visualizing_in_rviz/add_button.png b/doc/tutorials/visualizing_in_rviz/add_button.png index cd3135bfb8..b867b2c92c 100644 Binary files a/doc/tutorials/visualizing_in_rviz/add_button.png and b/doc/tutorials/visualizing_in_rviz/add_button.png differ diff --git a/doc/tutorials/visualizing_in_rviz/add_rviz_tools_gui.png b/doc/tutorials/visualizing_in_rviz/add_rviz_tools_gui.png index b4eb276c54..be420174e9 100644 Binary files a/doc/tutorials/visualizing_in_rviz/add_rviz_tools_gui.png and b/doc/tutorials/visualizing_in_rviz/add_rviz_tools_gui.png differ diff --git a/doc/tutorials/visualizing_in_rviz/executing.png b/doc/tutorials/visualizing_in_rviz/executing.png index bffeb167b0..4614bb6f89 100644 Binary files a/doc/tutorials/visualizing_in_rviz/executing.png and b/doc/tutorials/visualizing_in_rviz/executing.png differ diff --git a/doc/tutorials/visualizing_in_rviz/kinova_hello_moveit.cpp b/doc/tutorials/visualizing_in_rviz/kinova_hello_moveit.cpp new file mode 100644 index 0000000000..4a8f710a34 --- /dev/null +++ b/doc/tutorials/visualizing_in_rviz/kinova_hello_moveit.cpp @@ -0,0 +1,91 @@ +#include +#include + +#include +#include +#include + +int main(int argc, char* argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("hello_moveit"); + + // We spin up a SingleThreadedExecutor for the current state monitor to get + // information about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto spinner = std::thread([&executor]() { executor.spin(); }); + + // Create the MoveIt MoveGroup Interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "manipulator"); + + // Construct and initialize MoveItVisualTools + auto moveit_visual_tools = + moveit_visual_tools::MoveItVisualTools{ node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC, + move_group_interface.getRobotModel() }; + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.loadRemoteControl(); + + // Create a closure for updating the text in rviz + auto const draw_title = [&moveit_visual_tools](auto text) { + auto const text_pose = [] { + auto msg = Eigen::Isometry3d::Identity(); + msg.translation().z() = 1.0; // Place text 1m above the base link + return msg; + }(); + moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); + }; + auto const prompt = [&moveit_visual_tools](auto text) { moveit_visual_tools.prompt(text); }; + auto const draw_trajectory_tool_path = + [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("manipulator")]( + auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); }; + + // Set a target Pose + auto const target_pose = [] { + geometry_msgs::msg::Pose msg; + msg.orientation.w = 1.0; + msg.position.x = 0.28; + msg.position.y = -0.2; + msg.position.z = 0.5; + return msg; + }(); + move_group_interface.setPoseTarget(target_pose); + + // Create a plan to that target pose + prompt("Press 'next' in the RvizVisualToolsGui window to plan"); + draw_title("Planning"); + moveit_visual_tools.trigger(); + auto const [success, plan] = [&move_group_interface] { + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if (success) + { + draw_trajectory_tool_path(plan.trajectory); + moveit_visual_tools.trigger(); + prompt("Press 'next' in the RvizVisualToolsGui window to execute"); + draw_title("Executing"); + moveit_visual_tools.trigger(); + move_group_interface.execute(plan); + } + else + { + draw_title("Planning Failed!"); + moveit_visual_tools.trigger(); + RCLCPP_ERROR(logger, "Planning failed!"); + } + + // Shutdown ROS + rclcpp::shutdown(); + spinner.join(); + return 0; +} diff --git a/doc/tutorials/visualizing_in_rviz/marker_array.png b/doc/tutorials/visualizing_in_rviz/marker_array.png index eff6183881..a975721e93 100644 Binary files a/doc/tutorials/visualizing_in_rviz/marker_array.png and b/doc/tutorials/visualizing_in_rviz/marker_array.png differ diff --git a/doc/tutorials/visualizing_in_rviz/marker_array_topic.png b/doc/tutorials/visualizing_in_rviz/marker_array_topic.png index 3c115b2c13..50512214d5 100644 Binary files a/doc/tutorials/visualizing_in_rviz/marker_array_topic.png and b/doc/tutorials/visualizing_in_rviz/marker_array_topic.png differ diff --git a/doc/tutorials/visualizing_in_rviz/next_button.png b/doc/tutorials/visualizing_in_rviz/next_button.png index 058f3632c3..d71666c5c3 100644 Binary files a/doc/tutorials/visualizing_in_rviz/next_button.png and b/doc/tutorials/visualizing_in_rviz/next_button.png differ diff --git a/doc/tutorials/visualizing_in_rviz/hello_moveit.cpp b/doc/tutorials/visualizing_in_rviz/panda_hello_moveit.cpp similarity index 100% rename from doc/tutorials/visualizing_in_rviz/hello_moveit.cpp rename to doc/tutorials/visualizing_in_rviz/panda_hello_moveit.cpp diff --git a/doc/tutorials/visualizing_in_rviz/panel_menu.png b/doc/tutorials/visualizing_in_rviz/panel_menu.png index f53a8d8d77..613d5848b4 100644 Binary files a/doc/tutorials/visualizing_in_rviz/panel_menu.png and b/doc/tutorials/visualizing_in_rviz/panel_menu.png differ diff --git a/doc/tutorials/visualizing_in_rviz/planning.png b/doc/tutorials/visualizing_in_rviz/planning.png index c2dc533800..bd6bac119f 100644 Binary files a/doc/tutorials/visualizing_in_rviz/planning.png and b/doc/tutorials/visualizing_in_rviz/planning.png differ diff --git a/doc/tutorials/visualizing_in_rviz/uncheck_motion_planning.png b/doc/tutorials/visualizing_in_rviz/uncheck_motion_planning.png index e522ecc58b..3977098685 100644 Binary files a/doc/tutorials/visualizing_in_rviz/uncheck_motion_planning.png and b/doc/tutorials/visualizing_in_rviz/uncheck_motion_planning.png differ diff --git a/doc/tutorials/visualizing_in_rviz/unchecked_motion_planning.png b/doc/tutorials/visualizing_in_rviz/unchecked_motion_planning.png index 3789238620..a29e21358f 100644 Binary files a/doc/tutorials/visualizing_in_rviz/unchecked_motion_planning.png and b/doc/tutorials/visualizing_in_rviz/unchecked_motion_planning.png differ diff --git a/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.rst b/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.rst index 6ae6c48faa..6d632adf5e 100644 --- a/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.rst +++ b/doc/tutorials/visualizing_in_rviz/visualizing_in_rviz.rst @@ -101,11 +101,11 @@ Next, we will construct and initialize MoveItVisualTools after the construction // Create the MoveIt MoveGroup Interface using moveit::planning_interface::MoveGroupInterface; - auto move_group_interface = MoveGroupInterface(node, "panda_arm"); + auto move_group_interface = MoveGroupInterface(node, "manipulator"); // Construct and initialize MoveItVisualTools auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{ - node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, + node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group_interface.getRobotModel()}; moveit_visual_tools.deleteAllMarkers(); moveit_visual_tools.loadRemoteControl(); @@ -138,7 +138,7 @@ After we've constructed and initialized, we now create some closures (function o auto const draw_trajectory_tool_path = [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup( - "panda_arm")](auto const trajectory) { + "manipulator")](auto const trajectory) { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); }; diff --git a/doc/tutorials/your_first_project/kinova_hello_moveit.cpp b/doc/tutorials/your_first_project/kinova_hello_moveit.cpp new file mode 100644 index 0000000000..804bf84427 --- /dev/null +++ b/doc/tutorials/your_first_project/kinova_hello_moveit.cpp @@ -0,0 +1,51 @@ +#include + +#include +#include + +int main(int argc, char* argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("hello_moveit"); + + // Create the MoveIt MoveGroup Interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "manipulator"); + + // Set a target Pose + auto const target_pose = [] { + geometry_msgs::msg::Pose msg; + msg.orientation.w = 1.0; + msg.position.x = 0.28; + msg.position.y = -0.2; + msg.position.z = 0.5; + return msg; + }(); + move_group_interface.setPoseTarget(target_pose); + + // Create a plan to that target pose + auto const [success, plan] = [&move_group_interface] { + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if (success) + { + move_group_interface.execute(plan); + } + else + { + RCLCPP_ERROR(logger, "Planning failed!"); + } + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/doc/tutorials/your_first_project/hello_moveit.cpp b/doc/tutorials/your_first_project/panda_hello_moveit.cpp similarity index 100% rename from doc/tutorials/your_first_project/hello_moveit.cpp rename to doc/tutorials/your_first_project/panda_hello_moveit.cpp diff --git a/doc/tutorials/your_first_project/rviz_1.png b/doc/tutorials/your_first_project/rviz_1.png index 19857c2461..712fc24fc6 100644 Binary files a/doc/tutorials/your_first_project/rviz_1.png and b/doc/tutorials/your_first_project/rviz_1.png differ diff --git a/doc/tutorials/your_first_project/rviz_2.png b/doc/tutorials/your_first_project/rviz_2.png index 8efc32f492..321a3ea9fd 100644 Binary files a/doc/tutorials/your_first_project/rviz_2.png and b/doc/tutorials/your_first_project/rviz_2.png differ diff --git a/doc/tutorials/your_first_project/your_first_project.rst b/doc/tutorials/your_first_project/your_first_project.rst index b38f43c498..128048492f 100644 --- a/doc/tutorials/your_first_project/your_first_project.rst +++ b/doc/tutorials/your_first_project/your_first_project.rst @@ -139,7 +139,7 @@ In place of the comment that says "Next step goes here", add this code: // Create the MoveIt MoveGroup Interface using moveit::planning_interface::MoveGroupInterface; - auto move_group_interface = MoveGroupInterface(node, "panda_arm"); + auto move_group_interface = MoveGroupInterface(node, "manipulator"); // Set a target Pose auto const target_pose = []{ @@ -216,13 +216,13 @@ If it fails to find that within 10 seconds, it prints this error and terminates The first thing we do is create the ``MoveGroupInterface``. This object will be used to interact with ``move_group``, which allows us to plan and execute trajectories. Note that this is the only mutable object that we create in this program. -Another thing to take note of is the second argument to the ``MoveGroupInterface`` object we are creating here: ``"panda_arm"``. +Another thing to take note of is the second argument to the ``MoveGroupInterface`` object we are creating here: ``"manipulator"``. That is the group of joints as defined in the robot description that we are going to operate on with this ``MoveGroupInterface``. .. code-block:: C++ using moveit::planning_interface::MoveGroupInterface; - auto move_group_interface = MoveGroupInterface(node, "panda_arm"); + auto move_group_interface = MoveGroupInterface(node, "manipulator"); Then, we set our target pose and plan. Note that only the target pose is set (via ``setPoseTarget``). The starting pose is implicitly the position published by the joint state publisher, which could be changed using the diff --git a/package.xml b/package.xml index 05052d69f1..9f4fca44ab 100644 --- a/package.xml +++ b/package.xml @@ -60,6 +60,7 @@ ros2_control rviz2 xacro + kinova_gen3_7dof_robotiq_2f_85_moveit_config ament_cmake_gtest ros_testing