Skip to content
Snippets Groups Projects
Unverified Commit fe84ec9f authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #6 from nim65s/devel

update README
parents 56d0ea14 20e5eea0
No related branches found
No related tags found
No related merge requests found
......@@ -4,7 +4,7 @@
[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-bezier-com-traj/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-bezier-com-traj/master/coverage/)
Copyright 2018 LAAS-CNRS
Copyright 2018-2020 LAAS-CNRS
Authors: Pierre Fernbach and Steve Tonneau
......@@ -27,9 +27,9 @@ https://hal.archives-ouvertes.fr/hal-01726155v1
## Dependencies
* [centroidal-dynamics-lib] Centroidal dynamics computation library (https://github.com/stonneau/centroidal-dynamics-lib)
* [spline] Bezier curves library (https://github.com/stonneau/spline)
* [glpk] GNU Linear Programming Kit (https://www.gnu.org/software/glpk/)
* [centroidal-dynamics-lib](https://github.com/stonneau/centroidal-dynamics-lib) Centroidal dynamics computation library
* [curves](https://github.com/loco-3d/curves) Bezier curves library
* [glpk](https://www.gnu.org/software/glpk/) GNU Linear Programming Kit
## Additional dependencies for python bindings
* [Boost.Python](http://www.boost.org/doc/libs/1_63_0/libs/python/doc/html/index.html)
......@@ -77,41 +77,41 @@ to python.
For the zero step capturability, we will first define a contact phase using the objects from centroidal_dynamics:
```
#importing the libraries of interest
from centroidal_dynamics import * #centroidal dynamics utilities
from spline import * #bezier curves utilities
from bezier_com_traj import * #the actual library
import curves # noqa - necessary to register curves::bezier_curve
import numpy as np
from numpy import array
from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP
from hpp_bezier_com_traj import (SOLVER_QUADPROG, ConstraintFlag, Constraints, ContactData, ProblemData,
computeCOMTraj, zeroStepCapturability)
# create an Equilibrium solver, for a robot of 54 kilos. We linearize the friction cone to four generating rays
eq = Equilibrium("test", 54., 4)
# Now define some contact points ...
from numpy import array, asmatrix, matrix
import numpy as np
P = array([[x, y, 0] for x in [-0.05, 0.05] for y in [-0.1, 0.1]])
P = asmatrix(array([array([x,y,0]) for x in [-0.05,0.05] for y in [-0.1,0.1]]))
#and normals
z = array([0.,0.,1.])
N = asmatrix(array([z for _ in range(4)]))
z = array([0., 0., 1.])
N = array([[0., 0., 1.]] * 4)
#setting contact positions and normals, as well as friction coefficient of 0.3
#EQUILIBRIUM_ALGORITHM_PP is the algorithm that will always be used for our problems
eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
eq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
```
Then, we will define the initial state of our robot:
```
#c0 is the initial center of mass position
c0 = matrix([0.,0.,1.]) .T
c0 = array([0., 0., 1.])
#we set the inital speed dc0 to a rather slow 10 cm / s along the x axis
dc0 = matrix([0.1,0.,0.]).T
l0 = matrix([0.,0.,0.]).T
T = 1.2
tstep = -1.
dc0 = array([0.1, 0., 0.])
l0 = array([0., 0., 0.])
```
And finally, some optimization parameters:
......@@ -129,9 +129,8 @@ tstep = -1.
We can now solve the problem:
```
# the boolean value indicates whether to use or not angular momentum
result = zeroStepCapturability(eq,c0,dc0,l0,False,T,tstep)
print result.success
result = zeroStepCapturability(eq, c0, dc0, l0, False, T, tstep)
print(result.success)
#True the problem was feasible, and a trajectory was successfully computed
```
......@@ -139,16 +138,14 @@ print result.success
The found centroidal trajectory is accessible from the returned object, only if the problem
was feasible
```
res.c_of_t # a bezier curve object describing the com trajectory
result.c_of_t # a bezier curve object describing the com trajectory
#We can check that the end velocity is indeed zero:
dc_of_t = res.c_of_t.compute_derivate(1) # computing first derivative
print np.linalg.norm(dc_of_t(dc_of_t.max()))
dc_of_t = result.c_of_t.compute_derivate(1) # computing first derivative
print(np.linalg.norm(dc_of_t(dc_of_t.max())))
# 0.0
```
refer to the [test file](https://gitlab.com/stonneau/bezier_COM_traj/blob/master/python/test/binding_tests.py) for more advanced problems, including kinematic constraints,
mutiple contact phases handling and angular momentum
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment