Joint trajectory controller won't work with prismatic joint

Question

I tried to simulated a conveyor belt using a prismatic joint, unfortunately my configuration is having some sort of error:

Here is my Urdf:

<robot name="belt" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!--///////////////////////////AUFBAU DES FÖRDERBANDES///////////////////////////////////-->

    <!--link name="root_link"/>
    <joint name="root_joint" type="fixed">
        <origin xyz="1 0 0.035" rpy="0 0 0"/>
        <parent link="root_link"/>
        <child link="belt"/>
    </joint-->

    <link name="belt">
        <visual>
            <origin xyz="0 0 0.0" rpy="0 0 0" />
            <geometry>
                <box size="0.5 3 0.15" />
            </geometry>
            <material name="belt">
                <color rgba="0.3 0.3 0.3 1" />
            </material>
        </visual>
        <inertial>
            <origin xyz="0.4 0 0.0" rpy="0 0 0" />
            <mass value="50.0"/>
            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
        </inertial>

        <collision>
            <origin xyz="0.4 0 0.0" rpy="0 0 0" />
            <geometry>
                <box size="0.5 3 0.15" />
            </geometry>
        </collision>
    </link>


    <link name="cube">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <material name="belt">
                <color rgba="0.5 1.0 0.5 1" />
            </material>
        </visual>
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <mass value="0.5"/>
            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
        </inertial>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
        </collision>
    </link>



    <joint name="BeltPrismatic" type="prismatic">
        <origin rpy="0 0 0" xyz="0 -1.0 0.15"/> 
        <parent link="belt"/>
        <child link="cube"/>
        <axis xyz="0 1 0"/> 
        <limit lower="-1.0" upper="1.0" effort="100" velocity="11.0"/> 
    </joint>


    <!--////////////////////////////////////////Gazebo-Eigenschaften///////////////////////////////////////////-->
     <gazebo reference="belt">
        <material>Gazebo/DarkGrey</material>
    </gazebo>
    <gazebo reference="cube">
        <material>Gazebo/Grey</material>
    </gazebo>


    <ros2_control name="GazeboSystem" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        <joint name="BeltPrismatic">
            <command_interface name="position">
                <param name="min">-1.0</param>
                <param name="max">1.0</param>
            </command_interface>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>


</robot>

here is my controller.yaml:

  controller_manager:
      ros__parameters:
    
        update_rate: 100 # 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
    
        forward_velocity_controller:
          type: velocity_controllers/JointGroupVelocityController
    
        forward_position_controller:
          type: position_controllers/JointGroupPositionController
    
        belt_controller:
          type: joint_trajectory_controller/JointTrajectoryController
    
    
    
    speed_scaling_state_broadcaster:
      ros__parameters:
        state_publish_rate: 100.0
    
    
    force_torque_sensor_broadcaster:
      ros__parameters:
        sensor_name: tcp_fts_sensor
        state_interface_names:
          - force.x
          - force.y
          - force.z
          - torque.x
          - torque.y
          - torque.z
        frame_id: tool0
        topic_name: ft_data
    
    
    joint_trajectory_controller:
      ros__parameters:
        joints:
          - shoulder_pan_joint
          - shoulder_lift_joint
          - elbow_joint
          - wrist_1_joint
          - wrist_2_joint
          - wrist_3_joint
        command_interfaces:
          - position
        state_interfaces:
          - position
          - velocity
        state_publish_rate: 100.0
        action_monitor_rate: 20.0
        allow_partial_joints_goal: false
        constraints:
          stopped_velocity_tolerance: 0.2
          goal_time: 0.0
          shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
          shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
          elbow_joint: { trajectory: 0.2, goal: 0.1 }
          wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
          wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
          wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
    
    
    belt_controller:
      ros__parameters:
        joints:
          - BeltPrismatic
        command_interfaces:
          - position
          - velocity
        state_interfaces:
          - position
          - velocity
        state_publish_rate: 100.0
        action_monitor_rate: 20.0
        allow_partial_joints_goal: false
        constraints:
          stopped_velocity_tolerance: 0.2
          goal_time: 0.0
          BeltPrismatic: { trajectory: 0.2, goal: 0.1 }
    
    
    scaled_joint_trajectory_controller:
      ros__parameters:
        joints:
          - shoulder_pan_joint
          - shoulder_lift_joint
          - elbow_joint
          - wrist_1_joint
          - wrist_2_joint
          - wrist_3_joint
        command_interfaces:
          - position
        state_interfaces:
          - position
          - velocity
        state_publish_rate: 100.0
        action_monitor_rate: 20.0
        allow_partial_joints_goal: false
        constraints:
          stopped_velocity_tolerance: 0.2
          goal_time: 0.0
          shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
          shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
          elbow_joint: { trajectory: 0.2, goal: 0.1 }
          wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
          wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
          wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
    
    forward_velocity_controller:
      ros__parameters:
        joints:
          - shoulder_pan_joint
          - shoulder_lift_joint
          - elbow_joint
          - wrist_1_joint
          - wrist_2_joint
          - wrist_3_joint
        interface_name: velocity
    
    forward_position_controller:
      ros__parameters:
        joints:
          - shoulder_pan_joint
          - shoulder_lift_joint
          - elbow_joint
          - wrist_1_joint
          - wrist_2_joint
          - wrist_3_joint

and here's my node to control the jtc:

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from rclpy.time import Duration


class VelocityControllerNode(Node):
    def __init__(self):
        super().__init__('VelocityControllerNode')
        self.publischer = self.create_publisher(JointTrajectory, '/belt_controller/joint_trajectory', 10)
        self.subscription = self.create_subscription(Float64, 'box_velocity', self.velocity_callback, 10)
        self.get_logger().info('VCN gestartet...')

    def velocity_callback(self, msg):
        velocity = msg.data
        self.get_logger().info('Geschwindigkeit erhalten: {}'.format(msg.data))

        self.trajMsg = JointTrajectory()


        self.trajMsg.joint_names = ['BeltPrismatic']

        point = JointTrajectoryPoint()
        point.positions = [1.0]  # Die gewünschte Position hier ende des förderbandes

        point.velocities = [velocity]  # Sie können dies je nach Anforderung anpassen

        self.trajMsg.points = [point]

        self.publischer.publish(self.trajMsg)
        self.get_logger().info('Geschwindigkeit und Punkt umgesetzt: {}, {}'.format(velocity, point))


def main(args=None):
    rclpy.init(args=args)
    node = VelocityControllerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

the controller itself loads correctly when I run my launch file. when it try to send a velocity via the box_velocity topic to my node it processes the input correctly and publishes a trajectory, but unfortunately my cube does not move a bit. Would really appreciate your help. if you need outputs or more code just ask.

Answer 1

I can't really comment on your code as I don't use ros2_control, but wrt simulating a conveyor I can give you following suggestions:

I you are on Gazebo Sim, I suggest you have a look at the conveyor example.

There are two known issues wrt this plugin: 1662 and 2008, so you need to follow these instructions to get it running, but other than that it works flawlessly for me.

If you need to get rid of objects at the end of the conveyor: I wrote a waste bin plugin for Gazebo Sim.

If you are on Gazebo Classic, there's the Ariac conveyor belt plugin, which I think you'd need to adapt it a bit to your needs though. But in any case it is known to work as well. The Ariac repository also holds an object disposal plugin.

Another relevant Gazebo Classic example involving a conveyor was just recently announced in this post, so probably this is also a good reference example.