0
$\begingroup$

In ros2 Humble and Gazebo (11 I think is the latest). I have a gripper as shown in the images below. The images are from a very similar robot, it basically has the same structure! It's made out of grip_left, which should move finger_left and tendon_left as well as grip_right, tendon_right and finger_right.

However, by how I have it set up right now, it seems when grip_left moves, only finger_left moves.

enter image description here

This is my controllers.yaml:

controller_manager:
  ros__parameters:
    update_rate: 100
    use_sim_time: true

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    gripper_controller:
      type: joint_trajectory_controller/JointTrajectoryController

arm_controller:
  ros__parameters:
    publish_rate: 50.0
    base_frame_id: base_link
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    joints:
      - shoulder_pan
      - shoulder_lift
      - elbow
      - wrist_flex
      - wrist_roll

gripper_controller:
  ros__parameters:
    publish_rate: 50.0

    command_interfaces: 
      - position
    state_interfaces: 
      - position
      - velocity
    joints:
      - grip_left
    # pid: {p: 100.0, i: 0.01, d: 10.0}

I can see in my urdf that the transmission tag suggests that the joints work as EffortJointInterface, but in ros2 and according to this answer:

Relevant

you would not even need a custom system plugin; I did not test this, but it should be possible to just use the mimic functionality and apply the JointPositionController to the input joint.

And the relevant part of my urdf for the mimic joints:

<joint name="grip_left" type="revolute">
    <parent link="hand_link"/>
    <child link="grip_left_link"/>
    <origin rpy="-1.57 0 0" xyz="0 +0.015 0.030"/>
    <axis xyz="-1 0 0"/>
    <limit effort="1000" lower="-1.57" upper="0" velocity="0.5"/>
     <dynamics damping="50" friction="1"/>
  </joint>
  <joint name="grip_right" type="revolute">
    <parent link="hand_link"/>
    <child link="grip_right_link"/>
    <origin rpy="1.57 0 0" xyz="0 -0.015 0.030"/>
    <axis xyz="-1 0 0"/>
    <limit effort="1000" lower="0" upper="1.57" velocity="0.5"/>
    <mimic joint="grip_left" multiplier="-1"/>
  </joint>
  <joint name="tendon_left" type="revolute">
    <parent link="hand_link"/>
    <child link="tendon_left_link"/>
    <origin rpy="-1.57 0 0" xyz="0 +0.005 0.050"/>
    <axis xyz="-1 0 0"/>
    <limit effort="1000" lower="-1.57" upper="0" velocity="0.5"/>
    <mimic joint="grip_left"/>
  </joint>
  <joint name="tendon_right" type="revolute">
    <parent link="hand_link"/>
    <child link="tendon_right_link"/>
    <origin rpy="1.57 0 0" xyz="0 -0.005 0.050"/>
    <axis xyz="-1 0 0"/>
    <limit effort="1000" lower="0" upper="1.57" velocity="0.5"/>
    <mimic joint="grip_left" multiplier="-1"/>
  </joint>
  <joint name="finger_left" type="revolute">
    <parent link="grip_left_link"/>
    <child link="finger_left_link"/>
    <origin rpy="1.57 0 0" xyz="0 0 0.030"/>
    <axis xyz="-1 0 0"/>
    <limit effort="1000" lower="-1.57" upper="0" velocity="0.5"/>
    <mimic joint="grip_left" multiplier="-1"/>
  </joint>
  <joint name="finger_right" type="revolute">
    <parent link="grip_right_link"/>
    <child link="finger_right_link"/>
    <origin rpy="-1.57 0 0" xyz="0 0 0.030"/>
    <axis xyz="-1 0 0"/>
    <limit effort="1000" lower="0" upper="1.57" velocity="0.5"/>
    <mimic joint="grip_left"/>
  </joint>

With the appropriate ligbgazebo_ros2_control.so tags. Moving the arm works! Not the gripper.

And from reading the docs, it seems in this part:

<joint name="grip_right">
      <command_interface name="position">
        <param name="min">0</param>
        <param name="max">1.57</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-0.5</param>
        <param name="max">0.5</param>
      </command_interface>
      <command_interface name="effort">
        <param name="min">-1000</param>
        <param name="max">1000</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>

I should also add as specified in here:

  <param name="mimic">grip_left</param>
  <param name="multiplier">1</param>

But nothing seems to happen...

  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

And:

<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
</plugin>

The versions are the ones installed with sudo apt install I am running python3 send_trajectory_gripper.py as I do for the working arm:

import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

class TrajectoryPublisher(Node):
    def __init__(self):
        super().__init__('trajectory_publisher')
        self.publisher_ = self.create_publisher(JointTrajectory, '/gripper_controller/joint_trajectory', 10)
        timer_period = 2  # seconds
        self.timer = self.create_timer(timer_period, self.publish_trajectory)

    def publish_trajectory(self):
        msg = JointTrajectory()
        msg.joint_names = ['grip_left']
        point = JointTrajectoryPoint()
        point.positions = [-0.5] 
        point.time_from_start.sec = 2
        msg.points.append(point)
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing trajectory')

