Lab 4: Forward and Inverse Kinematics

Note

In this lab (and all the following labs), we will team up! So you can work with your teammate on the lab assignments. While both of you should contribute and TA will ask questions during your demo to make sure you know what you did.

Overview

In this lab, we will switch gears and work on the manipulator. Two algorithms will be designed:

You will write a script to solve the Forward Kinematics problem. Given joint angles and the dimension of the manipulator, the task is to compute the position of the end effector. We will be using Product of Exponentials approach.
You will also try to solve the Inverse Kinematics problem. The task is to find a set of feasible joint angles given the position of the end effector. We will be using the analytical approach (by trigonometry).

We will provide a couple of test cases for you on autograder. The script you submitted should be able to pass all test cases.

Set up robot arm in Gazebo

First, let's download the ROS packages for the robot arm.

cd ~/catkin_ws/src

git clone https://github.com/UCR-Robotics/interbotix_ros_arms.git

For M1/M2 users, run this command as well.

sudo apt-get install ros-noetic-moveit-ros

We can install the dependencies by the following commands.

cd ~/catkin_ws

rosdep update --include-eol-distros

rosdep install --from-paths src --ignore-src -r -y

We need to add one more package that is not currently supported by rosdep install.

sudo apt install python3-pip

sudo pip install modern_robotics

Lastly, with all dependencies ready, we can build the ROS package by the following commands.

cd ~/catkin_ws

catkin_make

Note: Do not panic if you see an error during the build as long as you see "[100%] Built target".

Play with robot arm in Gazebo

Launch the ReactorX 150 robot arm in Gazebo by the following command.

roslaunch interbotix_moveit interbotix_moveit.launch robot_name:=rx150 use_gazebo:=true

You will see the robot arm is ready in Gazebo but the RViz (the visualization software used in ROS) is still pending. This is because it is still waiting for Gazebo to start simulation. In the bottom left of Gazebo window, you will see a small Play ▶ button. Click it to let it run!
Once Gazebo starts simulation, the RViz will prompt you two panels on the left and a visualization of the robot arm on the right. On the top left panel, go to “MotionPlanning” -> “Planning Request” -> “Query Goal State” and check this box. Then you can drag the “ball” on the tip of the robot arm to wherever you want it to go.
Once a goal pose is set, in the bottom left panel, go to “Planning” tab and try buttons “Plan”, “Execute”, or “Plan and Execute”. Cool! The software can figure out a path for the arm to follow and reach the exact goal pose you just set. Spend some time playing with it!
You can also take a look at Gazebo to see the current status of the robot arm. RViz provides a tool for better interaction, but only Gazebo shows the real physical status.

Sample Code Forward and Inverse Kinematics

Open a new terminal and go to your ee144w25 package. We will start from a new python script.

roscd ee144w25/src

touch forward_kinematics.py

gedit forward_kinematics.py

Please copy and paste the following code, and complete the forward_kinematics function in this file. Find x, y, and z for joint angles passed into the function using the PoE methods learned in lectures 12 & 13. Refer to <Programming Tips>.

import numpy as np

from math import pi, cos, sin

import modern_robotics as mr

def forward_kinematics(joints):

# input: joint angles [joint1, joint2, joint3]

# output: the position of end effector [x, y, z]

# add your code here to complete the computation

link1z = 0.065

link2z = 0.039

link3x = 0.050

link3z = 0.150

link4x = 0.150

joint1 = joints[0]

joint2 = joints[1]

joint3 = joints[2]

return [x, y, z]

Similarly, open a new terminal and go to your ee144w25 package.

roscd ee144w25/src

touch inverse_kinematics.py

gedit inverse_kinematics.py

Please copy and paste the following code, and complete the inverse_kinematics function in this file. Find the joint angles for the position passed into the function using an analytical approach covered in lecture 14. Refer to <Programming Tips>.

import numpy as np

from math import pi, cos, sin, atan, atan2, sqrt, acos

def inverse_kinematics(position):

# input: the position of end effector [x, y, z]

# output: joint angles [joint1, joint2, joint3]

# add your code here to complete the computation

link1z = 0.065

link2z = 0.039

link3x = 0.050

link3z = 0.150

link4x = 0.150

x = position[0]

y = position[1]

z = position[2]

return [joint1, joint2, joint3]

Specification

The joint 1 could only rotate in the horizontal plane and joint 2, 3 could only rotate in the vertical plane. The dimension of the ReactorX 150 manipulator is shown below. We take joint4 as the end effector point (instead of the actual gripper).

XY plane

YZ plane

Take into account that the arm has moved in the xy plane.

Refer to origin of the arm when performing calculation.

Programming Tips

We provide two scripts forward_kinematics.py and inverse_kinematics.py for you, you need to complete and submit both of them.
To simplify computation, we will regard joint4 as the end effector. In other words, you need to return the position of joint4 instead of the actual end effector.
Options for Forward Kinematics
- Directly write down the transformation matrices between joints and links.
- Product of Exponentials in space frame.
- Product of Exponentials in body frame.
- You can either compute everything using program, or pre-compute some matrices by hand.
For Inverse Kinematics, we will use the Analytical approach by trigonometry. Please refer to lecture notes for further information.
Two math tools that might be helpful for the analytical approach.
- 1. - 2-argument arctangent θ = arctan(y,x) ∈ (-π,π] (we use atan2 in programming)
    - Law of cosines C = arccos((a^2+b^2−c^2)/2ab) ∈ (0,π)
