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

\begin{equation*} \ddot{c}(t) = \omega^2 (c(t) - r(t)) \end{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.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

## Discussion

You can use Markdown with $\LaTeX$ formulas in your comment.