diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 2371c93181..ce6675ffce 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -32,7 +32,7 @@ RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" &&\ # Remove the hello_world tutorial cpp file and replace it with the planning_around_objects file RUN rm src/hello_moveit/src/hello_moveit.cpp -COPY ./doc/tutorials/planning_around_objects/hello_moveit.cpp src/hello_moveit/src/hello_moveit.cpp +COPY ./doc/tutorials/planning_around_objects/hello_moveit_kinova.cpp src/hello_moveit/src/hello_moveit.cpp ######################### Pick and Place (MTC) Image ######################################### 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/examples/realtime_servo/CMakeLists.txt b/doc/examples/realtime_servo/CMakeLists.txt index 0dce73a746..a33cf18998 100644 --- a/doc/examples/realtime_servo/CMakeLists.txt +++ b/doc/examples/realtime_servo/CMakeLists.txt @@ -1,15 +1,10 @@ -add_executable(servo_keyboard_input src/servo_keyboard_input.cpp) -target_include_directories(servo_keyboard_input PUBLIC include) -ament_target_dependencies(servo_keyboard_input ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -add_executable(servo_cpp_interface_demo src/servo_cpp_interface_demo.cpp) -target_include_directories(servo_cpp_interface_demo PUBLIC include) -ament_target_dependencies(servo_cpp_interface_demo ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_executable(pose_tracking_tutorial src/pose_tracking_tutorial.cpp) +target_include_directories(pose_tracking_tutorial PUBLIC include) +ament_target_dependencies(pose_tracking_tutorial ${THIS_PACKAGE_INCLUDE_DEPENDS}) install( TARGETS - servo_keyboard_input - servo_cpp_interface_demo + pose_tracking_tutorial DESTINATION lib/${PROJECT_NAME} ) diff --git a/doc/examples/realtime_servo/config/demo_rviz_config.rviz b/doc/examples/realtime_servo/config/demo_rviz_config.rviz index e44c6604e8..a50d33565d 100644 --- a/doc/examples/realtime_servo/config/demo_rviz_config.rviz +++ b/doc/examples/realtime_servo/config/demo_rviz_config.rviz @@ -6,8 +6,10 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /TF1/Frames1 + - /MarkerArray1 Splitter Ratio: 0.5 - Tree Height: 628 + Tree Height: 549 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -21,6 +23,11 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -46,12 +53,12 @@ Visualization Manager: Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /moveit_servo/publish_planning_scene + Planning Scene Topic: /servo_node/publish_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 + Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -136,7 +143,7 @@ Visualization Manager: panda_leftfinger: Value: false panda_link0: - Value: false + Value: true panda_link1: Value: false panda_link2: @@ -156,7 +163,7 @@ Visualization Manager: panda_rightfinger: Value: false world: - Value: true + Value: false Marker Scale: 1 Name: TF Show Arrows: true @@ -180,6 +187,18 @@ Visualization Manager: {} Update Interval: 0 Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path_markers + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -195,6 +214,9 @@ Visualization Manager: - 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 @@ -223,39 +245,41 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.155569553375244 + Distance: 4.086756229400635 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.08608254045248032 - Y: -0.20677587389945984 - Z: 0.3424459993839264 + 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.4603978991508484 + Pitch: 0.785398006439209 Target Frame: Value: Orbit (rviz) - Yaw: 0.8753982782363892 + Yaw: 0.785398006439209 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 857 + Height: 846 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000002fffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ff000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000416000002ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Time: + collapsed: false Tool Properties: collapsed: false Views: - collapsed: true - Width: 1586 - X: 1179 - Y: 393 + collapsed: false + Width: 1200 + X: 74 + Y: 60 diff --git a/doc/examples/realtime_servo/config/panda_simulated_config.yaml b/doc/examples/realtime_servo/config/panda_simulated_config.yaml new file mode 100644 index 0000000000..35e8eedd25 --- /dev/null +++ b/doc/examples/realtime_servo/config/panda_simulated_config.yaml @@ -0,0 +1,62 @@ +############################################### +# Modify all parameters related to servoing here +############################################### + +# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) +# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + +## Properties of outgoing commands +publish_period: 0.034 # 1/Nominal publish rate [seconds] + +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +use_smoothing: true +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: true + +## MoveIt properties +move_group_name: panda_arm # Often 'manipulator' or 'arm' +planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame: panda_hand # The name of the end effector link, used to return the EE pose + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity. + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py b/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py new file mode 100644 index 0000000000..1a09a29175 --- /dev/null +++ b/doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py @@ -0,0 +1,152 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Launch Servo as a standalone node or as a "node component" for better latency/efficiency + launch_as_standalone_node = LaunchConfiguration( + "launch_as_standalone_node", default="false" + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit2_tutorials") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # 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 = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + # Example of launching Servo as a node component + # Launching as a node component makes ROS 2 intraprocess communication more efficient. + launch_ros.descriptions.ComposableNode( + package="moveit_servo", + plugin="moveit_servo::ServoNode", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + condition=UnlessCondition(launch_as_standalone_node), + ), + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="servo_node", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + condition=IfCondition(launch_as_standalone_node), + ) + + demo_node = launch_ros.actions.Node( + package="moveit2_tutorials", + executable="pose_tracking_tutorial", + name="pose_tracking_tutorial", + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + launch.actions.TimerAction(period=10.0, actions=[demo_node]), + ] + ) diff --git a/doc/examples/realtime_servo/launch/servo_cpp_interface_demo.launch.py b/doc/examples/realtime_servo/launch/servo_cpp_interface_demo.launch.py deleted file mode 100644 index 4e8aefc6e2..0000000000 --- a/doc/examples/realtime_servo/launch/servo_cpp_interface_demo.launch.py +++ /dev/null @@ -1,99 +0,0 @@ -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from launch.actions import ExecuteProcess -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - # Get parameters for the Servo node - servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config.yaml") - .to_dict() - } - - # A node to publish world -> panda_link0 transform - 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"], - ) - - # The servo cpp interface demo - # Creates the Servo node and publishes commands to it - servo_node = Node( - package="moveit2_tutorials", - executable="servo_cpp_interface_demo", - output="screen", - parameters=[ - servo_params, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - ], - ) - - # Publishes tf's for the robot - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[moveit_config.robot_description], - ) - - # RViz - rviz_config_file = ( - get_package_share_directory("moveit2_tutorials") - + "/config/demo_rviz_config.rviz" - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - ], - ) - - # ros2_control using FakeSystem as hardware - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[ - moveit_config.robot_description, - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "ros2_controllers.yaml", - ), - ], - output="both", - ) - - # Load controllers - load_controllers = [] - for controller in ["panda_arm_controller", "joint_state_broadcaster"]: - load_controllers += [ - ExecuteProcess( - cmd=["ros2 run controller_manager spawner {}".format(controller)], - shell=True, - output="screen", - ) - ] - - return LaunchDescription( - [rviz_node, static_tf, servo_node, ros2_control_node, robot_state_publisher] - + load_controllers - ) diff --git a/doc/examples/realtime_servo/realtime_servo_tutorial.rst b/doc/examples/realtime_servo/realtime_servo_tutorial.rst index 819a0fd0ea..186b7239ee 100644 --- a/doc/examples/realtime_servo/realtime_servo_tutorial.rst +++ b/doc/examples/realtime_servo/realtime_servo_tutorial.rst @@ -1,116 +1,55 @@ -Realtime Arm Servoing -===================== +Realtime Servo +=============== -MoveIt Servo allows you to stream End Effector (EEF) velocity commands to your manipulator and have it execute them concurrently. This enables teleoperation via a wide range of input schemes, or for other autonomous software to control the robot - in visual servoing or closed loop position control for instance. +MoveIt Servo facilitates realtime control of your robot arm. -This tutorial shows how to use MoveIt Servo to send real-time servo commands to a ROS-enabled robot. Some nice features of the servo node are singularity handling and collision checking that prevents the operator from breaking the robot. .. raw:: html
- +
-Getting Started ---------------- -If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. - -Launching a Servo Node ----------------------- -MoveIt Servo can be launched as a "node component" or a standalone node. The launch file, moveit_servo/servo_example.launch.py, launches a standalone node by default but also contains a commented component node. Commands are sent via ROS topics. The commands can come from anywhere, such as a joystick, keyboard, or other controller. - -This demo was written for an Xbox 1 controller, but can be easily modified to use any controller compatible with the `Joy package `_ by modifying the `joystick_servo_example.cpp file `_ - -To run the demo, make sure your controller is plugged in and can be detected by :code:`ros2 run joy joy_node`. Usually this happens automatically after plugging the controller in. Then launch with :: - - ros2 launch moveit_servo servo_example.launch.py - -Make a service request to start Servo :: - - ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {} - -You should be able to control the arm with your controller now, with MoveIt Servo automatically avoiding singularities and collisions. - -Without a Controller -^^^^^^^^^^^^^^^^^^^^ - -If you do not have a joystick or game controller, you can still try the demo using your keyboard. With the demo still running, in a new terminal, run :: - - ros2 run moveit2_tutorials servo_keyboard_input - -You will be able to use your keyboard to servo the robot. Send Cartesian commands with arrow keys and the :code:`.` and :code:`;` keys. Change the Cartesian command frame with :code:`W` for world and :code:`E` for End-Effector. Send joint jogging commands with keys 1-7 (use :code:`R` to reverse direction) - -.. raw:: html - - - -Note that the controller overlay here is just for demonstration purposes and is not actually included - -Introspection -------------- - -Here are some tips for inspecting and/or debugging the system. +MoveIt Servo accepts any of the following types of commands: -#. View the :code:`ros2_controllers` that are currently active with :code:`ros2 control list_controllers`. You will see a `JointTrajectoryController `_ that receives the joint position commands from Servo and handles them in the simulated robot driver. The JointTrajectoryController is very flexible; it can handle any combination of position/velocity/(position-and-velocity) input. Servo is also compatible with `JointGroupPosition `_ or `JointGroupVelocity `_-type controllers. + 1. Individual joint velocities. + 2. The desired velocity of end effector. + 3. The desired pose of end effector. -#. :code:`ros2 topic echo /servo_node/status` shows the current state of the Servo node. If :code:`0` is published, all is well. The definition for all enums can be seen :moveit_codedir:`here.` +This enables teleoperation via a wide range of input schemes, or for other autonomous software to control the robot - in visual servoing or closed loop position control for instance. -#. :code:`ros2 node list` shows the following. :code:`ros2 node info` can be used to get more information about any of these nodes. - - - :code:`/joy_node` handles commands from the XBox controller - - - :code:`/moveit_servo_demo_container` holds several ancillary ROS2 "component nodes" that are placed in a container for faster intra-process communication - - - :code:`/servo_node` which does the calculations and collision checking for this demo. :code:`servo_node` may be moved into the demo container in the future - -Using the C++ Interface ------------------------ -Instead of launching Servo as its own component, you can include Servo in your own nodes via the C++ interface. Sending commands to the robot is very similar in both cases, but for the C++ interface a little bit of setup for Servo is necessary. In exchange, you will be able to directly interact with Servo through its C++ API. - -This basic C++ interface demo moves the robot in a predetermined way and can be launched with :: - - ros2 launch moveit2_tutorials servo_cpp_interface_demo.launch.py +Getting Started +--------------- -An Rviz window should appear with a Panda arm and collision object. The arm will joint-jog for a few seconds before switching to a Cartesian movement. As the arm approaches the collision object, it slows and stops. +If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. -.. raw:: html - +Design overview +--------------- -Entire Code ------------ -The entire code is available :codedir:`here` +Moveit Servo consists of two main parts: The core implementation ``Servo`` which provides a C++ interface, and the ``ServoNode`` which +wraps the C++ interface and provides a ROS interface.The configuration of Servo is done through ROS parameters specified in :moveit_codedir:`servo_parameters.yaml ` -.. tutorial-formatter:: ./src/servo_cpp_interface_demo.cpp +In addition to the servoing capability, MoveIt Servo has some convenient features such as: + - Checking for singularities + - Checking for collisions + - Motion smoothing + - Joint position and velocity limits enforced -Servo Overview --------------- +Singularity checking and collision checking are safety features that scale down the velocities when approaching singularities or collisions (self collision or collision with other objects). +The collision checking and smoothing are optional features that can be disabled using the ``check_collisions`` parameter and the ``use_smoothing`` parameters respectively. -The following sections give some background information about MoveIt Servo and describe the first steps to set it up on your robot. +The inverse kinematics is handled through either the inverse Jacobain or the robot's IK solver if one was provided. -Servo includes a number of nice features: - 1. Cartesian End-Effector twist commands - 2. Joint commands - 3. Collision checking - 4. Singularity checking - 5. Joint position and velocity limits enforced - 6. Inputs are generic ROS messages Inverse Kinematics in Servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^ Inverse Kinematics may be handled internally by MoveIt Servo via inverse Jacobian calculations. However, you may also use an IK plugin. -To configure an IK plugin for use in Servo, your robot config package must define one in a :code:`kinematics.yaml` file, such as the one -in the `Panda config package `_. -Several IK plugins are available `within MoveIt `_, -as well as `externally `_. +To configure an IK plugin for use in MoveIt Servo, your robot config package must define one in a :code:`kinematics.yaml` file, such as the one +in the :moveit_resources_codedir:`Panda config package `. +Several IK plugins are available :moveit_codedir:`within MoveIt `, as well as `externally `_. :code:`bio_ik/BioIKKinematicsPlugin` is the most common choice. Once your :code:`kinematics.yaml` file has been populated, include it with the ROS parameters passed to the servo node in your launch file: @@ -124,9 +63,10 @@ Once your :code:`kinematics.yaml` file has been populated, include it with the R ) servo_node = Node( package="moveit_servo", - executable="servo_node_main", + executable="servo_node", parameters=[ servo_params, + low_pass_filter_coeff, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed @@ -134,8 +74,8 @@ Once your :code:`kinematics.yaml` file has been populated, include it with the R ) -The above excerpt is taken from `servo_example.launch.py `_ in MoveIt. -In the above example, the :code:`kinematics.yaml` file is taken from the `moveit_resources `_ repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`. +The above excerpt is taken from :moveit_codedir:`servo_example.launch.py ` in MoveIt. +In the above example, the :code:`kinematics.yaml` file is taken from the :moveit_resources_codedir:`moveit_resources ` repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`. The actual ROS parameter names that get passed by loading the yaml file are of the form :code:`robot_description_kinematics..`, e.g. :code:`robot_description_kinematics.panda_arm.kinematics_solver`. Since :code:`moveit_servo` does not allow undeclared parameters found in the :code:`kinematics.yaml` file to be set on the Servo node, custom solver parameters need to be declared from inside your plugin code. @@ -145,37 +85,164 @@ For example, :code:`bio_ik` defines a :code:`getROSParam()` function in `bio_ik/ Setup on a New Robot -------------------- -Preliminaries -^^^^^^^^^^^^^ - The bare minimum requirements for running MoveIt Servo with your robot include: - 1. A valid URDF and SRDF of the robot - 2. A controller that can accept joint positions or velocities from a ROS topic - 3. Joint encoders that provide rapid and accurate joint position feedback + 1. A valid URDF and SRDF of the robot. + 2. A controller that can accept joint positions or velocities. + 3. Joint encoders that provide rapid and accurate joint position feedback. Because the kinematics are handled by the core parts of MoveIt, it is recommended that you have a valid config package for your robot and you can run the demo launch file included with it. -Input Devices -^^^^^^^^^^^^^ -The two primary inputs to MoveIt Servo are Cartesian commands and joint commands. These come into Servo as `TwistStamped `_ and `JointJog `_ messages respectively. The source of the commands can be almost anything including: gamepads, voice commands, a SpaceNav mouse, or PID controllers (e.g. for visual servoing). +Using the C++ API +------------------ +This can be beneficial when there is a performance requirement to avoid the overhead of ROS communication infrastucture, or when the output generated by Servo needs to be fed into some other controller that does not have a ROS interface. + +When using MoveIt Servo with the C++ interface the three input command types are ``JointJogCommand``, ``TwistCommand`` and ``PoseCommand``. +The output from Servo when using the C++ interface is ``KinematicState``, a struct containing joint names, positions, velocities and accelerations. +As given by the definitions in :moveit_codedir:`datatypes ` header file. + +The first step is to create a ``Servo`` instance. + +.. code-block:: c++ + + // Import the Servo headers. + #include + #include + + // The node to be used by Servo. + rclcpp::Node::SharedPtr node = std::make_shared("servo_tutorial"); + + // Get the Servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // Create the planning scene monitor. + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(node, servo_params); + + // Create a Servo instance. + Servo servo = Servo(node, servo_param_listener, planning_scene_monitor); + + +Using the JointJogCommand + +.. code-block:: c++ + + using namespace moveit_servo; + + // Create the command. + JointJogCommand command; + command.joint_names = {"panda_link7"}; + command.velocities = {0.1}; + + // Set JointJogCommand as the input type. + servo.setCommandType(CommandType::JOINT_JOG); + + // Get the joint states required to follow the command. + // This is generally run in a loop. + KinematicState next_joint_state = servo.getNextJointState(command); + +Using the TwistCommand + +.. code-block:: c++ + + using namespace moveit_servo; + + // Create the command. + TwistCommand command{"panda_link0", {0.1, 0.0, 0.0, 0.0, 0.0, 0.0}; + + // Set the command type. + servo.setCommandType(CommandType::TWIST); + + // Get the joint states required to follow the command. + // This is generally run in a loop. + KinematicState next_joint_state = servo.getNextJointState(command); + + +Using the PoseCommand + +.. code-block:: c++ + + using namespace moveit_servo; + + // Create the command. + Eigen::Isometry3d ee_pose = Eigen::Isometry3d::Identity(); // This is a dummy pose. + PoseCommand command{"panda_link0", ee_pose}; + + // Set the command type. + servo.setCommandType(CommandType::POSE); + + // Get the joint states required to follow the command. + // This is generally run in a loop. + KinematicState next_joint_state = servo.getNextJointState(command); + +The ``next_joint_state`` result can then be used for further steps in the control pipeline. + +The status of MoveIt Servo resulting from the last command can be obtained by: + +.. code-block:: c++ + + StatusCode status = servo.getStatus(); + +The user can use status for higher-level decision making. + +See :moveit_codedir:`moveit_servo/demos ` for complete examples of using the C++ interface. +The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch `. + + - ``ros2 launch moveit_servo demo_joint_jog.launch.py`` + - ``ros2 launch moveit_servo demo_twist.launch.py`` + - ``ros2 launch moveit_servo demo_pose.launch.py`` + + +Using the ROS API +----------------- + +To use MoveIt Servo through the ROS interface, it must be launched as a ``Node`` or ``Component`` along with the required parameters as seen :moveit_codedir:`here `. + +When using MoveIt Servo with the ROS interface the commands are ROS messages of the following types published to respective topics specified by the Servo parameters. + + 1. ``control_msgs::msg::JointJog`` on the topic specified by the ``joint_command_in_topic`` parameter. + 2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by the ``cartesian_command_in_topic`` parameter. For now, the twist message must be in the planning frame of the robot. (This will be updated soon.) + 3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by the ``pose_command_in_topic`` parameter. + +Twist and Pose commands require that the ``header.frame_id`` is always specified. +The output from ``ServoNode`` (the ROS interface) can either be ``trajectory_msgs::msg::JointTrajectory`` or ``std_msgs::msg::Float64MultiArray`` +selected using the *command_out_type* parameter, and published on the topic specified by *command_out_topic* parameter. + +The command type can be selected using the ``ServoCommandType`` service, see definition :moveit_msgs_codedir:`ServoCommandType `. + +From cli : ``ros2 service call //switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"`` + +Programmatically: + +.. code-block:: c++ + + switch_input_client = node->create_client("//switch_command_type"); + auto request = std::make_shared(); + request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST; + if (switch_input_client->wait_for_service(std::chrono::seconds(1))) + { + auto result = switch_input_client->async_send_request(request); + if (result.get()->success) + { + RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Twist"); + } + else + { + RCLCPP_WARN_STREAM(node->get_logger(), "Could not switch input to: Twist"); + } + } + +Similarly, servoing can be paused using the pause service ``/pause_servo`` of type ``std_msgs::srv::SetBool``. + +When using the ROS interface, the status of Servo is available on the topic ``//status``, see definition :moveit_msgs_codedir:`ServoStatus `. -Requirements for incoming command messages, regardless of input device are: - 1. **TwistStamped and JointJog:** need a timestamp in the header that is updated when the message is published - 2. **JointJog:** must have valid joint names in the :code:`joint_names` field that correspond with the commands given in the :code:`displacements` or :code:`velocities` fields - 3. **(Optional) TwistStamped:** can provide an arbitrary :code:`frame_id` in the header that the twist will be applied to. If empty, the default from the configs is used +Launch ROS interface demo: ``ros2 launch moveit_servo demo_ros_api.launch.py``. -Servo Configs -^^^^^^^^^^^^^ +Once the demo is running, the robot can be teleoperated through the keyboard. -The `demo config file `_ shows the parameters needed for MoveIt Servo and is well documented. +Launch the keyboard demo: ``ros2 run moveit_servo servo_keyboard_input``. -Start with the parameters from the demo file, but some must be changed for your specific setup: - 1. :code:`robot_link_command_frame`: Update this to be a valid frame in your robot, recommended as the planning frame or EEF frame - 2. :code:`command_in_type`: Set to "unitless" if your input comes from a joystick, "speed_units" if the input will be in meters/second or radians/second - 3. :code:`command_out_topic`: Change this to be the input topic of your controller - 4. :code:`command_out_type`: Change this based on the type of message your controller needs - 5. :code:`publish_joint_positions` and :code:`publish_joint_velocities`: Change these based on what your controller needs. Note if :code:`command_out_type == std_msgs/Float64MultiArray`, only one of these can be True - 6. :code:`joint_topic`: Change this to be the joint_state topic for your arm, usually :code:`/joint_states` - 7. :code:`move_group_name`: Change this to be the name of your move group, as defined in your SRDF - 8. :code:`planning_frame`: This should be the planning frame of your group +An example of using the pose commands in the context of servoing to open a door can be seen in this :codedir:`example `. diff --git a/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp new file mode 100644 index 0000000000..a32bf45cb4 --- /dev/null +++ b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp @@ -0,0 +1,285 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Title : pose_tracking_tutorial.cpp + * Project : moveit2_tutorials + * Created : 08/07/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of using pose tracking via the ROS API in a door opening scenario. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * \brief Handles the simulation of the collision object representing the door. + */ +class Door +{ +public: + Door(const rclcpp::Node::SharedPtr& node) : node_(node) + { + dims_ = Eigen::Vector3d(0.5, 0.02, 0.8); + rotation_radius_ = dims_.x() / 2; + // Hinge is the bottom left corner + hinge_ = Eigen::Vector3d(0.8, 0.0, 0.0); + + center_.x() = hinge_.x() + dims_.x() / 2; + center_.y() = hinge_.y() + dims_.y() / 2 + rotation_radius_; + center_.z() = hinge_.z() + dims_.z() / 2; + + angle_ = (M_PI / 2); + + collision_object_publisher_ = + node_->create_publisher("/planning_scene", rclcpp::SystemDefaultsQoS()); + + createCollisionObject(); + } + + void rotateDoor(double angle) + { + angle_ = angle; + + collision_object_.primitives[0] = door_primitive_; + + center_.x() = hinge_.x() + (rotation_radius_ * cos(angle_)); + center_.y() = hinge_.y() + (rotation_radius_ * sin(angle_)); + geometry_msgs::msg::Pose door_pose; + door_pose.position.x = center_.x(); + door_pose.position.y = center_.y(); + door_pose.position.z = center_.z(); + auto orn = Eigen::Quaterniond(Eigen::AngleAxisd(angle_, Eigen::Vector3d::UnitZ())); + door_pose.orientation.w = orn.w(); + door_pose.orientation.x = orn.x(); + door_pose.orientation.y = orn.y(); + door_pose.orientation.z = orn.z(); + + collision_object_.operation = collision_object_.ADD; + collision_object_.primitive_poses[0] = door_pose; + collision_object_.header.stamp = node_->now(); + + moveit_msgs::msg::PlanningSceneWorld psw; + psw.collision_objects.push_back(collision_object_); + + moveit_msgs::msg::PlanningScene ps; + ps.world = psw; + ps.is_diff = true; + collision_object_publisher_->publish(ps); + } + +private: + void createCollisionObject() + { + collision_object_.id = "door"; + collision_object_.header.frame_id = "panda_link0"; + collision_object_.primitives.resize(1); + collision_object_.primitive_poses.resize(1); + + door_primitive_.type = shape_msgs::msg::SolidPrimitive::BOX; + door_primitive_.dimensions = { dims_[0], dims_[1], dims_[2] }; + } + + rclcpp::Node::SharedPtr node_; + Eigen::Vector3d hinge_, center_, dims_; + double angle_, step_, rotation_radius_; + rclcpp::Publisher::SharedPtr collision_object_publisher_; + moveit_msgs::msg::CollisionObject collision_object_; + shape_msgs::msg::SolidPrimitive door_primitive_; +}; + +/** + * \brief Generates the path to follow when opening the door. + */ +std::vector getPath() +{ + const double start_angle = M_PI / 2 + (M_PI / 8); + const double end_angle = M_PI; + const double step = 0.01745329; + std::vector traj; + + for (double i = start_angle; i < end_angle; i = i + step) + { + double x = 0.8 + (0.5 * cos(i)); + double y = 0.0 + (0.5 * sin(i)); + auto vec = Eigen::Vector3d(x, y, 0.4); + traj.push_back(vec); + } + return traj; +} + +/** + * \brief Creates an Rviz marker message to represent a waypoint in the path. + */ +visualization_msgs::msg::Marker getMarker(int id, const Eigen::Vector3d& position, const std::string& frame) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame; + marker.header.stamp = rclcpp::Time(0.0); + marker.id = id; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; + marker.scale.y = 0.01; + marker.scale.z = 0.01; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + id++; + return marker; +} + +/** + * \brief Generates a PoseStamped message with the given position and orientation. + */ +geometry_msgs::msg::PoseStamped getPose(const Eigen::Vector3d& position, const Eigen::Quaterniond& rotation) +{ + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = "panda_link0"; + target_pose.pose.orientation.w = rotation.w(); + target_pose.pose.orientation.x = rotation.x(); + target_pose.pose.orientation.y = rotation.y(); + target_pose.pose.orientation.z = rotation.z(); + target_pose.pose.position.x = position.x(); + target_pose.pose.position.y = position.y(); + target_pose.pose.position.z = position.z(); + + return target_pose; +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = std::make_shared("servo_tutorial"); + + auto marker_publisher = + node->create_publisher("/path_markers", rclcpp::SystemDefaultsQoS()); + auto pose_publisher = node->create_publisher("/servo_node/pose_target_cmds", + rclcpp::SystemDefaultsQoS()); + + auto switch_input_client = node->create_client("/servo_node/switch_command_type"); + + auto executor = std::make_shared(); + executor->add_node(node); + + // Spin the node. + std::thread executor_thread([&]() { executor->spin(); }); + + visualization_msgs::msg::MarkerArray marray; + std::vector path = getPath(); + + for (size_t i = 0; i < path.size(); i++) + { + marray.markers.push_back(getMarker(i, path[i], "panda_link0")); + } + marker_publisher->publish(marray); + + // Create the door. + Door door(node); + door.rotateDoor(M_PI / 2); + + // Default end-effector pose of the Panda arm. + auto ee_pose = Eigen::Isometry3d::Identity(); + ee_pose.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + ee_pose.translate(Eigen::Vector3d(0.3, 0.0, 0.4)); + + // Create the ROS message. + auto request = std::make_shared(); + request->command_type = moveit_msgs::srv::ServoCommandType::Request::POSE; + auto response = switch_input_client->async_send_request(request); + if (response.get()->success) + { + RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Pose"); + } + else + { + RCLCPP_WARN_STREAM(node->get_logger(), "Could not switch input to: Pose"); + } + + // Follow the trajectory + + const double publish_period = 0.15; + rclcpp::WallRate rate(1 / publish_period); + + // The path needs to be reversed since the last point in the path is where we want to start. + std::reverse(path.begin(), path.end()); + + for (auto& waypoint : path) + { + auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation())); + target_pose.header.stamp = node->now(); + pose_publisher->publish(target_pose); + rate.sleep(); + } + + // Simulated door opening + + double door_angle = M_PI / 2; + const double step = 0.01745329; // 1 degree in radian + + // Reverse again to so that we follow that path in reverse order. + std::reverse(path.begin(), path.end()); + for (auto& waypoint : path) + { + ee_pose.rotate(Eigen::AngleAxisd(-step, Eigen::Vector3d::UnitZ())); + auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation())); + target_pose.header.stamp = node->now(); + pose_publisher->publish(target_pose); + rate.sleep(); + + door.rotateDoor(door_angle); + door_angle += step; + } + + executor->cancel(); + if (executor_thread.joinable()) + { + executor_thread.join(); + } + rclcpp::shutdown(); +} diff --git a/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp b/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp deleted file mode 100644 index dcb05a2b86..0000000000 --- a/doc/examples/realtime_servo/src/servo_cpp_interface_demo.cpp +++ /dev/null @@ -1,194 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Title : servo_cpp_interface_demo.cpp - * Project : moveit2_tutorials - * Created : 07/13/2020 - * Author : Adam Pettinger - */ - -// ROS -#include - -// Auto-generated -#include -#include -#include - -using namespace std::chrono_literals; - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit2_tutorials.servo_demo_node.cpp"); - -// BEGIN_TUTORIAL - -// Setup -// ^^^^^ -// First we declare pointers to the node and publisher that will publish commands to Servo -rclcpp::Node::SharedPtr node_; -rclcpp::Publisher::SharedPtr joint_cmd_pub_; -rclcpp::Publisher::SharedPtr twist_cmd_pub_; -size_t count_ = 0; - -// BEGIN_SUB_TUTORIAL publishCommands -// Here is the timer callback for publishing commands. The C++ interface sends commands through internal ROS topics, -// just like if Servo was launched using ServoNode. -void publishCommands() -{ - // First we will publish 100 joint jogging commands. The :code:`joint_names` field allows you to specify individual - // joints to move, at the velocity in the corresponding :code:`velocities` field. It is important that the message - // contains a recent timestamp, or Servo will think the command is stale and will not move the robot. - if (count_ < 100) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->joint_names.push_back("panda_joint1"); - msg->velocities.push_back(0.3); - joint_cmd_pub_->publish(std::move(msg)); - ++count_; - } - - // After a while, we switch to publishing twist commands. The provided frame is the frame in which the twist is - // defined, not the robot frame that will follow the command. Again, we need a recent timestamp in the message - else - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->header.frame_id = "panda_link0"; - msg->twist.linear.x = 0.3; - msg->twist.angular.z = 0.5; - twist_cmd_pub_->publish(std::move(msg)); - } -} -// END_SUB_TUTORIAL - -// Next we will set up the node, planning_scene_monitor, and collision object -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - - // This is false for now until we fix the QoS settings in moveit to enable intra process comms - node_options.use_intra_process_comms(false); - node_ = std::make_shared("servo_demo_node", node_options); - - // Pause for RViz to come up. This is necessary in an integrated demo with a single launch file - rclcpp::sleep_for(std::chrono::seconds(4)); - - // Create the planning_scene_monitor. We need to pass this to Servo's constructor, and we should set it up first - // before initializing any collision objects - auto tf_buffer = std::make_shared(node_->get_clock()); - auto planning_scene_monitor = std::make_shared( - node_, "robot_description", tf_buffer, "planning_scene_monitor"); - - // Here we make sure the planning_scene_monitor is updating in real time from the joint states topic - if (planning_scene_monitor->getPlanningScene()) - { - planning_scene_monitor->startStateMonitor("/joint_states"); - planning_scene_monitor->setPlanningScenePublishingFrequency(25); - planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, - "/moveit_servo/publish_planning_scene"); - planning_scene_monitor->startSceneMonitor(); - planning_scene_monitor->providePlanningSceneService(); - } - else - { - RCLCPP_ERROR(LOGGER, "Planning scene not configured"); - return EXIT_FAILURE; - } - - // These are the publishers that will send commands to MoveIt Servo. Two command types are supported: JointJog - // messages which will directly jog the robot in the joint space, and TwistStamped messages which will move the - // specified link with the commanded Cartesian velocity. In this demo, we jog the end effector link. - joint_cmd_pub_ = node_->create_publisher("servo_demo_node/delta_joint_cmds", 10); - twist_cmd_pub_ = node_->create_publisher("servo_demo_node/delta_twist_cmds", 10); - - // Next we will create a collision object in the way of the arm. As the arm is servoed towards it, it will slow down - // and stop before colliding - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = "panda_link0"; - collision_object.id = "box"; - - // Make a box and put it in the way - shape_msgs::msg::SolidPrimitive box; - box.type = box.BOX; - box.dimensions = { 0.1, 0.4, 0.1 }; - geometry_msgs::msg::Pose box_pose; - box_pose.position.x = 0.6; - box_pose.position.y = 0.0; - box_pose.position.z = 0.6; - - // Add the box as a collision object - collision_object.primitives.push_back(box); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - - // Create the message to publish the collision object - moveit_msgs::msg::PlanningSceneWorld psw; - psw.collision_objects.push_back(collision_object); - moveit_msgs::msg::PlanningScene ps; - ps.is_diff = true; - ps.world = psw; - - // Publish the collision object to the planning scene - auto scene_pub = node_->create_publisher("planning_scene", 10); - scene_pub->publish(ps); - - // Initializing Servo - // ^^^^^^^^^^^^^^^^^^ - auto servo_param_listener = std::make_unique(node_, "moveit_servo"); - - // Initialize the Servo C++ interface by passing a pointer to the node, the parameters, and the PSM - moveit_servo::Servo servo(node_, planning_scene_monitor, std::move(servo_param_listener)); - - // You can start Servo directly using the C++ interface. If launched using the alternative ServoNode, a ROS - // service is used to start Servo. Before it is started, MoveIt Servo will not accept any commands or move the robot - servo.start(); - - // Sending Commands - // ^^^^^^^^^^^^^^^^ - // For this demo, we will use a simple ROS timer to send joint and twist commands to the robot - rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands); - - // CALL_SUB_TUTORIAL publishCommands - - // We use a multithreaded executor here because Servo has concurrent processes for moving the robot and avoiding collisions - auto executor = std::make_unique(); - executor->add_node(node_); - executor->spin(); - - // END_TUTORIAL - - rclcpp::shutdown(); - return 0; -} diff --git a/doc/examples/realtime_servo/src/servo_keyboard_input.cpp b/doc/examples/realtime_servo/src/servo_keyboard_input.cpp deleted file mode 100644 index 259952dd80..0000000000 --- a/doc/examples/realtime_servo/src/servo_keyboard_input.cpp +++ /dev/null @@ -1,313 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Title : servo_keyboard_input.cpp - * Project : moveit2_tutorials - * Created : 05/31/2021 - * Author : Adam Pettinger - */ - -#include -#include -#include - -#include -#include -#include -#include - -// Define used keys -#define KEYCODE_RIGHT 0x43 -#define KEYCODE_LEFT 0x44 -#define KEYCODE_UP 0x41 -#define KEYCODE_DOWN 0x42 -#define KEYCODE_PERIOD 0x2E -#define KEYCODE_SEMICOLON 0x3B -#define KEYCODE_1 0x31 -#define KEYCODE_2 0x32 -#define KEYCODE_3 0x33 -#define KEYCODE_4 0x34 -#define KEYCODE_5 0x35 -#define KEYCODE_6 0x36 -#define KEYCODE_7 0x37 -#define KEYCODE_Q 0x71 -#define KEYCODE_W 0x77 -#define KEYCODE_E 0x65 -#define KEYCODE_R 0x72 - -// Some constants used in the Servo Teleop demo -const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; -const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; -const size_t ROS_QUEUE_SIZE = 10; -const std::string EEF_FRAME_ID = "panda_hand"; -const std::string BASE_FRAME_ID = "panda_link0"; - -// A class for reading the key inputs from the terminal -class KeyboardReader -{ -public: - KeyboardReader() : kfd(0) - { - // get the console in raw mode - tcgetattr(kfd, &cooked); - struct termios raw; - memcpy(&raw, &cooked, sizeof(struct termios)); - raw.c_lflag &= ~(ICANON | ECHO); - // Setting a new line, then end of file - raw.c_cc[VEOL] = 1; - raw.c_cc[VEOF] = 2; - tcsetattr(kfd, TCSANOW, &raw); - } - void readOne(char* c) - { - int rc = read(kfd, c, 1); - if (rc < 0) - { - throw std::runtime_error("read failed"); - } - } - void shutdown() - { - tcsetattr(kfd, TCSANOW, &cooked); - } - -private: - int kfd; - struct termios cooked; -}; - -// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller -class KeyboardServo -{ -public: - KeyboardServo(); - int keyLoop(); - -private: - void spin(); - - rclcpp::Node::SharedPtr nh_; - - rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr joint_pub_; - - std::string frame_to_publish_; - double joint_vel_cmd_; -}; - -KeyboardServo::KeyboardServo() : frame_to_publish_(BASE_FRAME_ID), joint_vel_cmd_(1.0) -{ - nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); - - twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); - joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); -} - -KeyboardReader input; - -void quit(int sig) -{ - (void)sig; - input.shutdown(); - rclcpp::shutdown(); - exit(0); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - KeyboardServo keyboard_servo; - - signal(SIGINT, quit); - - int rc = keyboard_servo.keyLoop(); - input.shutdown(); - rclcpp::shutdown(); - - return rc; -} - -void KeyboardServo::spin() -{ - while (rclcpp::ok()) - { - rclcpp::spin_some(nh_); - } -} - -int KeyboardServo::keyLoop() -{ - char c; - bool publish_twist = false; - bool publish_joint = false; - - std::thread{ std::bind(&KeyboardServo::spin, this) }.detach(); - - puts("Reading from keyboard"); - puts("---------------------------"); - puts("Use arrow keys and the '.' and ';' keys to Cartesian jog"); - puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame"); - puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging."); - puts("'Q' to quit."); - - for (;;) - { - // get the next event from the keyboard - try - { - input.readOne(&c); - } - catch (const std::runtime_error&) - { - perror("read():"); - return -1; - } - - RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); - - // // Create the messages we might publish - auto twist_msg = std::make_unique(); - auto joint_msg = std::make_unique(); - - // Use read key-press - switch (c) - { - case KEYCODE_LEFT: - RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); - twist_msg->twist.linear.y = -1.0; - publish_twist = true; - break; - case KEYCODE_RIGHT: - RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); - twist_msg->twist.linear.y = 1.0; - publish_twist = true; - break; - case KEYCODE_UP: - RCLCPP_DEBUG(nh_->get_logger(), "UP"); - twist_msg->twist.linear.x = 1.0; - publish_twist = true; - break; - case KEYCODE_DOWN: - RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); - twist_msg->twist.linear.x = -1.0; - publish_twist = true; - break; - case KEYCODE_PERIOD: - RCLCPP_DEBUG(nh_->get_logger(), "PERIOD"); - twist_msg->twist.linear.z = -1.0; - publish_twist = true; - break; - case KEYCODE_SEMICOLON: - RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON"); - twist_msg->twist.linear.z = 1.0; - publish_twist = true; - break; - case KEYCODE_E: - RCLCPP_DEBUG(nh_->get_logger(), "E"); - frame_to_publish_ = EEF_FRAME_ID; - break; - case KEYCODE_W: - RCLCPP_DEBUG(nh_->get_logger(), "W"); - frame_to_publish_ = BASE_FRAME_ID; - break; - case KEYCODE_1: - RCLCPP_DEBUG(nh_->get_logger(), "1"); - joint_msg->joint_names.push_back("panda_joint1"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_2: - RCLCPP_DEBUG(nh_->get_logger(), "2"); - joint_msg->joint_names.push_back("panda_joint2"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_3: - RCLCPP_DEBUG(nh_->get_logger(), "3"); - joint_msg->joint_names.push_back("panda_joint3"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_4: - RCLCPP_DEBUG(nh_->get_logger(), "4"); - joint_msg->joint_names.push_back("panda_joint4"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_5: - RCLCPP_DEBUG(nh_->get_logger(), "5"); - joint_msg->joint_names.push_back("panda_joint5"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_6: - RCLCPP_DEBUG(nh_->get_logger(), "6"); - joint_msg->joint_names.push_back("panda_joint6"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_7: - RCLCPP_DEBUG(nh_->get_logger(), "7"); - joint_msg->joint_names.push_back("panda_joint7"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_R: - RCLCPP_DEBUG(nh_->get_logger(), "R"); - joint_vel_cmd_ *= -1; - break; - case KEYCODE_Q: - RCLCPP_DEBUG(nh_->get_logger(), "quit"); - return 0; - } - - // If a key requiring a publish was pressed, publish the message now - if (publish_twist) - { - twist_msg->header.stamp = nh_->now(); - twist_msg->header.frame_id = frame_to_publish_; - twist_pub_->publish(std::move(twist_msg)); - publish_twist = false; - } - else if (publish_joint) - { - joint_msg->header.stamp = nh_->now(); - joint_msg->header.frame_id = BASE_FRAME_ID; - joint_pub_->publish(std::move(joint_msg)); - publish_joint = false; - } - } - - return 0; -} diff --git a/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst b/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst index 157c29d45d..0738577e92 100644 --- a/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst +++ b/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.rst @@ -166,6 +166,7 @@ Open ``mtc_node.cpp`` in your editor of choice, and paste in the following code. geometry_msgs::msg::Pose pose; pose.position.x = 0.5; pose.position.y = -0.25; + pose.orientation.w = 1.0; object.pose = pose; moveit::planning_interface::PlanningSceneInterface psi; @@ -806,7 +807,10 @@ The next stages will be added to the serial container rather than the task. place->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" }); -This next stage generates the poses used to place the object and compute the inverse kinematics for those poses - it is somewhat similar to the ``generate grasp pose`` stage from the pick serial container. We start by creating a stage to generate the poses and inheriting the task properties. We specify the pose where we want to place the object with a ``PoseStamped`` message from ``geometry_msgs`` - in this case, we choose ``y = 0.5``. We then pass the target pose to the stage with ``setPose``. +This next stage generates the poses used to place the object and compute the inverse kinematics for those poses - it is somewhat similar to the ``generate grasp pose`` stage from the pick serial container. +We start by creating a stage to generate the poses and inheriting the task properties. +We specify the pose where we want to place the object with a ``PoseStamped`` message from ``geometry_msgs`` - in this case, we choose ``y = 0.5`` in the ``"object"`` frame. +We then pass the target pose to the stage with ``setPose``. Next, we use ``setMonitoredStage`` and pass it the pointer to the ``attach_object`` stage from earlier. This allows the stage to know how the object is attached. We then create a ``ComputeIK`` stage and pass it our ``GeneratePlacePose`` stage - the rest follows the same logic as above with the pick stages. @@ -823,6 +827,7 @@ We then create a ``ComputeIK`` stage and pass it our ``GeneratePlacePose`` stage geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = "object"; target_pose_msg.pose.position.y = 0.5; + target_pose_msg.pose.orientation.w = 1.0; stage->setPose(target_pose_msg); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage @@ -831,7 +836,7 @@ We then create a ``ComputeIK`` stage and pass it our ``GeneratePlacePose`` stage std::make_unique("place pose IK", std::move(stage)); wrapper->setMaxIKSolutions(2); wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(hand_frame); + wrapper->setIKFrame("object"); wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" }); place->insert(std::move(wrapper)); diff --git a/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp b/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp index 88022eca16..8d0e2e76e3 100644 --- a/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp +++ b/doc/tutorials/pick_and_place_with_moveit_task_constructor/src/mtc_node.cpp @@ -58,6 +58,7 @@ void MTCTaskNode::setupPlanningScene() geometry_msgs::msg::Pose pose; pose.position.x = 0.5; pose.position.y = -0.25; + pose.orientation.w = 1.0; object.pose = pose; moveit::planning_interface::PlanningSceneInterface psi; @@ -287,6 +288,7 @@ mtc::Task MTCTaskNode::createTask() geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = "object"; target_pose_msg.pose.position.y = 0.5; + target_pose_msg.pose.orientation.w = 1.0; stage->setPose(target_pose_msg); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage @@ -297,7 +299,7 @@ mtc::Task MTCTaskNode::createTask() // clang-format on wrapper->setMaxIKSolutions(2); wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(hand_frame); + wrapper->setIKFrame("object"); wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" }); place->insert(std::move(wrapper)); 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: 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 + 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: 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 + 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..3597c8e06e 100644 --- a/doc/tutorials/quickstart_in_rviz/test/bringup_test.cpp +++ b/doc/tutorials/quickstart_in_rviz/test/bringup_test.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include #include @@ -50,17 +52,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; @@ -72,7 +73,13 @@ TEST_F(BringupTestFixture, BasicBringupTest) }; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback = result_cb; - control_client->async_send_goal(joint_traj_request, send_goal_options); + + // Ensure the status of executing the trajectory is not a timeout. + auto goal_handle_future = control_client->async_send_goal(joint_traj_request, send_goal_options); + ASSERT_NE(goal_handle_future.wait_for(std::chrono::seconds(5)), std::future_status::timeout); + + // Sleeping for a bit helps prevent segfaults when shutting down the control node. + std::this_thread::sleep_for(std::chrono::seconds(1)); } } // namespace moveit2_tutorials::quickstart_in_rviz diff --git a/doc/tutorials/quickstart_in_rviz/test/bringup_test.test.py b/doc/tutorials/quickstart_in_rviz/test/bringup_test.test.py index f1c7972a7e..849628875a 100644 --- a/doc/tutorials/quickstart_in_rviz/test/bringup_test.test.py +++ b/doc/tutorials/quickstart_in_rviz/test/bringup_test.test.py @@ -43,7 +43,6 @@ def generate_test_description(): ), base_launch, # The test itself - bringup_test, TimerAction(period=2.0, actions=[bringup_test]), launch_testing.actions.ReadyToTest(), ] 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