Moveit servo with Universal Robot UR5, don't want to move robot
Hi. i have struggled for a while now with implementing moveit servo on my UR-robot. When i try to publish a message to move the robot, I only get messages about halting for collision and Stale command(full log further down). I have confirmed that communication between ROS and UR CB-controller is working by running a path planning with moveit.
My configuration:
ROS melodic on Ubuntu 18.04 in WSL2
I use the Universal_Robots_ROS_Driver to communicate with controller
I have a virtual machine running the CB-controller
ros-melodic-moveit-servo is installed
Here is how I start my setup
roslaunch telecoffee bringup_ur.launch
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=false
roslaunch telecoffee servo.launch
After everything is booted up i switch controller with
rosservice call /controller_manager/switch_controller "start_controllers: ['joint_group_vel_controller']
stop_controllers: ['scaled_pos_joint_traj_controller']
strictness: 0
start_asap: false
timeout: 0.0"
and confirms that controller is running
name: "joint_group_vel_controller"
state: "running"
type: "velocity_controllers/JointGroupVelocityController"
claimed_resources:
-
hardware_interface: "hardware_interface::VelocityJointInterface"
resources: [elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
This is servo.launch
<?xml version="1.0"?>
<launch>
<node name="servo_server" pkg="telecoffee" type="servo_server" output="screen" >
<param name="parameter_ns" type="string" value="optional_parameter_namespace" />
<rosparam ns="optional_parameter_namespace" command="load" file="$(find telecoffee)/config /ur_servo.yaml" />
</node>
</launch>
And this is my servo configuration file ur_servo.yaml
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
command_in_type: "speed_units"
scale: # Scale parameters are only used if command_in_type=="unitless"
linear: 0.0006
rotational: 0.003
joint: 0.001
low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less.
publish_period: 0.01 # 1/Nominal publish rate [seconds]
publish_delay: 0.005 # Delay between calculation and execution start of command
command_out_type: std_msgs/Float64MultiArray
publish_joint_positions: false
publish_joint_velocities: true
publish_joint_accelerations: false
move_group_name: manipulator # Often 'manipulator' or 'arm'
planning_frame: world
ee_frame_name: wrist_3_link
robot_link_command_frame: base_link
incoming_command_timeout: 1 # Stop servoing if X seconds elapse without a new command
num_outgoing_halt_msgs_to_publish: 0
lower_singularity_threshold: 17
hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
joint_limit_margin: 0.15
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_vel_controller/command # Publish outgoing commands here
check_collisions: true # Check collisions?
collision_check_rate: 40 # [Hz] Collision-checking can easily bog down a CPU if done too often.
collision_check_type: stop_distance
self_collision_proximity_threshold: 0.02 # 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]
collision_distance_safety_factor: 1000
min_allowable_collision_distance: 0.02 # Stop if a collision is closer than this [m]
When everything is up and running, I try to publish
rostopic pub -r 125 -s /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
twist:
linear:
x: 0.0
y: 0.0
z: 0.1
angular:
x: 0.0
y: 0.0
z: 0.0"
And the response I get from servo_server is
[ INFO] [1620828759.131166000]: Loading robot model 'ur5'...
[ WARN] [1620828759.132970700]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1620828759.133301200]: No root/virtual joint specified ...
