Problem Statement 1

Implement a forward kinematics node that:

Subscribes to the joint values topic and reads them from the robot's joint states topic.
Calculates the end effector pose
Publishes the pose as a ROS topic

Implement an inverse kinematics node that has a service client implementation that takes a desired pose of the end effector from the user and returns joint positions as response.
Place an object at a particular location on the chess board. Calculate the inverse kinematics for that gripper pose to find the point angles. Move the robot to that position, pick the object and lift it.

Implementation

1. Forward Kinematics

At each joint, the robot is assigned a frame and the DH-parameter table is calculated based on the frame assignments.
The homogeneous transformation matrix w.r.t the previous frame can then be calculated as:

$$ A_i = \left[\begin{array}{cccc} cos\theta_i & -sin\theta_i cos\alpha_i & sin\theta_i sin\alpha_i & a_i cos\theta_i \\ sin\theta_i & cos\theta_i cos\alpha_i & -cos\theta_i sin\alpha_i & a_i sin\theta_i \\ 0 & sin\alpha_i & cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{array} \right] $$

Finally, the pose of the end effector frame can be calclated w.r.t the base frame by multiplying all the frame transformation in between these frames.

The pose of the robotic arm is calculated for 3 different orientations as shown below:

Robot Pose at three different positions

2. Inverse Kinematics

The side view and top view of the robot arm are shown in the figure below:

Robot Top view and Side view

Using the geometry of the robot and the given pose, we can calculate the angles $\theta_1$, $\theta_2$, $\theta_3$ and $\theta_4$

The Inverse Kinematics node is tested with three different inputs using the ros2 service call command and the terminal outputs are shown below:

Terminal Output

3. Object Manipulation

This is done with the help of the forwardn and inverse kinematics node.
The position of the object is known.
The desired pose of the robot arm is calculated such that the gripper is just above the object.
This pose is sent to the inverse kinematics node to calculate the joint angles.
The joint angles are calculated and provided to the controller to move the robot arm.
Then the gripper is opened and desired pose at the object is sent to the inverse kinematics node, which provides the desired joint angles for the robot arm at the object position.
When the robot moves to that position, the gripper is closed and again the robot arm is moved to the initial position which results in the pick action on the object.

Picking and placing an object

Problem Statement 2

Implement a node wih two services:

One takes joint velocities and converts them to end effector velocities.
The other takes end effector velocities and converts them to joint velocities.

Create a node that provides incremental position references to the robot joints i.e. q_ref = q_ref_old + delta_q * sampling_time. The node would then send the q_ref to the joint position controllers of the robot as joint goals.
Give a constant velocity reference in the '+y' direction. Convert this velocity to the joint space velocities using the jacobian and feed it as a reference (detla_q) to the incremental position reference node.

Implementation

joint vel srv.py

end eff vel.py

The terminal output for the two services is shown below:

Terminal Output

For achieving a positive velocity in the y-direction, the steps taken to get to the solution are as follows: • Created a new node named IncrementalPositionClient
• Created a new client to request the SetJointPosition service
• Created a new client to call the cal joint vel service created before which takes in end effector Twist and provides joint velocities
• Developed a function to calculate forward kinematics from the current joint values
• In the main function, hard coded the Twist of the the end effector and calculated the joint velocities and updated the joint position using the given formula:

$$ qref = qref_{old} + ∆q ∗ sampling time $$

• Calculated the forward kinematics from the new joint position values and saved the end effector position in an array.
• Plotted the end effector position vs time.

The resutls are given below:

Terminal Output

Top right terminal window shows the joint output values from the service cal_joint_vel which takes in end effector twist and gives out joint velocities and the bottom right terminal window shows the output while running the incremental_pos_client node.

The trajectory of the end effector of the robot is saved in tractory.txt file and plotted with respect to time.
Figure below shows three plots x vs time, y vs time and z vs time.
The number of samples are 100 and sampling time is taken as 1 second.
The end effector velocity in y-direction is taken as 1.0

The resutls are given below:

Terminal Output

From the plot it can be seen that the robot is moving in y-direction in linear fashion to time.
The z-direction values are also stable after a while and the x-values are also approximately stable.

Incremental position change

Problem Statement 3

Implement a position controller for your robot joints. Here we want you to write PD Controllers yourselves. DO NOT use the readily available ROS packages.
You need to apply your controller only to one of the actuators of the robot: Actuator 4. That is the actuator right before the gripper.
Your package will read the joint position values from actuator 4, receive a position reference value for the actuator through a service, and publish joint efforts/currents (continuously with high sampling rates) to the actuator to make it move to the desired location.
You will need to tune the PD gains (you do not need to calculate them in this assignment; the dynamics equation of robot is not provided anyways). While tuning, start with very small $K_p$ gain and zero $K_d$ gain. Gradually increase the $K_p$ gain until you see some overshoot. Then, increase $K_p$ and $K_d$ gains together, and try to achieve a fast convergence with minimal overshoot.

Implementation

The dynamixel_sdk_examples.zip folder was used and modified for our implementation, where the read_write_node.cpp and the current_read_write_node.cpp file were combined to form a single file combined_read_write.cpp. The same process was done for combining the hpp files.
From these files it was made sure that the topic set_current and the service get_position are available and working.
We made sure that corresponding message and service are present in the dynamixel_sdk_custom_interfaces folder and they were named as SetCurrent.msg and GetPosition.srv.
The PD controller node contains a publisher to set the current values to be given to motor 4, a client for getting the position values from the encoder of motor 4 and a custom service "PosRef.srv" for setting the desired position.
While trying out different gain values for the PD controller, we found that the values $$K_p = 0.3$$ and $$K_d = 0.01$$ worked optimally for our case. This can also be verified by the video shown below.

The results are given below:

Desired Position 1200

PD control of the gripper