HW 3: Inverse Kinematics

CS 545: Introduction to Robotics 
HW 3: Inverse Kinematics
Submit all answers in a PDF named hw3 {USCNetID}.pdf (example: hw3 ttrojan.pdf).
In this assignment, you will compute numerically the inverse kinematics of a planar
robot with n = 3 rotational joints. Specifically, our goal is to compute the joint-positions
that bring the end-effector to a desired position pd. In this exercise we will ignore
the orientation of the end-effector. We will formulate the problem as an optimization
program as follows:
minimize
q
kFK(q) − pdk
2
subject to li ≤ qi ≤ ui
, i = {1, . . . , n}.
The objective is to find the values of the joints q that minimize the difference between
the actual position of the end-effector computed using the forward kinematics FK(q),
and the desired position pd, subject to the joint limits li
, ui
.
1. First, implement the forward kinematics function in the file fk.py. The function
should receive the joint values q and the lengths of the 3 links L, and it should
return the position of the robot’s end-effector. Report the results for the following
values. Note that q is in radians.
(a) q = [0.0, 0.0, 0.0], L = [1.0, 1.0, 1.0]
(b) q = [0.3, 0.4, 0.8], L = [0.8, 0.5, 1.0]
(c) q = [1.0, 0.0, 0.0], L = [3.0, 1.0, 1.0]
Hint: Express the pose of the first link relative to the reference frame using a rigid
transform with an elementary rotation about the z-axis, then the pose of the second
link relative to the first link and so on up to the end-effector.
2. For the inverse kinematics computation, you will use the scipy.optimize package.1
We will numerically compute a solution through the following function call:
solution = minimize(objective, q0, method=‘SLSQP’, bounds=bnds)
1https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.minimize.html
1
CS 545: Introduction to Robotics Fall 2021
objective refers to the objective function that the optimizer attempts to minimize.
q0 is the initial vector of values and bounds specify the range of allowable values.
The scipy.minimize function has problems handling type promotion, so make
sure all your numerical values are of type np.float64.
We specify the following parameters for the IK problem:
pd = [0.1, 1.33, 0.0], L = [0.7, 1.0, 1.0], q0 = [0, 0, 1.86],(li
, ui) = (−π, π) ∀i
We provide the file ik-a.py, which includes also code for plotting. The initial configuration of the robot should show up as follows, with the base of robot in red
and the desired end-effector position in green. To compute the inverse kinematics,
fill in the objective function. Visualize the result with the given plotting function.
Save your figure as ik-a solution.png and add it to the PDF.
Figure 1: Example visualization. A solution state should result
in the arm touching the green sphere.
3. You will now add an obstacle that the robot should avoid. You will approximate the
obstacle with a sphere, with circle c = (cx, cy, cz) and radius r. To detect whether
the robot collides with the obstacle, the easiest way is to implement a line-sphere
2
CS 545: Introduction to Robotics Fall 2021
intersection algorithm2
. If there is no real solution, then there is no intersection
between a line and the sphere. In the provided file collision.py, fill in the function
line sphere intersection. It should implement the algorithm and return the
value under the square-root (the discriminant).
4. To ensure that the robot does not collide with the obstacle, we will add three obstacle collision constraints, one for each robot link. The constraints are provided
as input to the optimization algorithm, as shown in the provided file ik-b.py. Fill
in the code for the constraints, using the collision detection algorithm from the
previous exercise. The parameters for the collision sphere are as follows:
c = (0.6, 0.5, 0),r = 0.2
Note that if the constraints are satisfied, they should return a non-negative value.
Using the following parameters for the obstacle, solve and visualize the solution.
Save your visualization as ik-b solution.png and add it to the PDF.
5. Try increasing the radius of the obstacle r. What do you observe? Also experiment
with different starting configurations q0. Add a figure to the PDF for at least one
increased obstacle radius, and add another figure for at least one different starting
configuration. Discuss the results in your PDF answers.
2https://en.wikipedia.org/wiki/Line-sphere_intersection
3