# Robotics assignment 1

All questions are formulated in Python with pymanoid but you can solve them using any other robotics environment. Stars indicate questions that are significantly harder than those before them.

## Problem 1: interpolation

### Question 1.1

Consider two points $$A$$ and $$B$$ in the 3D Euclidean space, as well as a duration $$T$$ in seconds. Derive the coefficients of a polynomial $$P(t)$$ of time $$t$$ such that:

• $$P(0) = A$$
• $$P(T) = B$$
• $$P'(0) = 0$$
• $$P'(T) = 0$$

### Question 1.2

Here are two transformations in the Euclidean space:

pose0 = array([ 0.65775439, -0.10466086, 0.05145381, 0.74414903, 1.4, 0.0, 1.4])
pose1 = array([ 0.47447667, -0.13290111, -0.05144997, 0.86865533, 1.22861559, 0.67119575, 1.68765532])


Each pose consists of a quaternion [qw qx qy qz] followed by the three position coordinates [x y z]. We will use poses to describe the position and orientation of robot frames (for instance, the heel frame under a foot link) with respect to the intertial frame.

Find a Slerp interpolation function for the language you are using, and use it to implement a function pose(t) such that:

• pose(0) = pose0
• pose(1) = pose1
• The quaternion pose(t) is the Slerp between pose0[:4] and pose1[:4]
• The linear velocity pose'(0)[4:] = 0
• The linear velocity pose'(1)[4:] = 0

### Question 1.3

In pymanoid, a Contact is a surface where the robot can put its feet or hands. A contact is defined by two things: its dimensions (width and length, in meters), and the location of its frame in the Euclidean space, as follows:

(half_length, half_width) = (0.1, 0.05)  # contact dimensions in [m]
c0 = Contact((half_width, half_length), pose=[ 0.65775439, -0.10466086, 0.05145381, 0.74414903, 1.4, 0.0, 1.4])
c1 = Contact((half_width, half_length), pose=[ 0.47447667, -0.13290111, -0.05144997, 0.86865533, 1.22861559, 0.67119575, 1.68765532])


Adapting your solution to Question 1, draw a trajectory between the two contacts c0 and c1 above. You can use the draw_line function.

## Problem 2: dynamics

The dynamics of a walking biped are summed up by the second-order differential equation:

$$\ddot{c}(t) = \omega^2 (c(t) - r(t))$$

where $$\omega^2$$ is a constant, $$c(t)$$ is the position of the center of mass (CoM) of the robot and $$r(t)$$ is the position of the center of pressure (CoP) at time $$t$$.

### Question 2.1

Let us take $$\omega = \sqrt{13} \ \text{s}^{-1}$$ for now. Assuming that $$r(t) = r_f$$ is stationary, solve the differential equation above, writing $$c(t)$$ as a function of $$t$$, $$\omega^2$$ and the CoP location $$r_f$$.

### Question 2.2

Implement your solution as a function integrate with the following specification:

• Parameters:
• Initial CoM position
• Initial CoM velocity
• CoP position
• Duration $$T$$
• Return values:
• CoM position at time $$T$$
• CoM velocity at time $$T$$

Run it on the following example:

init_com = [0.01, 0.002, 0.8]  # initial CoM position in [m]
init_comd = [0., 0.01, 0.1]  # initial CoM velocity in [m] / [s]
cop = [0., 0.0, 0.]  # CoP position in [m]
duration = 0.2  # duration in [s]

last_com, last_comd = integrate(init_com, init_comd, cop, duration)


What are the values of last_com and last_comd?

### Question 2.3

Draw the corresponding trajectory from the initial state (init_com, init_comd) to the terminal state (last_com, last_comd).

## Problem 3: planar walking

Here is a set of footsteps in the horizontal plane (x and y coordinates only):

footsteps = [
[0.0, -0.09],
[0.0, 0.09],
[0.2, -0.09],
[0.45, 0.09],
[0.725, -0.09],
[0.975, 0.09],
[1.25, -0.09],
[1.5, 0.09],
[1.775, -0.09],
[1.975, 0.09],
[1.975, -0.09],
[2.0, 0.09],
[0.035, -0.09],
[0.035, 0.09],
[0.335, -0.09],
[0.335, 0.09],
[0.635, -0.09],
[0.635, 0.09]]


Each footstep is a rectangle centered at coordinates (x, y) from the list above. As in Question 1.p, the width of each footstep rectangle (i.e. its dimension along the y-axis) is 10 cm, while the length of each rectangle (its dimension along the x-axis) is 20 cm.

### Question 3.1

Read the paper Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations by Pierre-Brice Wieber. Are there some parts you don't manage to understand? If so, pick one and pinpoint the step that you don't grasp.

### Question 3.2

What is the CoM height $$h$$ corresponding to the value of $$\omega$$ we used in Question 2.1?

### Question 3.3

Find a trajectory with piecewise-constant CoPs such that, starting from init_com=[0., 0., 0.8], the CoM ends above the fourth footstep of the sequence above, i.e. the 20 cm x 10 cm rectangle centered on [0.45, 0.09]. You can describe your trajectory as a list of (CoP, duration) pairs that can be played in Python as:

com = array([0., 0., 0.8])
comd = zeros(3)
for (cop, duration) in trajectory:
com, comd = integrate(com, comd, cop, duration)
assert(abs(com - 0.45) < 0.1 and abs(com - 0.09) < 0.05)


### Question 3.4 ★

Compute a CoM-CoP trajectory that traverses these footsteps using the method from the paper above. You can use the solve_qp function from qpsolvers in Python.

### Question 3.5

Combine your answers to the questions so far to display your walking trajectory in pymanoid, showing all of the following:

• Left foot trajectory in green
• Right foot trajectory in magenta
• CoP trajectory in red
• CoM trajectory in blue
Pages of this website are under the CC-BY 4.0 license.