From 20e5eea020d50c692705d23d3c6c50b3d418f42c Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@gmail.com>
Date: Fri, 10 Apr 2020 15:16:07 +0200
Subject: [PATCH] [README] fix 0step example in README

---
 README.md | 41 ++++++++++++++++++++---------------------
 1 file changed, 20 insertions(+), 21 deletions(-)

diff --git a/README.md b/README.md
index b617770..92cb957 100644
--- a/README.md
+++ b/README.md
@@ -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,11 +138,11 @@ 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
 
 ```
-- 
GitLab