Remove duplicate params and create launch file for joint_group_vel_co…

…ntroller based MoveIt servo.

ur5_moveit_tutorials/config/joint_group_pos_servo_params.yaml

@@ -0,0 +1,5 @@
+command_out_type: std_msgs/Float64MultiArray
+
+publish_joint_positions: true
+publish_joint_velocities: false
+command_out_topic: /joint_group_pos_controller/command # Publish outgoing commands here

ur5_moveit_tutorials/config/joint_group_vel_servo_params.yaml

@@ -0,0 +1,5 @@
+command_out_type: std_msgs/Float64MultiArray
+
+publish_joint_positions: false
+publish_joint_velocities: true
+command_out_topic: /joint_group_vel_controller/command # Publish outgoing commands here

ur5_moveit_tutorials/config/pos_joint_traj_servo_params.yaml

@@ -1,68 +1,5 @@
-###############################################
-# 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: 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: 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: /pos_joint_traj_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]
+command_out_topic: /pos_joint_traj_controller/command

ur5_moveit_tutorials/config/joint_group_servo_params.yaml → ur5_moveit_tutorials/config/servo_params_common.yaml

@@ -17,14 +17,6 @@ low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the
 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
@@ -51,7 +43,6 @@ cartesian_command_in_topic: delta_twist_cmds  # Topic for incoming Cartesian twi
 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?

ur5_moveit_tutorials/launch/ur5_moveit_servo_common.launch

@@ -2,14 +2,15 @@
 <launch>
     <arg name="controller" doc="Which additional controller to launch"/>
     <arg name="servo_params_file" doc="The servo parameters configuration file"/>
+    <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
     <include file="$(find ur_gazebo)/launch/ur5_bringup.launch">
         <arg name="controllers" value="joint_state_controller $(arg controller)"/>
         <arg name="stopped_controllers" value=""/>
-        <arg name="transmission_hw_interface" value="hardware_interface/PositionJointInterface"/>
+        <arg name="transmission_hw_interface" value="$(arg transmission_hw_interface)"/>
         <arg name="paused" value="true"/>
-        <arg name="initial_joint_positions" 
+        <arg name="initial_joint_positions"
             value=" -J shoulder_pan_joint -5.62  -J shoulder_lift_joint -1.785 -J elbow_joint 1.625 -J wrist_1_joint -2.737 -J wrist_2_joint 5.707 -J wrist_3_joint -3.92"/>
-    </include>        
+    </include>
     <include file="$(find ur5_moveit_config)/launch/moveit_planning_execution.launch">
         <arg name="sim" value="true"/>
         <arg name="controller_name" value="$(arg controller)"/>
@@ -22,6 +23,7 @@
 
     <!-- Include servo -->
     <node name="servo_server" pkg="moveit_servo" type="servo_server" output="screen" >
+        <rosparam command="load" file="$(find ur5_moveit_tutorials)/config/servo_params_common.yaml"/>
         <rosparam command="load" file="$(arg servo_params_file)"/>
     </node>
 

ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_controller.launch → ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_pos_controller.launch

@@ -2,6 +2,6 @@
 <launch>
     <include file="$(find ur5_moveit_tutorials)/launch/ur5_moveit_servo_common.launch">
         <arg name="controller" value="joint_group_pos_controller"/>
-        <arg name="servo_params_file" value="$(find ur5_moveit_tutorials)/config/joint_group_servo_params.yaml"/>
+        <arg name="servo_params_file" value="$(find ur5_moveit_tutorials)/config/joint_group_pos_servo_params.yaml"/>
     </include>
 </launch>

ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_vel_controller.launch

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+    <include file="$(find ur5_moveit_tutorials)/launch/ur5_moveit_servo_common.launch">
+        <arg name="controller" value="joint_group_vel_controller"/>
+        <arg name="servo_params_file" value="$(find ur5_moveit_tutorials)/config/joint_group_vel_servo_params.yaml"/>
+        <arg name="transmission_hw_interface" value="hardware_interface/VelocityJointInterface"/>
+    </include>
+</launch>