 #### 聯系方式 #### 您當前位置：首頁 >> Python編程Python編程

###### 日期：2019-10-30 10:20

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

2

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