CS 545: Introduction to Robotics Fall 2019
HW 3: Inverse Kinematics
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 qkFK(q) − pdk2
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:
(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)
CS 545: Introduction to Robotics Fall 2019
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 con-
figuration 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,
and save as ik-a solution.png.
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
CS 545: Introduction to Robotics Fall 2019
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.
5. Try increasing the radius of the obstacle r. What do you observe? Also experiment
with different starting configurations q0. Discuss the results in a file named
answers.pdf.