Commit 630674a4 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Demo] finish notebook 1

parent 464cd042
%% Cell type:markdown id: tags:
## Introduction
This package provide C++ structures with python bindings used to define and store contact phases and contact sequences.
The classes defined by this package are able to store a wide variety of data related to the locomotion of legged robots. This classes can be efficiently serialized and used to share motions or any "part" of motion (eg. contact plan or centroidal reference) between frameworks and users.
In this package, the different classes do not have access to any robot model and only store the data given by an user. There is nothing to check that the different data stored are consistent together or valid (eg. there could be a contact placement for a feet defined at a given time, but also a wholebody configuration defined for the same time where the feet is at a different position).
However, a couple of methods are available to check that part of the data are correctly defined, are continuous and/or have consistent time definition. See the last section of this notebook for more details.
## Main classes
This package define 4 main classes representing physical concepts of legged locomotion. Contact Model, Contact Patch, Contact Phase and Contact Sequence.
A Contact Model define the physical properties of a contact. A Contact Patch define completely a contact between a part of the robot and the environment, it contain a Contact Model. A Contact Phase is defined by a constant set of contacts, it contains one or more Contact Patches. Finally, a Contact Sequence is a sequence of Contact Phases.
### Contact Model
A Contact Model define the physical properties of a contact: the type of contact (Planar or Point), the physical positions of the contact points with respect to the "center" of the contact, and the coefficient of friction for this contact.
%% Cell type:code id: tags:
``` python
import numpy as np
from multicontact_api import ContactType, ContactModel
cm_default = ContactModel()
print("Contact Type: ", cm_default.contact_type)
print("Friction coefficient: ", cm_default.mu)
```
%%%% Output: stream
Contact Type: CONTACT_UNDEFINED
Friction coefficient: -1.0
%% Cell type:markdown id: tags:
The default constructor create a ContactModel with "undefined" values. Other constructors exist to build a ContactModel with a friction coefficient or a contact type:
%% Cell type:code id: tags:
``` python
cm1 = ContactModel(0.5)
print("cm1 friction coefficient:", cm1.mu)
print("cm1 Contact Type: ", cm1.contact_type)
cm2 = ContactModel(0.9, ContactType.CONTACT_POINT)
print("\ncm2 friction coefficient:", cm2.mu)
print("cm2 Contact Type: ", cm2.contact_type)
print("cm2 friction coefficient:", cm2.mu)
print("cm2 number of contact points: ", cm2.num_contact_points)
print("cm2 contact point position: ", cm2.contact_points_positions)
```
%%%% Output: stream
cm1 friction coefficient: 0.5
cm2 Contact Type: CONTACT_POINT
cm1 Contact Type: CONTACT_UNDEFINED
cm2 friction coefficient: 0.9
cm2 Contact Type: CONTACT_POINT
cm2 number of contact points: 1
cm2 contact point position: [0. 0. 0.]
%% Cell type:markdown id: tags:
The contact point position are expressed in the contact frame (see the next section for more details). By default they are positionned at the origin. It is also possible to create contact model for planar contacts and define the contacts positions at the corners of a rectangular feet:
%% Cell type:code id: tags:
``` python
cm_planar = ContactModel(0.5, ContactType.CONTACT_PLANAR)
# define 4 contacts points at the corners of a rectangle:
cm_planar.num_contact_points = 4
lx = 0.2 / 2. # half size of the feet along x axis
ly = 0.13 / 2. # half size of the feet along y axis
contact_points = np.zeros([3,4])
contact_points[0, :] = [-lx, -lx, lx, lx]
contact_points[1, :] = [-ly, ly, -ly, ly]
cm_planar.contact_points_positions = contact_points
# print the contact model data:
print("cm_planar: ", cm_planar)
print("cm_planar: \n", cm_planar)
```
%%%% Output: stream
cm_planar: ContactType: 1, mu: 0.5
cm_planar:
ContactType: 1, mu: 0.5
Number of contact points: 4, positions:
-0.1 -0.1 0.1 0.1
-0.065 0.065 -0.065 0.065
0 0 0 0
%% Cell type:markdown id: tags:
### Contact Patch
A contact patch define the placement (in SE(3), in the world frame) of a contact between a part of the robot and the environment. It contains a ContactModel.
The main constructor take as input the placement as an SE(3) object from Pinocchio and a ContactModel as previously defined.
%% Cell type:code id: tags:
``` python
from pinocchio import SE3, Quaternion
from multicontact_api import ContactPatch
placement = SE3()
placement.setRandom()
patch = ContactPatch(placement, cm_planar)
print("patch: ", patch)
print("patch: \n", patch)
```
%%%% Output: stream
patch: Placement:
patch:
Placement:
R =
0.740419 -0.428065 0.518208
0.421023 -0.305616 -0.854013
0.523946 0.850505 -0.0460591
p = -0.563486 0.0258648 0.678224
ContactModel : ContactType: 1, mu: 0.5
Number of contact points: 4, positions:
-0.1 -0.1 0.1 0.1
-0.065 0.065 -0.065 0.065
0 0 0 0
%% Cell type:markdown id: tags:
The placement defined here is the placement of the origin of the 'contact frame'. As explained in the previous section the physical contact points positions may be different than this placement and they are expressed in this contact frame. In the previous example, the position of the front left corner of the feet, in the world frame is computed as follow:
%% Cell type:code id: tags:
``` python
pos_RF = patch.placement * patch.contact_model.contact_points_positions[:, 3]
print(pos_RF)
```
%%%% Output: stream
[ 0.27524087 -0.30677788 0.13904126]
%% Cell type:markdown id: tags:
Other constructors exist to build a ContactPatch without requiring to build a ContactModel:
%% Cell type:code id: tags:
``` python
p2 = SE3()
p2.setRandom()
patch2 = ContactPatch(p2, 0.5) # constructor with only a placement and a friction coefficient
print("patch2:", patch2)
print("patch2:\n", patch2)
p3 = SE3()
p3.setRandom()
patch3 = ContactPatch(p3) # constructor with only a placement
print("patch3:", patch3)
print("patch3:\n", patch3)
```
%%%% Output: stream
patch2: Placement:
patch2:
Placement:
R =
-0.904459 -0.418759 -0.0812014
-0.136665 0.104151 0.985127
-0.404074 0.902105 -0.15143
p = 0.434594 -0.716795 0.213938
-0.22528 0.817598 -0.529889
-0.394879 0.420576 0.816814
0.890685 0.393254 0.228105
p = 0.0485744 -0.012834 0.94555
ContactModel : ContactType: 0, mu: 0.5
Number of contact points: 1, positions:
0
0
0
patch3: Placement:
patch3:
Placement:
R =
0.967399 -0.156046 0.199472
0.173229 0.982268 -0.071705
-0.184746 0.103922 0.977276
p = 0.608354 -0.686642 -0.198111
0.414966 -0.909316 0.0307914
0.868596 0.406004 0.28408
-0.27082 -0.0911383 0.958306
p = 0.539828 -0.199543 0.783059
ContactModel : ContactType: 0, mu: -1
Number of contact points: 1, positions:
0
0
0
%% Cell type:markdown id: tags:
With this constructors, the ContactModel type is undefined and there is a single contact point at the origin of the frame.
%% Cell type:markdown id: tags:
### Contact Phase
A contact phase is defined by a constant set of contact points. In the context of bipedal walking, two examples of contact phases are the single and double support phases.
The most important member of the ContactPhase is thus a map<EffectorName, ContactPatch> defining the set of contact between a specific part of the robot and the environment.
%% Cell type:code id: tags:
``` python
from multicontact_api import ContactPhase
cp = ContactPhase()
cp.addContact("right-feet", patch)
cp.addContact("left-feet", patch2)
```
%%%% Output: execute_result
True
%% Cell type:markdown id: tags:
The string argument choice is left to the user and is not required to match anything specifc. However, a good practice when working with a real robot model is to use the name of the frame of the robot creating the contact, as specified in the URDF.
The ContactPhase class have several methods to access to the contacts already added:
%% Cell type:code id: tags:
``` python
print("number of contacts: ", cp.numContacts())
print("effectors in contact: ", cp.effectorsInContact())
print("Is 'right-feet' in contact: ", cp.isEffectorInContact("right-feet"))
print("Is 'right-hand' in contact: ", cp.isEffectorInContact("right-hand"))
# Access the contact patch of a specific contact:
print("\n Contact placement of the right feet: \n", cp.contactPatch("right-feet").placement)
```
%%%% Output: stream
number of contacts: 2
effectors in contact: ['right-feet', 'left-feet']
Is 'right-feet' in contact: True
Is 'right-hand' in contact: False
Contact placement of the right feet:
R =
0.444451 -0.414554 0.794109
0.175408 -0.829041 -0.530964
0.878462 0.375281 -0.295751
p = 0.257742 -0.270431 0.0268018
%% Cell type:markdown id: tags:
The map<EffectorName, ContactPatch> can also be retrieved in Python as a Dictionary but this operation add a lot of overhead and should be avoided.
%% Cell type:code id: tags:
``` python
dict_patches = cp.contactPatches()
print("effectors in contact: ", dict_patches.keys())
```
%%%% Output: stream
effectors in contact: dict_keys(['left-feet', 'right-feet'])
%% Cell type:markdown id: tags:
The ContactPhase object also store different other optionnal data. For instance, a phase can be defined for a specific time interval:
%% Cell type:code id: tags:
``` python
cp.timeInitial = 1.
cp.timeFinal = 3.5
print("Duration of the phase: ",cp.duration)
```
%%%% Output: stream
Duration of the phase: 2.5
%% Cell type:markdown id: tags:
#### Centroidal data
Optionnaly, a Contact Phase can store data related to the centroidal dynamic. It store the following initial and final values as public member:
%% Cell type:code id: tags:
``` python
# Initial values: the values at the beginning of the phase
cp.c_init = np.random.rand(3) # CoM position
cp.dc_init = np.random.rand(3) # CoM velocity
cp.ddc_init = np.random.rand(3) # CoM acceleration
cp.L_init = np.random.rand(3) # Angular momentum
cp.dL_init = np.random.rand(3) # Angular momentum time derivative
# Final values: the values at the end of the phase:
cp.c_final = np.random.rand(3)
cp.dc_final = np.random.rand(3)
cp.ddc_final = np.random.rand(3)
cp.L_final = np.random.rand(3)
cp.dL_final = np.random.rand(3)
```
%% Cell type:markdown id: tags:
In addition to the initial and final values, a ContactPhase can store a trajectory for all of the following data:
```
```Python
cp.c_t # the CoM trajectory (dimension 3)
cp.dc_t # the CoM velocity (dimension 3)
cp.ddc_t # the CoM acceleration (dimension 3)
cp.L_t # the Angular Momentum (dimension 3)
cp.dL_t # the Angular Momentum time derivative (dimension 3)
cp.wrench_t # the centroidal wrench (dimension 6)
cp.zmp_t # the Zero Moment Point (dimension 3)
cp.root_t # the base position and orientation (in SE3)
```
All of this trajectories are represented with objects from the Curves library (https://github.com/loco-3d/curves). Example below add a third order polynomial trajectory as the CoM position:
%% Cell type:code id: tags:
``` python
import curves
from curves import polynomial
coefs = np.random.rand(3, 4) # generate random coefficients for the polynomial: 4 3D points
# build a 3rdf order polynomial of dimension 3 with random coefficients in a specific time interval:
c_t = polynomial(coefs, cp.timeInitial, cp.timeFinal)
cp.c_t = c_t
print("CoM position at t=2. : ", cp.c_t(2.))
```
%%%% Output: stream
CoM position at t=2. : [1.71418299 1.22685941 3.10291713]
CoM position at t=2. : [1.46198335 2.86437628 1.58724145]
%% Cell type:markdown id: tags:
#### Whole body data
A Contact Phase can also store data related to the wholebody motion, it store the initial and final wholebody configuration:
```
```Python
cp.q_init
cp.q_final
```
As long as the following trajectories:
```
```Python
cp.q_t # joint trajectory
cp.dq_t # joint velocity trajectory
cp.ddq_t # joint acceleration trajectory
cp.tau_t # joint torque trajectory
```
%% Cell type:markdown id: tags:
#### Effectors data
The ContactPhase class can also store the contact forces and contact normal forces, in a map<String,curve_ptr> with the effector name as Key. In order to add a contact force trajectory for an effector, this effector must be in contact (see subsection above to add contact to the phase).
%% Cell type:code id: tags:
``` python
coefs = np.random.rand(1, 4) # generate random coefficients for the polynomial: 4 1D points
# build a 3rdf order polynomial of dimension 1 with random coefficients in a specific time interval:
normal_force = polynomial(coefs, cp.timeInitial, cp.timeFinal)
cp.addContactNormalForceTrajectory("right-feet", normal_force)
print("Contact normal force at t=2. :", cp.contactNormalForce("right-feet")(2.))
```
%%%% Output: stream
Contact normal force at t=2. : [2.79459457]
Contact normal force at t=2. : [2.37947778]
%% Cell type:code id: tags:
``` python
coefs = np.random.rand(12, 4) # generate random coefficients for the polynomial: 4 12D points
forces = polynomial(coefs, cp.timeInitial, cp.timeFinal)
cp.addContactForceTrajectory("right-feet",forces)
```
%%%% Output: execute_result
True
%% Cell type:markdown id: tags:
The dimension of the trajectory for the contact forces is not fixed. It is up to the user to decide what is the representation of the forces stored. One of the main representation is to store the 3D force vector at each corner of the feet, leading to a curve of dimension 12.
The ContactModel class provide the generator matrix that could be used to map this 12D contact forces to the 6D wrench expressed in the contact frame:
%% Cell type:code id: tags:
``` python
contact_model = cp.contactPatch("right-feet").contact_model
contact_wrench = contact_model.generatorMatrix() @ cp.contactForce("right-feet")(2.)
print("Contact wrench for right-feet at t=2. : ", contact_wrench)
```
%%%% Output: stream
Contact wrench for right-feet at t=2. : [ 7.78325758 6.15952217 7.76835136 -0.03157656 0.19061326 -0.0740368 ]
Contact wrench for right-feet at t=2. : [ 7.52273183e+00 7.34567270e+00 9.78934959e+00 -7.35090018e-03
2.79734222e-02 -1.29831921e-03]
%% Cell type:markdown id: tags:
Finally, the contact phase can also store effector trajectory for the swinging limbs in a map<String,curve_SE3_ptr> with the effector name as Key. The effector cannot be in contact and have an effector trajectory.
%% Cell type:code id: tags:
``` python
from math import sqrt
from curves import SE3Curve
# Build two SE3 for the initial/final placement of the feet:
init_pose = SE3.Identity()
end_pose = SE3.Identity()
init_pose.translation = np.array([0.2, -0.7, 0.6])
end_pose.translation = np.array([3.6, -2.2, -0.9])
init_pose.rotation = Quaternion.Identity().matrix()
end_pose.rotation = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0).normalized().matrix()
# Build a curve in SE3 doing a linear interpolation between this two placements:
eff_HL = SE3Curve(init_pose, end_pose, cp.timeInitial,cp.timeFinal)
cp.addEffectorTrajectory("left-hand",eff_HL)
print("Left hand placement at t=2.5: \n", cp.effectorTrajectory("left-hand").evaluateAsSE3(2.5))
```
%%%% Output: stream
Left hand placement at t=2.5:
R =
1 0 0
0 0.587785 -0.809017
0 0.809017 0.587785
p = 2.24 -1.6 -0.3
%% Cell type:markdown id: tags:
### Contact Sequence
As soon as a creation or a rupture of contact point occurs the contact set is modified, defining a new contact phase.
The concatenation of contact phases describes what we name a contact sequence, inside which all the contact phases have their own duration.
A contact sequence is basically a Vector of Contact Phase, with several helper method which can be used to ease the creation of a Contact Sequence.
One can either create a Contact sequence with a know number of contact Phase and correctly set the members of the Contact Phases with:
%% Cell type:code id: tags:
``` python
from multicontact_api import ContactSequence
cs = ContactSequence(3)
cs.contactPhases[0] = cp # assign the phase defined below as the first one
print("CoM position at the beginning of the first phase: ", cs.contactPhases[0].c_init)
cp1 = cs.contactPhases[1]
cp1.c_final = np.array([1., 0. ,0.85])
print("CoM position at the end of the second phase: ", cs.contactPhases[1].c_final)
```
%%%% Output: stream
CoM position at the beginning of the first phase: [0.32799509 0.26935971 0.39995176]
CoM position at the beginning of the first phase: [0.21871697 0.86272939 0.97332564]
CoM position at the end of the second phase: [1. 0. 0.85]
%% Cell type:markdown id: tags:
Or simply append new Contact Phase at the end of the current Contact Sequence:
%% Cell type:code id: tags:
``` python
cp3 = ContactPhase()
cp3.c_final = np.array([5, 0, 0.85])
cs.append(cp3)
print("CoM position at the end of the last phase: ", cs.contactPhases[-1].c_final)
```
%%%% Output: stream
CoM position at the end of the last phase: [5. 0. 0.85]
%% Cell type:markdown id: tags:
**Helper methods to check a contact sequence**
The ContactSequence class contains several methods to check if the sequence contains some of the optional data, and if they are consistents across all the contact phases.
This methods should be used in order to check if a ContactSequence object given as input to your algorithm have been correctly initialized with all the data that you are going to use in your algorithm.
It may also be used to check if your algorithm output consistent data.
Examples of such methods are `haveCentroidalTrajectories` which check that the (c, dc, ddc, L, dL) have been initialized, have the correct duration, and that each trajectories of one phase correctly end at the same value as it begin in the next phase. Here is the list of all such methods:
```
```
* `haveConsistentContacts` check that there is always one contact change between adjacent phases in the
sequence and that there isn't any phase without any contact.
* `haveCOMvalues` Check that the initial and final CoM position values are defined for all phases
Also check that the initial values of one phase correspond to the final values of the previous ones.
* `haveAMvalues` Check that the initial and final AM values are defined for all phases
Also check that the initial values of one phase correspond to the final values of the previous ones.
* `haveCentroidalValue`s Check that the initial and final CoM position and AM values are defined for all phases
Also check that the initial values of one phase correspond to the final values of the previous ones.
* `haveConfigurationsValues` Check that the initial and final configuration are defined for all phases
Also check that the initial values of one phase correspond to the final values of the previous ones.
* `haveCOMtrajectories` check that a c, dc and ddc trajectories are defined for each phases
Also check that the time interval of this trajectories matches the one of the phase
and that the trajectories start and end and the correct values defined in each phase
* `haveAMtrajectories` check that a L and dL trajectories are defined for each phases
Also check that the time interval of this trajectories matches the one of the phase
and that the trajectories start and end and the correct values defined in each phase
* `haveCentroidalTrajectories` check that all centroidal trajectories are defined for each phases
Also check that the time interval of this trajectories matches the one of the phase
and that the trajectories start and end and the correct values defined in each phase
* `haveEffectorsTrajectories` check that for each phase preceeding a contact creation,
an SE3 trajectory is defined for the effector that will be in contact.
Also check that this trajectory is defined on the time-interval of the phase.
Also check that the trajectory correctly end at the placement defined for the contact in the next phase.
If this effector was in contact in the previous phase, it check that the trajectory start at the previous contact
placement.
* `haveJointsTrajectories` Check that a q trajectory is defined for each phases
Also check that the time interval of this trajectories matches the one of the phase
and that the trajectories start and end and the correct values defined in each phase
* `haveJointsDerivativesTrajectories` Check that a dq and ddq trajectories are defined for each phases
Also check that the time interval of this trajectories matches the one of the phase
and that the trajectories start and end and the correct values defined in each phase
* `haveJointsTrajectorie`s Check that a joint torque trajectories are defined for each phases
Also check that the time interval of this trajectories matches the one of the phase
and that the trajectories start and end and the correct values defined in each phase
* `haveJointsTrajectories` Check that a contact force trajectory exist for each active contact
Also check that the time interval of this trajectories matches the one of the phase
and that the trajectories start and end and the correct values defined in each phase
* `haveRootTrajectories` check that a root trajectory exist for each contact phases.
Also check that it start and end at the correct time interval
* `haveFriction` check that all the contact patch used in the sequence have
a friction coefficient initialized
* `haveContactModelDefined` check that all the contact patch have a contact_model defined
* `haveZMPtrajectories check` that all the contact phases have a zmp trajectory
**Helper methods to access Data**
The ContactSequence class also contains methods for easier access to the data contained in the ContactPhase vector. For example, `phaseAtTime` or `phaseIdAtTime` can be used to access a specific ContactPhase at a given time.
`getAllEffectorsInContact` output all the effectors used to create contact during the sequence.
Finally, methods exists to return the complete trajectory along the contact sequence, concatenating the trajectories of each phases (eg. `concatenateCtrajectories` return the complete c(t) trajectory for all the contact sequence). More details on this can be found on the Third notebook: load_from_file.
%% Cell type:markdown id: tags:
## Going further
Several helper methods have been added to the ContactSequence class to ease the contact creation process. See the next notebook for more information about this.
As said in the introduction, all the classes of this package can be serialized. The third notebook shows how to load a ContactSequence serialized and how to access some of the data, eg. plotting the centroidal trajectory or displaying the wholebody motion stored.
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment