Velocity command interface ROS2 foxy

[fafux]

Problem description
Hi,
I am trying to control an Universal Robots both in simulation (both on Rviz and Webots) and with the real hardware with the command interface in velocity with ROS2 foxy. The values of the joint velocities states, in simulation, are changing but the robot is not moving. While if I try to move the robot using the position command interface I can do it.
Is it possible to move the robot only using the velocity command interface?
Thanks.

Software versions

• OS: Ubuntu 20.04
• ROS version: ROS2 foxy
• Robot: Universal Robots

[stefanscherzinger]

Hi @fafux

Is it possible to move the robot only using the velocity command interface?

Yes, that should be possible. Here are some thoughts on this use case (it's ROS1, but is intended to work similarly for ROS2).

Could you post your controller configuration (.yaml file)? Maybe I can spot something unusual.
I'll try to test velocity control later. But it will be ROS2 Humble.

You could publish the internal controller state via the publish_state_feedback parameter. That will show what the controller is about to write to the command handles in each cycle. Maybe this gives further insights.

The values of the joint velocities states, in simulation, are changing but the robot is not moving.

Interesting. Are they changing with a meaningful speed, or are they maybe changing very slowly?

[fafux]

Hi @stefanscherzinger
Below you can see the first part of ur_controllers.yaml file:

controller_manager:
  ros__parameters:
    update_rate: 600  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    io_and_status_controller:
      type: ur_controllers/GPIOController

    speed_scaling_state_broadcaster:
      type: ur_controllers/SpeedScalingStateBroadcaster

    force_torque_sensor_broadcaster:
      type: ur_controllers/ForceTorqueStateBroadcaster

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    scaled_joint_trajectory_controller:
      type: ur_controllers/ScaledJointTrajectoryController

    cartesian_motion_controller:
      type: cartesian_motion_controller/CartesianMotionController

cartesian_motion_controller:
  ros__parameters:
    end_effector_link: "tool0"
    robot_base_link: "base_link"
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint

    # Choose: position or velocity.
    command_interfaces:
      #- position
      - velocity

    solver:
        error_scale: 1.0
        iterations: 10
        publish_state_feedback: True

    pd_gains:
        trans_x: {p: 1.0}
        trans_y: {p: 1.0}
        trans_z: {p: 1.0}
        rot_x: {p: 0.5}
        rot_y: {p: 0.5}
        rot_z: {p: 0.5}

# [...] the other controllers params

The speed data both from the publish_state_feedback (/cartesian_motion_controller/current_twist) and from the /joint_states are meaningful and are changing and different from 0 but the robot is not moving.

[stefanscherzinger]

@fafux

The controllers' specification looks alright. Let's clarify two things before we go into further debugging:

1. Why do you need velocity-based control as opposed to position-based control?

2. but the robot is not moving.

   Simulation or the real hardware?

[fafux]

@stefanscherzinger

1. Because afterwards I would like to do something on top of your controller.
2. Both simulation and real hardware.

[stefanscherzinger]

Alright, I'll see if I can reproduce this on a real setup.

Concerning 1.: The cartesian_controllers take force-torque vectors, poses, or both as input. On the lower level, they command what is most suitable for the respective hardware. But the inputs for the users do not change. What is it that you want to do on top that you cannot do with the position-based version?

[fafux]

First, I would like to avoid the numerical integration from joint velocities to joint positions based on a fixed control loop rate (in Foxy) that, depending on the hardware/application could be not respected precisely.
Then, I would like to add some null-space task that I would like to test directly with the velocity commands.

[fafux]

Thanks for your time. Universal Robots answered my issue and declared that velocity commands are not implemented in foxy.
UniversalRobots/Universal_Robots_ROS2_Driver#694