diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 1b48ea9cd3f77d5ae27a6bf7f4527ff7b9c60ed3..70507a2535984c7729fa79d65c3302f5ed3f269d 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -72,6 +72,9 @@ class FootTrajectoryGenerator {
MatrixN getFootPosition() { return position_; } // Get the next foot position
MatrixN getFootVelocity() { return velocity_; } // Get the next foot velocity
MatrixN getFootAcceleration() { return acceleration_; } // Get the next foot acceleration
+ MatrixN getFootJerk() { return jerk_; } // Get the next foot jerk
+ Vector4 getT0s() { return t0s; } // Get the t0s for each foot
+ Vector4 getTswing() { return t_swing; } // Get the flying period for each foot
private:
Gait *gait_; // Target lock before the touchdown
@@ -93,6 +96,7 @@ class FootTrajectoryGenerator {
Matrix34 position_; // Position computed in updateFootPosition
Matrix34 velocity_; // Velocity computed in updateFootPosition
Matrix34 acceleration_; // Acceleration computed in updateFootPosition
+ Matrix34 jerk_; // Jerk computed in updateFootPosition
Matrix34 position_base_; // Position computed in updateFootPosition in base frame
Matrix34 velocity_base_; // Velocity computed in updateFootPosition in base frame
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 48d25ae4006201bf6e61ba101872f870d3cd0f6e..d9fc8a7d13e741c1098e052de8c00193ca3e84d6 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -196,13 +196,18 @@ struct FootTrajectoryGeneratorPythonVisitor : public bp::def_visitor<FootTraject
.def("getFootPosition", &FootTrajectoryGenerator::getFootPosition, "Get position_ matrix.\n")
.def("getFootVelocity", &FootTrajectoryGenerator::getFootVelocity, "Get velocity_ matrix.\n")
.def("getFootAcceleration", &FootTrajectoryGenerator::getFootAcceleration, "Get acceleration_ matrix.\n")
+ .def("getFootJerk", &FootTrajectoryGenerator::getFootJerk, "Get jerk_ matrix.\n")
.def("initialize", &FootTrajectoryGenerator::initialize, bp::args("params", "gaitIn"),
"Initialize FootTrajectoryGenerator from Python.\n")
// Compute target location of footsteps from Python
.def("update", &FootTrajectoryGenerator::update, bp::args("k", "targetFootstep"),
- "Compute target location of footsteps from Python.\n");
+ "Compute target location of footsteps from Python.\n")
+
+ // Get flying period of the feet
+ .def("getT0s", &FootTrajectoryGenerator::getT0s, "Get the current timings of the flying feet.\n")
+ .def("getTswing", &FootTrajectoryGenerator::getTswing, "Get the flying period of the feet.\n");
}
@@ -454,6 +459,7 @@ struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
.def_readwrite("h_ref", &Params::h_ref)
.def_readwrite("shoulders", &Params::shoulders)
.def_readwrite("lock_time", &Params::lock_time)
+ .def_readwrite("vert_time", &Params::vert_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 334049024be0f18feb227e76c586074f8e2fbe3a..4de141bcd29c266fd1f8ae1dfff7bcfee76e3628 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -277,9 +277,14 @@ class Controller:
if self.type_MPC == 3:
# Compute the target foostep in local frame, to stop the optimisation around it when t_lock overpass
l_targetFootstep = oRh.transpose() @ (self.o_targetFootstep - oTh)
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, l_targetFootstep, oRh, oTh)
- else:
- self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3, 4)))
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, l_targetFootstep, oRh, oTh,
+ self.footTrajectoryGenerator.getFootPosition(),
+ self.footTrajectoryGenerator.getFootVelocity(),
+ self.footTrajectoryGenerator.getFootAcceleration(),
+ self.footTrajectoryGenerator.getFootJerk(),
+ self.footTrajectoryGenerator.getTswing() - self.footTrajectoryGenerator.getT0s())
+ else :
+ self.mpc_wrapper.solve(self.k, xref, fsteps, cgait, np.zeros((3,4)))
except ValueError:
print("MPC Problem")
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index b390dd34e223540c146beac29aaa2391e188be5a..d702a334879303b4c74b5be9546c1affab7741d4 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -1,4 +1,5 @@
'''This class will log 1d array in Nd matrix from device and qualisys object'''
+import pickle
import numpy as np
from datetime import datetime as datetime
from time import time
@@ -73,9 +74,10 @@ class LoggerControl():
self.planner_h_ref = np.zeros([logSize]) # reference height of the planner
# Foot Trajectory Generator
- self.planner_goals = np.zeros([logSize, 3, 4]) # 3D target feet positions
+ self.planner_goals = np.zeros([logSize, 3, 4]) # 3D target feet positions
self.planner_vgoals = np.zeros([logSize, 3, 4]) # 3D target feet velocities
self.planner_agoals = np.zeros([logSize, 3, 4]) # 3D target feet accelerations
+ self.planner_jgoals = np.zeros([logSize, 3, 4]) # 3D target feet accelerations
# Model Predictive Control
# output vector of the MPC (next state + reference contact force)
@@ -160,6 +162,7 @@ class LoggerControl():
self.planner_goals[self.i] = footTrajectoryGenerator.getFootPosition()
self.planner_vgoals[self.i] = footTrajectoryGenerator.getFootVelocity()
self.planner_agoals[self.i] = footTrajectoryGenerator.getFootAcceleration()
+ self.planner_jgoals[self.i] = footTrajectoryGenerator.getFootJerk()
self.planner_is_static[self.i] = gait.getIsStatic()
self.planner_h_ref[self.i] = loop.h_ref
@@ -747,6 +750,7 @@ class LoggerControl():
planner_goals=self.planner_goals,
planner_vgoals=self.planner_vgoals,
planner_agoals=self.planner_agoals,
+ planner_jgoals=self.planner_jgoals,
planner_is_static=self.planner_is_static,
planner_h_ref=self.planner_h_ref,
@@ -794,7 +798,7 @@ class LoggerControl():
import glob
fileName = np.sort(glob.glob('data_2021_*.npz'))[-1] # Most recent file
- data = np.load(fileName)
+ data = np.load(fileName, allow_pickle = True)
# Load LoggerControl arrays
self.joy_v_ref = data["joy_v_ref"]
@@ -842,6 +846,7 @@ class LoggerControl():
self.planner_goals = data["planner_goals"]
self.planner_vgoals = data["planner_vgoals"]
self.planner_agoals = data["planner_agoals"]
+ self.planner_jgoals = data["planner_jgoals"]
self.planner_is_static = data["planner_is_static"]
self.planner_h_ref = data["planner_h_ref"]
@@ -1074,13 +1079,20 @@ class LoggerControl():
if __name__ == "__main__":
import LoggerSensors
+ import sys
+ import os
+ from sys import argv
+ sys.path.insert(0, os.getcwd()) # adds current directory to python path
+
+ # Data file name to load
+ file_name = "/home/thomas_cbrs/Desktop/edin/quadruped-reactive-walking/scripts/crocoddyl_eval/logs/explore_weight_acc/data_2021_09_16_15_10_3.npz"
# Create loggers
loggerSensors = LoggerSensors.LoggerSensors(logSize=20000-3)
logger = LoggerControl(0.001, 30, logSize=20000-3)
# Load data from .npz file
- logger.loadAll(loggerSensors)
+ logger.loadAll(loggerSensors, fileName= file_name)
# Call all ploting functions
logger.plotAll(loggerSensors)
diff --git a/scripts/MPC_Wrapper.py b/scripts/MPC_Wrapper.py
index a82fde7422d6511d52447ed576cb4d7af2bd04e3..1dcc23a0020a6610a8f284b52ef8ada0ec1a65c8 100644
--- a/scripts/MPC_Wrapper.py
+++ b/scripts/MPC_Wrapper.py
@@ -1,5 +1,7 @@
# coding: utf8
+from operator import pos
+from typing import FrozenSet
import numpy as np
import libquadruped_reactive_walking as MPC
from multiprocessing import Process, Value, Array
@@ -8,6 +10,9 @@ import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
import pinocchio as pin
from time import time
from enum import Enum
+import libquadruped_reactive_walking as lqrw
+import ctypes
+from ctypes import Structure, c_double
class MPC_type(Enum):
OSQP = 0
@@ -16,6 +21,32 @@ class MPC_type(Enum):
CROCODDYL_PLANNER = 3
CROCODDYL_PLANNER_TIME = 4
+class DataInCtype(Structure):
+ ''' Ctype data structure for the shared memory between processes.
+ '''
+ params = lqrw.Params() # Object that holds all controller parameters
+ mpc_type = MPC_type(params.type_MPC) # MPC type
+ n_steps = np.int(params.T_mpc/params.dt_mpc) # Colomn size for xref (12 x n_steps)
+ N_gait = int(5 + params.T_mpc / params.dt_mpc) # Row size for fsteps (N_gait x 12), from utils_mpc.py
+
+ if mpc_type == MPC_type.CROCODDYL_PLANNER:
+ _fields_ = [('k', ctypes.c_int64 ),
+ ('xref', ctypes.c_double * 12 * (n_steps+1) ),
+ ('fsteps', ctypes.c_double * 12 * N_gait ),
+ ('l_fsteps_target', ctypes.c_double * 3 * 4 ),
+ ('oRh', ctypes.c_double * 3 * 3 ),
+ ('oTh', ctypes.c_double * 3 * 1 ),
+ ('position', ctypes.c_double * 3 * 4 ),
+ ('velocity', ctypes.c_double * 3 * 4 ),
+ ('acceleration', ctypes.c_double * 3 * 4 ),
+ ('jerk', ctypes.c_double * 3 * 4 ),
+ ('dt_flying', ctypes.c_double * 4)]
+ else:
+ _fields_ = [('k', ctypes.c_int64 ),
+ ('xref', ctypes.c_double * 12 * (n_steps+1) ),
+ ('fsteps', ctypes.c_double * 12 * N_gait )]
+
+
class Dummy:
"""Dummy class to store variables"""
@@ -60,18 +91,17 @@ class MPC_Wrapper:
self.T_gait = params.gait.shape[0] * params.dt_mpc
self.gait_past = np.zeros(4)
self.gait_next = np.zeros(4)
- self.mass = params.mass
+ self.mass = params.mass
self.mpc_type = MPC_type(params.type_MPC)
self.multiprocessing = params.enable_multiprocessing
if self.multiprocessing: # Setup variables in the shared memory
self.newData = Value('b', False)
- self.newResult = Value('b', False)
- if self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Need more space to store optimized footsteps and l_fsteps to stop the optimization around it (+ oTh and oRh)
- self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 12*self.N_gait + 12 + 9 + 3) )
+ self.newResult = Value('b', False)
+ self.dataIn = Value(DataInCtype)
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
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))
+ else:
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)
@@ -99,7 +129,8 @@ 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, l_targetFootstep, oRh = np.eye(3), oTh = np.zeros((3,1)) ):
+ def solve(self, k, xref, fsteps, gait, l_fsteps_target, oRh = np.eye(3), oTh = np.zeros((3,1)),
+ position = np.zeros((3,4)), velocity = np.zeros((3,4)) , acceleration = np.zeros((3,4)), jerk = np.zeros((3,4)) , dt_flying = np.zeros(4)):
"""Call either the asynchronous MPC or the synchronous MPC depending on the value of multiprocessing during
the creation of the wrapper
@@ -108,15 +139,15 @@ 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
+ l_fsteps_target (3x4 array) : 4*[x, y, z]^T target position in local frame, to stop the optimisation of the feet location around it
"""
self.t_mpc_solving_start = time()
if self.multiprocessing: # Run in parallel process
- self.run_MPC_asynchronous(k, xref, fsteps, l_targetFootstep, oRh, oTh)
+ self.run_MPC_asynchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
else: # Run in the same process than main loop
- self.run_MPC_synchronous(k, xref, fsteps, l_targetFootstep, oRh, oTh)
+ self.run_MPC_synchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
if not np.allclose(gait[0, :], self.gait_past): # If gait status has changed
if np.allclose(gait[0, :], self.gait_next): # If we're still doing what was planned the last time MPC was solved
@@ -157,14 +188,14 @@ class MPC_Wrapper:
self.not_first_iter = True
return self.last_available_result
- def run_MPC_synchronous(self, k, xref, fsteps, l_targetFootstep, oRh, oTh):
+ def run_MPC_synchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
"""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
+ l_fsteps_target (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
@@ -175,7 +206,7 @@ class MPC_Wrapper:
self.mpc.run(np.int(k), xref.copy(), fsteps.copy())
elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Add goal position to stop the optimisation
# Crocoddyl MPC
- self.mpc.solve(k, xref.copy(), fsteps.copy(), l_targetFootstep)
+ self.mpc.solve(k, xref.copy(), fsteps.copy(), l_fsteps_target, position, velocity, acceleration, jerk, oRh, oTh, dt_flying)
else:
# Crocoddyl MPC
self.mpc.solve(k, xref.copy(), fsteps.copy())
@@ -186,7 +217,7 @@ class MPC_Wrapper:
else:
self.f_applied = self.mpc.get_latest_result()
- def run_MPC_asynchronous(self, k, xref, fsteps, l_targetFootstep, oRh, oTh):
+ def run_MPC_asynchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
"""Run the MPC (asynchronous version) to get the desired contact forces for the feet currently in stance phase
Args:
@@ -194,7 +225,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
+ l_fsteps_target (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
@@ -204,7 +235,7 @@ class MPC_Wrapper:
p.start()
# Stacking data to send them to the parallel process
- self.compress_dataIn(k, xref, fsteps, l_targetFootstep, oRh, oTh)
+ self.compress_dataIn(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
self.newData.value = True
return 0
@@ -231,17 +262,9 @@ class MPC_Wrapper:
# Retrieve data thanks to the decompression function and reshape it
if self.mpc_type != MPC_type.CROCODDYL_PLANNER:
- kf, xref_1dim, fsteps_1dim = self.decompress_dataIn(dataIn)
+ k, xref, fsteps = self.decompress_dataIn(dataIn)
else:
- kf, xref_1dim, fsteps_1dim, l_target_1dim, oRh, oTh = self.decompress_dataIn(dataIn)
- l_target = np.reshape(l_target_1dim, (3,4))
- oRh = np.reshape(oRh, (3,3))
- oTh = np.reshape(oTh, (3,1))
-
- # Reshaping 1-dimensional data
- k = int(kf[0])
- xref = np.reshape(xref_1dim, (12, self.n_steps+1))
- fsteps = np.reshape(fsteps_1dim, (self.N_gait, 12))
+ k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying = self.decompress_dataIn(dataIn)
# Create the MPC object of the parallel process during the first iteration
if k == 0:
@@ -262,7 +285,9 @@ class MPC_Wrapper:
fsteps[np.isnan(fsteps)] = 0.0
loop_mpc.run(np.int(k), xref, fsteps)
elif self.mpc_type == MPC_type.CROCODDYL_PLANNER:
- loop_mpc.solve(k, xref.copy(), fsteps.copy(), l_target.copy())
+ loop_mpc.solve(k, xref.copy(), fsteps.copy(), l_fsteps_target.copy(),
+ position.copy(), velocity.copy(), acceleration.copy(), jerk.copy(),
+ oRh.copy(), oTh.copy(), dt_flying.copy())
else:
loop_mpc.solve(k, xref.copy(), fsteps.copy())
@@ -280,46 +305,69 @@ class MPC_Wrapper:
return 0
- def compress_dataIn(self, k, xref, fsteps, l_targetFootstep, oRh, oTh):
- """Compress data in a single C-type array that belongs to the shared memory to send data from the main control
+ def compress_dataIn(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
+ """Compress data in a C-type structure 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
+ l_fsteps_target (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
- if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
- self.dataIn[:] = np.concatenate([[(k/self.k_mpc)], xref.ravel(),
- fsteps.ravel(), l_targetFootstep.ravel(), oRh.ravel(), oTh.ravel()], axis=0)
- else:
- self.dataIn[:] = np.concatenate([[(k/self.k_mpc)], xref.ravel(),
- fsteps.ravel()], axis=0)
+ with self.dataIn.get_lock():
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
+ self.dataIn.k = k
+ np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))[:,:] = xref
+ np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))[:,:] = fsteps
+ np.frombuffer(self.dataIn.l_fsteps_target).reshape((3, 4))[:,:] = l_fsteps_target
+ np.frombuffer(self.dataIn.oRh).reshape((3, 3))[:,:] = oRh
+ np.frombuffer(self.dataIn.oTh).reshape((3, 1))[:,:] = oTh
+ np.frombuffer(self.dataIn.position).reshape((3, 4))[:,:] = position
+ np.frombuffer(self.dataIn.velocity).reshape((3, 4))[:,:] = velocity
+ np.frombuffer(self.dataIn.acceleration).reshape((3, 4))[:,:] = acceleration
+ np.frombuffer(self.dataIn.jerk).reshape((3, 4))[:,:] = jerk
+ np.frombuffer(self.dataIn.dt_flying).reshape(4)[:] = dt_flying
- return 0.0
+ else:
+ self.dataIn.k = k
+ np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))[:,:] = xref
+ np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))[:,:] = fsteps
def decompress_dataIn(self, dataIn):
- """Decompress data from a single C-type array that belongs to the shared memory to retrieve data from the main control
+ """Decompress data from a C-type structure that belongs to the shared memory to retrieve data from the main control
loop in the asynchronous MPC
Args:
- dataIn (Array): shared array that contains the data the asynchronous MPC will use as inputs
+ dataIn (Array): shared C-type structure that contains the data the asynchronous MPC will use as inputs
"""
- # Sizes of the different variables that are stored in the C-type array
- if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
- sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait, 12, 9, 3]
- else:
- sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 12*self.N_gait]
- csizes = np.cumsum(sizes)
+ with dataIn.get_lock():
+ if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
+ k = self.dataIn.k
+ xref = np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))
+ fsteps = np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))
+ l_fsteps_target = np.frombuffer(self.dataIn.l_fsteps_target).reshape((3,4))
+ oRh = np.frombuffer(self.dataIn.oRh).reshape((3,3))
+ oTh = np.frombuffer(self.dataIn.oTh).reshape((3,1))
+ position = np.frombuffer(self.dataIn.position).reshape((3,4))
+ velocity = np.frombuffer(self.dataIn.velocity).reshape((3,4))
+ acceleration = np.frombuffer(self.dataIn.acceleration).reshape((3,4))
+ jerk = np.frombuffer(self.dataIn.jerk).reshape((3,4))
+ dt_flying = np.frombuffer(self.dataIn.dt_flying).reshape(4)
+
+ return k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying
+
+ else:
+ k = self.dataIn.k
+ xref = np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps+1))
+ fsteps = np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))
- # Return decompressed variables in a list
- return [dataIn[csizes[i]:csizes[i+1]] for i in range(len(sizes)-1)]
+ return k, xref, fsteps
def convert_dataOut(self):
"""Return the result of the asynchronous MPC (desired contact forces) that is stored in the shared memory
diff --git a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
index 0b9ac28938ab1e655ec516c173d01ed880ebc5d1..dee9bfbbfeb08b3f8070431ca909f29a3d7d9021 100644
--- a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
+++ b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
@@ -5,6 +5,10 @@ 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.
@@ -13,29 +17,29 @@ class MPC_crocoddyl_planner():
params (obj): Params object containing the simulation parameters
mu (float): Friction coefficient
inner (float): Inside or outside approximation of the friction cone (mu * 1/sqrt(2))
- linearModel(bool) : Approximation in the cross product by using desired state
"""
- def __init__(self, params, mu=1, inner=True, warm_start=True, min_fz=0.0):
+ def __init__(self, params, mu=1, inner=True):
- self.dt = params.dt_mpc # Time step of the solver
- self.T_mpc = params.T_mpc # Period of the MPC
+ self.dt = params.dt_mpc # Time step of the solver
+ self.dt_wbc = params.dt_wbc # Time step of the WBC
+ self.T_mpc = params.T_mpc # Period of the MPC
+ self.T_gait = params.T_gait # Period of the gait
+ self.N_gait = params.N_gait # Number of gait row
self.n_nodes = int(self.T_mpc/self.dt) # Number of nodes
self.mass = params.mass # Mass of the robot
self.max_iteration = 10 # Max iteration ddp solver
- self.gI = np.array(params.I_mat.tolist()).reshape((3, 3)) # Inertia matrix in ody frame
+ self.gI = np.array(params.I_mat.tolist()).reshape((3, 3)) # Inertia matrix in body frame
# Friction coefficient
if inner:
self.mu = (1/np.sqrt(2))*mu
else:
- self.mu = mu
-
- # self.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])
+ self.mu = mu
# Weights Vector : States
+ # self.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])
self.w_x = 0.3
- # self.w_y = 0.3
self.w_y = 0.3
self.w_z = 20
self.w_roll = 0.9
@@ -50,33 +54,51 @@ class MPC_crocoddyl_planner():
self.stateWeights = np.array([self.w_x, self.w_y, self.w_z, self.w_roll, self.w_pitch, self.w_yaw,
self.w_vx, self.w_vy, self.w_vz, self.w_vroll, self.w_vpitch, self.w_vyaw])
- self.forceWeights = 1*np.array(4*[0.007, 0.007, 0.007]) # Weight Vector : Force Norm
- self.frictionWeights = 1. # Weight Vector : Friction cone cost
- self.heuristicWeights = 0.*np.array(4*[0.03, 0.04]) # Weights on the heuristic term
- self.stepWeights = 0.*np.full(8, 0.005) # Weight on the step command (distance between steps)
- self.stopWeights = 0.*np.ones(8) # Weights to stop the optimisation at the end of the flying phase
- self.shoulderContactWeight = 0.5*np.full(4,1.) # Weight for shoulder-to-contact penalty
- self.shoulder_hlim = 0.235
- self.shoulderReferencePosition = False # Use the reference trajectory of the Com (True) or not (False) for shoulder/contact cost
+ self.forceWeights = 1*np.array(4*[0.007, 0.007, 0.007]) # Weight vector : Force Norm
+ self.frictionWeights = 1. # Weight vector : Friction cone penalisation
+ self.heuristicWeights = 0.*np.array(4*[0.03, 0.04]) # Weight vector : Heuristic term
+ self.stepWeights = 0.*np.full(8, 0.005) # Weight vector : Norm of step command (distance between steps)
+ self.stopWeights = 0.*np.ones(8) # Weight vector : Stop the optimisation at the end of the flying phase (position penalisation)
+ self.shoulderContactWeight = 1.5*np.full(4,1.) # Weight vector : Shoulder-to-contact penalisation
+ self.shoulder_hlim = 0.235 # Distance to activate the shoulder-to-contact penalisation
+ self.shoulderReferencePosition = False # Use the reference trajectory of the Com (True) or not (False) for shoulder-to-contact penalisation
# TODO : create a proper warm-start with the previous optimisation
self.warm_start = True
- # Minimum normal force(N) and reference force vector bool
+ # Minimum/Maximal normal force(N) and relative_forces activation
self.min_fz = 0.
self.max_fz = 25.
- self.relative_forces = True
+ self.relative_forces = True # F_ref = m*g/nb_contact
+
+ # Acceleration penalty
+ self.is_acc_activated = False
+ self.acc_limit = np.array([60.,60.]) # Acceleration to activate the penalisation
+ self.acc_weight = 0.0001
+ self.n_sampling = 8
+ self.flying_foot_old = 4*[False]
+ self.dx_new_phase = 0.
+ self.dy_new_phase = 0.
+
+ # Velocity penalty
+ self.is_vel_activated = True
+ self.vel_limit = np.array([4.,4.]) # Velocity to activate the penalisation
+ self.vel_weight = 1.
+
+ # Jerk penalty
+ self.is_jerk_activated = True
+ self.jerk_weight = 1e-7
+ self.jerk_alpha = 23 # Define the slope of the cost, not used
+ self.jerk_beta = 0. # Define the slope of the cost, not used
# Offset CoM
self.offset_com = -0.03
+ self.vert_time = params.vert_time
# Gait matrix
self.gait = np.zeros((params.N_gait, 4))
self.gait_old = np.zeros(4)
- # Position of the feet in local frame
- self.fsteps = np.zeros((params.N_gait, 12))
-
# List to generate the problem
self.action_models = []
self.x_init = []
@@ -105,24 +127,21 @@ class MPC_crocoddyl_planner():
self.index_inital_stance_phase = [] # List of index to reset the stopWeights to 0 after optimisation
# Initialize the lists of models
- self.initialize_models(params)
+ self.initialize_models()
- def initialize_models(self, params):
+ def initialize_models(self):
''' 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 _ in range(params.N_gait):
+ for _ in range(self.N_gait):
model = quadruped_walkgen.ActionModelQuadrupedAugmented()
self.update_model_augmented(model)
self.models_augmented.append(model)
- for _ in range(4 * int(params.T_mpc / params.T_gait)):
+ for _ in range(4 * int(self.T_mpc / self.T_gait)):
model = quadruped_walkgen.ActionModelQuadrupedStep()
self.update_model_step(model)
self.models_step.append(model)
@@ -131,52 +150,60 @@ class MPC_crocoddyl_planner():
self.terminal_model = quadruped_walkgen.ActionModelQuadrupedAugmented()
self.update_model_augmented(self.terminal_model, terminal=True)
- def solve(self, k, xref, footsteps, l_stop):
- """ Solve the MPC problem
+ def solve(self, k, xref, footsteps, l_stop, position, velocity, acceleration, jerk, oRh, oTh, dt_flying):
+ """ Solve the MPC problem.
Args:
- k : Iteration
- xref : the desired state vector
- footsteps : current position of the feet (given by planner)
- l_stop : current and target position of the feet (given by footstepTragectory generator)
+ k (int) : Iteration
+ xref (Array 12xN) : Desired state vector
+ footsteps (Array 12xN) : current position of the feet (given by planner)
+ l_stop (Array 3x4) : Current target position of the feet
+ position (Array 3x4) : Position of the flying feet
+ velocity (Array 3x4) : Velocity of the flying feet
+ acceleration (Array 3x4): Acceleration position of the flying feet
+ jerk (Array 3x4) : Jerk position of the flying feet
+ oRh (Array 3x3) : Rotation matrix from base to ideal/world frame
+ oTh (Array 3x1) : Translation vector from base to ideal/world frame
+ dt_flying (Array 4x) : Remaining timing of flying feet
"""
self.xref[:,:] = xref
self.xref[2,:] += self.offset_com
- self.updateProblem(k, self.xref, footsteps, l_stop)
+ self.updateProblem(k, self.xref, footsteps, l_stop, position, velocity, acceleration, jerk, oRh, oTh, dt_flying)
self.ddp.solve(self.x_init, self.u_init, self.max_iteration)
-
- # np.set_printoptions(linewidth=300, precision=3)
- # print(self.gait[:30,:])
- # idx = 0
- # for model in self.action_models:
- # if model.__class__.__name__ == "ActionModelQuadrupedStep" :
- # print(model.__class__.__name__)
- # else:
- # print(model.__class__.__name__ + " : " + str(model.shoulderContactWeight) + " - " + str(self.gait[idx,:]))
- # idx += 1
- # from IPython import embed
- # embed()
-
- # Reset to 0 the stopWeights for next optimisation
+
+ # Reset to 0 the stopWeights for next optimisation (already 0.)
for index_stopped in self.index_stop_optimisation:
self.models_augmented[index_stopped].stopWeights = np.zeros(8)
for index_stance in self.index_inital_stance_phase:
self.models_augmented[index_stance].shoulderContactWeight = self.shoulderContactWeight
- def updateProblem(self, k, xref, footsteps, l_stop):
+ def updateProblem(self, k, xref, footsteps, l_stop, position, velocity, acceleration, jerk, oRh, oTh, dt_flying):
"""
- Update the dynamic of the model list according to the predicted position of the feet,
- and the desired state.
+ Update the models of the nodes according to parameters of the simulations.
Args:
+ k (int) : Iteration
+ xref (Array 12xN) : Desired state vector
+ footsteps (Array 12xN) : current position of the feet (given by planner)
+ l_stop (Array 3x4) : Current target position of the feet
+ position (Array 3x4) : Position of the flying feet
+ velocity (Array 3x4) : Velocity of the flying feet
+ acceleration (Array 3x4): Acceleration position of the flying feet
+ jerk (Array 3x4) : Jerk position of the flying feet
+ oRh (Array 3x3) : Rotation matrix from base to ideal/world frame
+ oTh (Array 3x1) : Translation vector from base to ideal/world frame
+ dt_flying (Array 4x) : Remaining timing of flying feet
"""
+ # Update internal matrix
self.compute_gait_matrix(footsteps)
+ # Set up intial feet position : shoulder for flying feet and current foot position for feet on the ground
p0 = (1.0 - np.repeat(self.gait[0, :], 2)) * self.shoulders \
+ np.repeat(self.gait[0, :], 2) * footsteps[0, [0, 1, 3, 4, 6, 7, 9, 10]]
+ # Clear lists of models
self.x_init.clear()
self.u_init.clear()
self.action_models.clear()
@@ -208,17 +235,76 @@ class MPC_crocoddyl_planner():
self.index_inital_stance_phase.append(index_augmented)
self.action_models.append(self.models_augmented[index_augmented])
-
index_augmented += 1
+
# Warm-start
self.x_init.append(np.concatenate([xref[:, 0], p0]))
self.u_init.append(np.repeat(self.gait[0, :], 3) * np.array(4*[0., 0., 2.5*9.81/np.sum(self.gait[0, :])]))
+ # Bool to activate the speed, acc or jerk on only current flying feet
+ is_first_step = True
+
while np.any(self.gait[j, :]):
- if np.any(self.gait[j, :] - self.gait[j-1, :]):
- # Step model
+ if np.any(self.gait[j, :] - self.gait[j-1, :]): # TODO Adapt this [1,1,1,1] --> [1,0,0,1] True here but not step model to add
+
+ flying_feet = np.where(self.gait[j-1, :] == 0)[0] # Current flying feet
+
+ # Get remaining dt incurrent flying phase
+ # TODO : dt_ is the same for all flying foot, could be different
+ dt_ = dt_flying[flying_feet[0]]
+
+ # part_curve explanation for foot trajectory :
+ # ------- <- 2 (foot stopped, during params.vert_time)
+ # /
+ # / <- 1 (foot moving)
+ # /
+ # ------ <- 0 (foot stopped, during params.vert_time)
+ # TODO : get total flying period from gait and not T_gait/2
+ part_curve = 0
+ dt_ -= self.dt_wbc # dt_ has been computed on the previous loop, dt_wbc late
+ if dt_ <= 0.001 or dt_ >= self.T_gait/2 - self.vert_time: # Because of the delay, dt_ = 0 --> new flying phase
+ dt_ = self.T_gait/2 - 2*self.vert_time
+ part_curve = 0
+ elif dt_ >= self.vert_time :
+ dt_ -= self.vert_time
+ part_curve = 1
+ else:
+ dt_ = 0.002 # Close to 0 to avoid high jerk and thus keep the optimised fstep from previous cycle
+ part_curve = 2
+
+ if is_first_step :
+ # Acceleration cost
+ # if self.flying_foot_old != self.flying_foot :
+ # self.dx_new_phase = abs(footsteps[j, 3*flying_feet[0] ] - position[0,flying_feet[0] ])
+ # self.dy_new_phase = abs(footsteps[j, 3*flying_feet[0]+1 ] - position[1,flying_feet[0] ])
+ # self.acc_limit = 2*5.625*np.array([ self.dx_new_phase / 0.24**2 , self.dy_new_phase / 0.24**2])
+ self.models_step[index_step].is_acc_activated = self.is_acc_activated
+ # self.models_step[index_step].acc_limit = self.acc_limit
+
+ # Velocity cost
+ self.models_step[index_step].is_vel_activated = self.is_vel_activated
+
+ if part_curve == 1 or part_curve == 2 :
+ self.models_step[index_step].is_jerk_activated = self.is_jerk_activated
+ # self.models_step[index_step].jerk_weight = self.exponential_cost(self.T_gait/2 - dt_)
+ # self.models_step[index_step].jerk_weight = self.jerk_weight
+ else :
+ self.models_step[index_step].jerk_weight = 0.
+
+ # Update is_first_step bool
+ is_first_step = False
+
+ else:
+ # Cost on the feet trajectory disabled
+ # TODO : Only activate cost on the maximum speed (simplify the equations in step model, vmax = T/2)
+ self.models_step[index_step].is_acc_activated = False
+ self.models_step[index_step].is_vel_activated = False
+ self.models_step[index_step].is_jerk_activated = False
+ dt_ = 0.24
+
self.models_step[index_step].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
- xref[:, j], self.gait[j, :] - self.gait[j-1, :])
+ xref[:, j], self.gait[j, :] - self.gait[j-1, :],
+ position, velocity, acceleration, jerk, oRh, oTh, dt_)
self.action_models.append(self.models_step[index_step])
# Augmented model
@@ -366,6 +452,15 @@ class MPC_crocoddyl_planner():
model.heuristicWeights = np.zeros(8)
model.stateWeights = self.stateWeights
model.stepWeights = self.stepWeights
+ model.acc_limit = self.acc_limit
+ model.acc_weight = self.acc_weight
+ model.is_acc_activated = False
+ model.vel_limit = self.vel_limit
+ model.vel_weight = self.vel_weight
+ model.is_vel_activated = False
+ model.is_jerk_activated = False
+ model.jerk_weight = self.jerk_weight
+ model.set_sample_feet_traj(self.n_sampling)
def compute_gait_matrix(self, footsteps):
"""
@@ -383,6 +478,7 @@ class MPC_crocoddyl_planner():
j += 1
# Get the current flying feet and the number of nodes
+ self.flying_foot_old[:] = self.flying_foot[:] # Update old matrix, usefull when new phase start
for foot in range(4):
row = 0
if self.gait[0, foot] == 0:
@@ -403,5 +499,14 @@ class MPC_crocoddyl_planner():
self.stance_foot_nodes[foot] = int(row)
else:
self.stance_foot[foot] = False
+
+ def exponential_cost(self, t):
+ """
+ Returns weight_jerk * exp(alpha(t-beta)) - 1
+ Args:
+ t (float) : time in s
+ """
+
+ return self.jerk_weight * (np.exp(self.jerk_alpha*(t - self.jerk_beta)) - 1)
diff --git a/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py b/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
index 1a5516583981677812909d69cb5e186e882437fd..ab222f1631b8a9370a3b8043057ef59932a46383 100644
--- a/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
+++ b/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
@@ -11,11 +11,12 @@ import libquadruped_reactive_walking as lqrw
import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
import crocoddyl
import utils_mpc
+np.set_printoptions(formatter={'float': lambda x: "{0:0.7f}".format(x)}, linewidth = 450)
##############
# Parameters
##############
-iteration_mpc = 2 # Control cycle
+iteration_mpc = 350 # Control cycle
Relaunch_DDP = True # Compare a third MPC with != parameters
linear_mpc = True
params = lqrw.Params() # Object that holds all controller parameters
@@ -29,7 +30,7 @@ solo = utils_mpc.init_robot(q_init, params)
######################
# Recover Logged data
######################
-file_name = "crocoddyl_eval/logs/chute_mpc_nl_noShoulder.npz"
+file_name = "/home/thomas_cbrs/Desktop/edin/quadruped-reactive-walking/scripts/crocoddyl_eval/logs/explore_weight_acc/data_cost.npz"
logs = np.load(file_name)
planner_gait = logs.get("planner_gait")
planner_xref = logs.get("planner_xref")
@@ -42,14 +43,14 @@ k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) # simulation iteratio
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])
+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=False)
+mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=True)
# Without warm-start :
# mpc_ddp.warm_start = False
# mpc_ddp.solve(k, planner_xref[k] , planner_fsteps[k] ) # Without warm-start
@@ -57,8 +58,14 @@ mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=F
# 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
+planner_xref_offset = planner_xref[k].copy()
+planner_xref_offset[2,:] += mpc_ddp.offset_com
+mpc_ddp.updateProblem( planner_fsteps[k], planner_xref_offset) # Update dynamics
+
+print("\n\n")
+print("model 0 :")
+print(mpc_ddp.problem.runningModels[0].B)
x_init = []
u_init = []
mpc_ddp.ddp.solve(x_init, u_init, mpc_ddp.max_iteration)
@@ -70,28 +77,30 @@ 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(mpc_x_f[k_previous,12:24,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
+x_init.insert(0, planner_xref_offset[:, 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_xs = np.vstack([planner_xref_offset[:,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_2 = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False , linearModel=False) # To modify the linear model if wanted, recreate a list with proper model
+mpc_ddp_2 = 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
mpc_ddp_2.implicit_integration = False
-mpc_ddp_2.shoulderWeights = 0.
+mpc_ddp_2.offset_com = 0.0
+# mpc_ddp_2.stateWeights = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 1.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3])
+# mpc_ddp_2.shoulderWeights = 1.
# Weight Vector : State
# w_x = 0.2
# w_y = 0.2
@@ -142,12 +151,15 @@ mpc_ddp_2.problem = crocoddyl.ShootingProblem(np.zeros(12), mpc_ddp_2.ListActio
mpc_ddp_2.ddp = crocoddyl.SolverDDP(mpc_ddp_2.problem)
# Run ddp solver
# Update the dynamic depending on the predicted feet position
+planner_xref_offset = planner_xref[k]
+planner_xref_offset[2,:] += mpc_ddp_2.offset_com
+
if Relaunch_DDP :
- mpc_ddp_2.updateProblem( planner_fsteps[k], planner_xref[k])
+ mpc_ddp_2.updateProblem( planner_fsteps[k], planner_xref_offset)
mpc_ddp_2.ddp.solve(x_init, u_init, mpc_ddp_2.max_iteration, isFeasible = False)
ddp_xs_relaunch = mpc_ddp_2.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_xs_relaunch = np.vstack([planner_xref_offset[:,0] , ddp_xs_relaunch.transpose()]).transpose() # Add current state
ddp_us_relaunch = mpc_ddp_2.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
#############
diff --git a/scripts/crocoddyl_eval/test_2/unittest_augmented.py b/scripts/crocoddyl_eval/test_2/unittest_augmented.py
index cf364c1e7d57c35bec623bcb422f04166104de84..1c9aa18c611bf5b158ffd4684f658a4d0f321e3f 100644
--- a/scripts/crocoddyl_eval/test_2/unittest_augmented.py
+++ b/scripts/crocoddyl_eval/test_2/unittest_augmented.py
@@ -16,6 +16,9 @@ model = quadruped_walkgen.ActionModelQuadrupedAugmented()
model.stateWeights = 2*np.ones(12)
model.heuristicWeights = 2*np.ones(8)
model.stepWeights = np.ones(8)
+model.shoulderContactWeight = np.array([0.5,0.1,0.9,0.])
+model.shoulderReferencePosition = False
+model.max_fz = 0.5
# Update the dynamic of the model
fstep = np.random.rand(12).reshape((3,4))
diff --git a/scripts/crocoddyl_eval/test_2/unittest_step.py b/scripts/crocoddyl_eval/test_2/unittest_step.py
index c31d6d2031eca2855bff357274910528d91055fc..453338a81cfa1b47ca28c3465c76a3b0a639ef15 100644
--- a/scripts/crocoddyl_eval/test_2/unittest_step.py
+++ b/scripts/crocoddyl_eval/test_2/unittest_step.py
@@ -17,11 +17,36 @@ model.stateWeights = 2*np.ones(12)
model.heuristicWeights = 2*np.ones(8)
model.stepWeights = np.ones(8)
+model.is_acc_activated = False
+model.acc_limit = np.array([0.09,0.])
+model.acc_weight = 1.88
+
+model.is_vel_activated = True
+model.vel_limit = np.array([0.,0.])
+model.vel_weight = 0.0015
+
+model.is_jerk_activated = True
+model.jerk_weight = 0.000001
+
+
# 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)
+position = np.random.rand(12).reshape((3,4))
+velocity = np.random.rand(12).reshape((3,4))
+acceleration = np.random.rand(12).reshape((3,4))
+jerk = np.random.rand(12).reshape((3,4))
+oRh = np.zeros((3,3))
+oTh = np.zeros((3,1))
+oRh[0,0] = 0.88
+oRh[1,1] = 0.88
+oRh[0,1] = -0.2
+oRh[1,0] = 0.2
+oTh[0,0] = 0.1
+oTh[1,0] = 0.09
+Dt = 0.16
+model.updateModel(fstep, xref , gait, position, velocity, acceleration, jerk, oRh, oTh, Dt )
################################################
## CHECK DERIVATIVE WITH NUM_DIFF
@@ -29,7 +54,7 @@ model.updateModel(fstep, xref , gait)
a = 1
b = -1
-N_trial = 50
+N_trial = 1
epsilon = 10e-6
model_diff = crocoddyl.ActionModelNumDiff(model)
@@ -63,7 +88,12 @@ def run_calcDiff_numDiff(epsilon) :
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)
+ position = np.random.rand(12).reshape((3,4))
+ velocity = np.random.rand(12).reshape((3,4))
+ acceleration = np.random.rand(12).reshape((3,4))
+ jerk = np.random.rand(12).reshape((3,4))
+ Dt = 0.16
+ model.updateModel(fstep, xref , gait, position, velocity, acceleration, jerk, oRh, oTh, Dt )
model_diff = crocoddyl.ActionModelNumDiff(model)
# Run calc & calcDiff function : numDiff
@@ -78,6 +108,11 @@ def run_calcDiff_numDiff(epsilon) :
Lx_err += np.sum( abs((data.Lx - data_diff.Lx )) )
Lu += np.sum( abs((data.Lu - data_diff.Lu )) >= epsilon )
+ print("\n")
+ print("Lu :")
+ print(data.Lu)
+ print("Lu diff :")
+ print(data_diff.Lu )
Lu_err += np.sum( abs((data.Lu - data_diff.Lu )) )
Lxu += np.sum( abs((data.Lxu - data_diff.Lxu )) >= epsilon )
diff --git a/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py b/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
index a3d7792485b16125552f69c21c5e556d36599736..c4977ec9c059a0a8dc24388674c7ac4bb7a77a0d 100644
--- a/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
+++ b/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
@@ -1,161 +1,692 @@
# coding: utf8
import sys
-import os
+import os
from sys import argv
+from matplotlib.cbook import ls_mapper
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
+import time
import utils_mpc
-
-
-##############
+import pinocchio as pin
+from matplotlib.collections import LineCollection
+import matplotlib
+import LoggerSensors
+from LoggerControl import LoggerControl
+
+class Poly_5():
+ """
+ Class to evaluate 5th degree polynomial curve for the foot trajectory.
+ x(0) = x0
+ x'(0) = v0
+ x''(0) = a0
+ x(Dt) = x1
+ Args :
+ - x0 (float) : initial position
+ - v0 (float) : initial velocity
+ - a0 (float) : initial acceleration
+ - x1 (float) : final position
+ - Dt (float) : Time of the trajectory
+ """
+
+ def __init__(self, x0, v0, a0, x1, Dt) -> None:
+ self.A0 = x0
+ self.A1 = v0
+ self.A2 = a0/2
+ self.A3 = ( 20*(x1 - x0) - 12*Dt*v0 -3*a0*Dt**2) / (2*Dt**3)
+ self.A4 = - ( 30*(x1 - x0) - 16*Dt*v0 -3*a0*Dt**2) / (2*Dt**4)
+ self.A5 = ( 12*(x1 - x0) - 6*Dt*v0 -a0*Dt**2) / (2*Dt**5)
+
+ def update_coeff(self, x0, v0, a0, x1, Dt ):
+ """
+ Re-compute the internal coefficients
+ Args :
+ - x0 (float) : initial position
+ - v0 (float) : initial velocity
+ - a0 (float) : initial acceleration
+ - x1 (float) : final position
+ - Dt (float) : Time of the trajectory
+ """
+ self.A0 = x0
+ self.A1 = v0
+ self.A2 = a0/2
+ self.A3 = ( 20*(x1 - x0) - 12*Dt*v0 -3*a0*Dt**2) / (2*Dt**3)
+ self.A4 = - ( 30*(x1 - x0) - 16*Dt*v0 -3*a0*Dt**2) / (2*Dt**4)
+ self.A5 = ( 12*(x1 - x0) - 6*Dt*v0 -a0*Dt**2) / (2*Dt**5)
+
+ def compute(self, t, index):
+ """
+ Evaluate the trajectory : x(index)(t)
+ Args :
+ - t (float) : time
+ - index (int) : Derivative order
+ """
+ if index == 0:
+ return self.A0 + self.A1*t + self.A2*t**2 + self.A3*t**3 + self.A4*t**4 + self.A5*t**5
+
+ elif index == 1:
+ return self.A1 + 2*self.A2*t + 3*self.A3*t**2 + 4*self.A4*t**3 + 5*self.A5*t**4
+
+ elif index == 2:
+ return 2*self.A2 + 6*self.A3*t + 12*self.A4*t**2 + 20*self.A5*t**3
+
+ else:
+ return 6*self.A3 + 24*self.A4*t + 60*self.A5*t**2
+
+################
# Parameters
-##############
-iteration_mpc = 45 # Control cycle
-Relaunch_DDP = False # Compare a third MPC with != parameters
-linear_mpc = True
+################
+iteration_mpc = 180 # Control cycle
+iteration_init = iteration_mpc
params = lqrw.Params() # Object that holds all controller parameters
+vert_time = params.vert_time
# Default position after calibration
q_init = np.array(params.q_init.tolist())
# Update I_mat, etc...
-solo = utils_mpc.init_robot(q_init, params)
+solo = utils_mpc.init_robot(q_init, params)
+
+#######################
+# Recover Logged data
+#######################
+file_name = "/home/thomas_cbrs/Desktop/edin/quadruped-reactive-walking/scripts/crocoddyl_eval/logs/explore_weight_acc/data_cost.npz"
-######################
-# Recover Logged data
-######################
-file_name = "crocoddyl_eval/logs/data_2021_08_18_13_23.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")
+planner_vgoals = logs.get("planner_vgoals")
+planner_agoals = logs.get("planner_agoals")
+planner_jgoals = logs.get("planner_jgoals")
+loop_o_q = logs.get("loop_o_q")
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) )
+# Create loggers and process mocap
+loggerSensors = LoggerSensors.LoggerSensors(logSize=20000-3)
+logger = LoggerControl(0.001, 30, logSize=20000-3)
+N_logger = logger.tstamps.shape[0]
+logger.processMocap(N_logger, loggerSensors)
+logger.loadAll(loggerSensors, fileName= file_name)
+
+mocap_RPY = logger.mocap_RPY
+mocap_pos = logger.mocap_pos
+
+k_0 = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) # initial simulation iteration corresponding
+N_mpc = 12 # Number of mpc to launch
+
+
+t_init = k_0*params.dt_wbc
+t_end = k_0*params.dt_wbc + N_mpc*params.dt_mpc
+T = np.linspace(t_init, t_end - params.dt_wbc , int( (t_end - t_init)/params.dt_wbc ) + 1)
+
+plt.figure()
+k_end = int(k_0 + N_mpc * params.dt_mpc / params.dt_wbc)
+
+gait = planner_gait[k_0,:,:]
+flying_feet = np.where(gait[0,:] == 0.)[0]
+foot = flying_feet[0]
+
+lgd = ["Position X", "Velocity X", "Accleration X", "Jerk X", "Position Y", "Velocity Y", "Accleration Y", "Jerk Y"]
+
+plt.subplot(2,4,1)
+plt.plot(T, planner_goals[k_0:k_end, 0,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[0])
+plt.subplot(2,4,2)
+plt.plot(T, planner_vgoals[k_0:k_end, 0,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[1])
+plt.subplot(2,4,3)
+plt.plot(T, planner_agoals[k_0:k_end, 0,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[2])
+plt.subplot(2,4,4)
+plt.plot(T, planner_jgoals[k_0:k_end, 0,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[3])
+
+plt.subplot(2,4,5)
+plt.plot(T, planner_goals[k_0:k_end, 1,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[4])
+plt.subplot(2,4,6)
+plt.plot(T, planner_vgoals[k_0:k_end, 1,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[5])
+plt.subplot(2,4,7)
+plt.plot(T, planner_agoals[k_0:k_end, 1,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[6])
+plt.subplot(2,4,8)
+plt.plot(T, planner_jgoals[k_0:k_end, 1,foot], linewidth=4, color="r" )
+plt.xlabel(lgd[7])
+
+
+cmap = plt.cm.get_cmap("hsv", N_mpc)
+
+for i in range(N_mpc):
+
+ k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) - 1# simulation iteration corresponding
+ gait = planner_gait[k,:,:]
+ id = 0
+ while gait[id,foot] == 0 :
+ id += 1
+ o_fsteps = mpc_x_f[k,24 + 2*foot:24+2*foot+2, id+1]
+ print(k)
+ x0 = planner_goals[k,0,foot]
+ y0 = planner_goals[k,1,foot]
+ xv0 = planner_vgoals[k,0,foot]
+ yv0 = planner_vgoals[k,1,foot]
+ xa0 = planner_agoals[k,0,foot]
+ ya0 = planner_agoals[k,1,foot]
+ dt_ = 0.24-params.dt_mpc*i
+
+ # part curve :
+ part_curve = 0
+ if dt_ >= 0.24 - vert_time :
+ dt_ = 0.24 - 2*vert_time
+ part_curve = 0
+ elif dt_ >= vert_time :
+ dt_ -= vert_time
+ part_curve = 1
+ else :
+ part_curve = 2
+ poly_x = Poly_5(x0,xv0,xa0, o_fsteps[0],dt_)
+ poly_y = Poly_5(y0,yv0,ya0, o_fsteps[1],dt_)
+
+ T = np.linspace(0,dt_, 50)
+
+
+ X = [poly_x.compute(t,0) for t in T]
+ VX = [poly_x.compute(t,1) for t in T]
+ AX = [poly_x.compute(t,2) for t in T]
+ JX = [poly_x.compute(t,3) for t in T]
+
+ Y = [poly_y.compute(t,0) for t in T]
+ VY = [poly_y.compute(t,1) for t in T]
+ AY = [poly_y.compute(t,2) for t in T]
+ JY = [poly_y.compute(t,3) for t in T]
+
+
+ if part_curve == 0 :
+ t0 = t_init + vert_time
+ T += t0
+ elif part_curve == 1 :
+ t0 = t_init + params.dt_mpc*i
+ T += t0
+ else :
+ t0 = t_init + params.dt_mpc*i
+ T += t0
+
+
+ plt.subplot(2,4,1)
+ plt.plot(T,X, color = cmap(i) )
+ plt.plot(T[0], X[0],marker = "o", color = cmap(i) )
+
+ plt.subplot(2,4,2)
+ plt.plot(T,VX, color = cmap(i) )
+ plt.plot(T[0], VX[0],marker = "o", color = cmap(i) )
+ plt.subplot(2,4,3)
+ plt.plot(T,AX, color = cmap(i) )
+ plt.plot(T[0], AX[0],marker = "o", color = cmap(i) )
+ plt.subplot(2,4,4)
+ plt.plot(T,JX, color = cmap(i) )
+ plt.plot(T[0], JX[0],marker = "o", color = cmap(i) )
+
+
+ plt.subplot(2,4,5)
+ plt.plot(T,Y, color = cmap(i) )
+ plt.plot(T[0], Y[0],marker = "o", color = cmap(i) )
+
+ plt.subplot(2,4,6)
+ plt.plot(T,VY, color = cmap(i) )
+ plt.plot(T[0], VY[0],marker = "o", color = cmap(i) )
+
+ plt.subplot(2,4,7)
+ plt.plot(T,AY, color = cmap(i) )
+ plt.plot(T[0], AY[0],marker = "o", color = cmap(i) )
+
+ plt.subplot(2,4,8)
+ plt.plot(T,JY, color = cmap(i) )
+ plt.plot(T[0], JY[0],marker = "o", color = cmap(i) )
+
+ iteration_mpc += 1
+
+
+
+fig_f, ax_f = plt.subplots(2,4)
+iteration_mpc = iteration_init
+t_init = k_0*params.dt_wbc
+t_end = k_0*params.dt_wbc + N_mpc*params.dt_mpc
+
+for i in range(N_mpc):
+
+ k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) - 1# simulation iteration corresponding
+ gait = planner_gait[k,:,:]
+ id = 0
+ while gait[id,foot] == 0 :
+ id += 1
+ o_fsteps = mpc_x_f[k,24 + 2*foot:24+2*foot+2, id+1]
+ print(k)
+ x0 = planner_goals[k,0,foot]
+ y0 = planner_goals[k,1,foot]
+ xv0 = planner_vgoals[k,0,foot]
+ yv0 = planner_vgoals[k,1,foot]
+ xa0 = planner_agoals[k,0,foot]
+ ya0 = planner_agoals[k,1,foot]
+ dt_ = 0.24-params.dt_mpc*i
+
+ # part curve :
+ part_curve = 0
+ if dt_ >= 0.24 - vert_time :
+ dt_ = 0.24 - 2*vert_time
+ part_curve = 0
+ elif dt_ >= vert_time :
+ dt_ -= vert_time
+ part_curve = 1
+ else :
+ part_curve = 2
+ poly_x = Poly_5(x0,xv0,xa0, o_fsteps[0],dt_)
+ poly_y = Poly_5(y0,yv0,ya0, o_fsteps[1],dt_)
+
+ T = np.linspace(0,dt_, 20)
+
+
+ X = [poly_x.compute(t,0) for t in T]
+ VX = [poly_x.compute(t,1) for t in T]
+ AX = [poly_x.compute(t,2) for t in T]
+ JX = [poly_x.compute(t,3) for t in T]
+
+ Y = [poly_y.compute(t,0) for t in T]
+ VY = [poly_y.compute(t,1) for t in T]
+ AY = [poly_y.compute(t,2) for t in T]
+ JY = [poly_y.compute(t,3) for t in T]
+
+
+ if part_curve == 0 :
+ t0 = t_init + vert_time
+ T += t0
+ elif part_curve == 1 :
+ t0 = t_init + params.dt_mpc*i
+ T += t0
+ else :
+ t0 = t_init + params.dt_mpc*i
+ T += t0
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, X]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+
+
+ # Scatter to highlight points
+ colors = np.r_[np.linspace(0.1, 0.65, 19), 1]
+ my_colors = cm(colors)
+
+
+ ax_f[0,0].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[0,0].scatter(T, X, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+
+ #####
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, VX]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+ ax_f[0,1].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[0,1].scatter(T, VX, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+ #####
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, AX]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+ ax_f[0,2].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[0,2].scatter(T, AX, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+ #####
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, JX]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+ ax_f[0,3].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[0,3].scatter(T, JX, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+##################################################################################################
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, Y]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+
+
+ # Scatter to highlight points
+ colors = np.r_[np.linspace(0.1, 0.65, 19), 1]
+ my_colors = cm(colors)
+
+
+ ax_f[1,0].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[1,0].scatter(T, Y, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+
+ #####
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, VY]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+ ax_f[1,1].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[1,1].scatter(T, VY, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+ #####
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, AY]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+ ax_f[1,2].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[1,2].scatter(T, AY, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+ #####
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([T, JY]).transpose().reshape(-1,1,2)
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+ lc_q.set_array(T)
+ # Customize
+ lc_q.set_linestyle('-')
+ ax_f[1,3].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_f[1,3].scatter(T, JY, s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+
+
+ # plt.subplot(2,4,1)
+ # plt.plot(T,X, color = cmap(i) )
+ # plt.plot(T[0], X[0],marker = "o", color = cmap(i) )
+
+ # plt.subplot(2,4,2)
+ # plt.plot(T,VX, color = cmap(i) )
+ # plt.plot(T[0], VX[0],marker = "o", color = cmap(i) )
+ # plt.subplot(2,4,3)
+ # plt.plot(T,AX, color = cmap(i) )
+ # plt.plot(T[0], AX[0],marker = "o", color = cmap(i) )
+ # plt.subplot(2,4,4)
+ # plt.plot(T,JX, color = cmap(i) )
+ # plt.plot(T[0], JX[0],marker = "o", color = cmap(i) )
+
+
+ # plt.subplot(2,4,5)
+ # plt.plot(T,Y, color = cmap(i) )
+ # plt.plot(T[0], Y[0],marker = "o", color = cmap(i) )
+
+ # plt.subplot(2,4,6)
+ # plt.plot(T,VY, color = cmap(i) )
+ # plt.plot(T[0], VY[0],marker = "o", color = cmap(i) )
+
+ # plt.subplot(2,4,7)
+ # plt.plot(T,AY, color = cmap(i) )
+ # plt.plot(T[0], AY[0],marker = "o", color = cmap(i) )
-############
-# 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])
+ # plt.subplot(2,4,8)
+ # plt.plot(T,JY, color = cmap(i) )
+ # plt.plot(T[0], JY[0],marker = "o", color = cmap(i) )
-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
+ iteration_mpc += 1
+t_init = k_0*params.dt_wbc
+t_end = k_0*params.dt_wbc + N_mpc*params.dt_mpc
+T = np.linspace(t_init, t_end - params.dt_wbc , int( (t_end - t_init)/params.dt_wbc ) + 1)
-###########
-# 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.initialize_models(params) # re-initialize the model list with the new gains
+ax_f[0,0].plot(T, planner_goals[k_0:k_end, 0,foot], linewidth=2, color="b" )
+ax_f[0,0].set(xlabel=lgd[0])
+ax_f[0,1].plot(T, planner_vgoals[k_0:k_end, 0,foot], linewidth=2, color="b" )
+ax_f[0,1].set(xlabel=lgd[1])
+ax_f[0,2].plot(T, planner_agoals[k_0:k_end, 0,foot], linewidth=2, color="b" )
+ax_f[0,2].set(xlabel=lgd[2])
+ax_f[0,3].plot(T, planner_jgoals[k_0:k_end, 0,foot], linewidth=2, color="b" )
+ax_f[0,3].set(xlabel=lgd[3])
-mpc_ddp.gait = planner_gait[k_previous].copy() # gait_old will be initialised with that
-print(mpc_ddp.gait)
-mpc_ddp.updateProblem(k , planner_xref[k] , planner_fsteps[k], planner_goals[k])
+ax_f[1,0].plot(T, planner_goals[k_0:k_end, 1,foot], linewidth=2, color="b" )
+ax_f[1,0].set(xlabel=lgd[4])
+ax_f[1,1].plot(T, planner_vgoals[k_0:k_end, 1,foot], linewidth=2, color="b" )
+ax_f[1,1].set(xlabel=lgd[5])
+ax_f[1,2].plot(T, planner_agoals[k_0:k_end, 1,foot], linewidth=2, color="b" )
+ax_f[1,2].set(xlabel=lgd[6])
+ax_f[1,3].plot(T, planner_jgoals[k_0:k_end, 1,foot], linewidth=2, color="b" )
+ax_f[1,3].set(xlabel=lgd[7])
-mpc_ddp.ddp.solve(mpc_ddp.x_init, mpc_ddp.u_init, mpc_ddp.max_iteration)
-oRh = np.eye(3)
-oTh = np.zeros((3,1))
-ddp_xs = mpc_ddp.get_latest_result(oRh, oTh)[: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(oRh, oTh)[12:,:] # Forces computed over the whole predicted horizon
-ddp_fsteps = mpc_ddp.get_latest_result(oRh, oTh)[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"]
+
+
+
+
+iteration_mpc = iteration_init
+t_0 = k_0*params.dt_wbc
+N = mpc_x_f.shape[2]
+
+l_str = ["Position X", "Position Y", "Position Z",
+ "Position Roll", "Position Pitch", "Position Yaw",
+ "Linear velocity X", "Linear velocity Y", "Linear velocity Z",
+ "Angular velocity Roll", "Angular velocity Pitch", "Angular velocity Yaw"]
+
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])
+
+# plt.figure()
+# for i in range(N_mpc):
+# k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) - 1# simulation iteration corresponding
+# xref = planner_xref[k].copy() # From logged value
+# xs = mpc_x_f[k,:12,:]
+# xs = np.vstack([xref[:,0] , xs.transpose()]).transpose()
+
+# t_init = t_0 + i*params.dt_mpc
+# t_end = t_init + N*params.dt_mpc
+# T = np.linspace(t_init, t_end,N+1)
+
+# for j in range(12):
+# plt.subplot(3,4,index[j])
+# plt.plot(T,xs[j,:])
- pl1, = plt.plot(l_t, ddp_xs[i,:], linewidth=2, marker='x')
- pl2, = plt.plot(l_t, osqp_xs[i,:], linewidth=2, marker='x')
+# iteration_mpc +=1
+
+
+# for j in range(12):
+# plt.subplot(3,4,index[j])
+# plt.xlabel(l_str[j])
+
+iteration_mpc = iteration_init
+t_0 = k_0*params.dt_wbc
+N = mpc_x_f.shape[2]
+
+data_xs_mpc = np.zeros((12,25, N_mpc))
+iteration_mpc = iteration_init
+# Extract state prediction
+for i in range(N_mpc):
+ k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) - 1 # simulation iteration corresponding
+ xref = planner_xref[k].copy() # From logged value
+ xs = mpc_x_f[k,:12,:]
+ xs = np.vstack([xref[:,0] , xs.transpose()]).transpose()
+ data_xs_mpc[:,:,i] = xs
+ iteration_mpc +=1
+
- 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]
+
+fig_x, ax_x = plt.subplots(3,4)
+com_offset = 0.03
+# For each states
+for i in range(12):
+ # Extract state prediction
+ xs_i = data_xs_mpc[i,:,:]
+ if i == 2 :
+ xs_i[1:,:] += com_offset
+ # For each planning step in the trajectory
+ for j in range(N_mpc):
+ # Receding horizon = [j,j+N_h]
+ t0_horizon = t_init + j*params.dt_mpc
+ t_end = t0_horizon + N*params.dt_mpc
+ tspan_x_pred = np.linspace(t0_horizon, t_end,N+1)
+
+ # Set up lists of (x,y) points for predicted state i
+ points_j = np.array([tspan_x_pred, xs_i[:,j]]).transpose().reshape(-1,1,2)
+
+ # Set up lists of segments
+ segs_j = np.concatenate([points_j[:-1], points_j[1:]], axis=1)
+
+ # Make collections segments
+ cm = plt.get_cmap('Greys_r')
+ lc_q = LineCollection(segs_j, cmap=cm, zorder=-1)
+
+ lc_q.set_array(tspan_x_pred)
+
+ # Customize
+ lc_q.set_linestyle('-')
+
+ # Scatter to highlight points
+ colors = np.r_[np.linspace(0.1, 1, N), 1]
+ my_colors = cm(colors)
+
+ if i < 3:
+ # Plot collections
+ ax_x[i,0].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_x[i,0].scatter(tspan_x_pred, xs_i[:,j], s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+ elif i < 6:
+ # Plot collections
+ ax_x[i-3,1].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_x[i-3,1].scatter(tspan_x_pred, xs_i[:,j], s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+ elif i < 9:
+ # Plot collections
+ ax_x[i-6,2].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_x[i-6,2].scatter(tspan_x_pred, xs_i[:,j], s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+ else:
+ # Plot collections
+ ax_x[i-9,3].add_collection(lc_q)
+ # Scatter to highlight points
+ ax_x[i-9,3].scatter(tspan_x_pred, xs_i[:,j], s=10, zorder=1, c=my_colors, cmap=matplotlib.cm.Greys)
+
+
+t_init = k_0*params.dt_wbc
+t_end = k_0*params.dt_wbc + N_mpc*params.dt_mpc
+k_end = int(k_0 + N_mpc * params.dt_mpc / params.dt_wbc)
+T = np.linspace(t_init, t_end - params.dt_wbc , int( (t_end - t_init)/params.dt_wbc ) + 1)
+
+
+####
+# Measured & Reference position and orientation (ideal world frame)
+####
+
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" ])
+ if i < 2 :
+ pass
+ elif i < 3:
+ ax_x[i,0].plot(T, planner_xref[k_0:k_end, i, 0], "b", linewidth=3)
+ ax_x[i,0].legend(["Robot state"], prop={'size': 8})
+ ax_x[i,0].set(xlabel=l_str[i])
+ elif i < 6:
+ ax_x[i-3,1].plot(T, planner_xref[k_0:k_end, i, 0], "b", linewidth=3)
+ ax_x[i-3,1].legend(["Robot state"], prop={'size': 8})
+ ax_x[i-3,1].set(xlabel=l_str[i])
+ elif i < 9:
+ ax_x[i-6,2].plot(T, planner_xref[k_0:k_end, i, 0], "b", linewidth=3)
+ ax_x[i-6,2].legend(["Robot state"], prop={'size': 8})
+ ax_x[i-6,2].set(xlabel=l_str[i])
+ else:
+ ax_x[i-9,3].plot(T, planner_xref[k_0:k_end, i, 0], "b", linewidth=3)
+ ax_x[i-9,3].legend(["Robot state"], prop={'size': 8})
+ ax_x[i-9,3].set(xlabel=l_str[i])
- 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)
+
+
+
+
+
+
+
+
+
+plt.show()
+
+
+
+
+# T = np.linspace()
+# for it in range(N_mpc):
+
+
+# k_0 = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) # simulation iteration corresponding
+# gait = planner_gait[k_0]
diff --git a/scripts/crocoddyl_eval/test_6/compare_mpcs.py b/scripts/crocoddyl_eval/test_6/compare_mpcs.py
index a9a68a3156ded2ad8564f74535c1c582193ce983..01743216c262df9312a12e7529d6043810394f2e 100644
--- a/scripts/crocoddyl_eval/test_6/compare_mpcs.py
+++ b/scripts/crocoddyl_eval/test_6/compare_mpcs.py
@@ -88,7 +88,6 @@ for j in range(4):
for t in range(N):
if np.isnan(data_[t,i,j]):
data_[t,i,j] = data_[t-1,i,j]
-
##########
# PLOTS
##########
@@ -166,7 +165,7 @@ for i in range(6):
for j in range(4):
if MPCs[j]:
- plt.plot(t_range, data_diff[:,index_error[i],j], color[j], linewidth=3)
+ plt.plot(t_range, abs(data_diff[:,index_error[i],j]), color[j], linewidth=3)
# Add mean on graph
# for i in range(6):
@@ -177,7 +176,7 @@ for i in range(6):
plt.legend(legend, prop={'size': 8})
plt.ylabel(lgd[i])
-plt.suptitle("Error wrt reference state")
+plt.suptitle("Absolute Error wrt reference state")
plt.figure()
for i in range(6):
@@ -212,7 +211,7 @@ bars = []
bars_names = ["Lin", "NL", "Plan", "OSQP"]
for j in range(4):
if MPCs[j]:
- bars.append(bars_names[j])
+ bars.append(MPCs_names[j])
plt.figure()
for i in range(6):
@@ -256,30 +255,30 @@ plt.suptitle("NORMALIZED RMSE -MEAN: sqrt( (mes - ref - mean(mes-ref)) **2).me
####
# FF torques & FB torques & Sent torques & Meas torques
####
-index12 = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
-lgd1 = ["HAA", "HFE", "Knee"]
-lgd2 = ["FL", "FR", "HL", "HR"]
-plt.figure()
-my_axs = []
-for i in range(12):
- if i == 0:
- ax = plt.subplot(3, 4, index12[i])
- my_axs.append(ax)
- elif i in [1, 2]:
- ax = plt.subplot(3, 4, index12[i], sharex=my_axs[0])
- my_axs.append(ax)
- else:
- plt.subplot(3, 4, index12[i], sharex=my_axs[0], sharey=my_axs[int(i % 3)])
+# index12 = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
+# lgd1 = ["HAA", "HFE", "Knee"]
+# lgd2 = ["FL", "FR", "HL", "HR"]
+# plt.figure()
+# my_axs = []
+# for i in range(12):
+# if i == 0:
+# ax = plt.subplot(3, 4, index12[i])
+# my_axs.append(ax)
+# elif i in [1, 2]:
+# ax = plt.subplot(3, 4, index12[i], sharex=my_axs[0])
+# my_axs.append(ax)
+# else:
+# plt.subplot(3, 4, index12[i], sharex=my_axs[0], sharey=my_axs[int(i % 3)])
- for j in range(4):
- if MPCs[j]:
- plt.plot(t_range, tau_ff_[:,i,j], color[j], linewidth=3)
+# for j in range(4):
+# if MPCs[j]:
+# plt.plot(t_range, tau_ff_[:,i,j], color[j], linewidth=3)
- plt.xlabel("Time [s]")
- plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [Nm]")
- tmp = lgd1[i % 3]+" "+lgd2[int(i/3)]
- plt.legend(legend, prop={'size': 8})
- plt.ylim([-8.0, 8.0])
+# plt.xlabel("Time [s]")
+# plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [Nm]")
+# tmp = lgd1[i % 3]+" "+lgd2[int(i/3)]
+# plt.legend(legend, prop={'size': 8})
+# plt.ylim([-8.0, 8.0])
####
# Contact forces (MPC command) & WBC QP output
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index 299ca04a1af0ee6a6fbdf1d882a30187cd81143e..1ad0c5b5bc69582425244d0bd88d4084cf0b26c2 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -17,6 +17,7 @@ FootTrajectoryGenerator::FootTrajectoryGenerator()
position_(Matrix34::Zero()),
velocity_(Matrix34::Zero()),
acceleration_(Matrix34::Zero()),
+ jerk_(Matrix34::Zero()),
position_base_(Matrix34::Zero()),
velocity_base_(Matrix34::Zero()),
acceleration_base_(Matrix34::Zero()) {}
@@ -152,6 +153,8 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
velocity_(1, j) = 0.0;
acceleration_(0, j) = 0.0;
acceleration_(1, j) = 0.0;
+ jerk_(0, j) = 0.0;
+ jerk_(1, j) = 0.0;
} else {
position_(0, j) = Ax(5, j) + Ax(4, j) * ev + Ax(3, j) * std::pow(ev, 2) + Ax(2, j) * std::pow(ev, 3) +
Ax(1, j) * std::pow(ev, 4) + Ax(0, j) * std::pow(ev, 5);
@@ -165,11 +168,15 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
2 * Ax(3, j) + 3 * 2 * Ax(2, j) * ev + 4 * 3 * Ax(1, j) * std::pow(ev, 2) + 5 * 4 * Ax(0, j) * std::pow(ev, 3);
acceleration_(1, j) =
2 * Ay(3, j) + 3 * 2 * Ay(2, j) * ev + 4 * 3 * Ay(1, j) * std::pow(ev, 2) + 5 * 4 * Ay(0, j) * std::pow(ev, 3);
+
+ jerk_(0, j) = 3 * 2 * Ax(2, j) + 4 * 3 * 2 * Ax(1, j) * ev + 5 * 4 * 3 * Ax(0, j) * std::pow(ev, 2);
+ jerk_(1, j) = 3 * 2 * Ay(2, j) + 4 * 3 * 2 * Ay(1, j) * ev + 5 * 4 * 3 * Ay(0, j) * std::pow(ev, 2);
}
velocity_(2, j) = 3 * Az(3, j) * std::pow(evz, 2) + 4 * Az(2, j) * std::pow(evz, 3) +
5 * Az(1, j) * std::pow(evz, 4) + 6 * Az(0, j) * std::pow(evz, 5);
acceleration_(2, j) = 2 * 3 * Az(3, j) * evz + 3 * 4 * Az(2, j) * std::pow(evz, 2) +
4 * 5 * Az(1, j) * std::pow(evz, 3) + 5 * 6 * Az(0, j) * std::pow(evz, 4);
+ jerk_(2, j) = 2 * 3 * Az(3, j) + 3 * 4 * 2 * Az(2, j) * evz + 4 * 5 * 3 * Az(1, j) * std::pow(evz, 2) + 5 * 6 * 4 * Az(0, j) * std::pow(evz, 3);
position_(2, j) = Az(3, j) * std::pow(evz, 3) + Az(2, j) * std::pow(evz, 4) + Az(1, j) * std::pow(evz, 5) +
Az(0, j) * std::pow(evz, 6);
}