## Description

1. (75 Points) On the homework web page, you find four files: kinematic_arm_simulation.mdl,

ArmAnimation.m, forward_kinematics.m, and inverse_kinematics.m. This Simulink simulation is a kinematic simulation of a planar 4-joint-arm (note that a “kinematic simulation” is a

simulation where no mass and inertia properties are included – it basically assumes that there

is a perfect inverse dynamics model in the control loop such that desired states and current

states always coincide). Details of the robot arm are in the figure below. To simplify the inertial properties of the arm, we assume that each link’s mass is a point mass located at pi ,

which also means that each link’s center of mass is at pi . The goal of this assignment will be

to create inverse kinematics control for the center of mass of the entire arm, which prepares

controlling the center of mass of the NAO.

a) Write down the formula for computing center of center of mass pcm of the entire robot arm

using your knowledge of the center of mass of the individual links. Assume that the mass mi

of each link is 1kg, i.e., all links weigh the same amount.

2

b) Write down the generic formula for the geometric Jacobian ∂p4

∂θ of a system with 4 revolute

joints arranged in an open-chain as shown for the 4 degrees-of-freedom system above – just

assume that all the n links form an open-loop chain, where each link is of length li and the

corresponding joint angle is θi with origin of the local coordinate system at pi-1 and joint axis

zi . Note that you do not need to give the part of the Jacobian that deals with orientations.

Explain the symbols in the formula. Use the notation in the figure above for this formula.

c) Expand the formula for the Jacobian in b) such that it is only a function of the joint angles

and link lengths, i.e., eliminate the pi variables

d) From your result in c), what are the Jacobians for the points p1,p2 ,p3 ?

e) Given your previous results, what is the center of mass Jacobian ∂pcm

∂θ

?

f) Implement this center of mass Jacobian for the 4 DOF system above in the Matlab program

inverse_kinematics.m at the indicated position by using the (vector) variables “links” and

“theta” in this function. Provide a printout of the program.

g) Give the general formula of the Jacobian transpose for inverse kinematics computations, and

implement this method in the “inverse_kinematics.m” program. Provide a printout of your

program. Run the Simulink simulation (which implements a figure-8 tracking task for the

center of mass) and provide print-outs of the x-y-Graph and the joint trajectories in the

“scope” block. How well does the method perform, and why does it perform this way?

h) Give the general formula of the pseudo-inverse for inverse kinematics computations, implement this method in the “inverse_kinematics.m” program. Provide a printout of your program. Run the Simulink simulation and provide print-outs of the x-y-Graph and the joint trajectories in the “scope” block. How well does the method perform, and why does it perform

this way?

i) Give the general formula of the pseudo-inverse with Null-space optimization for inverse kinematics computations, and implement this method in the “inverse_kinematics.m” program —

use all joint angles to be 0.5 as a desired optimization posture (see Slide 13, Lecture-9). Provide a printout of your program. Run the Simulink and provide print-outs of the x-y-Graph

and the joint trajectories in the “scope” block. How well does the method perform, and why

does it perform this way?

j) Using a weighted pseudo-inverse allows you changing how much each degree-of-freedom

should contribute to the inverse kinematics. Assume a weight vector for the four joints of the

robot as w=[0.1 0.2 0.3 0.5]. Derive a weighted version of the inverse kinematics of h) and

repeat subproblem h) for this weighted version. How well does the method perform, and why

does it perform this way?

k) You can also use the weights w in j) for weighting the null-space optimization criterion. Repeat the subproblem f) with this method. How well does the method perform, and why does it

perform this way?

2. (50 Points) On the homework web page, you find the file: draw_task.cpp. This task implements a simple operational space reaching task with minimum jerk trajectories. You need to

3

add this task to your CMakeLists.txt compilation, and also in naoUser/src/initUserTasks.c. A

simple point-to-point movement is currently implemented and should work. Modify this program to create a figure-8 drawing in Cartesian space. Create the figure-8 in the X-Z plane of

the workspace, as big as possible, but make sure that the NAO can realize the figure-8 without violating the max/min joint space limits it has. Create phase plots of the figure-8 the same

as in Homework 1.i. Provide the phase plot printouts and the printout of your modified

draw_task.cpp. Comment on how the performance of accomplishing this task.