diff --git a/doc/realtime_servo/realtime_servo_tutorial.rst b/doc/realtime_servo/realtime_servo_tutorial.rst index 493eba03a..1dfd29e74 100644 --- a/doc/realtime_servo/realtime_servo_tutorial.rst +++ b/doc/realtime_servo/realtime_servo_tutorial.rst @@ -38,25 +38,25 @@ Re-build and re-source the workspace. :: Launch the Gazebo simulation: :: - roslaunch ur_gazebo ur5.launch + roslaunch ur_gazebo ur5_bringup.launch - roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true + roslaunch ur5_moveit_config moveit_planning_execution.launch sim:=true - roslaunch ur5_moveit_config moveit_rviz.launch config:=true + roslaunch ur5_moveit_config moveit_rviz.launch In RViz, grab the red/blue/green "interactive marker" and drag the robot to a non-singular position (not all zero joint angles) that is not close to a joint limit. Click "plan and execute" to move the robot to that pose. Switch to a compatible type of `ros-control` controller. It should be a `JointGroupVelocityController` or a `JointGroupPositionController`, not a trajectory controller like MoveIt usually requires. :: rosservice call /controller_manager/switch_controller "start_controllers: ['joint_group_position_controller'] - stop_controllers: ['arm_controller'] + stop_controllers: ['eff_joint_traj_controller'] strictness: 0 start_asap: false timeout: 0.0" Launch the servo node. This example uses commands from a `SpaceNavigator `_ joystick-like device: :: - roslaunch moveit_servo spacenav_cpp.launch + roslaunch moveit_servo spacenav_cpp.launch config:=$(rospack find moveit_tutorials)/doc/realtime_servo/ur_config.yaml If you do not have a SpaceNav 3D mouse, you can publish example servo commands from the command line with: :: diff --git a/doc/realtime_servo/ur_config.yaml b/doc/realtime_servo/ur_config.yaml new file mode 100644 index 000000000..8578ca0ff --- /dev/null +++ b/doc/realtime_servo/ur_config.yaml @@ -0,0 +1,68 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: true # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +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.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. + +## Properties of outgoing commands +publish_period: 0.008 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) +# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) +command_out_type: std_msgs/Float64MultiArray + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## MoveIt properties +move_group_name: manipulator # Often 'manipulator' or 'arm' +planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30 # 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. + +## 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: /joint_group_position_controller/command # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: threshold_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]