# 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.

When you are done, send me a compressed archive `YourName.zip`

containing:

- Your written answers in a PDF document (kudos if you write it with LaTeX)
- Your source code, with comments indicating which part corresponds to which question

Do not hesitate to e-mail me if one of the questions below is unclear.

## Problem 0: getting started

### Question 0.1

Install pymanoid.

This question is *optional*, for instance if you are not a Linux user (yet?).
If you skip this step, answer programming questions in your favorite
language/robotics environment.

## 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:

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] - 0.45) < 0.1 and abs(com[1] - 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

## Bonus questions

### Motion planning

Solve the 2D motion planning problem using PRM question from the online book Introduction to Open Source Robotics.