ROS2 Tutorial: Simple Gripper

# Following the Trajectory

Here comes the fun part. Our basic_trajectory_action_service.py takes array of target coordinates, and before it used 6 joints of the robotic arm. Now we have added an extra joint... so all it takes to make the arm work with gripper, is to pass an extra target value in an array of target coordinates: 7 values instead of 6.

Just keep in mind that our gripper uses linear actuator, so, unlike for other joints, targer values for it should be in meters, not in radians.

```

...

def send_goal(self):

joint_names = ['joint1','joint2','joint3','joint4',
'joint5','joint6', 'gripper_joint_left_1']

...

def send_effector_pos(self, arrAngles):
joint_names = ['joint1','joint2','joint3','joint4',
'joint5','joint6', 'gripper_joint_left_1']

...

def main(args=None):
Uncomment one of the following two sections:

# --- IK with robot, notice the "-0.03" as the last target:
# rclpy.init()
# action_client = TrajectoryActionClient()

# future = action_client.send_effector_pos(
#     [np.pi / 2, np.pi / 2, -np.pi / 2, np.pi / 2,
#        np.pi / 2, np.pi / 2, -0.03 ])
# rclpy.spin(action_client)

# --- Writing our own IK code:

arrJointCoords = [
'z', (0, 0, 0.114),
'y', (0, 0.082, 0.082),
'y', (0, 0, 0.5),
'z', (0, -0.082, 0.26),
'y', (0, 0.082, 0.240),
'z', (0, 0.082, 0.082)
]

dictJointLimits = {
'joint1': (-6.2832, 6.2832),
'joint2': (-6.2832, 6.2832),
'joint3': (-6.2832, 6.2832),
'joint4': (-6.2832, 6.2832),
'joint5': (-6.2832, 6.2832),
'joint6': (-6.2832, 6.2832),
'gripper_joint_left_1': (-0.04, 0.06)
}

# 7 target coordinates now
arrTargetAngles = [0,0,0,0,0,0,0]

objKinematics = inverse_kinematics.Kinematics(
arrJointCoords, dictJointLimits)

joint_coordinates = objKinematics.forward_kinematics(arrTargetAngles)

# Print the joint coordinates
for i, joint_coord in enumerate(joint_coordinates):
print(f"Joint {i+1}: {joint_coord}")

# --- Do it in simulator
rclpy.init()
action_client = TrajectoryActionClient()

# 7 targets
future = action_client.send_effector_pos(
[np.pi / 2, np.pi / 2, -np.pi / 2, np.pi / 2, np.pi / 2, np.pi / 2, -0.3 ])
rclpy.spin(action_client)

if __name__ == '__main__':
main()

```

To run this script, run the launch file first (see "building and running below). The robot will move the first joint, then second and so on. This is just the way we programmed it, same code, if you change values of two target elements simultaneously, will move few joints in the same time.