Commit 5c775fba authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Pierre Fernbach
Browse files

Update readme to refactor of V2

parent aefd45d4
......@@ -5,33 +5,216 @@
This package is extracted from an original work of Justin Carpentier (jcarpent@laas.fr),
with the goal to simplify the library and remove old dependencies.
with the goal to simplify the library, make it more generic and remove old dependencies.
This package installs a python module used to define, store and use ContactSequence objects.
This package provide C++ structures with python bindings used to define and store contact phases and contact sequences.
Basic usage of the ContactSequence object is to initialize either an empty sequence of given size :
# Dependencies
``` Python
cs = ContactSequenceHumanoid(num_contact_phases)
* 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
```
Or load it from file :
Clone the repository and build the package:
``` Python
cs = ContactSequenceHumanoid(0)
cs.loadFromXML(filename, "ContactSequence") # From an XML file
cs.loadFromBinary(filename) # From a binary file (with .cs extension)
```
git clone --recursive git@gepgitlab.laas.fr:loco-3d/multicontact-api.git
cd multicontact-api && mkdir build && cd build
cmake .. && make
make test
```
This ContactSequence object store a sequence of ContactPhases, defining a set of active contact with their placement.
A ContactPhase can also store centroidal data : state trajectory (c,dc,L) and control trajectory (ddc,dL).
This trajectories are stored as a set of discretized points, the time corresponding to each point can be found in 'time trajectory'.
# Usage
For example, to access the position of the center of mass during the first contact phase of a motion :
## Main classes
``` Python
phase = cs.contact_phases[0]
for k in range(len(phase.state_trajectory)):
print "c ("+str(phase.time_trajectory[k])+") = "+str(phase.state_trajectory[k][0:3])
```
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);
```
**Helper methods**
_TODO_ Several helper methods will be added to easy the contact sequence creation process.
Supports Markdown
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