ros2_control with position and velocity controller

Question

I`m trying to use position and velocity control with joint_trajectory_controller, I have several problems: it seems to me that controller doesn't take velocity into account, while in Gazebo logs both interfaces are in command interfaces list: Loading joint: joint1 [gazebo-5] [INFO] [1709040839.028377435] [gazebo_ros2_control]: State: [gazebo-5] [INFO] [1709040839.028386829] [gazebo_ros2_control]: position [gazebo-5] [INFO] [1709040839.028395098] [gazebo_ros2_control]: velocity [gazebo-5] [INFO] [1709040839.028401741] [gazebo_ros2_control]: effort [gazebo-5] [INFO] [1709040839.028407850] [gazebo_ros2_control]: Command: [gazebo-5] [INFO] [1709040839.028413598] [gazebo_ros2_control]: position [gazebo-5] [INFO] [1709040839.028457275] [gazebo_ros2_control]: velocity This is my controller config:

controller_manager:
  ros__parameters:
    update_rate: 100 # Hz
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    write_op_modes:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
      - position
      - velocity
    interface_name: position/velocity
    command_interfaces:
      - position
      - velocity
    state_interfaces:
      - position
      - velocity

    state_publish_rate: 50.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    hardware_state_has_offset: false
    deduce_states_from_derivatives: false

    constraints:
      stopped_velocity_tolerance: 0.01
      allow_nonzero_velocity_at_trajectory_end: true

This is place in my code, where I`m trying to move each of my robot joints with fixed velocity from start position to end (1 joint in 1 time, step-by-step):

j, k = 0, 0
        while j < 6:
            joints_limit = [(-2.96706, 2.96706), (-1.74533, 2.61799), (-3.1415, 1.5708), (-3.31613, 3.31613),\
                            (-2.35619, 2.35619), (-6.28319, 6.28319)]
            step = 0.0872 #5 degrees in radians
            radians_range = create_position_range(joints_limit[j][0], joints_limit[j][1], step)
            velocity = 0.1
            # time_from_start = math.ceil((abs(radians_range[0]) + abs(radians_range[-1]) / velocity) / len(radians_range))
            time_from_start = math.ceil((abs(radians_range[0]) + abs(radians_range[-1]) / velocity))
            for i in radians_range:
                goal_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
                joint_velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
                goal_positions[k] = i
                joint_velocities[k] = 0.1
                point_msg = JointTrajectoryPoint()
                point_msg.positions = goal_positions
                point_msg.velocities = joint_velocities
                my_trajectory_msg = JointTrajectory()
                my_trajectory_msg.joint_names = self.joints
                my_trajectory_msg.points.append(point_msg)
                point_msg.time_from_start = Duration(sec=time_from_start)
                self.get_logger().info(f'***{point_msg.time_from_start}***')
                self.trajectory_publisher.publish(my_trajectory_msg)

                # Publish desire pose - point position and rotation before IK calculation
                desire_pose_msg = InverseKinematics()
                a = radians_range.tolist().index(i)
                desire_pose_msg.x = 1.0 + a / 2
                desire_pose_msg.y = 1.0 + a / 2
                desire_pose_msg.z = 1.0 + a / 2
                desire_pose_msg.r = 0.0
                desire_pose_msg.p = 0.0
                desire_pose_msg.w = 0.0
                self.desire_pose_publisher.publish(desire_pose_msg)

            # point_msg = JointTrajectoryPoint()
            # point_msg.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
            # point_msg.time_from_start = Duration(sec=time_from_start + 1)
            # my_trajectory_msg.joint_names = self.joints
            # my_trajectory_msg.points.append(point_msg)
            # self.trajectory_publisher.publish(my_trajectory_msg)

            j += 1
            k += 1
            time.sleep(3)

I`m using ros2 Humble, Ubuntu 22. Thanks in advance!

Answer 1

Why are you giving these options:

write_op_modes:
  - joint1
  - joint2
  - joint3
  - joint4
  - joint5
  - joint6
  - position
  - velocity
interface_name: position/velocity

Please have a look at the docs.

When I understand your code, you publish single trajectory points, but without a time-delay between the points? consider adding the points to a single message, and sum-up the time deltas for time_from_start. And I'm not sure now what happens with append in python, but you alter the point_msg after you append it to the trajectory message.