-
Notifications
You must be signed in to change notification settings - Fork 193
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[Servo] Update servo tutorials for refactor (#734)
- Loading branch information
1 parent
93675a1
commit 3e38458
Showing
9 changed files
with
728 additions
and
749 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
62 changes: 62 additions & 0 deletions
62
doc/examples/realtime_servo/config/panda_simulated_config.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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] |
152 changes: 152 additions & 0 deletions
152
doc/examples/realtime_servo/launch/pose_tracking_tutorial.launch.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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]), | ||
| ] | ||
| ) |
Oops, something went wrong.