From 2e45b57e9faafb4faeb74e1224530290d3634067 Mon Sep 17 00:00:00 2001
From: Pierre-Alexandre Leziart <paleziart@laas.fr>
Date: Thu, 15 Jul 2021 07:24:10 +0000
Subject: [PATCH] Merge from Devel Croco 14/07/2021
---
CMakeLists.txt | 3 +
python/gepadd.cpp | 1 +
scripts/Controller.py | 16 +-
scripts/MPC_Wrapper.py | 49 +-
.../crocoddyl_class/MPC_crocoddyl_planner.py | 467 +++++++-----------
scripts/crocoddyl_eval/README.md | 35 +-
.../test_1/analyse_control_cycle.py | 188 +++++++
scripts/crocoddyl_eval/test_1/analyse_simu.py | 137 -----
scripts/crocoddyl_eval/test_1/main.py | 193 --------
.../crocoddyl_eval/test_1/run_scenarios.py | 72 ---
scripts/crocoddyl_eval/test_2/unit_test.py | 190 -------
.../test_2/unittest_augmented.py | 146 ++++++
.../crocoddyl_eval/test_2/unittest_linear.py | 144 ++++++
.../test_2/unittest_nonlinear.py | 144 ++++++
.../crocoddyl_eval/test_2/unittest_step.py | 144 ++++++
.../test_3/analyse_control_cycle.py | 148 ++++++
scripts/crocoddyl_eval/test_3/analyse_simu.py | 355 -------------
scripts/crocoddyl_eval/test_3/main.py | 231 ---------
.../crocoddyl_eval/test_3/run_scenarios.py | 63 ---
scripts/main_solo12_control.py | 2 +-
20 files changed, 1157 insertions(+), 1571 deletions(-)
create mode 100644 scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
delete mode 100644 scripts/crocoddyl_eval/test_1/analyse_simu.py
delete mode 100644 scripts/crocoddyl_eval/test_1/main.py
delete mode 100644 scripts/crocoddyl_eval/test_1/run_scenarios.py
delete mode 100644 scripts/crocoddyl_eval/test_2/unit_test.py
create mode 100644 scripts/crocoddyl_eval/test_2/unittest_augmented.py
create mode 100644 scripts/crocoddyl_eval/test_2/unittest_linear.py
create mode 100644 scripts/crocoddyl_eval/test_2/unittest_nonlinear.py
create mode 100644 scripts/crocoddyl_eval/test_2/unittest_step.py
create mode 100644 scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
delete mode 100644 scripts/crocoddyl_eval/test_3/analyse_simu.py
delete mode 100644 scripts/crocoddyl_eval/test_3/main.py
delete mode 100644 scripts/crocoddyl_eval/test_3/run_scenarios.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 96761f71..1a17b43c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,6 +23,9 @@ include(cmake/base.cmake)
include(cmake/boost.cmake)
include(cmake/python.cmake)
+# Disable -Werror on Unix.
+SET(CXX_DISABLE_WERROR True)
+
# Project definition
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
project(${PROJECT_NAME} ${PROJECT_ARGS})
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 9d4791cf..ae0478e9 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -274,6 +274,7 @@ struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
.def_readwrite("I_mat", &Params::I_mat)
.def_readwrite("h_ref", &Params::h_ref)
.def_readwrite("shoulders", &Params::shoulders)
+ .def_readwrite("lock_time", &Params::lock_time);
.def_readwrite("footsteps_init", &Params::footsteps_init)
.def_readwrite("footsteps_under_shoulders", &Params::footsteps_under_shoulders);
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 9f8a19f1..15d2c0a3 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -232,7 +232,13 @@ class Controller:
# Solve MPC problem once every k_mpc iterations of the main loop
if (self.k % self.k_mpc) == 0:
try:
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait)
+ if self.type_MPC == 3 :
+ # Compute the target foostep in local frame, to stop the optimisation around it when t_lock overpass
+ l_targetFootstep = self.footstepPlanner.getRz().transpose() @ self.footTrajectoryGenerator.getFootPosition() - self.q[0:3,0:1]
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, l_targetFootstep)
+ else :
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)))
+
except ValueError:
print("MPC Problem")
@@ -249,8 +255,12 @@ class Controller:
t_mpc = time.time()
# If the MPC optimizes footsteps positions then we use them
- if self.k > 100 and self.type_MPC == 3:
- o_targetFootstep[:2, :] = self.footstepPlanner.getRz()[:2, :2] @ self.x_f_mpc[24:, 0].reshape((2, 4), order='F') + self.q[0:2, 0:1]
+ if self.k > 100 and self.type_MPC == 3 :
+ for foot in range(4):
+ id = 0
+ while cgait[id,foot] == 0 :
+ id += 1
+ o_targetFootstep[:2,foot] = np.array(self.footstepPlanner.getRz()[:2, :2]) @ self.x_f_mpc[24 + 2*foot:24+2*foot+2, id] + np.array([self.q[0, 0] , self.q[1,0] ])
# Update pos, vel and acc references for feet
self.footTrajectoryGenerator.update(self.k, o_targetFootstep)
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index 6aabb66d..f3004d1c 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -54,11 +54,12 @@ class MPC_Wrapper:
self.multiprocessing = params.enable_multiprocessing
if self.multiprocessing: # Setup variables in the shared memory
self.newData = Value('b', False)
- self.newResult = Value('b', False)
- self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait))
- if self.mpc_type == 3: # Need more space to store optimized footsteps
+ self.newResult = Value('b', False)
+ if self.mpc_type == 3: # Need more space to store optimized footsteps and l_fsteps to stop the optimization around it
+ self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait + 12) )
self.dataOut = Array('d', [0] * 32 * (np.int(self.n_steps)))
else:
+ self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait))
self.dataOut = Array('d', [0] * 24 * (np.int(self.n_steps)))
self.fsteps_future = np.zeros((self.N_gait, 12))
self.running = Value('b', True)
@@ -83,7 +84,7 @@ class MPC_Wrapper:
self.last_available_result = np.zeros((24, (np.int(self.n_steps))))
self.last_available_result[:24, 0] = np.hstack((x_init, np.array([0.0, 0.0, 8.0] * 4)))
- def solve(self, k, xref, fsteps, gait):
+ def solve(self, k, xref, fsteps, gait, l_targetFootstep):
"""Call either the asynchronous MPC or the synchronous MPC depending on the value of multiprocessing during
the creation of the wrapper
@@ -92,12 +93,13 @@ class MPC_Wrapper:
xref (12xN): Desired state vector for the whole prediction horizon
fsteps (12xN array): the [x, y, z]^T desired position of each foot for each time step of the horizon
gait (4xN array): Contact state of feet (gait matrix)
+ l_targetFootstep (3x4 array) : 4*[x, y, z]^T target position in local frame, to stop the optimisation of the feet location around it
"""
if self.multiprocessing: # Run in parallel process
- self.run_MPC_asynchronous(k, xref, fsteps)
+ self.run_MPC_asynchronous(k, xref, fsteps, l_targetFootstep)
else: # Run in the same process than main loop
- self.run_MPC_synchronous(k, xref, fsteps)
+ self.run_MPC_synchronous(k, xref, fsteps, l_targetFootstep)
if k > 2:
self.last_available_result[12:(12+self.n_steps), :] = np.roll(self.last_available_result[12:(12+self.n_steps), :], -1, axis=1)
@@ -139,13 +141,14 @@ class MPC_Wrapper:
self.not_first_iter = True
return self.last_available_result
- def run_MPC_synchronous(self, k, xref, fsteps):
+ def run_MPC_synchronous(self, k, xref, fsteps, l_targetFootstep):
"""Run the MPC (synchronous version) to get the desired contact forces for the feet currently in stance phase
Args:
k (int): Number of inv dynamics iterations since the start of the simulation
xref (12xN): Desired state vector for the whole prediction horizon
fsteps (12xN array): the [x, y, z]^T desired position of each foot for each time step of the horizon
+ l_targetFootstep (3x4 array) : [x, y, z]^T target position in local frame, to stop the optimisation of the feet location around it
"""
# Run the MPC to get the reference forces and the next predicted state
@@ -154,6 +157,9 @@ class MPC_Wrapper:
if self.mpc_type == 0:
# OSQP MPC
self.mpc.run(np.int(k), xref.copy(), fsteps.copy())
+ elif self.mpc_type == 3: # Add goal position to stop the optimisation
+ # Crocoddyl MPC
+ self.mpc.solve(k, xref.copy(), fsteps.copy(), l_targetFootstep)
else:
# Crocoddyl MPC
self.mpc.solve(k, xref.copy(), fsteps.copy())
@@ -161,7 +167,7 @@ class MPC_Wrapper:
# Output of the MPC
self.f_applied = self.mpc.get_latest_result()
- def run_MPC_asynchronous(self, k, xref, fsteps):
+ def run_MPC_asynchronous(self, k, xref, fsteps, l_targetFootstep):
"""Run the MPC (asynchronous version) to get the desired contact forces for the feet currently in stance phase
Args:
@@ -169,6 +175,7 @@ class MPC_Wrapper:
xref (12xN): Desired state vector for the whole prediction horizon
fsteps (12xN array): the [x, y, z]^T desired position of each foot for each time step of the horizon
params (object): stores parameters
+ l_targetFootstep (3x4 array) : [x, y, z]^T target position in local frame, to stop the optimisation of the feet location around it
"""
# If this is the first iteration, creation of the parallel process
@@ -178,7 +185,7 @@ class MPC_Wrapper:
p.start()
# Stacking data to send them to the parallel process
- self.compress_dataIn(k, xref, fsteps)
+ self.compress_dataIn(k, xref, fsteps, l_targetFootstep)
self.newData.value = True
return 0
@@ -204,7 +211,11 @@ class MPC_Wrapper:
# print("New data detected")
# Retrieve data thanks to the decompression function and reshape it
- kf, xref_1dim, fsteps_1dim = self.decompress_dataIn(dataIn)
+ if self.mpc_type != 3:
+ kf, xref_1dim, fsteps_1dim = self.decompress_dataIn(dataIn)
+ else:
+ kf, xref_1dim, fsteps_1dim, l_target_1dim = self.decompress_dataIn(dataIn)
+ l_target = np.reshape(l_target_1dim, (3,4))
# Reshaping 1-dimensional data
k = int(kf[0])
@@ -227,6 +238,8 @@ class MPC_Wrapper:
if self.mpc_type == 0:
fsteps[np.isnan(fsteps)] = 0.0
loop_mpc.run(np.int(k), xref, fsteps)
+ elif self.mpc_type == 3:
+ loop_mpc.solve(k, xref.copy(), fsteps.copy(), l_target.copy())
else:
loop_mpc.solve(k, xref.copy(), fsteps.copy())
@@ -241,21 +254,26 @@ class MPC_Wrapper:
return 0
- def compress_dataIn(self, k, xref, fsteps):
+ def compress_dataIn(self, k, xref, fsteps, l_targetFootstep):
"""Compress data in a single C-type array that belongs to the shared memory to send data from the main control
loop to the asynchronous MPC
Args:
k (int): Number of inv dynamics iterations since the start of the simulation
fstep_planner (object): FootstepPlanner object of the control loop
+ l_targetFootstep (3x4 array) : [x, y, z]^T target position in local frame, to stop the optimisation of the feet location around it
"""
# Replace NaN values by 0.0 to be stored in C-type arrays
fsteps[np.isnan(fsteps)] = 0.0
# Compress data in the shared input array
- self.dataIn[:] = np.concatenate([[(k/self.k_mpc)], xref.ravel(),
- fsteps.ravel()], axis=0)
+ if self.mpc_type == 3:
+ self.dataIn[:] = np.concatenate([[(k/self.k_mpc)], xref.ravel(),
+ fsteps.ravel(), l_targetFootstep.ravel()], axis=0)
+ else:
+ self.dataIn[:] = np.concatenate([[(k/self.k_mpc)], xref.ravel(),
+ fsteps.ravel()], axis=0)
return 0.0
@@ -268,7 +286,10 @@ class MPC_Wrapper:
"""
# Sizes of the different variables that are stored in the C-type array
- sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait]
+ if self.mpc_type == 3:
+ sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait, 12]
+ else:
+ sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait]
csizes = np.cumsum(sizes)
# Return decompressed variables in a list
diff --git a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
index 1493b198..b3f46193 100644
--- a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
+++ b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
@@ -5,7 +5,6 @@ import numpy as np
import quadruped_walkgen as quadruped_walkgen
import pinocchio as pin
-
class MPC_crocoddyl_planner():
"""Wrapper class for the MPC problem to call the ddp solver and
retrieve the results.
@@ -17,7 +16,7 @@ class MPC_crocoddyl_planner():
inner(bool): Inside or outside approximation of the friction cone
"""
- def __init__(self, params, mu=1, inner=True, warm_start=False, min_fz=0.0):
+ def __init__(self, params, mu=1, inner=True, warm_start=True, min_fz=0.0):
# Time step of the solver
self.dt = params.dt_mpc
@@ -29,7 +28,6 @@ class MPC_crocoddyl_planner():
self.mass = 2.50000279
# Inertia matrix of the robot in body frame
- # self.gI = np.diag([0.00578574, 0.01938108, 0.02476124])
self.gI = np.array([[3.09249e-2, -8.00101e-7, 1.865287e-5],
[-8.00101e-7, 5.106100e-2, 1.245813e-4],
[1.865287e-5, 1.245813e-4, 6.939757e-2]])
@@ -40,8 +38,6 @@ class MPC_crocoddyl_planner():
else:
self.mu = mu
- # Weights Vector : States
- # self.stateWeights = np.array([1,1,150,35,30,8,20,20,15,4,4,8])
# Weights Vector : States
self.w_x = 0.3
self.w_y = 0.3
@@ -59,63 +55,47 @@ class MPC_crocoddyl_planner():
self.w_vx, self.w_vy, self.w_vz, self.w_vroll, self.w_vpitch, self.w_vyaw])
# Weight Vector : Force Norm
- # self.forceWeights = np.full(12,0.02)
self.forceWeights = np.array(4*[0.01, 0.01, 0.01])
# Weight Vector : Friction cone cost
- # self.frictionWeights = 10
self.frictionWeights = 0.5
+ # Weights on the heuristic term
+ self.heuristicWeights = np.array(4*[0.3, 0.4])
+
+ # Weight on the step command (distance between steps)
+ self.stepWeights = np.full(8, 0.05)
+
+ # Weights to stop the optimisation at the end of the flying phase
+ self.stopWeights = np.full(8, 1.)
+
+ # Weight for shoulder-to-contact penalty
+ self.shoulderContactWeight = 5
+ self.shoulder_hlim = 0.225
+
# Max iteration ddp solver
self.max_iteration = 10
- # Warm Start for the solver
+ # Warm-start for the solver
+ # Always on, just an approximation, not the previous result
+ # TODO : create a proper warm-start with the previous optimisation
self.warm_start = warm_start
- # Minimum normal force (N)
+ # Minimum normal force(N) and reference force vector bool
self.min_fz = min_fz
+ self.relative_forces = True
# Gait matrix
self.gait = np.zeros((params.N_gait, 4))
self.gait_old = np.zeros((1, 4))
- self.index = 0
# Position of the feet in local frame
self.fsteps = np.full((params.N_gait, 12), 0.0)
- # List of the actionModel
+ # List to generate the problem
self.ListAction = []
-
- # Warm start
self.x_init = []
- self.u_init = []
-
- # Weights on the shoulder term : term 1
- self.shoulderWeights = np.array(4*[0.3, 0.4])
-
- # symmetry & centrifugal term in foot position heuristic
- self.centrifugal_term = True
- self.symmetry_term = True
-
- # Weight on the step command
- self.stepWeights = np.full(4, 0.8)
-
- # Weights on the previous position predicted : term 2
- self.lastPositionWeights = np.full(8, 2.)
-
- # When the the foot reaches 10% of the flying phase, the optimisation of the foot
- # positions stops by setting the "lastPositionWeight" on.
- # For exemple, if T_mpc = 0.32s, dt = 0.02s, one flying phase period lasts 7 nodes.
- # When there are 6 knots left before changing steps, the next knot will have its relative weight activated
- self.stop_optim = 0.1
- self.index_stop = int((1 - self.stop_optim)*(int(0.5*self.T_mpc/self.dt) - 1))
-
- # Index of the control cycle to start the "stopping optimisation"
- self.start_stop_optim = 20
-
- # Predicted position of feet computed by previous cycle, it will be used with
- # the self.lastPositionWeights weight.
- self.oMl = pin.SE3.Identity() # Â transform from world to local frame ("L")
+ self.u_init = []
self.l_fsteps = np.zeros((3, 4))
self.o_fsteps = np.zeros((3, 4))
@@ -133,62 +113,75 @@ class MPC_crocoddyl_planner():
# Initial foot location (local frame, X,Y plan)
self.p0 = [0.1946, 0.14695, 0.1946, -0.14695, -0.1946, 0.14695, -0.1946, -0.14695]
- def solve(self, k, xref, l_feet, oMl=pin.SE3.Identity()):
+ # Index to stop the feet optimisation
+ self.index_lock_time = int(params.lock_time / params.dt_mpc ) # Row index in the gait matrix when the optimisation of the feet should be stopped
+ self.index_stop_optimisation = [] # List of index to reset the stopWeights to 0 after optimisation
+
+ self.initializeModels(params)
+
+ def initializeModels(self, params):
+ ''' Reset the two lists of augmented and step-by-step models, to avoid recreating them at each loop.
+ Not all models here will necessarily be used.
+
+ Args :
+ - params : object containing the parameters of the simulation
+ '''
+ self.models_augmented = []
+ self.models_step = []
+
+ for j in range(params.N_gait) :
+ model = quadruped_walkgen.ActionModelQuadrupedAugmented()
+
+ self.update_model_augmented(model)
+ self.models_augmented.append(model)
+
+ for j in range(4 * int(params.T_gait / params.T_mpc) ) :
+ model = quadruped_walkgen.ActionModelQuadrupedStep()
+
+ self.update_model_step(model)
+ self.models_step.append(model)
+
+ # Terminal node
+ self.terminalModel = quadruped_walkgen.ActionModelQuadrupedAugmented()
+ self.update_model_augmented(self.terminalModel)
+ # Weights vectors of terminal node (no command cost)
+ self.terminalModel.forceWeights = np.zeros(12)
+ self.terminalModel.frictionWeights = 0.
+ self.terminalModel.heuristicWeights = np.full(8, 0.0)
+ self.terminalModel.stopWeights = np.full(8, 0.0)
+
+ return 0
+
+ def solve(self, k, xref, l_feet, l_stop):
""" Solve the MPC problem
Args:
k : Iteration
xref : the desired state vector
- l_feet : current position of the feet
+ l_feet : current position of the feet (given by planner)
+ l_stop : current and target position of the feet (given by footstepTragectory generator)
"""
# Update the dynamic depending on the predicted feet position
- self.updateProblem(k, xref, l_feet, oMl)
+ self.updateProblem(k, xref, l_feet, l_stop)
# Solve problem
self.ddp.solve(self.x_init, self.u_init, self.max_iteration)
- """
- cpt = 0
- N = int(self.T_mpc / self.dt)
- result = np.zeros((8, N))
- for i in range(len(self.ListAction)):
- if self.ListAction[i].__class__.__name__ != "ActionModelQuadrupedStep" :
- if cpt >= N:
- raise ValueError("Too many action model considering the current MPC prediction horizon")
- result[:, cpt] = self.ddp.xs[i][12:]
- cpt += 1
-
- from matplotlib import pyplot as plt
- plt.figure()
- plt.subplot(1, 2, 1)
- for i in range(4):
- plt.plot(result[2*i, :], linewidth=2)
- plt.legend(["FL", "FR", "HL", "HR"])
- plt.subplot(1, 2, 2)
- for i in range(4):
- plt.plot(result[2*i+1, :], linewidth=2)
- plt.legend(["FL", "FR", "HL", "HR"])
- plt.show()
- from IPython import embed
- embed()"""
-
- # Get the results
- self.get_fsteps()
+ # Reset to 0 the stopWeights for next optimisation
+ for index_stopped in self.index_stop_optimisation :
+ self.models_augmented[index_stopped].stopWeights = np.zeros(8)
return 0
- def updateProblem(self, k, xref, l_feet, oMl=pin.SE3.Identity()):
+ def updateProblem(self, k, xref, l_feet, l_stop):
"""Update the dynamic of the model list according to the predicted position of the feet,
and the desired state.
Args:
"""
-
- self.oMl = oMl
-
# Save previous gait state before updating the gait
- self.gait_old[0, :] = self.gait[0, :].copy()
+ self.gait_old[0, :] = self.gait[0, :].copy()
# Recontruct the gait based on the computed footsteps
a = 0
@@ -200,70 +193,101 @@ class MPC_crocoddyl_planner():
# On swing phase before --> initialised below shoulder
p0 = (1.0 - np.repeat(self.gait[0, :], 2)) * self.p0
# On the ground before --> initialised with the current feet position
- p0 += np.repeat(self.gait[0, :], 2) * l_feet[0, [0, 1, 3, 4, 6, 7, 9, 10]] # (x, y) of each foot
+ p0 += np.repeat(self.gait[0, :], 2) * l_feet[0, [0, 1, 3, 4, 6, 7, 9, 10]] # (x, y) of each foot
+
+ self.x_init.clear()
+ self.u_init.clear()
+ self.ListAction.clear()
+ self.index_stop_optimisation.clear()
- if k == 0:
- # Create the list of models
- self.create_List_model()
+ index_step = 0
+ index_augmented = 0
+ j = 0
- # By default we suppose we were in the same state previously
- self.gait_old[0, :] = self.gait[0, :].copy()
- else:
- # Update list of models
- self.roll_models()
-
- j = 0
# Iterate over all phases of the gait
- # The first column of xref correspond to the current state
- # Gap introduced to take into account the Step model (more nodes than gait phases )
- self.x_init = []
- self.u_init = []
- gap = 0
-
- next_step_flag = True
- next_step_index = 0 # Get index of incoming step for updatePositionWeights
-
while np.any(self.gait[j, :]):
-
- if self.ListAction[j+gap].__class__.__name__ == "ActionModelQuadrupedStep":
-
- if next_step_flag:
- next_step_index = j+gap
- next_step_flag = False
-
- self.x_init.append(np.zeros(20))
- self.u_init.append(np.zeros(4))
-
- if j == 0:
- self.ListAction[j+gap].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- xref[:, j], self.gait[0, :] - self.gait_old[0, :])
- else:
- self.ListAction[j+gap].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- xref[:, j], self.gait[j, :] - self.gait[j-1, :])
-
- self.ListAction[j+gap+1].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
- xref[:, j], self.gait[j, :])
- self.x_init.append(np.zeros(20))
- self.u_init.append(np.zeros(12))
-
- gap += 1
- # self.ListAction[j+1].shoulderWeights = 2*np.array(4*[0.25,0.3])
-
- else:
- self.ListAction[j+gap].updateModel(np.reshape(l_feet[j, :], (3, 4),
- order='F'), xref[:, j], self.gait[j, :])
- self.x_init.append(np.zeros(20))
- self.u_init.append(np.zeros(12))
-
+ if j == 0 : # First row, need to take into account previous gait
+ if np.any(self.gait[0,:] - self.gait_old[0,:]) :
+ # Step model
+ self.models_step[index_step].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
+ xref[:, j+1], self.gait[0, :] - self.gait_old[0, :])
+ self.ListAction.append(self.models_step[index_step])
+
+ # Augmented model
+ self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
+ l_stop, xref[:, j+1], self.gait[j, :])
+
+ # Activation of the cost to stop the optimisation around l_stop (position locked by the footstepGenerator)
+ if j < self.index_lock_time :
+ self.models_augmented[index_augmented].stopWeights = self.stopWeights
+ self.index_stop_optimisation.append(index_augmented)
+
+ self.ListAction.append(self.models_augmented[index_augmented])
+
+
+ index_step += 1
+ index_augmented += 1
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.zeros(8))
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
+
+ else :
+ # Augmented model
+ self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
+ l_stop, xref[:, j+1], self.gait[j, :])
+ self.ListAction.append(self.models_augmented[index_augmented])
+
+ index_augmented += 1
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
+
+ else :
+ if np.any(self.gait[j,:] - self.gait[j-1,:]) :
+ # Step model
+ self.models_step[index_step].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
+ xref[:, j+1], self.gait[j, :] - self.gait[j-1, :])
+ self.ListAction.append(self.models_step[index_step])
+
+ # Augmented model
+ self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
+ l_stop, xref[:, j+1], self.gait[j, :])
+
+ # Activation of the cost to stop the optimisation around l_stop (position locked by the footstepGenerator)
+ if j < self.index_lock_time :
+ self.models_augmented[index_augmented].stopWeights = self.stopWeights
+ self.index_stop_optimisation.append(index_augmented)
+
+ self.ListAction.append(self.models_augmented[index_augmented])
+
+ index_step += 1
+ index_augmented += 1
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.zeros(8))
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
+
+
+ else :
+ self.models_augmented[index_augmented].updateModel(np.reshape(l_feet[j, :], (3, 4), order='F'),
+ l_stop, xref[:, j+1], self.gait[j, :])
+ self.ListAction.append(self.models_augmented[index_augmented])
+
+ index_augmented += 1
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, j+1], p0]))
+ self.u_init.append(np.repeat(self.gait[j, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[j, :])] ))
+
+ # Update row matrix
j += 1
- if k > self.start_stop_optim:
- # Update the lastPositionweight
- self.updatePositionWeights(next_step_index)
-
- # Update model of the terminal model # TODO: Check if correct row of l_feet
- self.terminalModel.updateModel(np.reshape(l_feet[j-1, :], (3, 4), order='F'), xref[:, -1], self.gait[j-1, :])
- self.x_init.append(np.zeros(20))
+ # Update terminal model
+ self.terminalModel.updateModel(np.reshape(l_feet[j-1, :], (3, 4), order='F'), l_stop, xref[:, -1], self.gait[j-1, :])
+ # Warm-start
+ self.x_init.append(np.concatenate([xref[:, j-1], p0]))
# Shooting problem
self.problem = crocoddyl.ShootingProblem(np.zeros(20), self.ListAction, self.terminalModel)
@@ -292,7 +316,8 @@ class MPC_crocoddyl_planner():
result[24:, cpt] = self.ddp.xs[i][12:]
cpt += 1
if i > 0 and self.ListAction[i-1].__class__.__name__ == "ActionModelQuadrupedStep":
- print(self.ddp.xs[i][12:])
+ # print(self.ddp.xs[i][12:])
+ pass
return result
@@ -305,6 +330,8 @@ class MPC_crocoddyl_planner():
model.gI = self.gI
model.mu = self.mu
model.min_fz = self.min_fz
+ model.relative_forces = self.relative_forces
+ model.shoulderContactWeight = self.shoulderContactWeight
# Weights vectors
model.stateWeights = self.stateWeights
@@ -312,178 +339,16 @@ class MPC_crocoddyl_planner():
model.frictionWeights = self.frictionWeights
# Weight on feet position
- # will be set when needed
- model.lastPositionWeights = np.full(8, 0.0)
- model.shoulderWeights = self.shoulderWeights
- model.symmetry_term = self.symmetry_term
- model.centrifugal_term = self.centrifugal_term
+ model.stopWeights = np.full(8, 0.0) # Will be updated when necessary
+ model.heuristicWeights = self.heuristicWeights
return 0
def update_model_step(self, model):
"""Set intern parameters for step model type
"""
- model.shoulderWeights = self.shoulderWeights
+ model.heuristicWeights = np.zeros(8)
model.stateWeights = self.stateWeights
model.stepWeights = self.stepWeights
- model.symmetry_term = self.symmetry_term
- model.centrifugal_term = self.centrifugal_term
-
- return 0
-
- def create_List_model(self):
- """Create the List model using ActionQuadrupedModel()
- The same model cannot be used [model]*(T_mpc/dt) because the dynamic changes for each nodes
- """
-
- j = 0
-
- # Iterate over all phases of the gait
- # The first column of xref correspond to the current state
- while np.any(self.gait[j, :]):
-
- model = quadruped_walkgen.ActionModelQuadrupedAugmented()
-
- # Update intern parameters
- self.update_model_augmented(model)
-
- # Add model to the list of model
- self.ListAction.append(model)
-
- # No optimisation on the first line
- if np.any(self.gait[j+1, :]) and not np.array_equal(self.gait[j, :], self.gait[j+1, :]):
-
- model = quadruped_walkgen.ActionModelQuadrupedStep()
- # Update intern parameters
- self.update_model_step(model)
-
- # Add model to the list of model
- self.ListAction.append(model)
-
- j += 1
-
- # Model parameters of terminal node
- self.terminalModel = quadruped_walkgen.ActionModelQuadrupedAugmented()
- self.update_model_augmented(self.terminalModel)
- # Weights vectors of terminal node
- self.terminalModel.forceWeights = np.zeros(12)
- self.terminalModel.frictionWeights = 0.
- self.terminalModel.shoulderWeights = np.full(8, 0.0)
- self.terminalModel.lastPositionWeights = np.full(8, 0.0)
-
- # Shooting problem
- self.problem = crocoddyl.ShootingProblem(np.zeros(20), self.ListAction, self.terminalModel)
-
- # DDP Solver
- self.ddp = crocoddyl.SolverDDP(self.problem)
-
- return 0
-
- def roll_models(self):
- """
- Move one step further in the gait cycle
- Add and remove corresponding model in ListAction
- """
-
- # Remove first model
- flag = False
- if self.ListAction[0].__class__.__name__ == "ActionModelQuadrupedStep":
- self.ListAction.pop(0)
- flag = True
- model = self.ListAction.pop(0)
-
- # Add last model & step model if needed
- if flag: # not np.array_equal(self.gait_old[0, :], self.gait[0, :]):
- modelStep = quadruped_walkgen.ActionModelQuadrupedStep()
- self.update_model_step(modelStep)
-
- # Add model to the list of model
- self.ListAction.append(modelStep)
-
- # reset to 0 the weight lastPosition
- model.lastPositionWeights = np.full(8, 0.0)
- self.ListAction.append(model)
-
- return 0
-
- def get_fsteps(self):
- """Create the matrices fstep, the position of the feet predicted during the control cycle.
-
- To be used after the solve function.
- """
- ##################################################
- # Get command vector without actionModelStep node
- ##################################################
- Us = self.ddp.us
- for elt in Us:
- if len(elt) == 4:
- Us.remove(elt)
- self.Us = np.array(Us)[:, :].transpose()
-
- ################################################
- # Get state vector without actionModelStep node
- ################################################
- # self.Xs[:,0 ] = np.array(self.ddp.xs[0])
- k = 1
- gap = 1
- for elt in self.ListAction:
- if elt.__class__.__name__ != "ActionModelQuadrupedStep":
- self.Xs[:, k - gap] = np.array(self.ddp.xs[k])
- else:
- gap += 1
- k = k + 1
-
- ########################################
- # Compute fsteps using the state vector
- ########################################
-
- j = 0
-
- # Iterate over all phases of the gait
- while np.any(self.gait[j, :]):
- self.fsteps[j, :] = np.repeat(self.gait[j, :], 3)*np.concatenate([self.Xs[12:14, j], [0.],
- self.Xs[14:16, j], [0.],
- self.Xs[16:18, j], [0.],
- self.Xs[18:20, j], [0.]])
- j += 1
-
- ####################################################
- # Compute the current position of feet in contact
- # and the position of desired feet in flying phase
- # in local frame
- #####################################################
-
- for i in range(4):
- index = next((idx for idx, val in np.ndenumerate(
- self.fsteps[:, 3*i]) if ((not (val == 0)) and (not np.isnan(val)))), [-1])[0]
- pos_tmp = np.reshape(
- np.array(self.oMl * (np.array([self.fsteps[index, (i*3):(3+i*3)]]).transpose())), (3, 1))
- self.o_fsteps[:2, i] = pos_tmp[0:2, 0]
-
- return self.fsteps
-
- def updatePositionWeights(self, next_step_index):
- """Update the parameters in the ListAction to keep the next foot position at the same position computed by the
- previous control cycle and avoid re-optimization at the end of the flying phase
- """
-
- if next_step_index == self.index_stop:
- self.ListAction[next_step_index + 1].lastPositionWeights = np.repeat(
- (np.array([1, 1, 1, 1]) - self.gait[0, :]), 2) * self.lastPositionWeights
-
- return 0
-
- def get_xrobot(self):
- """Returns the state vectors predicted by the mpc throughout the time horizon, the initial column is
- deleted as it corresponds initial state vector
- Args:
- """
-
- return np.array(self.ddp.xs)[1:, :].transpose()
-
- def get_fpredicted(self):
- """Returns the force vectors command predicted by the mpc throughout the time horizon,
- Args:
- """
- return np.array(self.ddp.us)[:, :].transpose()[:, :]
+ return 0
\ No newline at end of file
diff --git a/scripts/crocoddyl_eval/README.md b/scripts/crocoddyl_eval/README.md
index b28f6580..284e351e 100644
--- a/scripts/crocoddyl_eval/README.md
+++ b/scripts/crocoddyl_eval/README.md
@@ -1,21 +1,34 @@
-test_1 : Some tests to compare the two MPC solvers in simple case (no obstacles) : ddp & osqp. The solvers operate in parallel and control is looped over the ddp algorithm. All data is stored during the simulation inside the log_eval folder, the algorithms have exactly the same input parameters for each control cycle. Then, to analyze the behavior of solvers, the file analyse_simu.py is used to evaluate each control cycle. Both algorithms can be relaunched and weights on the ddp algorithm can be changed to evaluate the modifications (line 55, Relaunch_DDP boolean to relaunch the ddp algorithm)
+### EVALUATION 1 :
- -> Run python3 crocoddyl_eval/test_1/run_scenarios.py
- -> Run python3 crocoddyl_eval/test_1/analyse_simu.py
+Some tests to compare the two MPC solvers: ddp & osqp. The data collected during the simulation is used to restart and analyze each control cycle with different MPCs or different parameters. A third DDP MPC can be run to evaluate and compare the parameters. (see bool *Relaunch_DDP*)
+ -> Run the simulation with boolean LOGGING on true.
+ -> Modify the path in *Recover Logged data* section.
+ -> Select the MPC control cycle to be analysed and tune the gains if necessary.
+ -> Run python3 crocoddyl_eval/test_1/analyse_control_cycle.py
-test_2 : Test to check the derivatives using the crocoddyl.ActionModelNumDiff (derivatives computed with finite differences). The simulation is run for a short time to get initializations of the foot position. The Luu term (hessian of the cost with regards to the command vector) cannot be accurate because it uses the residual cost (Luu ~= R^T R) which is not written that way in that case.
- -> Run python3 crocoddyl_eval/test_2/unit_test.py
+### EVALUATION 2 :
+Test to check the derivatives computed with the action models. The derivatives are computed with finite differences using the crocodile.ActionMode NumDiff class. The derivatived tested coressponds to :
-test_3 : Test to run the simulation using the ddp as foot step planner too. It involves new c++ models with augmented states to optimises the feet position.
+- Lx : Derivative of the cost wrt states x
+- Lu : Derivative of the cost wrt command u
+- Fx : Derivative of the dynamics wrt states x
+- Fu : Derivative of the dynamics wrt command u
- -> Run python3 crocoddyl_eval/test_3/run_scenarios.py
+For the Hessian of the cost Lxx and Luu, the results are not accurate. With NumDiff, the hessian is approximated with the residual vector : Lxx ~= R^T R, which is not the case here because of the friction cost, non linear terms...
+ -> Modify the parameters of the model in crocoddyl_eval/test_2/unittest_model.py (to evaluate each cost)
+ -> Run python3 crocoddyl_eval/test_2/unittest_model.py
-test_5 : Test to run the simulation using the ddp at the frequency of TSID. The number of node in the ddp has been adjusted to avoid
-a too large number of them. Each control cycle can be monitored after the log in the analyse_simu file.
- -> Run python3 crocoddyl_eval/test_5/run_scenarios.py
- -> Run ipython3 crocoddyl_eval/test_5/analyse_simu.py -i
+### EVALUATION 3 :
+
+Test to evaluate the MPC with the optimisation of the foosteps. Comparison of : ddp + fstep optimization & osqp. The data collected during the simulation is used to restart and analyze each control cycle. The parameters can be modified in *DDP MPC* section.
+
+ -> Run the simulation with boolean LOGGING on true and type_MPC == 3.
+ -> Modify the path in *Recover Logged data* section.
+ -> Select the MPC control cycle to be analysed and tune the gains if necessary.
+ -> Run python3 crocoddyl_eval/test_1/analyse_control_cycle.py
+
diff --git a/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py b/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
new file mode 100644
index 00000000..df51c87e
--- /dev/null
+++ b/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
@@ -0,0 +1,188 @@
+# coding: utf8
+
+import sys
+import os
+from sys import argv
+sys.path.insert(0, os.getcwd()) # adds current directory to python path
+
+import numpy as np
+import matplotlib.pylab as plt
+import libquadruped_reactive_walking as lqrw
+import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
+
+
+##############
+# Parameters
+##############
+iteration_mpc = 205 # Control cycle
+Relaunch_DDP = True # Compare a third MPC with != parameters
+linear_mpc = True
+params = lqrw.Params() # Object that holds all controller parameters
+
+######################
+# Recover Logged data
+######################
+file_name = "crocoddyl_eval/logs/logs_2021_07_05_22_43.npz"
+logs = np.load(file_name)
+planner_gait = logs.get("planner_gait")
+planner_xref = logs.get("planner_xref")
+planner_fsteps = logs.get("planner_fsteps")
+mpc_x_f = logs.get("mpc_x_f")
+
+k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) # simulation iteration corresponding
+
+# OSQP MPC
+mpc_osqp = lqrw.MPC(params)
+mpc_osqp.run(0, planner_xref[0] , planner_fsteps[0]) # Initialization of the matrix
+# mpc_osqp.run(1, planner_xref[1] , planner_fsteps[1])
+mpc_osqp.run(k, planner_xref[k] , planner_fsteps[k])
+
+osqp_xs = mpc_osqp.get_latest_result()[:12,:] # States computed over the whole predicted horizon
+osqp_xs = np.vstack([planner_xref[k,:,0] , osqp_xs.transpose()]).transpose() # Add current state
+osqp_us = mpc_osqp.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
+
+# DDP MPC
+mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=linear_mpc)
+# Without warm-start :
+# mpc_ddp.warm_start = False
+# mpc_ddp.solve(k, planner_xref[k] , planner_fsteps[k] ) # Without warm-start
+
+# Using warm-start logged :
+#( Exactly the function solve from MPC_crocoddyl)
+# But cannot be used because there areis no previous value inside ddp.xs and ddp.us
+mpc_ddp.updateProblem( planner_fsteps[k], planner_xref[k]) # Update dynamics
+
+x_init = []
+u_init = []
+mpc_ddp.ddp.solve(x_init, u_init, mpc_ddp.max_iteration)
+
+k_previous = int( (iteration_mpc - 1) * (params.dt_mpc / params.dt_wbc) )
+
+i = 0
+while planner_gait[k,i,:].any() :
+ i +=1
+
+for j in range(mpc_x_f[0,:,:].shape[1] - 1 ) :
+ u_init.append(mpc_x_f[k_previous,12:,j+1]) # Drag through an iteration (remove first)
+u_init.append(np.repeat(planner_gait[k,i-1,:], 3)*np.array(4*[0.5, 0.5, 5.])) # Add last node with average values, depending on the gait
+
+for j in range(mpc_x_f[0,:,:].shape[1] - 1 ) :
+ x_init.append(mpc_x_f[k_previous,:12,j+1]) # Drag through an iteration (remove first)
+x_init.append(mpc_x_f[k_previous,:12,-1]) # repeat last term
+x_init.insert(0, planner_xref[k,:, 0]) # With ddp, first value is the initial value
+
+# Solve problem
+mpc_ddp.ddp.solve(x_init, u_init, mpc_ddp.max_iteration)
+
+ddp_xs = mpc_ddp.get_latest_result()[:12,:] # States computed over the whole predicted horizon
+ddp_xs = np.vstack([planner_xref[k,:,0] , ddp_xs.transpose()]).transpose() # Add current state
+ddp_us = mpc_ddp.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
+
+######################################
+# Relaunch DDP to adjust the gains
+######################################
+
+mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=True) # To modify the linear model if wanted, recreate a list with proper model
+
+# Weight Vector : State
+# w_x = 0.2
+# w_y = 0.2
+# w_z = 2
+# w_roll = 0.8
+# w_pitch = 1.5
+# w_yaw = 0.11
+# w_vx = 1*np.sqrt(w_x)
+# w_vy = 2*np.sqrt(w_y)
+# w_vz = 1*np.sqrt(w_z)
+# w_vroll = 0.05*np.sqrt(w_roll)
+# w_vpitch = 0.05*np.sqrt(w_pitch)
+# w_vyaw = 0.03*np.sqrt(w_yaw)
+# mpc_ddp.stateWeight = np.array([w_x,w_y,w_z,w_roll,w_pitch,w_yaw,
+# w_vx,w_vy,w_vz,w_vroll,w_vpitch,w_vyaw])
+# OSQP values, in ddp formulation, terms are put in square
+mpc_ddp.stateWeight = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3])
+
+# Friction coefficient
+mpc_ddp.mu = 0.9
+mpc_ddp.max_iteration = 550
+mpc_ddp.warm_start = False
+
+# Minimum normal force (N)
+mpc_ddp.min_fz = 0.
+mpc_ddp.max_fz = 25
+
+# Integration scheme
+# V+ = V + dt*B*u ; P+ = P + dt*V+ != explicit : P+ = P + dt*V
+mpc_ddp.implicit_integration = False
+
+# Weight on the shoulder term :
+mpc_ddp.shoulderWeights = 0.
+mpc_ddp.shoulder_hlim = 0.27
+
+# Weight Vector : Force Norm
+# mpc_ddp.forceWeights = np.array(4*[0.01,0.01,0.01])
+mpc_ddp.forceWeights = np.sqrt(4*[0.00005, 0.00005, 0.00005]) # OSQP values
+
+# Weight Vector : Friction cone cost
+mpc_ddp.frictionWeights = 0.5
+
+mpc_ddp.relative_forces = False
+
+# Update weights and params inside the models
+mpc_ddp.updateActionModel()
+
+# Run ddp solver
+# Update the dynamic depending on the predicted feet position
+if Relaunch_DDP :
+ mpc_ddp.updateProblem( planner_fsteps[k], planner_xref[k])
+ mpc_ddp.ddp.solve(x_init, u_init, mpc_ddp.max_iteration)
+ ddp_xs_relaunch = mpc_ddp.get_latest_result()[:12,:] # States computed over the whole predicted horizon
+ ddp_xs_relaunch = np.vstack([planner_xref[k,:,0] , ddp_xs_relaunch.transpose()]).transpose() # Add current state
+ ddp_us_relaunch = mpc_ddp.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
+
+#############
+# Plot #
+#############
+
+# Predicted evolution of state variables
+l_t = np.linspace(0., params.T_gait, np.int(params.T_gait/params.dt_mpc)+1)
+l_str = ["X_osqp", "Y_osqp", "Z_osqp", "Roll_osqp", "Pitch_osqp", "Yaw_osqp", "Vx_osqp", "Vy_osqp", "Vz_osqp", "VRoll_osqp", "VPitch_osqp", "VYaw_osqp"]
+l_str2 = ["X_ddp", "Y_ddp", "Z_ddp", "Roll_ddp", "Pitch_ddp", "Yaw_ddp", "Vx_ddp", "Vy_ddp", "Vz_ddp", "VRoll_ddp", "VPitch_ddp", "VYaw_ddp"]
+
+index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
+plt.figure()
+for i in range(12):
+ plt.subplot(3, 4, index[i])
+
+ pl1, = plt.plot(l_t, ddp_xs[i,:], linewidth=2, marker='x')
+ pl2, = plt.plot(l_t, osqp_xs[i,:], linewidth=2, marker='x')
+
+ if Relaunch_DDP :
+ pl3, = plt.plot(l_t, ddp_xs_relaunch[i,:], linewidth=2, marker='x')
+ plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
+
+ else :
+ plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
+
+
+
+# Desired evolution of contact forces
+l_t = np.linspace(params.dt_mpc, params.T_gait, np.int(params.T_gait/params.dt_mpc))
+l_str = ["FL_X_osqp", "FL_Y_osqp", "FL_Z_osqp", "FR_X_osqp", "FR_Y_osqp", "FR_Z_osqp", "HL_X_osqp", "HL_Y_osqp", "HL_Z_osqp", "HR_X_osqp", "HR_Y_osqp", "HR_Z_osqp"]
+l_str2 = ["FL_X_ddp", "FL_Y_ddp", "FL_Z_ddp", "FR_X_ddp", "FR_Y_ddp", "FR_Z_ddp", "HL_X_ddp", "HL_Y_ddp", "HL_Z_ddp", "HR_X_ddp", "HR_Y_ddp", "HR_Z_ddp"]
+index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
+plt.figure()
+for i in range(12):
+ plt.subplot(3, 4, index[i])
+ pl1, = plt.plot(l_t, ddp_us[i,:], linewidth=2, marker='x')
+ pl2, = plt.plot(l_t, osqp_us[i,:], linewidth=2, marker='x')
+
+ if Relaunch_DDP :
+ pl3, = plt.plot(l_t, ddp_us_relaunch[i,:], linewidth=2, marker='x')
+ plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
+
+ else :
+ plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
+
+
+plt.show(block=True)
diff --git a/scripts/crocoddyl_eval/test_1/analyse_simu.py b/scripts/crocoddyl_eval/test_1/analyse_simu.py
deleted file mode 100644
index 9468bbe8..00000000
--- a/scripts/crocoddyl_eval/test_1/analyse_simu.py
+++ /dev/null
@@ -1,137 +0,0 @@
-# coding: utf8
-# coding: utf8
-
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd()) # adds current directory to python path
-
-
-import numpy as np
-import matplotlib.pylab as plt
-import utils
-import time
-# from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-import MPC_Wrapper
-import FootstepPlanner
-
-####################
-# Recovery of Data
-####################
-
-folder_name = "log_eval/"
-pathIn = "crocoddyl_eval/test_1/"
-ddp_xs = np.load(pathIn + folder_name + "ddp_xs.npy")
-ddp_us = np.load(pathIn + folder_name + "ddp_us.npy")
-osqp_xs = np.load(pathIn + folder_name + "osqp_xs.npy")
-osqp_us = np.load(pathIn + folder_name + "osqp_us.npy")
-fsteps = np.load(pathIn + folder_name + "fsteps.npy")
-xref = np.load(pathIn + folder_name + "xref.npy")
-l_feet = np.load(pathIn + folder_name + "l_feet.npy")
-
-
-####################
-# Iteration
-####################
-
-iteration = 202
-
-dt_mpc = 0.02 # Time step of the MPC
-dt = 0.004
-k_mpc = int(dt_mpc / dt) # dt is dt_tsid, defined in the TSID controller script
-n_periods = 1 # Number of periods in the prediction horizon
-T_gait = 0.64 # Duration of one gait period
-
-# Create footstep planner object
-fstep_planner = FootstepPlanner.FootstepPlanner(dt_mpc, n_periods, T_gait)
-fstep_planner.xref = xref[:,:, iteration]
-fstep_planner.fsteps = fsteps[:,:,iteration]
-fstep_planner.fsteps_mpc = fstep_planner.fsteps.copy()
-
-######################################
-# Relaunch DDP to adjust the gains #
-######################################
-
-Relaunch_DDP = True
-
-enable_multiprocessing = False
-mpc_wrapper_ddp = MPC_Wrapper.MPC_Wrapper(False, dt_mpc, fstep_planner.n_steps,
- k_mpc, fstep_planner.T_gait, enable_multiprocessing)
-
-w_x = 0.2
-w_y = 0.2
-w_z = 2
-w_roll = 0.8 # diff from MPC_crocoddyl
-w_pitch = 1.5 # diff from MPC_crocoddyl
-w_yaw = 0.11
-w_vx = 1*np.sqrt(w_x)
-w_vy = 2*np.sqrt(w_y)
-w_vz = 1*np.sqrt(w_z)
-w_vroll = 0.05*np.sqrt(w_roll)
-w_vpitch = 0.05*np.sqrt(w_pitch)
-w_vyaw = 0.03*np.sqrt(w_yaw)
-
-# Weight Vector : State
-mpc_wrapper_ddp.mpc.stateWeight = np.array([w_x,w_y,w_z,w_roll,w_pitch,w_yaw,
- w_vx,w_vy,w_vz,w_vroll,w_vpitch,w_vyaw])
-
-# Weight Vector : Force Norm
-mpc_wrapper_ddp.mpc.forceWeights = np.array(4*[0.01,0.01,0.01])
-
-# Weight Vector : Friction cone cost
-mpc_wrapper_ddp.mpc.frictionWeights = 0.5
-
-# Update weights inside the models
-mpc_wrapper_ddp.mpc.updateActionModel()
-
-# Run ddp solver
-mpc_wrapper_ddp.mpc.max_iteration = 10 # The solver is not warm started here
-mpc_wrapper_ddp.solve(0,fstep_planner)
-
-#############
-# Plot #
-#############
-
-# Predicted evolution of state variables
-l_t = np.linspace(dt_mpc, T_gait, np.int(T_gait/dt_mpc))
-l_str = ["X_osqp", "Y_osqp", "Z_osqp", "Roll_osqp", "Pitch_osqp", "Yaw_osqp", "Vx_osqp", "Vy_osqp", "Vz_osqp", "VRoll_osqp", "VPitch_osqp", "VYaw_osqp"]
-l_str2 = ["X_ddp", "Y_ddp", "Z_ddp", "Roll_ddp", "Pitch_ddp", "Yaw_ddp", "Vx_ddp", "Vy_ddp", "Vz_ddp", "VRoll_ddp", "VPitch_ddp", "VYaw_ddp"]
-
-index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
-plt.figure()
-for i in range(12):
- plt.subplot(3, 4, index[i])
-
- pl1, = plt.plot(l_t, ddp_xs[i,:,iteration], linewidth=2, marker='x')
- pl2, = plt.plot(l_t, osqp_xs[i,:,iteration], linewidth=2, marker='x')
-
- if Relaunch_DDP :
- pl3, = plt.plot(l_t, mpc_wrapper_ddp.mpc.get_xrobot()[i,:], linewidth=2, marker='x')
- plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
-
- else :
- plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
-
-
-
-
-# Desired evolution of contact forces
-l_t = np.linspace(dt_mpc, T_gait, np.int(T_gait/dt_mpc))
-l_str = ["FL_X_osqp", "FL_Y_osqp", "FL_Z_osqp", "FR_X_osqp", "FR_Y_osqp", "FR_Z_osqp", "HL_X_osqp", "HL_Y_osqp", "HL_Z_osqp", "HR_X_osqp", "HR_Y_osqp", "HR_Z_osqp"]
-l_str2 = ["FL_X_ddp", "FL_Y_ddp", "FL_Z_ddp", "FR_X_ddp", "FR_Y_ddp", "FR_Z_ddp", "HL_X_ddp", "HL_Y_ddp", "HL_Z_ddp", "HR_X_ddp", "HR_Y_ddp", "HR_Z_ddp"]
-index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
-plt.figure()
-for i in range(12):
- plt.subplot(3, 4, index[i])
- pl1, = plt.plot(l_t, ddp_us[i,:,iteration], linewidth=2, marker='x')
- pl2, = plt.plot(l_t, osqp_us[i,:,iteration], linewidth=2, marker='x')
-
- if Relaunch_DDP :
- pl3, = plt.plot(l_t, mpc_wrapper_ddp.mpc.get_fpredicted()[i,:], linewidth=2, marker='x')
- plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
-
- else :
- plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
-
-
-plt.show(block=True)
diff --git a/scripts/crocoddyl_eval/test_1/main.py b/scripts/crocoddyl_eval/test_1/main.py
deleted file mode 100644
index 4f694f9a..00000000
--- a/scripts/crocoddyl_eval/test_1/main.py
+++ /dev/null
@@ -1,193 +0,0 @@
-# coding: utf8
-import Logger
-import pybullet as pyb
-import MPC_Wrapper
-import processing as proc
-import ForceMonitor
-import EmergencyStop_controller
-import Safety_controller
-from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-import time
-import utils
-import matplotlib.pylab as plt
-import numpy as np
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd()) # adds current directory to python path
-
-
-def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback,
- on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI):
-
- ########################################################################
- # Parameters definition #
- ########################################################################
- """# Time step
- dt_mpc = 0.02
- k_mpc = int(dt_mpc / dt) # dt is dt_tsid, defined in the TSID controller script
- t = 0.0 # Time
- n_periods = 1 # Number of periods in the prediction horizon
- T_gait = 0.48 # Duration of one gait period
- # Simulation parameters
- N_SIMULATION = 1000 # number of time steps simulated"""
-
- # Initialize the error for the simulation time
- time_error = False
-
- # Lists to log the duration of 1 iteration of the MPC/TSID
- t_list_tsid = [0] * int(N_SIMULATION)
- t_list_loop = [0] * int(N_SIMULATION)
- t_list_mpc = [0] * int(N_SIMULATION)
-
- # List to store the IDs of debug lines
- ID_deb_lines = []
-
- # Enable/Disable Gepetto viewer
- enable_gepetto_viewer = True
-
- # Which MPC solver you want to use
- # True to have PA's MPC, to False to have Thomas's MPC
- """type_MPC = True"""
-
- # Create Joystick, FootstepPlanner, Logger and Interface objects
- joystick, fstep_planner, logger_ddp, interface, estimator = utils.init_objects(
- dt, dt_mpc, N_SIMULATION, k_mpc, n_periods, T_gait, type_MPC, on_solo8,
- predefined_vel)
-
- # Create a new logger type for the second solver
- logger_osqp = Logger.Logger(N_SIMULATION, dt, dt_mpc, k_mpc, n_periods, T_gait, True)
-
- # Wrapper that makes the link with the solver that you want to use for the MPC
- # First argument to True to have PA's MPC, to False to have Thomas's MPC
- enable_multiprocessing = True
-
- # Initialize the two algorithms
- mpc_wrapper_ddp = MPC_Wrapper.MPC_Wrapper(False, dt_mpc, fstep_planner.n_steps,
- k_mpc, fstep_planner.T_gait, enable_multiprocessing)
-
- mpc_wrapper_osqp = MPC_Wrapper.MPC_Wrapper(True, dt_mpc, fstep_planner.n_steps,
- k_mpc, fstep_planner.T_gait, enable_multiprocessing)
-
- # Enable/Disable hybrid control
- enable_hybrid_control = True
-
- ########################################################################
- # Gepetto viewer #
- ########################################################################
-
- # Initialisation of the Gepetto viewer
- solo = utils.init_viewer(enable_gepetto_viewer)
-
- ########################################################################
- # PyBullet #
- ########################################################################
-
- # Initialisation of the PyBullet simulator
- pyb_sim = utils.pybullet_simulator(envID, use_flat_plane, enable_pyb_GUI, dt=dt)
-
- # Force monitor to display contact forces in PyBullet with red lines
- myForceMonitor = ForceMonitor.ForceMonitor(pyb_sim.robotId, pyb_sim.planeId)
-
- ########################################################################
- # Simulator #
- ########################################################################
-
- # Define the default controller as well as emergency and safety controller
- myController = controller(int(N_SIMULATION), k_mpc, n_periods, T_gait, on_solo8)
-
- for k in range(int(N_SIMULATION)):
- time_loop = time.time()
-
- if (k % 1000) == 0:
- print("Iteration: ", k)
-
- # Process state estimator
- if k == 1:
- estimator.run_filter(k, fstep_planner.gait[0, 1:], pyb_sim.robotId,
- myController.invdyn.data(), myController.model)
- else:
- estimator.run_filter(k, fstep_planner.gait[0, 1:], pyb_sim.robotId)
-
- # Process states update and joystick
- proc.process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, myController, estimator, pyb_feedback)
-
- if np.isnan(interface.lC[2, 0]):
- print("NaN value for the position of the center of mass. Simulation likely crashed. Ending loop.")
- break
-
- # Process footstep planner
- proc.process_footsteps_planner(k, k_mpc, pyb_sim, interface, joystick, fstep_planner)
-
- # Process MPC once every k_mpc iterations of TSID
- if (k % k_mpc) == 0:
- time_mpc = time.time()
- # Run both algorithms
- proc.process_mpc(k, k_mpc, interface, joystick, fstep_planner, mpc_wrapper_ddp,
- dt_mpc, ID_deb_lines)
- proc.process_mpc(k, k_mpc, interface, joystick, fstep_planner, mpc_wrapper_osqp,
- dt_mpc, ID_deb_lines)
- t_list_mpc[k] = time.time() - time_mpc
-
- if k == 0:
- f_applied = mpc_wrapper_ddp.get_latest_result()
- # elif (k % k_mpc) == 0:
- else:
- # Output of the MPC (with delay)
- f_applied = mpc_wrapper_ddp.get_latest_result()
-
- # Process Inverse Dynamics
- time_tsid = time.time()
- jointTorques = proc.process_invdyn(solo, k, f_applied, pyb_sim, interface, fstep_planner,
- myController, enable_hybrid_control, enable_gepetto_viewer)
- t_list_tsid[k] = time.time() - time_tsid # Logging the time spent to run this iteration of inverse dynamics
-
- # Process PD+ (feedforward torques and feedback torques)
- for i_step in range(1):
-
- # Process the PD+
- jointTorques = proc.process_pdp(pyb_sim, myController, estimator)
-
- if myController.error:
- print('NaN value in feedforward torque. Ending loop.')
- break
-
- # Process PyBullet
- proc.process_pybullet(pyb_sim, k, envID, velID, jointTorques)
-
- # Call logger object to log various parameters
- logger_ddp.call_log_functions(k, pyb_sim, joystick, fstep_planner, interface, mpc_wrapper_ddp, myController,
- False, pyb_sim.robotId, pyb_sim.planeId, solo)
-
- if (k % k_mpc) == 0:
- logger_ddp.log_fstep_planner(k, fstep_planner)
- # logger_osqp.log_predicted_trajectories(k, mpc_wrapper_osqp)
-
- t_list_loop[k] = time.time() - time_loop
-
- ####################
- # END OF MAIN LOOP #
- ####################
-
- # Close the parallel process if it is running
- if enable_multiprocessing:
- print("Stopping parallel process")
- mpc_wrapper_osqp.stop_parallel_loop()
-
- print("END")
-
- pyb.disconnect()
-
- return logger_ddp, logger_osqp
-
-
-"""# Display what has been logged by the logger
-logger.plot_graphs(enable_multiprocessing=False)
-
-quit()
-
-# Display duration of MPC block and Inverse Dynamics block
-plt.figure()
-plt.plot(t_list_tsid, 'k+')
-plt.title("Time TSID")
-plt.show(block=True)"""
diff --git a/scripts/crocoddyl_eval/test_1/run_scenarios.py b/scripts/crocoddyl_eval/test_1/run_scenarios.py
deleted file mode 100644
index 7296c18e..00000000
--- a/scripts/crocoddyl_eval/test_1/run_scenarios.py
+++ /dev/null
@@ -1,72 +0,0 @@
-# coding: utf8
-from IPython import embed
-from crocoddyl_eval.test_1.main import run_scenario
-from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-import matplotlib.pylab as plt
-import numpy as np
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd()) # adds current directory to python path
-
-
-envID = 0
-velID = 0
-
-dt_mpc = 0.02 # Time step of the MPC
-k_mpc = int(dt_mpc / dt) # dt is dt_tsid, defined in the TSID controller script
-t = 0.0 # Time
-n_periods = 1 # Number of periods in the prediction horizon
-T_gait = 0.64 # Duration of one gait period
-N_SIMULATION = 2000 # number of simulated TSID time steps
-
-# Which MPC solver you want to use
-# True to have PA's MPC, to False to have Thomas's MPC
-type_MPC = False
-
-# Whether PyBullet feedback is enabled or not
-pyb_feedback = True
-
-# Whether we are working with solo8 or not
-on_solo8 = False
-
-# If True the ground is flat, otherwise it has bumps
-use_flat_plane = True
-
-# If we are using a predefined reference velocity (True) or a joystick (False)
-predefined_vel = True
-
-# Enable or disable PyBullet GUI
-enable_pyb_GUI = False
-
-#################
-# RUN SCENARIOS #
-#################
-
-# Run a scenario and retrieve data thanks to the logger
-logger_ddp, logger_osqp = run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods,
- T_gait, N_SIMULATION, type_MPC, pyb_feedback,
- on_solo8, use_flat_plane, predefined_vel, enable_pyb_GUI)
-
-#################
-# RECORD LOGGERS
-#################
-
-pathIn = "crocoddyl_eval/test_1/log_eval/"
-
-print("Saving logs...")
-
-np.save(pathIn + "ddp_xs.npy", logger_ddp.pred_trajectories)
-np.save(pathIn + "ddp_us.npy", logger_ddp.pred_forces)
-
-np.save(pathIn + "osqp_xs.npy", logger_osqp.pred_trajectories)
-np.save(pathIn + "osqp_us.npy", logger_osqp.pred_forces)
-
-np.save(pathIn + "l_feet.npy", logger_ddp.feet_pos)
-np.save(pathIn + "fsteps.npy", logger_ddp.fsteps)
-np.save(pathIn + "xref.npy", logger_ddp.xref)
-
-logger_ddp.plot_state()
-plt.show(block=True)
-
-quit()
diff --git a/scripts/crocoddyl_eval/test_2/unit_test.py b/scripts/crocoddyl_eval/test_2/unit_test.py
deleted file mode 100644
index f99c81ad..00000000
--- a/scripts/crocoddyl_eval/test_2/unit_test.py
+++ /dev/null
@@ -1,190 +0,0 @@
-# coding: utf8
-# coding: utf8
-
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd()) # adds current directory to python path
-
-import numpy as np
-import MPC_Wrapper
-
-import numpy as np
-import matplotlib.pylab as plt
-from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-from main import run_scenario
-from IPython import embed
-
-import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
-import FootstepPlanner
-import random
-import crocoddyl
-
-envID = 0
-velID = 0
-
-t = 0.0
-dt_mpc = 0.02 # Time step of the MPC
-k_mpc = int(dt_mpc / dt) # dt is dt_tsid, defined in the TSID controller script
-n_periods = 1 # Number of periods in the prediction horizon
-T_gait = 0.32 # Duration of one gait period
-N_SIMULATION = 100 # number of simulated TSID time steps
-# Which MPC solver you want to use
-# True to have PA's MPC, to False to have Thomas's MPC
-type_MPC = False
-
-# Whether PyBullet feedback is enabled or not
-pyb_feedback = True
-
-#################
-# RUN SCENARIOS #
-#################
-
-# Run a scenario and retrieve data thanks to the logger
-logger_ddp = run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback)
-
-
-# Create footstep planner object
-fstep_planner = FootstepPlanner.FootstepPlanner(dt_mpc, n_periods, T_gait)
-
-mpc_wrapper_ddp = MPC_crocoddyl.MPC_crocoddyl(dt_mpc, T_gait, 1, inner = True , linearModel = False)
-
-a = 1
-b = -1
-
-N_trial = 10
-
-
-epsilon = 10e-6
-
-
-################################################
-## CHECK DERIVATIVE WITH NUM_DIFF
-#################################################
-
-
-model_diff = crocoddyl.ActionModelNumDiff(mpc_wrapper_ddp.ListAction[0])
-data = model_diff.createData()
-dataCpp = mpc_wrapper_ddp.ListAction[0].createData()
-
-# RUN CALC DIFF
-def run_calcDiff_numDiff(epsilon) :
- Lx = 0
- Lx_err = 0
- Lu = 0
- Lu_err = 0
- Lxx = 0
- Lxx_err = 0
- Luu = 0
- Luu_err = 0
- Luu_noFri = 0
- Luu_err_noFri = 0
- Fx = 0
- Fx_err = 0
- Fu = 0
- Fu_err = 0
-
- for k in range(N_trial):
-
- N_iteration = random.randint(0, int(N_SIMULATION/k_mpc) - 1 )
- N_model = random.randint(0, np.int(T_gait/dt_mpc)- 1 )
-
- x = a + (b-a)*np.random.rand(12)
- u = a + (b-a)*np.random.rand(12)
-
- mpc_wrapper_ddp.updateProblem(logger_ddp.fsteps[:,:,N_iteration] , logger_ddp.xref[:,:, N_iteration] )
- actionModel = mpc_wrapper_ddp.ListAction[N_model]
- model_diff = crocoddyl.ActionModelNumDiff(actionModel)
-
-
- # Run calc & calcDiff function : numDiff
-
- model_diff.calc(data , x , u )
- model_diff.calcDiff(data , x , u )
-
- # Run calc & calcDiff function : c++
- actionModel.calc(dataCpp , x , u )
- actionModel.calcDiff(dataCpp , x , u )
-
- Lx += np.sum( abs((data.Lx - dataCpp.Lx )) >= epsilon )
- Lx_err += np.sum( abs((data.Lx - dataCpp.Lx )) )
-
- Lu += np.sum( abs((data.Lu - dataCpp.Lu )) >= epsilon )
- Lu_err += np.sum( abs((data.Lu - dataCpp.Lu )) )
-
- Lxx += np.sum( abs((data.Lxx - dataCpp.Lxx )) >= epsilon )
- Lxx_err += np.sum( abs((data.Lxx - dataCpp.Lxx )) )
-
- Luu += np.sum( abs((data.Luu - dataCpp.Luu )) >= epsilon )
- Luu_err += np.sum( abs((data.Luu - dataCpp.Luu )) )
-
- Fx += np.sum( abs((data.Fx - dataCpp.Fx )) >= epsilon )
- Fx_err += np.sum( abs((data.Fx - dataCpp.Fx )) )
-
- Fu += np.sum( abs((data.Fu - dataCpp.Fu )) >= epsilon )
- Fu_err += np.sum( abs((data.Fu - dataCpp.Fu )) )
-
- # No friction cone :
- actionModel.frictionWeights = 0.0
- model_diff = crocoddyl.ActionModelNumDiff(actionModel)
-
- # Run calc & calcDiff function : numDiff
- model_diff.calc(data , x , u )
- model_diff.calcDiff(data , x , u )
-
- # Run calc & calcDiff function : c++
- actionModel.calc(dataCpp , x , u )
- actionModel.calcDiff(dataCpp , x , u )
- Luu_noFri += np.sum( abs((data.Luu - dataCpp.Luu )) >= epsilon )
- Luu_err_noFri += np.sum( abs((data.Luu - dataCpp.Luu )) )
-
-
-
- Lx_err = Lx_err /N_trial
- Lu_err = Lu_err/N_trial
- Lxx_err = Lxx_err/N_trial
- Luu_err = Luu_err/N_trial
- Fx_err = Fx_err/N_trial
- Fu_err = Fu_err/N_trial
-
- return Lx , Lx_err , Lu , Lu_err , Lxx , Lxx_err , Luu , Luu_err, Luu_noFri , Luu_err_noFri, Fx, Fx_err, Fu , Fu_err
-
-
-Lx , Lx_err , Lu , Lu_err , Lxx , Lxx_err , Luu , Luu_err , Luu_noFri , Luu_err_noFri , Fx, Fx_err, Fu , Fu_err = run_calcDiff_numDiff(epsilon)
-
-print("\n \n ------------------------------------------ " )
-print(" Checking implementation of the derivatives ")
-print(" Using crocoddyl NumDiff class")
-print(" ------------------------------------------ " )
-
-print("\n Luu is calculated with the residual cost and can not be exact")
-print(" Because of the friction cost term")
-print("\n")
-print("Espilon : %f" %epsilon)
-print("N_trial : %f" %N_trial)
-print("\n")
-
-if Fx == 0: print("Fx : OK (error : %f)" %Fx_err)
-else : print("Fx : NOT OK !!! (error : %f)" %Fx_err)
-
-if Fu == 0: print("Fu : OK (error : %f)" %Fu_err)
-else : print("Fu : NOT OK !!! (error : %f)" %Fu_err)
-if Lx == 0: print("Lx : OK (error : %f)" %Lx_err)
-else : print("Lx : NOT OK !!! (error : %f)" %Lx_err )
-
-if Lu == 0: print("Lu : OK (error : %f)" %Lu_err)
-else : print("Lu : NOT OK !!! (error : %f)" %Lu_err)
-
-if Lxx == 0: print("Lxx : OK (error : %f)" %Lxx_err)
-else : print("Lxx : NOT OK !!! (error : %f)" %Lxx_err)
-
-if Luu == 0: print("Luu : OK (error : %f)" %Luu_err)
-else : print("Luu : NOT OK !!! (error : %f)" %Luu_err)
-
-if Luu_noFri == 0: print("Luu : OK (error : %f) , no friction cone" %Luu_err_noFri)
-else : print("Luu : NOT OK !!! (error : %f) , no friction cone" %Luu_err_noFri )
-
-
-
-if Lx == 0 and Lu == 0 and Lxx == 0 and Luu_noFri == 0 and Fu == 0 and Fx == 0: print("\n --> Derivatives : OK")
-else : print("\n --> Derivatives : NOT OK !!!")
\ No newline at end of file
diff --git a/scripts/crocoddyl_eval/test_2/unittest_augmented.py b/scripts/crocoddyl_eval/test_2/unittest_augmented.py
new file mode 100644
index 00000000..cf364c1e
--- /dev/null
+++ b/scripts/crocoddyl_eval/test_2/unittest_augmented.py
@@ -0,0 +1,146 @@
+# coding: utf8
+
+import numpy as np
+
+import quadruped_walkgen as quadruped_walkgen
+import libquadruped_reactive_walking as lqrw
+import crocoddyl
+
+#####################
+# Select MPC type #
+#####################
+
+model = quadruped_walkgen.ActionModelQuadrupedAugmented()
+
+# Tune the weights
+model.stateWeights = 2*np.ones(12)
+model.heuristicWeights = 2*np.ones(8)
+model.stepWeights = np.ones(8)
+
+# Update the dynamic of the model
+fstep = np.random.rand(12).reshape((3,4))
+fstop = np.random.rand(12).reshape((3,4))
+xref = np.random.rand(12)
+gait = np.random.randint(2, size=4)
+model.updateModel(fstep, fstop, xref , gait)
+
+################################################
+## CHECK DERIVATIVE WITH NUM_DIFF
+#################################################
+
+a = 1
+b = -1
+N_trial = 50
+epsilon = 10e-6
+
+model_diff = crocoddyl.ActionModelNumDiff(model)
+data_diff = model_diff.createData()
+data = model.createData()
+
+# RUN CALC DIFF
+def run_calcDiff_numDiff(epsilon) :
+ Lx = 0
+ Lx_err = 0
+ Lu = 0
+ Lu_err = 0
+ Lxu = 0
+ Lxu_err = 0
+ Lxx = 0
+ Lxx_err = 0
+ Luu = 0
+ Luu_err = 0
+ Luu_noFri = 0
+ Luu_err_noFri = 0
+ Fx = 0
+ Fx_err = 0
+ Fu = 0
+ Fu_err = 0
+
+ for k in range(N_trial):
+
+ x = a + (b-a)*np.random.rand(20)
+ u = a + (b-a)*np.random.rand(12)
+
+ fstep = np.random.rand(12).reshape((3,4))
+ fstop = np.random.rand(12).reshape((3,4))
+ xref = np.random.rand(12)
+ gait = np.random.randint(2, size=4)
+ model.updateModel(fstep, fstop, xref , gait)
+ model_diff = crocoddyl.ActionModelNumDiff(model)
+
+ # Run calc & calcDiff function : numDiff
+ model_diff.calc(data_diff , x , u )
+ model_diff.calcDiff(data_diff , x , u )
+
+ # Run calc & calcDiff function : c++ model
+ model.calc(data , x , u )
+ model.calcDiff(data , x , u )
+
+ Lx += np.sum( abs((data.Lx - data_diff.Lx )) >= epsilon )
+ Lx_err += np.sum( abs((data.Lx - data_diff.Lx )) )
+
+ Lu += np.sum( abs((data.Lu - data_diff.Lu )) >= epsilon )
+ Lu_err += np.sum( abs((data.Lu - data_diff.Lu )) )
+
+ Lxu += np.sum( abs((data.Lxu - data_diff.Lxu )) >= epsilon )
+ Lxu_err += np.sum( abs((data.Lxu - data_diff.Lxu )) )
+
+ Lxx += np.sum( abs((data.Lxx - data_diff.Lxx )) >= epsilon )
+ Lxx_err += np.sum( abs((data.Lxx - data_diff.Lxx )) )
+
+ Luu += np.sum( abs((data.Luu - data_diff.Luu )) >= epsilon )
+ Luu_err += np.sum( abs((data.Luu - data_diff.Luu )) )
+
+ Fx += np.sum( abs((data.Fx - data_diff.Fx )) >= epsilon )
+ Fx_err += np.sum( abs((data.Fx - data_diff.Fx )) )
+
+ Fu += np.sum( abs((data.Fu - data_diff.Fu )) >= epsilon )
+ Fu_err += np.sum( abs((data.Fu - data_diff.Fu )) )
+
+ Lx_err = Lx_err /N_trial
+ Lu_err = Lu_err/N_trial
+ Lxx_err = Lxx_err/N_trial
+ Luu_err = Luu_err/N_trial
+ Fx_err = Fx_err/N_trial
+ Fu_err = Fu_err/N_trial
+
+ return Lx , Lx_err , Lu , Lu_err , Lxu , Lxu_err, Lxx , Lxx_err , Luu , Luu_err, Luu_noFri , Luu_err_noFri, Fx, Fx_err, Fu , Fu_err
+
+
+Lx, Lx_err, Lu, Lu_err, Lxu, Lxu_err, Lxx, Lxx_err , Luu , Luu_err , Luu_noFri , Luu_err_noFri , Fx, Fx_err, Fu , Fu_err = run_calcDiff_numDiff(epsilon)
+
+print("\n \n ------------------------------------------ " )
+print(" Checking implementation of the derivatives ")
+print(" Using crocoddyl NumDiff class")
+print(" ------------------------------------------ " )
+
+print("\n Luu and Lxx are calculated with the residual cost and cannot be exact")
+print("\n")
+print("Espilon : %f" %epsilon)
+print("N_trial : %f" %N_trial)
+print("\n")
+
+if Fx == 0: print("Fx : OK (error : %f)" %Fx_err)
+else : print("Fx : NOT OK !!! (error : %f)" %Fx_err)
+
+if Fu == 0: print("Fu : OK (error : %f)" %Fu_err)
+else : print("Fu : NOT OK !!! (error : %f)" %Fu_err)
+if Lx == 0: print("Lx : OK (error : %f)" %Lx_err)
+else : print("Lx : NOT OK !!! (error : %f)" %Lx_err )
+
+if Lu == 0: print("Lu : OK (error : %f)" %Lu_err)
+else : print("Lu : NOT OK !!! (error : %f)" %Lu_err)
+
+if Lxu == 0: print("Lxu : OK (error : %f)" %Lxu_err)
+else : print("Lxu : NOT OK !!! (error : %f)" %Lxu_err)
+
+if Lxx == 0: print("Lxx : OK (error : %f)" %Lxx_err)
+else : print("Lxx : NOT OK !!! (error : %f)" %Lxx_err)
+
+if Luu == 0: print("Luu : OK (error : %f)" %Luu_err)
+else : print("Luu : NOT OK !!! (error : %f)" %Luu_err)
+
+if Lx == 0 and Lu == 0 and Fx == 0 and Fu == 0:
+ print("\n --> Derivatives 1st order : OK")
+else:
+ print("\n --> Derivatives : NOT OK !!!")
\ No newline at end of file
diff --git a/scripts/crocoddyl_eval/test_2/unittest_linear.py b/scripts/crocoddyl_eval/test_2/unittest_linear.py
new file mode 100644
index 00000000..d74bd10e
--- /dev/null
+++ b/scripts/crocoddyl_eval/test_2/unittest_linear.py
@@ -0,0 +1,144 @@
+# coding: utf8
+
+import numpy as np
+
+import quadruped_walkgen as quadruped_walkgen
+import libquadruped_reactive_walking as lqrw
+import crocoddyl
+
+#####################
+# Select MPC type #
+#####################
+
+model = quadruped_walkgen.ActionModelQuadruped()
+
+# Tune the weights
+# model.stateWeights = 2*np.ones(12)
+# model.heuristicWeights = 2*np.ones(8)
+# model.stepWeights = np.ones(8)
+
+# Update the dynamic of the model
+fstep = np.random.rand(12).reshape((3,4))
+xref = np.random.rand(12)
+gait = np.random.randint(2, size=4)
+model.updateModel(fstep, xref , gait)
+
+################################################
+## CHECK DERIVATIVE WITH NUM_DIFF
+#################################################
+
+a = 1
+b = -1
+N_trial = 50
+epsilon = 10e-4
+
+model_diff = crocoddyl.ActionModelNumDiff(model)
+data_diff = model_diff.createData()
+data = model.createData()
+
+# RUN CALC DIFF
+def run_calcDiff_numDiff(epsilon) :
+ Lx = 0
+ Lx_err = 0
+ Lu = 0
+ Lu_err = 0
+ Lxu = 0
+ Lxu_err = 0
+ Lxx = 0
+ Lxx_err = 0
+ Luu = 0
+ Luu_err = 0
+ Luu_noFri = 0
+ Luu_err_noFri = 0
+ Fx = 0
+ Fx_err = 0
+ Fu = 0
+ Fu_err = 0
+
+ for k in range(N_trial):
+
+ x = a + (b-a)*np.random.rand(12)
+ u = a + (b-a)*np.random.rand(12)
+
+ fstep = np.random.rand(12).reshape((3,4))
+ xref = np.random.rand(12)
+ gait = np.random.randint(2, size=4)
+ model.updateModel(fstep, xref , gait)
+ model_diff = crocoddyl.ActionModelNumDiff(model)
+
+ # Run calc & calcDiff function : numDiff
+ model_diff.calc(data_diff , x , u )
+ model_diff.calcDiff(data_diff , x , u )
+
+ # Run calc & calcDiff function : c++ model
+ model.calc(data , x , u )
+ model.calcDiff(data , x , u )
+
+ Lx += np.sum( abs((data.Lx - data_diff.Lx )) >= epsilon )
+ Lx_err += np.sum( abs((data.Lx - data_diff.Lx )) )
+
+ Lu += np.sum( abs((data.Lu - data_diff.Lu )) >= epsilon )
+ Lu_err += np.sum( abs((data.Lu - data_diff.Lu )) )
+
+ Lxu += np.sum( abs((data.Lxu - data_diff.Lxu )) >= epsilon )
+ Lxu_err += np.sum( abs((data.Lxu - data_diff.Lxu )) )
+
+ Lxx += np.sum( abs((data.Lxx - data_diff.Lxx )) >= epsilon )
+ Lxx_err += np.sum( abs((data.Lxx - data_diff.Lxx )) )
+
+ Luu += np.sum( abs((data.Luu - data_diff.Luu )) >= epsilon )
+ Luu_err += np.sum( abs((data.Luu - data_diff.Luu )) )
+
+ Fx += np.sum( abs((data.Fx - data_diff.Fx )) >= epsilon )
+ Fx_err += np.sum( abs((data.Fx - data_diff.Fx )) )
+
+ Fu += np.sum( abs((data.Fu - data_diff.Fu )) >= epsilon )
+ Fu_err += np.sum( abs((data.Fu - data_diff.Fu )) )
+
+ Lx_err = Lx_err /N_trial
+ Lu_err = Lu_err/N_trial
+ Lxx_err = Lxx_err/N_trial
+ Luu_err = Luu_err/N_trial
+ Fx_err = Fx_err/N_trial
+ Fu_err = Fu_err/N_trial
+
+ return Lx , Lx_err , Lu , Lu_err , Lxu , Lxu_err, Lxx , Lxx_err , Luu , Luu_err, Luu_noFri , Luu_err_noFri, Fx, Fx_err, Fu , Fu_err
+
+
+Lx, Lx_err, Lu, Lu_err, Lxu, Lxu_err, Lxx, Lxx_err , Luu , Luu_err , Luu_noFri , Luu_err_noFri , Fx, Fx_err, Fu , Fu_err = run_calcDiff_numDiff(epsilon)
+
+print("\n \n ------------------------------------------ " )
+print(" Checking implementation of the derivatives ")
+print(" Using crocoddyl NumDiff class")
+print(" ------------------------------------------ " )
+
+print("\n Luu and Lxx are calculated with the residual cost and cannot be exact")
+print("\n")
+print("Espilon : %f" %epsilon)
+print("N_trial : %f" %N_trial)
+print("\n")
+
+if Fx == 0: print("Fx : OK (error : %f)" %Fx_err)
+else : print("Fx : NOT OK !!! (error : %f)" %Fx_err)
+
+if Fu == 0: print("Fu : OK (error : %f)" %Fu_err)
+else : print("Fu : NOT OK !!! (error : %f)" %Fu_err)
+if Lx == 0: print("Lx : OK (error : %f)" %Lx_err)
+else : print("Lx : NOT OK !!! (error : %f)" %Lx_err )
+
+if Lu == 0: print("Lu : OK (error : %f)" %Lu_err)
+else : print("Lu : NOT OK !!! (error : %f)" %Lu_err)
+
+if Lxu == 0: print("Lxu : OK (error : %f)" %Lxu_err)
+else : print("Lxu : NOT OK !!! (error : %f)" %Lxu_err)
+
+if Lxx == 0: print("Lxx : OK (error : %f)" %Lxx_err)
+else : print("Lxx : NOT OK !!! (error : %f)" %Lxx_err)
+
+if Luu == 0: print("Luu : OK (error : %f)" %Luu_err)
+else : print("Luu : NOT OK !!! (error : %f)" %Luu_err)
+
+if Lx == 0 and Lu == 0 and Fx == 0 and Fu == 0:
+ print("\n --> Derivatives 1st order : OK")
+else:
+ print("\n --> Derivatives : NOT OK !!!")
\ No newline at end of file
diff --git a/scripts/crocoddyl_eval/test_2/unittest_nonlinear.py b/scripts/crocoddyl_eval/test_2/unittest_nonlinear.py
new file mode 100644
index 00000000..02649e33
--- /dev/null
+++ b/scripts/crocoddyl_eval/test_2/unittest_nonlinear.py
@@ -0,0 +1,144 @@
+# coding: utf8
+
+import numpy as np
+
+import quadruped_walkgen as quadruped_walkgen
+import libquadruped_reactive_walking as lqrw
+import crocoddyl
+
+#####################
+# Select MPC type #
+#####################
+
+model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
+
+# Tune the weights
+# model.stateWeights = 2*np.ones(12)
+# model.heuristicWeights = 2*np.ones(8)
+# model.stepWeights = np.ones(8)
+
+# Update the dynamic of the model
+fstep = np.random.rand(12).reshape((3,4))
+xref = np.random.rand(12)
+gait = np.random.randint(2, size=4)
+model.updateModel(fstep, xref , gait)
+
+################################################
+## CHECK DERIVATIVE WITH NUM_DIFF
+#################################################
+
+a = 1
+b = -1
+N_trial = 50
+epsilon = 10e-4
+
+model_diff = crocoddyl.ActionModelNumDiff(model)
+data_diff = model_diff.createData()
+data = model.createData()
+
+# RUN CALC DIFF
+def run_calcDiff_numDiff(epsilon) :
+ Lx = 0
+ Lx_err = 0
+ Lu = 0
+ Lu_err = 0
+ Lxu = 0
+ Lxu_err = 0
+ Lxx = 0
+ Lxx_err = 0
+ Luu = 0
+ Luu_err = 0
+ Luu_noFri = 0
+ Luu_err_noFri = 0
+ Fx = 0
+ Fx_err = 0
+ Fu = 0
+ Fu_err = 0
+
+ for k in range(N_trial):
+
+ x = a + (b-a)*np.random.rand(12)
+ u = a + (b-a)*np.random.rand(12)
+
+ fstep = np.random.rand(12).reshape((3,4))
+ xref = np.random.rand(12)
+ gait = np.random.randint(2, size=4)
+ model.updateModel(fstep, xref , gait)
+ model_diff = crocoddyl.ActionModelNumDiff(model)
+
+ # Run calc & calcDiff function : numDiff
+ model_diff.calc(data_diff , x , u )
+ model_diff.calcDiff(data_diff , x , u )
+
+ # Run calc & calcDiff function : c++ model
+ model.calc(data , x , u )
+ model.calcDiff(data , x , u )
+
+ Lx += np.sum( abs((data.Lx - data_diff.Lx )) >= epsilon )
+ Lx_err += np.sum( abs((data.Lx - data_diff.Lx )) )
+
+ Lu += np.sum( abs((data.Lu - data_diff.Lu )) >= epsilon )
+ Lu_err += np.sum( abs((data.Lu - data_diff.Lu )) )
+
+ Lxu += np.sum( abs((data.Lxu - data_diff.Lxu )) >= epsilon )
+ Lxu_err += np.sum( abs((data.Lxu - data_diff.Lxu )) )
+
+ Lxx += np.sum( abs((data.Lxx - data_diff.Lxx )) >= epsilon )
+ Lxx_err += np.sum( abs((data.Lxx - data_diff.Lxx )) )
+
+ Luu += np.sum( abs((data.Luu - data_diff.Luu )) >= epsilon )
+ Luu_err += np.sum( abs((data.Luu - data_diff.Luu )) )
+
+ Fx += np.sum( abs((data.Fx - data_diff.Fx )) >= epsilon )
+ Fx_err += np.sum( abs((data.Fx - data_diff.Fx )) )
+
+ Fu += np.sum( abs((data.Fu - data_diff.Fu )) >= epsilon )
+ Fu_err += np.sum( abs((data.Fu - data_diff.Fu )) )
+
+ Lx_err = Lx_err /N_trial
+ Lu_err = Lu_err/N_trial
+ Lxx_err = Lxx_err/N_trial
+ Luu_err = Luu_err/N_trial
+ Fx_err = Fx_err/N_trial
+ Fu_err = Fu_err/N_trial
+
+ return Lx , Lx_err , Lu , Lu_err , Lxu , Lxu_err, Lxx , Lxx_err , Luu , Luu_err, Luu_noFri , Luu_err_noFri, Fx, Fx_err, Fu , Fu_err
+
+
+Lx, Lx_err, Lu, Lu_err, Lxu, Lxu_err, Lxx, Lxx_err , Luu , Luu_err , Luu_noFri , Luu_err_noFri , Fx, Fx_err, Fu , Fu_err = run_calcDiff_numDiff(epsilon)
+
+print("\n \n ------------------------------------------ " )
+print(" Checking implementation of the derivatives ")
+print(" Using crocoddyl NumDiff class")
+print(" ------------------------------------------ " )
+
+print("\n Luu and Lxx are calculated with the residual cost and cannot be exact")
+print("\n")
+print("Espilon : %f" %epsilon)
+print("N_trial : %f" %N_trial)
+print("\n")
+
+if Fx == 0: print("Fx : OK (error : %f)" %Fx_err)
+else : print("Fx : NOT OK !!! (error : %f)" %Fx_err)
+
+if Fu == 0: print("Fu : OK (error : %f)" %Fu_err)
+else : print("Fu : NOT OK !!! (error : %f)" %Fu_err)
+if Lx == 0: print("Lx : OK (error : %f)" %Lx_err)
+else : print("Lx : NOT OK !!! (error : %f)" %Lx_err )
+
+if Lu == 0: print("Lu : OK (error : %f)" %Lu_err)
+else : print("Lu : NOT OK !!! (error : %f)" %Lu_err)
+
+if Lxu == 0: print("Lxu : OK (error : %f)" %Lxu_err)
+else : print("Lxu : NOT OK !!! (error : %f)" %Lxu_err)
+
+if Lxx == 0: print("Lxx : OK (error : %f)" %Lxx_err)
+else : print("Lxx : NOT OK !!! (error : %f)" %Lxx_err)
+
+if Luu == 0: print("Luu : OK (error : %f)" %Luu_err)
+else : print("Luu : NOT OK !!! (error : %f)" %Luu_err)
+
+if Lx == 0 and Lu == 0 and Fx == 0 and Fu == 0:
+ print("\n --> Derivatives 1st order : OK")
+else:
+ print("\n --> Derivatives : NOT OK !!!")
\ No newline at end of file
diff --git a/scripts/crocoddyl_eval/test_2/unittest_step.py b/scripts/crocoddyl_eval/test_2/unittest_step.py
new file mode 100644
index 00000000..c31d6d20
--- /dev/null
+++ b/scripts/crocoddyl_eval/test_2/unittest_step.py
@@ -0,0 +1,144 @@
+# coding: utf8
+
+import numpy as np
+
+import quadruped_walkgen as quadruped_walkgen
+import libquadruped_reactive_walking as lqrw
+import crocoddyl
+
+#####################
+# Select MPC type #
+#####################
+
+model = quadruped_walkgen.ActionModelQuadrupedStep()
+
+# Tune the weights
+model.stateWeights = 2*np.ones(12)
+model.heuristicWeights = 2*np.ones(8)
+model.stepWeights = np.ones(8)
+
+# Update the dynamic of the model
+fstep = np.random.rand(12).reshape((3,4))
+xref = np.random.rand(12)
+gait = np.random.randint(2, size=4)
+model.updateModel(fstep, xref , gait)
+
+################################################
+## CHECK DERIVATIVE WITH NUM_DIFF
+#################################################
+
+a = 1
+b = -1
+N_trial = 50
+epsilon = 10e-6
+
+model_diff = crocoddyl.ActionModelNumDiff(model)
+data_diff = model_diff.createData()
+data = model.createData()
+
+# RUN CALC DIFF
+def run_calcDiff_numDiff(epsilon) :
+ Lx = 0
+ Lx_err = 0
+ Lu = 0
+ Lu_err = 0
+ Lxu = 0
+ Lxu_err = 0
+ Lxx = 0
+ Lxx_err = 0
+ Luu = 0
+ Luu_err = 0
+ Luu_noFri = 0
+ Luu_err_noFri = 0
+ Fx = 0
+ Fx_err = 0
+ Fu = 0
+ Fu_err = 0
+
+ for k in range(N_trial):
+
+ x = a + (b-a)*np.random.rand(20)
+ u = a + (b-a)*np.random.rand(8)
+
+ fstep = np.random.rand(12).reshape((3,4))
+ xref = np.random.rand(12)
+ gait = np.random.randint(2, size=4)
+ model.updateModel(fstep, xref , gait)
+ model_diff = crocoddyl.ActionModelNumDiff(model)
+
+ # Run calc & calcDiff function : numDiff
+ model_diff.calc(data_diff , x , u )
+ model_diff.calcDiff(data_diff , x , u )
+
+ # Run calc & calcDiff function : c++ model
+ model.calc(data , x , u )
+ model.calcDiff(data , x , u )
+
+ Lx += np.sum( abs((data.Lx - data_diff.Lx )) >= epsilon )
+ Lx_err += np.sum( abs((data.Lx - data_diff.Lx )) )
+
+ Lu += np.sum( abs((data.Lu - data_diff.Lu )) >= epsilon )
+ Lu_err += np.sum( abs((data.Lu - data_diff.Lu )) )
+
+ Lxu += np.sum( abs((data.Lxu - data_diff.Lxu )) >= epsilon )
+ Lxu_err += np.sum( abs((data.Lxu - data_diff.Lxu )) )
+
+ Lxx += np.sum( abs((data.Lxx - data_diff.Lxx )) >= epsilon )
+ Lxx_err += np.sum( abs((data.Lxx - data_diff.Lxx )) )
+
+ Luu += np.sum( abs((data.Luu - data_diff.Luu )) >= epsilon )
+ Luu_err += np.sum( abs((data.Luu - data_diff.Luu )) )
+
+ Fx += np.sum( abs((data.Fx - data_diff.Fx )) >= epsilon )
+ Fx_err += np.sum( abs((data.Fx - data_diff.Fx )) )
+
+ Fu += np.sum( abs((data.Fu - data_diff.Fu )) >= epsilon )
+ Fu_err += np.sum( abs((data.Fu - data_diff.Fu )) )
+
+ Lx_err = Lx_err /N_trial
+ Lu_err = Lu_err/N_trial
+ Lxx_err = Lxx_err/N_trial
+ Luu_err = Luu_err/N_trial
+ Fx_err = Fx_err/N_trial
+ Fu_err = Fu_err/N_trial
+
+ return Lx , Lx_err , Lu , Lu_err , Lxu , Lxu_err, Lxx , Lxx_err , Luu , Luu_err, Luu_noFri , Luu_err_noFri, Fx, Fx_err, Fu , Fu_err
+
+
+Lx, Lx_err, Lu, Lu_err, Lxu, Lxu_err, Lxx, Lxx_err , Luu , Luu_err , Luu_noFri , Luu_err_noFri , Fx, Fx_err, Fu , Fu_err = run_calcDiff_numDiff(epsilon)
+
+print("\n \n ------------------------------------------ " )
+print(" Checking implementation of the derivatives ")
+print(" Using crocoddyl NumDiff class")
+print(" ------------------------------------------ " )
+
+print("\n Luu and Lxx are calculated with the residual cost and cannot be exact")
+print("\n")
+print("Espilon : %f" %epsilon)
+print("N_trial : %f" %N_trial)
+print("\n")
+
+if Fx == 0: print("Fx : OK (error : %f)" %Fx_err)
+else : print("Fx : NOT OK !!! (error : %f)" %Fx_err)
+
+if Fu == 0: print("Fu : OK (error : %f)" %Fu_err)
+else : print("Fu : NOT OK !!! (error : %f)" %Fu_err)
+if Lx == 0: print("Lx : OK (error : %f)" %Lx_err)
+else : print("Lx : NOT OK !!! (error : %f)" %Lx_err )
+
+if Lu == 0: print("Lu : OK (error : %f)" %Lu_err)
+else : print("Lu : NOT OK !!! (error : %f)" %Lu_err)
+
+if Lxu == 0: print("Lxu : OK (error : %f)" %Lxu_err)
+else : print("Lxu : NOT OK !!! (error : %f)" %Lxu_err)
+
+if Lxx == 0: print("Lxx : OK (error : %f)" %Lxx_err)
+else : print("Lxx : NOT OK !!! (error : %f)" %Lxx_err)
+
+if Luu == 0: print("Luu : OK (error : %f)" %Luu_err)
+else : print("Luu : NOT OK !!! (error : %f)" %Luu_err)
+
+if Lx == 0 and Lu == 0 and Fx == 0 and Fu == 0:
+ print("\n --> Derivatives 1st order : OK")
+else:
+ print("\n --> Derivatives : NOT OK !!!")
\ No newline at end of file
diff --git a/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py b/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
new file mode 100644
index 00000000..9dbcac1b
--- /dev/null
+++ b/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
@@ -0,0 +1,148 @@
+# coding: utf8
+
+import sys
+import os
+from sys import argv
+sys.path.insert(0, os.getcwd()) # adds current directory to python path
+
+import numpy as np
+import matplotlib.pylab as plt
+import libquadruped_reactive_walking as lqrw
+import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
+import time
+
+##############
+# Parameters
+##############
+iteration_mpc = 62 # Control cycle
+Relaunch_DDP = False # Compare a third MPC with != parameters
+linear_mpc = True
+params = lqrw.Params() # Object that holds all controller parameters
+
+######################
+# Recover Logged data
+######################
+file_name = "crocoddyl_eval/logs/data_2021_07_12_09_25.npz"
+logs = np.load(file_name)
+planner_gait = logs.get("planner_gait")
+planner_xref = logs.get("planner_xref")
+planner_fsteps = logs.get("planner_fsteps")
+planner_goals = logs.get("planner_goals")
+mpc_x_f = logs.get("mpc_x_f")
+
+k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) # simulation iteration corresponding
+k_previous = int( (iteration_mpc - 1) * (params.dt_mpc / params.dt_wbc) )
+
+############
+# OSQP MPC
+###########
+mpc_osqp = lqrw.MPC(params)
+mpc_osqp.run(0, planner_xref[0] , planner_fsteps[0]) # Initialization of the matrix
+# mpc_osqp.run(1, planner_xref[1] , planner_fsteps[1])
+mpc_osqp.run(k, planner_xref[k] , planner_fsteps[k])
+
+osqp_xs = mpc_osqp.get_latest_result()[:12,:] # States computed over the whole predicted horizon
+osqp_xs = np.vstack([planner_xref[k,:,0] , osqp_xs.transpose()]).transpose() # Add current state
+osqp_us = mpc_osqp.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
+
+
+###########
+# DDP MPC
+##########
+mpc_ddp = MPC_crocoddyl_planner.MPC_crocoddyl_planner(params, mu=0.9, inner=False)
+
+# Tune the weights
+mpc_ddp.heuristicWeights = np.array(4*[0.3, 0.4])
+mpc_ddp.stepWeights = np.full(8, 0.5)
+mpc_ddp.stateWeights = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3]) # fit osqp gains
+mpc_ddp.initializeModels(params) # re-initialize the model list with the new gains
+
+mpc_ddp.gait = planner_gait[k_previous].copy() # gait_old will be initialised with that
+mpc_ddp.updateProblem(k , planner_xref[k] , planner_fsteps[k], planner_goals[k])
+
+mpc_ddp.ddp.solve(mpc_ddp.x_init, mpc_ddp.u_init, mpc_ddp.max_iteration)
+
+ddp_xs = mpc_ddp.get_latest_result()[:12,:] # States computed over the whole predicted horizon
+ddp_xs = np.vstack([planner_xref[k,:,0] , ddp_xs.transpose()]).transpose() # Add current state
+ddp_us = mpc_ddp.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
+ddp_fsteps = mpc_ddp.get_latest_result()[24:,:]
+ddp_fsteps = np.vstack([planner_fsteps[k,0,:][[0,1,3,4,6,7,9,10]] , ddp_fsteps.transpose()]).transpose() # Add current state
+
+
+#############
+# Plot #
+#############
+
+# Predicted evolution of state variables
+l_t = np.linspace(0., params.T_gait, np.int(params.T_gait/params.dt_mpc)+1)
+l_str = ["X_osqp", "Y_osqp", "Z_osqp", "Roll_osqp", "Pitch_osqp", "Yaw_osqp", "Vx_osqp", "Vy_osqp", "Vz_osqp", "VRoll_osqp", "VPitch_osqp", "VYaw_osqp"]
+l_str2 = ["X_ddp", "Y_ddp", "Z_ddp", "Roll_ddp", "Pitch_ddp", "Yaw_ddp", "Vx_ddp", "Vy_ddp", "Vz_ddp", "VRoll_ddp", "VPitch_ddp", "VYaw_ddp"]
+
+index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
+plt.figure()
+for i in range(12):
+ plt.subplot(3, 4, index[i])
+
+ pl1, = plt.plot(l_t, ddp_xs[i,:], linewidth=2, marker='x')
+ pl2, = plt.plot(l_t, osqp_xs[i,:], linewidth=2, marker='x')
+
+ if Relaunch_DDP :
+ pl3, = plt.plot(l_t, ddp_xs_relaunch[i,:], linewidth=2, marker='x')
+ plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
+
+ else :
+ plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
+
+# Desired evolution of contact forces
+l_t = np.linspace(params.dt_mpc, params.T_gait, np.int(params.T_gait/params.dt_mpc))
+l_str = ["FL_X_osqp", "FL_Y_osqp", "FL_Z_osqp", "FR_X_osqp", "FR_Y_osqp", "FR_Z_osqp", "HL_X_osqp", "HL_Y_osqp", "HL_Z_osqp", "HR_X_osqp", "HR_Y_osqp", "HR_Z_osqp"]
+l_str2 = ["FL_X_ddp", "FL_Y_ddp", "FL_Z_ddp", "FR_X_ddp", "FR_Y_ddp", "FR_Z_ddp", "HL_X_ddp", "HL_Y_ddp", "HL_Z_ddp", "HR_X_ddp", "HR_Y_ddp", "HR_Z_ddp"]
+index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
+plt.figure()
+for i in range(12):
+ plt.subplot(3, 4, index[i])
+ pl1, = plt.plot(l_t, ddp_us[i,:], linewidth=2, marker='x')
+ pl2, = plt.plot(l_t, osqp_us[i,:], linewidth=2, marker='x')
+
+ if Relaunch_DDP :
+ pl3, = plt.plot(l_t, ddp_us_relaunch[i,:], linewidth=2, marker='x')
+ plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
+
+ else :
+ plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
+
+plt.figure()
+# CoM position predicted
+l_t = np.linspace(0., params.T_gait, np.int(params.T_gait/params.dt_mpc)+1)
+for j in range(len(l_t)) :
+ if j == 6 : # middle sizie of the cross, for the legend
+ pl1, = plt.plot(ddp_xs[0,j], ddp_xs[1,j] ,color = "k" , marker='x', markersize= int(20/np.sqrt(j+3)) )
+ pl2, = plt.plot(planner_xref[k,0,j], planner_xref[k,1,j] ,color = "g" , marker='x', markersize= int(20/np.sqrt(j+3)) )
+ else :
+ plt.plot(ddp_xs[0,j], ddp_xs[1,j] ,color = "k" , marker='x', markersize= int(20/np.sqrt(j+3)) )
+ plt.plot(planner_xref[k,0,j], planner_xref[k,1,j] ,color = "g" , marker='x', markersize= int(20/np.sqrt(j+3)) )
+
+plt.legend([pl1,pl2] , ["CoM ddp" , "CoM ref" ])
+
+# First foot on the ground using previous gait matrix : iteration - 1
+for j in range(4) :
+ if planner_gait[k_previous,0,j] == 1 :
+ pl3, = plt.plot(planner_fsteps[k_previous,0 , 3*j] , planner_fsteps[k_previous,0 , 3*j] , 'ro', markersize= 8 )
+ # plt.plot(mpc_planner.Xs[12+2*i , 0 ] , mpc_planner.Xs[12+2*i + 1 , 0 ] , 'yo', markerSize= 8 )
+
+# Foostep computed by ddp and planner fsteps
+for j in range(len(l_t)) :
+ for foot in range(4):
+ if j == 6 : # middle sizie of the cross, for the legend
+ pl4, = plt.plot(ddp_fsteps[2*foot,j], ddp_fsteps[2*foot+ 1,j] ,color = "k" , marker='o', markersize= int(20/np.sqrt(j+3)),markerfacecolor='none' )
+ pl5, = plt.plot(planner_fsteps[k,j , 3*foot] , planner_fsteps[k,j , 3*foot+1],color = "r" , marker='o', markersize= int(20/np.sqrt(j+3)) ,markerfacecolor='none' )
+ else :
+ plt.plot(ddp_fsteps[2*foot,j], ddp_fsteps[2*foot+ 1,j] ,color = "k" , marker='o', markersize= int(20/np.sqrt(j+3)),markerfacecolor='none' )
+ plt.plot(planner_fsteps[k,j , 3*foot] , planner_fsteps[k,j , 3*foot+1],color = "r" , marker='o', markersize= int(20/np.sqrt(j+3)) ,markerfacecolor='none' )
+
+plt.legend([pl1,pl2,pl3] , ["CoM ddp" , "CoM ref" , "previous fstep"])
+plt.grid()
+plt.show(block=True)
+
+
+
diff --git a/scripts/crocoddyl_eval/test_3/analyse_simu.py b/scripts/crocoddyl_eval/test_3/analyse_simu.py
deleted file mode 100644
index 57317463..00000000
--- a/scripts/crocoddyl_eval/test_3/analyse_simu.py
+++ /dev/null
@@ -1,355 +0,0 @@
-# coding: utf8
-# coding: utf8
-
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd()) # adds current directory to python path
-
-import random
-import numpy as np
-import matplotlib.pylab as plt
-import utils
-import time
-from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-import MPC_Wrapper
-import FootstepPlanner
-from crocoddyl_class.MPC_crocoddyl_planner import *
-####################
-# Recovery of Data
-####################
-
-folder_name = "log_eval/"
-pathIn = "crocoddyl_eval/test_3/"
-ddp_xs = np.load(pathIn + folder_name + "ddp_xs.npy")
-ddp_us = np.load(pathIn + folder_name + "ddp_us.npy")
-ddp_fsteps = np.load(pathIn + folder_name + "ddp_fsteps.npy")
-ddp_gait = np.load(pathIn + folder_name + "ddp_gait.npy")
-l_feet_local = np.load(pathIn + folder_name + "l_feet_local.npy")
-
-osqp_xs = np.load(pathIn + folder_name + "osqp_xs.npy")
-osqp_us = np.load(pathIn + folder_name + "osqp_us.npy")
-fsteps = np.load(pathIn + folder_name + "fsteps.npy")
-xref = np.load(pathIn + folder_name + "xref.npy")
-l_feet = np.load(pathIn + folder_name + "l_feet.npy")
-
-
-####################
-# Iteration
-####################
-
-iteration = 172
-dt_mpc = 0.02 # Time step of the MPC
-k_mpc = int(dt_mpc / dt) # dt is dt_tsid, defined in the TSID controller script
-n_periods = 1 # Number of periods in the prediction horizon
-T_gait = 0.64 # Duration of one gait period
-
-# Create footstep planner object
-fstep_planner = FootstepPlanner.FootstepPlanner(dt_mpc, n_periods, T_gait)
-fstep_planner.xref = xref[:,:, iteration]
-fstep_planner.fsteps = fsteps[:,:,iteration]
-fstep_planner.fsteps_mpc = fstep_planner.fsteps.copy()
-gait_matrix = ddp_gait[:,:,iteration]
-
-######################################
-# Relaunch DDP to adjust the gains #
-######################################
-Relaunch_DDP = False
-
-# Initialization of the crocoddyl gait problem
-
-# Ref problem
-
-mpc_planner = MPC_crocoddyl_planner(dt = dt_mpc , T_mpc = T_gait)
-# Weights on the shoulder term : term 1
-mpc_planner.shoulderWeights = np.array(4*[1,10])
-
-# Weights on the previous position predicted : term 2
-mpc_planner.lastPositionWeights = np.full(8,0.0)
-
-# Weight on the step command
-mpc_planner.stepWeights = np.full(4,10)
-
-
-
-for i in range(iteration + 1) :
-
- mpc_planner.updateProblem(i, xref[:,:, i] , l_feet_local[:,:, i ] )
- mpc_planner.ddp.solve()
- mpc_planner.get_fsteps()
-
-
-
-
-
-# xref_test = np.zeros((12,int(T_gait/dt_mpc) + 1))
-# xref_test[2,:] = xref[2,5, iteration]
-# xref_test[6,0] = 0.4
-# xref_test[0,0] = 0.0
-# mpc_planner.updateProblem(iteration, xref_test[:,: ] , l_feet_local[:,:, iteration ] )
-# mpc_planner.ddp.solve()
-# mpc_planner.get_fsteps()
-
-# gaitProblem.updateProblem(iteration, xref[:,:, i], l_feet_local[:,:, iteration ])
-# gaitProblem.ddp.solve()
-# gaitProblem.get_fsteps()
-
-# print(mpc_planner.gait)
-# for elt in mpc_planner.ListAction :
-# print(elt.__class__.__name__)
-# if(elt.__class__.__name__ != "ActionModelQuadrupedStep") :
-# print(elt.stateWeights)
-# print(elt.forceWeights)
-# print(elt.frictionWeights)
-# print(elt.lastPositionWeights )
-# print(elt.shoulderWeights )
-# else :
-# print(elt.lastPositionWeights )
-# print(elt.shoulderWeights )
-# print(elt.stateWeights)
-# print(elt.stepWeights)
-
-
-# ################################################
-# ## CHECK DERIVATIVE WITH NUMDDIFF
-# #################################################
-
-
-# model_diff = crocoddyl.ActionModelNumDiff(gaitProblemCpp.ListAction[0])
-# data = model_diff.createData()
-# N_trial = 100
-# epsilon = 0.001
-# a = -1
-# b = 1
-
-# # RUN CALC DIFF
-# def run_calcDiff_numDiff(epsilon) :
-# Lx = 0
-# Lx_err = 0
-# Lu = 0
-# Lu_err = 0
-# Lxx = 0
-# Lxx_err = 0
-# Luu = 0
-# Luu_err = 0
-# Fx = 0
-# Fx_err = 0
-# Fu = 0
-# Fu_err = 0
-
-# for k in range(N_trial):
-
-
-# x = a + (b-a)*np.random.rand(20)
-# u = a + (b-a)*np.random.rand(12)
-# N_model = random.randint(0,16)
-
-# while (mpc_planner.ListAction[N_model].__class__.__name__ == "ActionModelQuadrupedStep") :
-# N_model = random.randint(0,16)
-
-# # Run calc & calcDiff function
-# actionCpp = mpc_planner.ListAction[N_model]
-# model_diff = crocoddyl.ActionModelNumDiff(actionCpp)
-
-# dataCpp = mpc_planner.ListAction[N_model].createData()
-# data = model_diff.createData()
-
-# model_diff.calc(data , x , u )
-# model_diff.calcDiff(data , x , u )
-
-
-# actionCpp.calc(dataCpp ,x , u )
-# actionCpp.calcDiff(dataCpp ,x , u )
-
-
-# Lx += np.sum( abs((data.Lx - dataCpp.Lx )) >= epsilon )
-# Lx_err += np.sum( abs((data.Lx - dataCpp.Lx )) )
-
-# Lu += np.sum( abs((data.Lu - dataCpp.Lu )) >= epsilon )
-# Lu_err += np.sum( abs((data.Lu - dataCpp.Lu )) )
-
-# Lxx += np.sum( abs((data.Lxx - dataCpp.Lxx )) >= epsilon )
-# Lxx_err += np.sum( abs((data.Lxx - dataCpp.Lxx )) )
-
-# Luu += np.sum( abs((data.Luu - dataCpp.Luu )) >= epsilon )
-# Luu_err += np.sum( abs((data.Luu - dataCpp.Luu )) )
-
-# Fx += np.sum( abs((data.Fx - dataCpp.Fx )) >= epsilon )
-# Fx_err += np.sum( abs((data.Fx - dataCpp.Fx )) )
-
-# Fu += np.sum( abs((data.Fu - dataCpp.Fu )) >= epsilon )
-# Fu_err += np.sum( abs((data.Fu - dataCpp.Fu )) )
-
-# Lx_err = Lx_err /N_trial
-# Lu_err = Lu_err/N_trial
-# Lxx_err = Lxx_err/N_trial
-# Luu_err = Luu_err/N_trial
-# Fx_err = Fx_err/N_trial
-# Fu_err = Fu_err/N_trial
-
-# return Lx , Lx_err , Lu , Lu_err , Lxx , Lxx_err , Luu , Luu_err, Fx, Fx_err, Fu , Fu_err
-
-
-# Lx , Lx_err , Lu , Lu_err , Lxx , Lxx_err , Luu , Luu_err , Fx, Fx_err, Fu , Fu_err = run_calcDiff_numDiff(epsilon)
-
-# print("\n \n ---------- Check derivative with NumDiff method : cpp check -------------------")
-# print("\n------------CalcDiff Function :-----------")
-# if Lx == 0: print("Lx : OK (error : %f)" %Lx_err)
-# else : print("Lx : NOT OK !!! (error : %f)" %Lx_err )
-
-# if Lu == 0: print("Lu : OK (error : %f)" %Lu_err)
-# else : print("Lu : NOT OK !!! (error : %f)" %Lu_err)
-
-# if Lxx == 0: print("Lxx : OK (error : %f)" %Lxx_err)
-# else : print("Lxx : NOT OK !!! (error : %f)" %Lxx_err)
-
-# if Luu == 0: print("Luu : OK (error : %f)" %Luu_err)
-# else : print("Luu : NOT OK !!! (error : %f)" %Luu_err)
-
-# if Fx == 0: print("Fx : OK (error : %f)" %Fx_err)
-# else : print("Fx : NOT OK !!! (error : %f)" %Fx_err)
-
-# if Fu == 0: print("Fu : OK (error : %f)" %Fu_err)
-# else : print("Fu : NOT OK !!! (error : %f)" %Fu_err)
-
-
-# if Lx == 0 and Lu == 0 and Lxx == 0 and Luu == 0 and Fu == 0 and Fx == 0: print("\n Calc function : OK")
-# else : print("\n Calc Diff function : NOT OK !!!")
-
-
-
-# Logger ddp
-# pred_trajectories[:,:,int(k/k_mpc)] = mpc_planner.Xs
-# pred_forces[:,:,int(k/k_mpc)] = mpc_planner.Us
-# fsteps[:,:,int(k/k_mpc)] = mpc_planner.fsteps.copy()
-# gait_[:,:,int(k/k_mpc)] = mpc_planner.gait
-
-
-#############
-# Plot #
-#############
-
-# Predicted evolution of state variables
-l_t = np.linspace(dt_mpc, T_gait, np.int(T_gait/dt_mpc))
-l_str = ["X_osqp", "Y_osqp", "Z_osqp", "Roll_osqp", "Pitch_osqp", "Yaw_osqp", "Vx_osqp", "Vy_osqp", "Vz_osqp", "VRoll_osqp", "VPitch_osqp", "VYaw_osqp"]
-l_str2 = ["X_ddp", "Y_ddp", "Z_ddp", "Roll_ddp", "Pitch_ddp", "Yaw_ddp", "Vx_ddp", "Vy_ddp", "Vz_ddp", "VRoll_ddp", "VPitch_ddp", "VYaw_ddp"]
-
-index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
-plt.figure()
-for i in range(12):
- plt.subplot(3, 4, index[i])
-
- pl2, = plt.plot(l_t, osqp_xs[i,:,iteration], linewidth=2, marker='x')
- pl1, = plt.plot(l_t, ddp_xs[i,:,iteration], linewidth=2, marker='x')
-
-
- if Relaunch_DDP :
- pl3, = plt.plot(l_t, mpc_planner.Xs[i,:], linewidth=2, marker='x')
- plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
-
- else :
- plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
-
-
-
-
-# Desired evolution of contact forces
-l_t = np.linspace(dt_mpc, T_gait, np.int(T_gait/dt_mpc))
-l_str = ["FL_X_osqp", "FL_Y_osqp", "FL_Z_osqp", "FR_X_osqp", "FR_Y_osqp", "FR_Z_osqp", "HL_X_osqp", "HL_Y_osqp", "HL_Z_osqp", "HR_X_osqp", "HR_Y_osqp", "HR_Z_osqp"]
-l_str2 = ["FL_X_ddp", "FL_Y_ddp", "FL_Z_ddp", "FR_X_ddp", "FR_Y_ddp", "FR_Z_ddp", "HL_X_ddp", "HL_Y_ddp", "HL_Z_ddp", "HR_X_ddp", "HR_Y_ddp", "HR_Z_ddp"]
-index = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
-plt.figure()
-for i in range(12):
- plt.subplot(3, 4, index[i])
-
- pl2, = plt.plot(l_t, osqp_us[i,:,iteration], linewidth=2, marker='x')
- pl1, = plt.plot(l_t, ddp_us[i,:,iteration], linewidth=2, marker='x')
-
-
- if Relaunch_DDP :
- pl3, = plt.plot(l_t,mpc_planner.Us[i,:], linewidth=2, marker='x')
- plt.legend([pl1,pl2,pl3] , [l_str2[i] , l_str[i], "ddp_redo" ])
-
- else :
- plt.legend([pl1,pl2] , [l_str2[i] , l_str[i] ])
-
-
-
-# d = 7
-
-# for i in range(4) :
-# # plt.plot(log_fsteps[:,0,3*i+1] + log_xref[:,0,0] , log_fsteps[:,0,3*i+2] + log_xref[:,1,0] , 'rx' , markerSize = 8)
-# # plt.plot(log_fsteps_ref[:,0,3*i+1] + log_xref[:,0,0] , log_fsteps_ref[:,0,3*i+2] + log_xref[:,1,0] , 'bo', markerSize= 8 , markerfacecolor='none')
-
-# if i == 0 or i == 1 :
-# for k in range(iteration_begin , iteration) :
-# if ddp_gait[k,0,i+1] == 1 :
-# if ddp_gait[k,0,0] == 7 :
-# pl1, = plt.plot(ofeet[k,0,i] , ofeet[k,1,i] , 'bo' , markerSize = 8)
-# # plt.plot(ofeet_ref[k,0,i] , ofeet_ref[k,1,i] , 'go', markerSize= 8 , markerfacecolor='none')
-# if ddp_gait[k,0,i+1] == 0 :
-# pl2, = plt.plot(ofeet[k,0,i] , ofeet[k,1,i] , 'gx' , markerSize = 8)
-# pl3, = plt.plot(ofeet_ref[k,0,i] , ofeet_ref[k,1,i] , 'go', markerSize= 8 , markerfacecolor='none')
-# if i == 2 or i == 3 :
-# for k in range(iteration_begin , iteration) :
-# if ddp_gait[k,0,i+1] == 1 :
-# if ddp_gait[k,0,0] == 7 :
-# pl11, = plt.plot(ofeet[k,0,i] , ofeet[k,1,i] , 'mo' , markerSize = 8)
-# # plt.plot(ofeet_ref[k,0,i] , ofeet_ref[k,1,i] , 'go', markerSize= 8 , markerfacecolor='none')
-# if ddp_gait[k,0,i+1] == 0 :
-# pl22, = plt.plot(ofeet[k,0,i] , ofeet[k,1,i] , 'rx' , markerSize = 8)
-# pl33, = plt.plot(ofeet_ref[k,0,i] , ofeet_ref[k,1,i] , 'ro', markerSize= 8 , markerfacecolor='none')
-
-# # for k in range(gait_matrix.shape[0]) :
-# pl4, = plt.plot(oC[iteration_begin:iteration+d,0] , oC[iteration_begin:iteration+d,1] , "kx")
-
-# plt.legend([pl1,pl2,pl3,pl4] , ["Feet on the ground" , "MPC planner" , "1st planner" , "CoM" ])
-# plt.grid()
-
-plt.figure()
-
-# CoM position predicted
-for k in range(len(l_t)) :
- plt.plot(ddp_xs[0,k,iteration], ddp_xs[1,k,iteration] ,"kx" , markerSize= int(20/np.sqrt(k+1)) )
- plt.plot(xref[0,k+1,iteration], xref[1,k+1,iteration] ,"gx" , markerSize= int(20/(k+1)) )
-
-# First foot on the ground using previous gait matrix : iteration - 1
-for i in range(4) :
- if gait_matrix[0,i+1] == 1 :
- plt.plot(ddp_xs[12+2*i , 0 , iteration ] , ddp_xs[12+2*i + 1 , 0 , iteration ] , 'ro', markerSize= 8 )
- # plt.plot(mpc_planner.Xs[12+2*i , 0 ] , mpc_planner.Xs[12+2*i + 1 , 0 ] , 'yo', markerSize= 8 )
-
-print(gait_matrix)
-j = 0
-k_cum = 0
-L = []
-# Iterate over all phases of the gait
-# The first column of xref correspond to the current state
-while (gait_matrix[j, 0] != 0):
- for k in range(k_cum, k_cum+np.int(gait_matrix[j, 0])):
-
- for l in range(4) :
- if gait_matrix[j,l+1] == 1 : # next position
- plt.plot(ddp_xs[12+2*l , k , iteration ] , ddp_xs[12+2*l + 1 , k , iteration ] , 'rx', markerSize= int(16/(j+1)) )
- # plt.plot(mpc_planner.Xs[12+2*l , k ] , mpc_planner.Xs[12+2*l + 1 , k ] , 'yo', markerSize= int(16/(j+1)) ,markerfacecolor='none' )
- if gait_matrix[j,l+1] == 0 : # state before optimisation
- plt.plot(ddp_xs[12+2*l , k , iteration ] , ddp_xs[12+2*l + 1 , k , iteration ] , 'bx', markerSize= int(16/(j+1)) )
- # plt.plot(mpc_planner.Xs[12+2*l , k ] , mpc_planner.Xs[12+2*l + 1 , k ] , 'yo', markerSize= int(16/(j+1)) , markerfacecolor='none' )
-
- k_cum += np.int(gait_matrix[j, 0])
- j += 1
-
-# Adding shoulder
-p0 = [ 0.1946,0.15005, 0.1946,-0.15005, -0.1946, 0.15005 ,-0.1946, -0.15005]
-for i in range(4) :
- pl4, = plt.plot(p0[2*i] , p0[2*i+1] , "ko " , markerSize= 10 , markerfacecolor='none' )
-
-plt.grid()
-
-
-
-
-plt.show(block=True)
-
-
-
diff --git a/scripts/crocoddyl_eval/test_3/main.py b/scripts/crocoddyl_eval/test_3/main.py
deleted file mode 100644
index c158ee41..00000000
--- a/scripts/crocoddyl_eval/test_3/main.py
+++ /dev/null
@@ -1,231 +0,0 @@
-# coding: utf8
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd()) # adds current directory to python path
-
-import numpy as np
-import matplotlib.pylab as plt
-import utils
-import time
-
-from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-import Safety_controller
-import EmergencyStop_controller
-import ForceMonitor
-import processing as proc
-import MPC_Wrapper
-import pybullet as pyb
-from crocoddyl_class.MPC_crocoddyl_planner import *
-
-def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback):
-
- ########################################################################
- # Parameters definition #
- ########################################################################
- """# Time step
- dt_mpc = 0.02
- k_mpc = int(dt_mpc / dt) # dt is dt_tsid, defined in the TSID controller script
- t = 0.0 # Time
- n_periods = 1 # Number of periods in the prediction horizon
- T_gait = 0.48 # Duration of one gait period
- # Simulation parameters
- N_SIMULATION = 1000 # number of time steps simulated"""
-
- # Initialize the error for the simulation time
- time_error = False
-
- # Lists to log the duration of 1 iteration of the MPC/TSID
- t_list_tsid = [0] * int(N_SIMULATION)
- t_list_loop = [0] * int(N_SIMULATION)
- t_list_mpc = [0] * int(N_SIMULATION)
-
- # List to store the IDs of debug lines
- ID_deb_lines = []
-
- # Enable/Disable Gepetto viewer
- enable_gepetto_viewer = True
-
- # Which MPC solver you want to use
- # True to have PA's MPC, to False to have Thomas's MPC
- """type_MPC = True"""
-
- # Create Joystick, FootstepPlanner, Logger and Interface objects
- joystick, fstep_planner, logger, interface = utils.init_objects(
- dt, dt_mpc, N_SIMULATION, k_mpc, n_periods, T_gait, type_MPC)
-
- # Wrapper that makes the link with the solver that you want to use for the MPC
- # First argument to True to have PA's MPC, to False to have Thomas's MPC
- enable_multiprocessing = False
- mpc_wrapper = MPC_Wrapper.MPC_Wrapper(type_MPC, dt_mpc, fstep_planner.n_steps,
- k_mpc, fstep_planner.T_gait, enable_multiprocessing)
-
- # MPC with augmented states
- mpc_planner = MPC_crocoddyl_planner(dt = dt_mpc , T_mpc = fstep_planner.T_gait)
-
- # Enable/Disable hybrid control
- enable_hybrid_control = True
-
- ########################################################################
- # Logger DDP #
- ########################################################################
-
- pred_trajectories = np.zeros((20, int(T_gait/dt_mpc), int(N_SIMULATION/k_mpc)))
- pred_forces = np.zeros((12, int(T_gait/dt_mpc), int(N_SIMULATION/k_mpc)))
- fsteps = np.zeros((20,13,int(N_SIMULATION/k_mpc)))
- gait_ = np.zeros((20,5,int(N_SIMULATION/k_mpc)))
- l_feet_ = np.zeros((3,4,int(N_SIMULATION/k_mpc)))
- o_feet_ = np.zeros((3,4,int(N_SIMULATION/k_mpc)))
-
- ########################################################################
- # Gepetto viewer #
- ########################################################################
-
- # Initialisation of the Gepetto viewer
- solo = utils.init_viewer(enable_gepetto_viewer)
-
- ########################################################################
- # PyBullet #
- ########################################################################
-
- # Initialisation of the PyBullet simulator
- pyb_sim = utils.pybullet_simulator(envID, dt=0.001)
-
- # Force monitor to display contact forces in PyBullet with red lines
- myForceMonitor = ForceMonitor.ForceMonitor(pyb_sim.robotId, pyb_sim.planeId)
-
- ########################################################################
- # Simulator #
- ########################################################################
-
- # Define the default controller as well as emergency and safety controller
- myController = controller(int(N_SIMULATION), k_mpc, n_periods, T_gait)
- mySafetyController = Safety_controller.controller_12dof()
- myEmergencyStop = EmergencyStop_controller.controller_12dof()
-
- tic = time.time()
- for k in range(int(N_SIMULATION)):
- time_loop = time.time()
-
- if (k % 1000) == 0:
- print("Iteration: ", k)
-
- # Process states update and joystick
- proc.process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, myController, pyb_feedback)
-
- if np.isnan(interface.lC[2]):
- print("NaN value for the position of the center of mass. Simulation likely crashed. Ending loop.")
- break
-
- # Process footstep planner
- proc.process_footsteps_planner(k, k_mpc, pyb_sim, interface, joystick, fstep_planner)
-
- # Process MPC once every k_mpc iterations of TSID
- if (k % k_mpc) == 0:
- time_mpc = time.time()
- if k == 0 :
- proc.process_mpc(k, k_mpc, interface, joystick, fstep_planner, mpc_wrapper,
- dt_mpc, ID_deb_lines)
-
- else :
- # if not proc.proc
- proc.process_mpc(k, k_mpc, interface, joystick, fstep_planner, mpc_wrapper,
- dt_mpc, ID_deb_lines)
- t_list_mpc[k] = time.time() - time_mpc
-
- # running mpc planner in parallel (xref has been updated in process_mpc)
- start_time = time.time()
- mpc_planner.solve(k, fstep_planner.xref , interface.l_feet )
- print("Temps d execution : %s secondes ---" % (time.time() - start_time))
-
- #############
- # ddp logger
- #############
- pred_trajectories[:,:,int(k/k_mpc)] = mpc_planner.Xs
- pred_forces[:,:,int(k/k_mpc)] = mpc_planner.Us
- fsteps[:,:,int(k/k_mpc)] = mpc_planner.fsteps.copy()
- gait_[:,:,int(k/k_mpc)] = mpc_planner.gait
- l_feet_[:,:,int(k/k_mpc)] = interface.l_feet
- for i in range(4):
- index = next((idx for idx, val in np.ndenumerate(mpc_planner.fsteps[:, 3*i+1]) if ((not (val==0)) and (not np.isnan(val)))), [-1])[0]
- pos_tmp = np.reshape(np.array(interface.oMl * (np.array([mpc_planner.fsteps[index, (1+i*3):(4+i*3)]]).transpose())) , (3,1) )
- o_feet_[:2, i , int(k/k_mpc)] = pos_tmp[0:2, 0]
-
- # Replace the fstep_invdyn by the ddp one
- fstep_planner.fsteps_invdyn = mpc_planner.fsteps.copy()
-
- if k == 0:
- # f_applied = mpc_wrapper.get_latest_result()
- f_applied = mpc_planner.get_latest_result()
- else:
- # Output of the MPC (with delay)
- # f_applied = mpc_wrapper.get_latest_result()
- f_applied = mpc_planner.get_latest_result()
-
-
- # Process Inverse Dynamics
- time_tsid = time.time()
- jointTorques = proc.process_invdyn(solo, k, f_applied, pyb_sim, interface, fstep_planner,
- myController, enable_hybrid_control)
- t_list_tsid[k] = time.time() - time_tsid # Logging the time spent to run this iteration of inverse dynamics
-
- # Process PD+ (feedforward torques and feedback torques)
- for i_step in range(1):
-
- # Process the PD+
- jointTorques = proc.process_pdp(pyb_sim, myController)
-
- if myController.error:
- print('NaN value in feedforward torque. Ending loop.')
- break
-
- # Process PyBullet
- proc.process_pybullet(pyb_sim, k, envID, jointTorques)
-
- # Call logger object to log various parameters
- logger.call_log_functions(k, pyb_sim, joystick, fstep_planner, interface, mpc_wrapper, myController,
- False, pyb_sim.robotId, pyb_sim.planeId, solo)
-
- t_list_loop[k] = time.time() - time_loop
-
- #########################
- # Camera
- #########################
-
- # if (k % 20) == 0:
- # img = pyb.getCameraImage(width=1920, height=1080, renderer=pyb.ER_BULLET_HARDWARE_OPENGL)
- # #if k == 0:
- # # newpath = r'/tmp/recording'
- # # if not os.path.exists(newpath):
- # # os.makedirs(newpath)
- # if (int(k/20) < 10):
- # plt.imsave('tmp/recording/frame_00'+str(int(k/20))+'.png', img[2])
- # elif int(k/20) < 100:
- # plt.imsave('tmp/recording/frame_0'+str(int(k/20))+'.png', img[2])
- # else:
- # plt.imsave('tmp/recording/frame_'+str(int(k/20))+'.png', img[2])
-
-
- ####################
- # END OF MAIN LOOP #
- ####################
-
- print("Computation duration: ", time.time()-tic)
- print("Simulated duration: ", N_SIMULATION*0.001)
- print("END")
-
- pyb.disconnect()
-
- return logger , pred_trajectories , pred_forces , fsteps , gait_ , l_feet_ , o_feet_
-
-
-"""# Display what has been logged by the logger
-logger.plot_graphs(enable_multiprocessing=False)
-
-quit()
-
-# Display duration of MPC block and Inverse Dynamics block
-plt.figure()
-plt.plot(t_list_tsid, 'k+')
-plt.title("Time TSID")
-plt.show(block=True)"""
diff --git a/scripts/crocoddyl_eval/test_3/run_scenarios.py b/scripts/crocoddyl_eval/test_3/run_scenarios.py
deleted file mode 100644
index ea62479a..00000000
--- a/scripts/crocoddyl_eval/test_3/run_scenarios.py
+++ /dev/null
@@ -1,63 +0,0 @@
-# coding: utf8
-import sys
-import os
-from sys import argv
-sys.path.insert(0, os.getcwd()) # adds current directory to python path
-
-import numpy as np
-import matplotlib.pylab as plt
-from TSID_Debug_controller_four_legs_fb_vel import controller, dt
-from crocoddyl_eval.test_3.main import run_scenario
-from IPython import embed
-
-envID = 0
-velID = 0
-
-dt_mpc = 0.02 # Time step of the MPC
-k_mpc = int(dt_mpc / dt) # dt is dt_tsid, defined in the TSID controller script
-t = 0.0 # Time
-n_periods = 1 # Number of periods in the prediction horizon
-T_gait = 0.64 # Duration of one gait period
-N_SIMULATION = 6000 # number of simulated TSID time steps
-
-# Which MPC solver you want to use
-# True to have PA's MPC, to False to have Thomas's MPC
-type_MPC = True
-
-# Whether PyBullet feedback is enabled or not
-pyb_feedback = True
-
-#################
-# RUN SCENARIOS #
-#################
-
-# Run a scenario and retrieve data thanks to the logger
-logger_osqp , pred_trajectories , pred_forces , fsteps, gait, l_feet_ , o_feet = run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION, type_MPC, pyb_feedback)
-
-#################
-# RECORD LOGGERS
-#################
-
-pathIn = "crocoddyl_eval/test_3/log_eval/"
-
-print("Saving logs...")
-
-np.save(pathIn + "ddp_xs.npy" , pred_trajectories )
-np.save(pathIn + "ddp_us.npy" , pred_forces )
-np.save(pathIn + "ddp_fsteps.npy" , fsteps )
-np.save(pathIn + "ddp_gait.npy" , gait )
-np.save(pathIn + "l_feet_local.npy" , l_feet_ )
-np.save(pathIn + "o_feet_.npy" , o_feet )
-
-np.save(pathIn + "osqp_xs.npy" , logger_osqp.pred_trajectories )
-np.save(pathIn + "osqp_us.npy" , logger_osqp.pred_forces )
-
-np.save(pathIn + "l_feet.npy" , logger_osqp.feet_pos )
-np.save(pathIn + "fsteps.npy" , logger_osqp.fsteps )
-np.save(pathIn + "xref.npy" , logger_osqp.xref )
-
-# logger_osqp.plot_graphs(False)
-logger_osqp.plot_state()
-plt.show(block=True)
-
-quit()
\ No newline at end of file
diff --git a/scripts/main_solo12_control.py b/scripts/main_solo12_control.py
index cb44b062..07d0be99 100644
--- a/scripts/main_solo12_control.py
+++ b/scripts/main_solo12_control.py
@@ -345,7 +345,7 @@ def main():
help='Name of the clone interface that will reproduce the movement of the first one \
(use ifconfig in a terminal), for instance "enp1s0"')
- f, v = control_loop(parser.parse_args().interface, parser.parse_args().clone, np.array([1.3, 0.0, 0.0, 0.0, 0.0, 0.0]))
+ f, v = control_loop(parser.parse_args().interface, parser.parse_args().clone, np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print(f, v)
quit()
--
GitLab