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

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 Wieber (2006). Are there some parts you don’t understand? If so, pick one and pinpoint what you don’t grasp. Are there some points you disagree with? If so, detail why.

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