Math library and functions that might be helpful for the numerical approach.
- 1. - import sympy as sym the math library sympy can help with symbolic mathematics
    - sym.cos and sym.sin the symbolic version operations (distinct from those in math library)
    - sym.symbols create symbolic variables
    - sym.diff compute the derivative of a function with respect to a variable
    - sym.lambdify transform a symbolic function in sympy into a native Python function (using lambda expression), such that the Python function can be evaluated at given points later on.
    - If you do want to proceed with the numerical approach, you need to read the documentation of these functions in order to properly use them in programming.
Please pay attention to the data type used in your computation.
- 1. - If two matrices A and B are of the type np.array, then A * B will not perform matrix multiplication, but element-wise multiplication.
    - If two matrices A and B are of the type np.array, then np.dot(A, B) will perform matrix multiplication.
    - If two matrices A and B are of the type np.matrix, then A * B will perform matrix multiplication.
Functions available in numpy and modern_robotics library.
- 1. - np.cross(A, B) the cross product of two vectors
    - np.concatenate([A, B]) the concatenation of two vectors
    - mr.VecTose3(S) converts a 6x1 twist vector S to a 4x4 matrix in se(3)
    - mr.MatrixExp6(M) the matrix exponential of a 4x4 matrix in se(3)

Sample Code to run Kinematics on Manipulator

Open a new terminal and go to your ee144w25 package. We will start from a new python script.

roscd ee144w25/src

touch manipulator.py

gedit manipulator.py

Copy and paste the following code to the manipulator.py , then modify the code to make the manipulator move:

#!/usr/bin/env python

import rospy

import numpy as np

from math import pi

from std_msgs.msg import Header

from sensor_msgs.msg import JointState

#Import the forward and inverse kinematics python scripts that you have created !

from forward_kinematics import forward_kinematics

from inverse_kinematics import inverse_kinematics

class Manipulator():

def __init__(self):

rospy.init_node('manipulator')

rospy.loginfo("Press Ctrl + C to terminate")

self.joint_pub = rospy.Publisher("/rx150/joint_states", JointState, queue_size=10)

self.rate = rospy.Rate(10)

joint_msg = JointState() #Creates an instance of JointState

joint_msg.header = Header() # Initializes header attribute of JointState

joint_msg.name = [['waist', 'shoulder', 'elbow', 'wrist_angle',\

'wrist_rotate', 'gripper', 'left_finger', 'right_finger']] # List of joint names

joint_msg.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.026, -0.026 ] # Initial positions of joints

# Test Cases

angles = [pi/6, -pi/3, -pi/6] # joint angle in radians

pos = [0.02165, 0.01250, 0.29721]

# Fill in the missing parts of the code below

################################################

# Compute forward and inverse kinematics

position = forward_kinematics(__________) # Fill in function call
joint_angle = inverse_kinematics(__________) # Fill in function call

print('Position of end-effector = ', position)

print('Joint angles = ', joint_angle)

while not rospy.is_shutdown():

joint_msg.header.stamp = rospy.Time.now()

joint_msg.position[0:3] = angles

#WRITE THE LINES FOR PUBLISHING joint_msg

#________________________________

#________________

if __name__ == '__main__':

whatever = Manipulator()

Submission

Submision: group submission via Gradescope
Demo: Both of the teammates will be asked questions.
Due: 11:59 pm, Feb 17 2025 Monday
Files to submit: (please submit exactly the same file)

lab4_report.pdf
forward_kinematics.py
inverse_kinematics.py

Grading rubric:

+ 50% Clearly describe your approach and *provide your manipulator.py* code and explanation in the lab report.
+ 25% Implement forward kinematics and pass test cases.
+ 25% Implement inverse kinematics and pass test cases.
- 10% Penalty applies for each late day up to 2 late days.

Autograder

All code submissions will be graded automatically by an autograder uploaded to Gradescope. Your scripts will be tested on a Ubuntu cloud server using a similar ROS environment. The grading results will be available in a couple of minutes after submission.

Testing parameters are as follows.

The tolerance for distance error is set to 0.002m (Manhattan distance on x, y, z axes).
- The autograder will take the maximum of the error in x, y, z axes respectively, and check if the maximum error is less than 0.002m.
- For example, if the computed position is [0.020, 0.013, 0.298], and the ground truth is [0.021, 0.012, 0.297], it should pass the test.
For the forward kinematics test, three test cases are visible to you; two test cases are hidden. The hidden ones are designed to test some “corner cases” and to make sure there is no hard-coded computation in the submitted script.
The autograder works in the way that it sends joint angles to the manipulator and reads the results after the manipulator actually moves according to these angles. It will compare the ground truth position with the position after moving according to the computed angles.
The time limit is not set in this lab, as the script should be able to get it done in seconds.

Report abuse