The goal of homework1 is to practice and implement what you have learned in the recent courses. In the following exercises, you will implement your own forward kinematics(FK) algorithm step by step and test the algorithm in simulation using a Franka Panda 7DoF arm.

Exercise 0

Let’s take a look at what we have now. You will get:

1. py : this file is a template where you need to write your own code.
2. py : build a simulation environment. This file does not need to be modified, but you can call some functions provided for you.
3. py : this file provides some demonstrations to show whether your algorithms work well in the simulation by calling functions in robot_sim.py and core.py .
4. Folder /robot_model : contains the URDF files and its meshes. Do not change anything in this folder.

Then you need to install the dependencies. Python version 3.7 or 3.8 is recommended. It is recommended to use conda and virtual environment to configure the programming environment.

The third-party libraries you need are: pybullet and numpy. For numpy version, I am using version

1.20.1. New versions and some slightly older versions are also available.

For pybullet, just run:

Exercise 1: Rigid-Body Motions 【

You need to implement some basics functions of rigid-body motions.

NOTE1: write your code in the template file core.py . DO NOT MODIFY THE FUNCTION NAME!!! Otherwise the autotest program will not work and you will lose your score 🙂

NOTE2: The input and output of the function are numpy arrays!

Here is what you need to finish:

1. invR=rot_inv(R) : Inverts a rotation matrix, use the property of rotation matrix.

input: R: Rotation matrix R return: The inverse of R

2. so3mat=vec_to_so3(omg) : Converts a 3-vector to an so(3) representation.

input: omg: A 3-vector return: The corresponding 3 × 3 skew-symmetric matrix in so(3).

3. omg=so3_to_vec(so3mat) : Converts an so(3) representation to a 3-vector.

input: so3mat: A 3 × 3 skew-symmetric matrix (an element of so(3)).

return: The corresponding 3-vector.

4. [omghat,theta]=axis_ang3(expc3) : Converts a 3-vector of exponential coordinates for rotation into axis-angle form input: expc3: A 3-vector of exponential coordinates for rotation

return1: omghat: The corresponding unit rotation axis

return2: theta: The corresponding rotation angle

5. R=matrix_exp3(so3mat) : Computes the matrix exponential of a matrix in so(3) input: so3mat: An so(3) representation of exponential coordinates for rotation, . return: The that is achieved by rotating about by from an initial orientation .
6. R=matrix_log3(R) : Computes the matrix logarithm of a rotation matrix input: R: Rotation matrix.

return: so3mat: The corresponding so(3) representation of exponential coordinates.

7. T=rp_to_trans(R,p) : Converts a rotation matrix and a position vector into homogeneous transformation matrix. input1: R: Rotation matrix.

input2: p: A 3-vector

return: T: The corresponding homogeneous transformation matrix .

8. [R,p]=TransToRp(T) : Converts a homogeneous transformation matrix into a rotation matrix and position vector.

input: T: Transformation matrix.

return1: R: The corresponding rotation matrix.

return2: p: The corresponding position.

9. invT = trans_inv(T) : Inverts a homogeneous transformation matrix.

input: T: Transformation matrix.

return: invT: Inverse of T.

10. se3mat=vec_to_se3(V) : Converts a spatial velocity vector into a 4×4 matrix in se3.

input: V: A 6-vector (e.g. representing a twist).

return: se3mat: The corresponding 4 × 4 se(3) matrix.

11. V = se3_to_vec(se3mat) : Converts an se3 matrix into a spatial velocity vector.

input: se3mat: A 4 × 4 se(3) matrix. return: V: The corresponding 6-vector.

12. AdT = adjoint(T) : Computes the adjoint representation of a homogeneous transformation matrix.

input: T: Transformation matrix.

return: AdT: The corresponding 6 × 6 adjoint representation .

13. S=screw_to_axis(q, s, h): Takes a parametric description of a screw axis and converts it to a normalized screw axis.

input1: q: A point lying on the screw axis. input2: An unit vector in the direction of the screw axis.

input3: The pitch (linear velocity divided by angular velocity) of the screw axis. return: S: The corresponding normalized screw axis .

14. [S,theta] = axis_ang6(expc6): Converts a 6-vector of exponential coordinates into screw axis-angle form.

input: expc6: A 6-vector of exponential coordinates for rigid-body motion, return1: S: The corresponding normalized screw axis S. return2: theta: The distance traveled along/about S.

15. T=matrix_exp6(se3mat) : Computes the matrix exponential of an se3 representation of exponential coordinates.

input: se3mat: An se(3) representation of exponential coordinates for rigid-body motion return: T: The that is achieved by traveling along/about the screw axis a distance from an initial configuration .

16. se3mat=matrix_log6(T) : Computes the matrix logarithm of a homogeneous transformation matrix.

input: T: Transformation matrix. return: se3mat: The corresponding se(3) representation of exponential coordinates.

That’s the end of exercise1. You may find this list is a bit long, but these implementations are quite simple. So, don’t worry 🙂

Exercise 2: Forward Kinematics

Now is time to use the code in exercise 1 to perform your own forward kinematics(FK) algorithm. Here are the functions you need to implement in the template:

1. T=FK_in_body(M, Blist, thetalist): Computes forward kinematics in the body frame for an open chain robot.

input1: M: The home configuration of the end-effector.

input2: Blist: The joint screw axes in the end-effector frame when the manipulator is at the home position. input3: thetalist: A list of joint coordinate values.

return: The representing the end-effector frame when the joints are at the specified coordinates.

2. T=FK_in_space(M, Slist, thetalist) : Computes forward kinematics in the space frame for an open chain robot.

input1:M: The home configuration of the end-effector.

input2: Slist: The joint screw axes in the space frame when the manipulator is at the home position. input3: thetalist: A list of joint coordinate values.

return: The representing the end-effector frame when the joints are at the specified coordinates.

Exercise 3: Play with the demo

Congratulations! You have completed your coding part. It‘s time to play with a robot arm! (in simulation)

Run the code in demo.py , especially using function: FK_test(robot, physicsClientId)

In this demo, you will send commands to set the position of each joint and see whether the real configuration caused by commands is the same as the result of your analytical calculation using your own FK algorithm.

After running the demo, you will see a robot(Franka Panda Arm) like this.

You can press the CTRL and left mouse button and drag the mouse to change the view angle. You can also zoom in or out by sliding the mouse wheel.

If your FK algorithm is correct, you will find something like this:

This is the end of homework1~