README.md 9.81 KB
Newer Older
Guilhem Saurel's avatar
Guilhem Saurel committed
1 2
# Multicontact API

Guilhem Saurel's avatar
badges  
Guilhem Saurel committed
3 4 5 6
[![Pipeline status](https://gepgitlab.laas.fr/loco-3d/multicontact-api/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/loco-3d/multicontact-api/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/loco-3d/multicontact-api/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/loco-3d/multicontact-api/master/coverage/)


Justin Carpentier's avatar
Justin Carpentier committed
7
This package is extracted from an original work of Justin Carpentier (jcarpent@laas.fr),
8
with the goal to simplify the library, make it more generic and remove old dependencies.
Pierre Fernbach's avatar
Pierre Fernbach committed
9

10
This package provide C++ structures with python bindings used to define and store contact phases and contact sequences. 
11

12
# Dependencies
13

14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
* Eigen
* [Pinocchio](https://github.com/stack-of-tasks/pinocchio)
* [Curves](https://github.com/loco-3d/curves)
* [Eigenpy](https://github.com/stack-of-tasks/eigenpy) (Only for python bindings)

# Installation procedure

## From binary
This package is available as binary in [robotpkg/wip](http://robotpkg.openrobots.org/robotpkg-wip.html)

## From sources
Install the required dependencies, eg. (choose for python version):

```
sudo apt-get install robotpkg-py35-pinocchio robotpkg-py35-curves
29 30
```

31
Clone the repository and build the package:
32

33 34 35 36 37
```
git clone --recursive git@gepgitlab.laas.fr:loco-3d/multicontact-api.git
cd multicontact-api && mkdir build && cd build
cmake .. && make
make test
38 39
```

40
# Usage
41

42
## Main classes
43

44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217
This package define 3 main classes representing physical concepts of legged locomotion. Contact Patch, Contact Phase and Contact Sequence.

### Contact Patch

A contact patch define the placement (in SE(3), in the world frame) of a contact as long as the friction coefficient for this contact. 

Future work will also include the type of contact (eg planar, punctual, bilateral ...), a representation of the friction cone or additional information about the contact.

### 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.
A contact phase store several data, many of which are optional. 

It store the set of active contacts as a map<String, ContactPatch> with the effector name as Keys:

```python
cp = ContactPhase()
p = SE3()
p.setRandom()
patchRF = ContactPatch(p,0.5) # create a new contact patch at the placement p with a friction coefficient of 0.5
cp.addContact("right-feet",patchRF)
# check if an effector is in contact:
cp.isEffectorInContact("right-feet")
# access to the contact patch from the effector name: 
cp.contactPatch("right-feet")
``` 

A contact phase can be defined in a specific time interval: 

```python
cp = ContactPhase()
cp.timeInitial = 1.
cp.timeFinal =3.5
``` 

**Centroidal dynamic data**

Optionnaly, a Contact Phase can store data related to the centroidal dynamic. It store the following initial and final values as public member:

```c
 /// \brief Initial position of the center of mass for this contact phase
  point3_t m_c_init;
  /// \brief Initial velocity of the center of mass for this contact phase
  point3_t m_dc_init;
  /// \brief Initial acceleration of the center of mass for this contact phase
  point3_t m_ddc_init;
  /// \brief Initial angular momentum for this contact phase
  point3_t m_L_init;
  /// \brief Initial angular momentum derivative for this contact phase
  point3_t m_dL_init;
  /// \brief Final position of the center of mass for this contact phase
  point3_t m_c_final;
  /// \brief Final velocity of the center of mass for this contact phase
  point3_t m_dc_final;
  /// \brief Final acceleration of the center of mass for this contact phase
  point3_t m_ddc_final;
  /// \brief Final angular momentum for this contact phase
  point3_t m_L_final;
  /// \brief Final angular momentum derivative for this contact phase
  point3_t m_dL_final;
``` 

It also store centroidal trajectories, represented with objects from the [Curves](https://github.com/loco-3d/curves) library:


```c
/// \brief trajectory for the center of mass position
  curve_ptr m_c;
  /// \brief trajectory for the center of mass velocity
  curve_ptr m_dc;
  /// \brief trajectory for the center of mass acceleration
  curve_ptr m_ddc;
  /// \brief trajectory for the angular momentum
  curve_ptr m_L;
  /// \brief trajectory for the angular momentum derivative
  curve_ptr m_dL;
  /// \brief trajectory for the centroidal wrench
  curve_ptr m_wrench;
  /// \brief trajectory for the zmp
  curve_ptr m_zmp;
  /// \brief SE3 trajectory of the root of the robot
  curve_SE3_ptr m_root;
``` 

**Wholebody data:**

A Contact Phase can also store data related to the wholebody motion, it store the following initial and final wholebody configuration as public member: 

```c
  /// \brief Initial whole body configuration of this phase
  pointX_t m_q_init;
  /// \brief Final whole body configuration of this phase
  pointX_t m_q_final;
``` 

And the following trajectories:

```c
/// \brief trajectory for the joint positions
  curve_ptr m_q;
  /// \brief trajectory for the joint velocities
  curve_ptr m_dq;
  /// \brief trajectory for the joint accelerations
  curve_ptr m_ddq;
  /// \brief trajectory for the joint torques
  curve_ptr m_tau;
``` 

It can also store the contact forces and contact normal forces, in a map<String,curve_ptr> with the effector name as Key:


```python
fR = createRandomPiecewisePolynomial(12)
cp.addContactForceTrajectory("right-feet",fR)
# access the trajectory : 
cp.contactForce("right-feet")
# contact normal force :
fnR = createRandomPiecewisePolynomial(1)
cp.addContactNormalForceTrajectory("right-feet",fnR)
# access the trajectory : 
cp.contactNormalForce("right-feet")
``` 

And the effector trajectory for the swinging limbs, also in a map<String,curve_ptr> with the effector name as Key:

```python
# create a SE3 trajectory:
init_pose = SE3.Identity()
end_pose = SE3.Identity()
init_pose.translation = array([0.2, -0.7, 0.6])
end_pose.translation = 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()
effL = SE3Curve(init_pose, end_pose, cp.timeInitial,cp.timeFinal)
# add it to the contact phase:
cp.addEffectorTrajectory("left-feet",effL)
# access the trajectory : 
cp.effectorTrajectory("left-feet")
``` 

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

```c
ContactSequence cs = ContactSequence(3);
ContactPhase cp0;
/* set the contact phase members ... */
cs.contactPhase(0) = cp0;
// or:
cs.contactPhase(1).m_c_init = Point3_t(1,0,0.7);
cs.contactPhase(1).timeFinal(3.);
// or : 
ContactPhase& cp2 = cs.contactPhase(2);
/* set the contact phase members ... */

``` 

Or simply append new Contact Phase at the end of the current Contact Sequence;

```c
ContactSequence cs; // create empty contact sequence
ContactPhase cp0;
/* set the contact phase members ... */
cs.append(cp0);
``` 

218
**Helper methods to create contact sequence**
219

220 221 222 223 224 225 226
Several helper methods have been added to the ContactSequence class to ease the contact creation process:

* `breakContact(eeName, phaseDuration)` Add a new contactPhase at the end of the current ContactSequence, the new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact removed.

* `createContact(eeName, contactPatch, phaseDuration)` Add a new contactPhase at the end of the current ContactSequence, the new ContactPhase have the same ContactPatchs as the last phase of the sequence, with the exeption of the given contact added.

* `moveEffectorToPlacement(eeName, placement, durationBreak, durationCreate)`  Add two new phases at the end of the current ContactSequence:
Pierre Fernbach's avatar
Pierre Fernbach committed
227 228 229
  * it break the contact with eeName
  * it create the contact with eeName at the given placement.

230 231 232 233 234

* `moveEffectorOf(eeName, transform, durationBreak, durationCreate)` Similar to moveEffectorToPlacement but use a transform from the previous contact placement instead of a new placement. 



235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262
**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 `haveConsistentContacts` or `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. 


**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 effector 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).


## Examples

[Examples](examples/README.md) provide several serialized ContactSequence files with descriptions. 







263 264 265