def main(args=None):
    rclpy.init(args=args)
    trajectory_publisher = TrajectoryPublisher()
    rclpy.spin(trajectory_publisher)
    trajectory_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

And this is how it moves:

enter image description here

It somehow seems that the tendon of the left side, and that the whole right side are either not connected at at all physically? Wouldn't the moving part, not move at all if the other joints that are theoretically in contact, do not move as well?

$\endgroup$
2
  • $\begingroup$ Just for reference, which ROS distro and gazebo_ros2_control-version are you using? $\endgroup$ Commented Apr 14 at 13:47
  • $\begingroup$ Updated @ChristophFroehlich $\endgroup$
    – M.K
    Commented Apr 14 at 13:48

1 Answer 1

0
$\begingroup$

After testing the joints individually, I could see some missmatch in the limits of the joints. For example: If grip_left moved, (circular movement to the left), then finger_left will go horizontal moving to the left. If the finger needs to stay upright, it means it needs to counter this movement to the right. But the limits were on the opposite.

  <joint name="grip_left" type="revolute">
    <parent link="hand_link"/>
    <child link="grip_left_link"/>
    <origin xyz="0 +0.015 0.030" rpy="-1.57 0 0"/>
    <axis xyz="-1 0 0"/>
    <limit lower="-1.57" upper="0" effort="1000" velocity="0.5"/>
  </joint>

The limit of finger_left should be the opposite:

  <joint name="finger_left" type="revolute">
    <parent link="grip_left_link"/>
    <child link="finger_left_link"/>
    <origin xyz="0 0 0.030" rpy="1.57 0 0"/>
    <axis xyz="-1 0 0"/>
    <limit lower="-1.57" upper="0" effort="1000" velocity="0.5"/>
    <mimic joint="grip_left" multiplier="-1"/>
  </joint>

As: <limit lower="0" upper="-1.57" effort="1000" velocity="0.5"/>

My only concern is that if rviz worked well, it means hopefully these constraints were also not taken into account. If they were, I do not know how it will work then...

This was the first realisation, which was great, I could use a gripper with all the gripper joints and move them each, but the mimicking was not working still, although I fixed the limits.

I remembered that in the tutorials:

<joint name="left_finger_joint">
  <param name="mimic">right_finger_joint</param>
  <param name="multiplier">1</param>
  <command_interface name="position"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
  <state_interface name="effort"/>
</joint>

I had not added the params for the gazebo_ros2_control:

  <param name="mimic">right_finger_joint</param>
  <param name="multiplier">1</param>

It seems that the mimicking params should be established in here, the urdf get's overriden wtih default, which I do not know what is it.

My final controllers.yaml:

controller_manager:
  ros__parameters:
    update_rate: 100
    use_sim_time: true

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    gripper_controller:
      # type: joint_trajectory_controller/JointTrajectoryController
      # type: effort_controllers/JointGroupEffortController
      type: joint_trajectory_controller/JointTrajectoryController
      # type: effort_controllers/GripperActionController

arm_controller:
  ros__parameters:
    publish_rate: 50.0
    base_frame_id: base_link

    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    joints:
      - shoulder_pan
      - shoulder_lift
      - elbow
      - wrist_flex
      - wrist_roll

gripper_controller:
  ros__parameters:
    publish_rate: 50.0

    command_interfaces: 
      - position
    state_interfaces: 
      - position
      - velocity
    joints:
      - grip_left # Add pids?? 
# gains:
#   grip_left: {p: 100.0, i: 0.01, d: 10.0, i_clamp: 1}

And test it with a command or creating a .py file to run with ros2 run:

ros2 topic pub /gripper_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
  header: {
    stamp: {sec: 0, nanosec: 0},
    frame_id: ''
  },

  joint_names: ['grip_left'],

  points: [
    {
      positions: [-2],
      velocities: [],
      accelerations: [],
      effort: [],
      time_from_start: {sec: 1, nanosec: 0}
    }
  ]
}" --once

enter image description here

I will have to test in regards of efforts once the robot is interacting with objects.

$\endgroup$
2
  • 1
    $\begingroup$ The necessary parameters in the ros2_control tag is old legacy code ("override" as you called it), but from Jazzy on we will use the URDF definition. It is still not included in gz_ros2_control, but there is an open discussion for gazebo_ros2_control how to properly interpret the mimic definition. Fyi: The mentioned PR won't be backported to humble, and gazebo_ros2_control won't be released for rolling because gazebo classic is not released for noble any more. $\endgroup$ Commented Apr 15 at 16:50
  • $\begingroup$ Very nice, thank you for the info @ChristophFroehlich . Once I read "So I implemented it that independently of the interface type of the mimicked joint, the mimic joint's position is set." I wondered what about effort when interacting with real objects (a "effort until position is met" kind of thing). I'll have some reads and thoughts! I hope not to bother more with this topic! Thanks! $\endgroup$
    – M.K
    Commented Apr 15 at 17:49

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.