diff --git a/ur5_moveit_tutorials/config/joint_group_servo_params.yaml b/ur5_moveit_tutorials/config/joint_group_servo_params.yaml new file mode 100644 index 00000000..f20cf637 --- /dev/null +++ b/ur5_moveit_tutorials/config/joint_group_servo_params.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.5 # 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_pos_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] diff --git a/ur5_moveit_tutorials/config/moveit_servo.rviz b/ur5_moveit_tutorials/config/moveit_servo.rviz index 43be18ef..1e2b7228 100644 --- a/ur5_moveit_tutorials/config/moveit_servo.rviz +++ b/ur5_moveit_tutorials/config/moveit_servo.rviz @@ -110,14 +110,14 @@ Visualization Manager: Update Interval: 0 Value: true - Alpha: 1 - Angular Arrow Scale: 0.5 + Angular Arrow Scale: 1 Angular Color: 204; 204; 51 Arrow Width: 0.10000000149011612 Class: rviz/TwistStamped Enabled: true Hide Small Values: false History Length: 10 - Linear Arrow Scale: 1 + Linear Arrow Scale: 2 Linear Color: 173; 127; 168 Name: TwistStamped Queue Size: 10 @@ -243,9 +243,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6853972673416138 + Pitch: 0.6603972911834717 Target Frame: - Yaw: 1.435401439666748 + Yaw: 2.030400276184082 Saved: ~ Window Geometry: Displays: diff --git a/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_controller.launch b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_controller.launch new file mode 100644 index 00000000..c0c503e8 --- /dev/null +++ b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_controller.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file