From db79e1621b28d567102c2037a41e1d9503c0765e Mon Sep 17 00:00:00 2001
From: thomascbrs <thomas.corberes@laposte.net>
Date: Wed, 18 Aug 2021 17:24:37 +0100
Subject: [PATCH] New set of gains for MPCs (for h_ref), new velocities tests
in joystick, scripts to eval RMSE, small fix in control cycle eval scripts
---
scripts/Joystick.py | 77 ++--
scripts/crocoddyl_class/MPC_crocoddyl.py | 8 +-
.../crocoddyl_class/MPC_crocoddyl_planner.py | 14 +-
.../test_1/analyse_control_cycle.py | 18 +-
.../test_3/analyse_control_cycle.py | 25 +-
.../crocoddyl_eval/test_6/analyse_slalom.py | 338 ++++++++++++++++++
6 files changed, 416 insertions(+), 64 deletions(-)
create mode 100644 scripts/crocoddyl_eval/test_6/analyse_slalom.py
diff --git a/scripts/Joystick.py b/scripts/Joystick.py
index f12592fd..32fec77b 100644
--- a/scripts/Joystick.py
+++ b/scripts/Joystick.py
@@ -201,46 +201,45 @@ class Joystick:
0.0, 0.0, 0.0, 0.0, R_max, R_max, 0.0, 0.0,
-R_max, 0.0])
elif velID == 2:
- self.t_switch = np.array([0, 5, 8, 10, 12])
- self.v_switch = np.array([[0.0, 0.8, 0.8, 0.8, 0.8 ],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0., 1., 0., -1. ]])
- elif velID == 3:
- self.t_switch = np.array([0, 2, 6, 8, 12, 60])
- self.v_switch = np.array([[0.0, 0.0, 0.4, 0.4, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]])
- elif velID == 4:
- self.t_switch = np.array([0, 2, 6, 14, 18, 60])
- self.v_switch = np.array([[0.0, 0.0, 1.5, 1.5, 1.5, 1.5],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.4, 0.4]])
- elif velID == 5:
- self.t_switch = np.array([0, 1, 3, 5.2, 10, 13, 14, 16, 18])
- self.v_switch = np.array([[0.0, 0.0, 0.5, 0.6, 0.3, 0.6, -0.5, 0.7, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.2, 0.7, 0.7, 0.0, -0.4, -0.6, 0.0]])
+ self.t_switch = np.array([0, 3, 4, 5, 10,15])
+ self.v_switch = np.array([[0.0, 0.6, 0.6, 0.6, 0., 0. ],
+ [0.0, 0.0, 0.0, 0.0, 0.5, 0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0., 0.4, -0.4, 0. ,0.]])
+ elif velID == 3: # PLANNER > NL > LIN
+ self.t_switch = np.array([0, 3, 4, 5, 10,15])
+ self.v_switch = np.array([[0.0, 0., 0., 0., 0., 0. ],
+ [0.0, 0.0, 0.0, 0.0, 0., 0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.8, 1.5, 1.8, 2.5 ,2.8]])
+ elif velID == 4: # PLANNER > NL == LIN
+ self.t_switch = np.array([0, 3, 4, 5, 10,15])
+ self.v_switch = np.array([[0.0, 0.4, 0.6, 0.6, 0., 0. ],
+ [0.0, 0.4, 0.6, 0.6, 0.6 ,0.6],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.5, 0.5 ,0.5]])
+ elif velID == 5: # PLANNER GOOD ROLL / PITCH
+ self.t_switch = np.array([0, 3, 4, 5, 10,15])
+ self.v_switch = np.array([[0.0, 0.4, 0.4, 0.4, 0.4, 0. ],
+ [0.0, 0.4, 0.4, 0.4, 0. ,0.4],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.7, 0.7 ,0.7]])
elif velID == 6:
- self.t_switch = np.array([0, 2, 5, 10, 15, 16, 20])
- self.v_switch = np.array([[0.0, 0.0, 0.8, 0.4, 0.8, 0.8, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.55, 0.3, 0.0, 0.0]])
-
+ self.t_switch = np.array([0, 3, 4, 5, 10,15])
+ self.v_switch = np.array([[0.0, 0.4, 0.5, 0.6, 0.6, 0.6 ],
+ [0.0, 0., 0., 0., 0. ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0.0, 0.0 ,0.],
+ [0.0, 0.0, 0.0, 0., 0. ,0.]])
self.k_switch = (self.t_switch / self.dt_wbc).astype(int)
self.handle_v_switch(k_loop)
return 0
diff --git a/scripts/crocoddyl_class/MPC_crocoddyl.py b/scripts/crocoddyl_class/MPC_crocoddyl.py
index 346a2f49..c478763b 100644
--- a/scripts/crocoddyl_class/MPC_crocoddyl.py
+++ b/scripts/crocoddyl_class/MPC_crocoddyl.py
@@ -35,7 +35,7 @@ class MPC_crocoddyl:
# Weights on the state vector
self.w_x = 0.3
self.w_y = 0.3
- self.w_z = 2
+ self.w_z = 20
self.w_roll = 0.9
self.w_pitch = 1.
self.w_yaw = 0.4
@@ -44,20 +44,20 @@ class MPC_crocoddyl:
self.w_vz = 2*np.sqrt(self.w_z)
self.w_vroll = 0.05*np.sqrt(self.w_roll)
self.w_vpitch = 0.07*np.sqrt(self.w_pitch)
- self.w_vyaw = 0.05*np.sqrt(self.w_yaw)
+ self.w_vyaw = 0.08*np.sqrt(self.w_yaw)
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 = np.array(4*[0.007, 0.007, 0.007]) # Weight Vector : Force Norm
+ self.forceWeights = 1*np.array(4*[0.007, 0.007, 0.007]) # Weight Vector : Force Norm
self.frictionWeights = 1.0 # Weight Vector : Friction cone cost
self.min_fz = 0.2 # Minimum normal force (N)
self.max_fz = 25 # Maximum normal force (N)
self.shoulderWeights = 5. # Weight on the shoulder term :
- self.shoulder_hlim = 0.23 # shoulder maximum height
+ self.shoulder_hlim = 0.235 # shoulder maximum height
# Integration scheme
self.implicit_integration = True
diff --git a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
index de68d695..8356cb8a 100644
--- a/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
+++ b/scripts/crocoddyl_class/MPC_crocoddyl_planner.py
@@ -31,29 +31,29 @@ class MPC_crocoddyl_planner():
else:
self.mu = mu
- # self.stateWeight = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 0.25, 0.2, 0.2, 5., 0.0, 0.0, 0.3])
+ # self.stateWeight = np.sqrt([0., 0., 20.0, 1., 1., 0., 0.5, 0.5, 0., 0.0, 0.0, 0.5])
# Weights Vector : States
self.w_x = 0.3
self.w_y = 0.3
- self.w_z = 2
+ self.w_z = 20
self.w_roll = 0.9
self.w_pitch = 1.
- self.w_yaw = 0.4
+ self.w_yaw = 0.5
self.w_vx = 1.5*np.sqrt(self.w_x)
self.w_vy = 2*np.sqrt(self.w_y)
self.w_vz = 1*np.sqrt(self.w_z)
self.w_vroll = 0.05*np.sqrt(self.w_roll)
self.w_vpitch = 0.07*np.sqrt(self.w_pitch)
- self.w_vyaw = 0.05*np.sqrt(self.w_yaw)
+ self.w_vyaw = 0.08*np.sqrt(self.w_yaw)
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 = 2*np.array(4*[0.01, 0.01, 0.01]) # Weight Vector : Force Norm
+ 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 = np.array(4*[0.3, 0.4]) # Weights on the heuristic term
+ self.heuristicWeights = np.array(4*[0.03, 0.04]) # Weights on the heuristic term
self.stepWeights = np.full(8, 0.005) # Weight on the step command (distance between steps)
- self.stopWeights = 2.5*np.ones(8) # Weights to stop the optimisation at the end of the flying phase
+ self.stopWeights = 2.*np.ones(8) # Weights to stop the optimisation at the end of the flying phase
self.shoulderContactWeight = 5. # Weight for shoulder-to-contact penalty
self.shoulder_hlim = 0.235
diff --git a/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py b/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
index 8068739f..1a551658 100644
--- a/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
+++ b/scripts/crocoddyl_eval/test_1/analyse_control_cycle.py
@@ -15,7 +15,7 @@ import utils_mpc
##############
# Parameters
##############
-iteration_mpc = 250 # Control cycle
+iteration_mpc = 2 # 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 +29,7 @@ solo = utils_mpc.init_robot(q_init, params)
######################
# Recover Logged data
######################
-file_name = "crocoddyl_eval/logs/new_controller_test1.npz"
+file_name = "crocoddyl_eval/logs/chute_mpc_nl_noShoulder.npz"
logs = np.load(file_name)
planner_gait = logs.get("planner_gait")
planner_xref = logs.get("planner_xref")
@@ -42,14 +42,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=True)
+mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=False)
# Without warm-start :
# mpc_ddp.warm_start = False
# mpc_ddp.solve(k, planner_xref[k] , planner_fsteps[k] ) # Without warm-start
@@ -90,7 +90,8 @@ ddp_us = mpc_ddp.get_latest_result()[12:,:] # Forces computed over the whole pre
######################################
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.implicit_integration = False
+mpc_ddp_2.shoulderWeights = 0.
# Weight Vector : State
# w_x = 0.2
# w_y = 0.2
@@ -107,7 +108,7 @@ mpc_ddp_2 = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False , linearMod
# 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_2.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])
+# mpc_ddp_2.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_2.mu = 0.9
@@ -136,8 +137,9 @@ mpc_ddp_2.stateWeight = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2
# mpc_ddp_2.relative_forces = False
# Update weights and params inside the models
-# mpc_ddp_2.updateActionModel()
-
+mpc_ddp_2.updateActionModels()
+mpc_ddp_2.problem = crocoddyl.ShootingProblem(np.zeros(12), mpc_ddp_2.ListAction, mpc_ddp_2.terminalModel)
+mpc_ddp_2.ddp = crocoddyl.SolverDDP(mpc_ddp_2.problem)
# Run ddp solver
# Update the dynamic depending on the predicted feet position
if Relaunch_DDP :
diff --git a/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py b/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
index 9dbcac1b..a3d77924 100644
--- a/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
+++ b/scripts/crocoddyl_eval/test_3/analyse_control_cycle.py
@@ -10,19 +10,27 @@ import matplotlib.pylab as plt
import libquadruped_reactive_walking as lqrw
import crocoddyl_class.MPC_crocoddyl_planner as MPC_crocoddyl_planner
import time
+import utils_mpc
+
##############
# Parameters
##############
-iteration_mpc = 62 # Control cycle
+iteration_mpc = 45 # Control cycle
Relaunch_DDP = False # Compare a third MPC with != parameters
linear_mpc = True
params = lqrw.Params() # Object that holds all controller parameters
+# Default position after calibration
+q_init = np.array(params.q_init.tolist())
+
+# Update I_mat, etc...
+solo = utils_mpc.init_robot(q_init, params)
+
######################
# Recover Logged data
######################
-file_name = "crocoddyl_eval/logs/data_2021_07_12_09_25.npz"
+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")
@@ -55,17 +63,22 @@ mpc_ddp = MPC_crocoddyl_planner.MPC_crocoddyl_planner(params, mu=0.9, inner=Fals
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.initialize_models(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
+
+print(mpc_ddp.gait)
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
+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()[12:,:] # Forces computed over the whole predicted horizon
-ddp_fsteps = mpc_ddp.get_latest_result()[24:,:]
+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
diff --git a/scripts/crocoddyl_eval/test_6/analyse_slalom.py b/scripts/crocoddyl_eval/test_6/analyse_slalom.py
new file mode 100644
index 00000000..bc47ff5a
--- /dev/null
+++ b/scripts/crocoddyl_eval/test_6/analyse_slalom.py
@@ -0,0 +1,338 @@
+# 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 pinocchio as pin
+import libquadruped_reactive_walking as lqrw
+
+
+######################
+# Recover Logged data, mpc lin
+######################
+file_name = "crocoddyl_eval/logs/vel6/data_lin_vel6.npz"
+logs = np.load(file_name)
+
+
+# Common data
+
+joy_v_ref = logs.get('joy_v_ref')
+planner_xref = logs.get("planner_xref")
+
+
+
+mocapPosition = logs.get("mocapPosition")
+mocapOrientationQuat = logs.get("mocapOrientationQuat")
+mocapOrientationMat9 = logs.get("mocapOrientationMat9")
+mocapVelocity = logs.get("mocapVelocity")
+mocapAngularVelocity = logs.get('mocapAngularVelocity')
+
+params = lqrw.Params() # Object that holds all controller parameters
+
+
+N = mocapPosition.shape[0]
+
+mocap_pos_mpc_lin = np.zeros([N, 3])
+mocap_h_v_mpc_lin = np.zeros([N, 3])
+mocap_b_w_mpc_lin = np.zeros([N, 3])
+mocap_RPY_mpc_lin = np.zeros([N, 3])
+
+for i in range(N):
+ mocap_RPY_mpc_lin[i] = pin.rpy.matrixToRpy(pin.Quaternion(mocapOrientationQuat[i]).toRotationMatrix())
+
+# Robot world to Mocap initial translationa and rotation
+mTo = np.array([mocapPosition[0, 0], mocapPosition[0, 1], 0.02])
+mRo = pin.rpy.rpyToMatrix(0.0, 0.0, mocap_RPY_mpc_lin[0, 2])
+
+for i in range(N):
+ oRb = mocapOrientationMat9[i]
+
+ oRh = pin.rpy.rpyToMatrix(0.0, 0.0, mocap_RPY_mpc_lin[i, 2] - mocap_RPY_mpc_lin[0, 2])
+
+ mocap_h_v_mpc_lin[i] = (oRh.transpose() @ mRo.transpose() @ mocapVelocity[i].reshape((3, 1))).ravel()
+ mocap_b_w_mpc_lin[i] = (oRb.transpose() @ mocapAngularVelocity[i].reshape((3, 1))).ravel()
+ mocap_pos_mpc_lin[i] = (mRo.transpose() @ (mocapPosition[i, :] - mTo).reshape((3, 1))).ravel()
+
+
+######################
+# Recover Logged data, mpc non linear
+######################
+file_name = "crocoddyl_eval/logs/vel6/data_nl_vel6.npz"
+logs = np.load(file_name)
+
+mocapPosition = logs.get("mocapPosition")
+mocapOrientationQuat = logs.get("mocapOrientationQuat")
+mocapOrientationMat9 = logs.get("mocapOrientationMat9")
+mocapVelocity = logs.get("mocapVelocity")
+mocapAngularVelocity = logs.get('mocapAngularVelocity')
+
+params = lqrw.Params() # Object that holds all controller parameters
+
+
+N = mocapPosition.shape[0]
+
+mocap_pos_mpc_nl = np.zeros([N, 3])
+mocap_h_v_mpc_nl = np.zeros([N, 3])
+mocap_b_w_mpc_nl = np.zeros([N, 3])
+mocap_RPY_mpc_nl = np.zeros([N, 3])
+
+for i in range(N):
+ mocap_RPY_mpc_nl[i] = pin.rpy.matrixToRpy(pin.Quaternion(mocapOrientationQuat[i]).toRotationMatrix())
+
+# Robot world to Mocap initial translationa and rotation
+mTo = np.array([mocapPosition[0, 0], mocapPosition[0, 1], 0.02])
+mRo = pin.rpy.rpyToMatrix(0.0, 0.0, mocap_RPY_mpc_nl[0, 2])
+
+for i in range(N):
+ oRb = mocapOrientationMat9[i]
+
+ oRh = pin.rpy.rpyToMatrix(0.0, 0.0, mocap_RPY_mpc_nl[i, 2] - mocap_RPY_mpc_nl[0, 2])
+
+ mocap_h_v_mpc_nl[i] = (oRh.transpose() @ mRo.transpose() @ mocapVelocity[i].reshape((3, 1))).ravel()
+ mocap_b_w_mpc_nl[i] = (oRb.transpose() @ mocapAngularVelocity[i].reshape((3, 1))).ravel()
+ mocap_pos_mpc_nl[i] = (mRo.transpose() @ (mocapPosition[i, :] - mTo).reshape((3, 1))).ravel()
+
+
+######################
+# Recover Logged data, mpc planner
+######################
+file_name = "crocoddyl_eval/logs/vel6/data_planner_vel6.npz"
+logs = np.load(file_name)
+
+mocapPosition = logs.get("mocapPosition")
+mocapOrientationQuat = logs.get("mocapOrientationQuat")
+mocapOrientationMat9 = logs.get("mocapOrientationMat9")
+mocapVelocity = logs.get("mocapVelocity")
+mocapAngularVelocity = logs.get('mocapAngularVelocity')
+
+params = lqrw.Params() # Object that holds all controller parameters
+
+
+N = mocapPosition.shape[0]
+
+mocap_pos_mpc_planner = np.zeros([N, 3])
+mocap_h_v_mpc_planner = np.zeros([N, 3])
+mocap_b_w_mpc_planner = np.zeros([N, 3])
+mocap_RPY_mpc_planner = np.zeros([N, 3])
+
+for i in range(N):
+ mocap_RPY_mpc_planner[i] = pin.rpy.matrixToRpy(pin.Quaternion(mocapOrientationQuat[i]).toRotationMatrix())
+
+# Robot world to Mocap initial translationa and rotation
+mTo = np.array([mocapPosition[0, 0], mocapPosition[0, 1], 0.02])
+mRo = pin.rpy.rpyToMatrix(0.0, 0.0, mocap_RPY_mpc_planner[0, 2])
+
+for i in range(N):
+ oRb = mocapOrientationMat9[i]
+
+ oRh = pin.rpy.rpyToMatrix(0.0, 0.0, mocap_RPY_mpc_planner[i, 2] - mocap_RPY_mpc_planner[0, 2])
+
+ mocap_h_v_mpc_planner[i] = (oRh.transpose() @ mRo.transpose() @ mocapVelocity[i].reshape((3, 1))).ravel()
+ mocap_b_w_mpc_planner[i] = (oRb.transpose() @ mocapAngularVelocity[i].reshape((3, 1))).ravel()
+ mocap_pos_mpc_planner[i] = (mRo.transpose() @ (mocapPosition[i, :] - mTo).reshape((3, 1))).ravel()
+
+
+
+lgd = ["Position X", "Position Y", "Position Z", "Position Roll", "Position Pitch", "Position Yaw"]
+index6 = [1, 3, 5, 2, 4, 6]
+t_range = np.array([k*params.dt_wbc for k in range(N)])
+
+plt.figure()
+for i in range(6):
+ if i == 0:
+ ax0 = plt.subplot(3, 2, index6[i])
+ else:
+ plt.subplot(3, 2, index6[i], sharex=ax0)
+
+ if i < 3:
+ plt.plot(t_range, mocap_pos_mpc_lin[:, i], "k", linewidth=3)
+ plt.plot(t_range, mocap_pos_mpc_nl[:, i], "b", linewidth=3)
+ plt.plot(t_range, mocap_pos_mpc_planner[:, i], "r", linewidth=3)
+ else:
+ plt.plot(t_range, mocap_RPY_mpc_lin[:, i-3], "k", linewidth=3)
+ plt.plot(t_range, mocap_RPY_mpc_nl[:, i-3], "b", linewidth=3)
+ plt.plot(t_range, mocap_RPY_mpc_planner[:, i-3], "r", linewidth=3)
+
+ plt.legend(["MOCAP LIN", "MOCAP NL", "MOCAP PLANNER"], prop={'size': 8})
+ plt.ylabel(lgd[i])
+plt.suptitle("Measured & Reference position and orientation")
+
+lgd = ["Linear vel X", "Linear vel Y", "Linear vel Z",
+ "Andiff_pos_lingular vel Roll", "Angular vel Pitch", "Angular vel Yaw"]
+plt.figure()
+for i in range(6):
+ if i == 0:
+ ax0 = plt.subplot(3, 2, index6[i])
+ else:
+ plt.subplot(3, 2, index6[i], sharex=ax0)
+
+ if i < 3:
+ plt.plot(t_range, mocap_h_v_mpc_lin[:, i], "k", linewidth=3)
+ plt.plot(t_range, mocap_h_v_mpc_nl[:, i], "b", linewidth=3)
+ plt.plot(t_range, mocap_h_v_mpc_planner[:, i], "r", linewidth=3)
+ else:
+ plt.plot(t_range, mocap_b_w_mpc_lin[:, i-3], "k", linewidth=3)
+ plt.plot(t_range, mocap_b_w_mpc_nl[:, i-3], "b", linewidth=3)
+ plt.plot(t_range, mocap_b_w_mpc_planner[:, i-3], "r", linewidth=3)
+
+ """N = 2000
+ y = np.convolve(self.mocap_b_w[:, i-3], np.ones(N)/N, mode='valid')
+ plt.plot(t_range[int(N/2)-1:-int(N/2)], y, linewidth=3, linestyle="--")"""
+
+ # plt.plot(t_range, self.log_dq[i, :], "g", linewidth=2)
+ # plt.plot(t_range[:-2], self.log_dx_invkin[i, :-2], "g", linewidth=2)
+ # plt.plot(t_range[:-2], self.log_dx_ref_invkin[i, :-2], "violet", linewidth=2, linestyle="--")
+ plt.legend(["MOCAP LIN", "MOCAP NL", "MOCAP PLANNER"], prop={'size': 8})
+ plt.ylabel(lgd[i])
+plt.suptitle("Measured & Reference linear and angular velocities")
+
+
+diff_vel_lin = abs(mocap_h_v_mpc_lin[:,:] - joy_v_ref[:,:3])
+diff_vel_nl = abs(mocap_h_v_mpc_nl[:,:] - joy_v_ref[:,:3])
+diff_vel_planner = abs(mocap_h_v_mpc_planner[:,:] - joy_v_ref[:,:3])
+
+diff_ang_lin = abs(mocap_b_w_mpc_lin[:,:] - joy_v_ref[:,3:6])
+diff_ang_nl = abs(mocap_b_w_mpc_nl[:,:] - joy_v_ref[:,3:6])
+diff_ang_planner = abs(mocap_b_w_mpc_planner[:,:] - joy_v_ref[:,3:6])
+
+diff_rpy_lin = abs(mocap_RPY_mpc_lin[:,:] - planner_xref[:, 3:6, 1] )
+diff_rpy_nl = abs(mocap_RPY_mpc_nl[:,:] - planner_xref[:, 3:6, 1] )
+diff_rpy_planner = abs(mocap_RPY_mpc_planner[:,:] - planner_xref[:, 3:6, 1] )
+
+diff_pos_lin = abs(mocap_pos_mpc_lin[:,:] - planner_xref[:, :3, 1] )
+diff_pos_nl = abs(mocap_pos_mpc_nl[:,:] - planner_xref[:, :3, 1] )
+diff_pos_planner = abs(mocap_pos_mpc_planner[:,:] - planner_xref[:, :3, 1] )
+
+# print('RMSE Vx [Lin, Nl, PLanner]: ', np.linalg.norm(diff_vel_lin))
+
+lgd = ["Linear vel X", "Linear vel Y", "POsition Z",
+ "Position Roll", "Position Pitch", "Ang vel Yaw"]
+plt.figure()
+for i in range(6):
+ if i == 0:
+ ax0 = plt.subplot(3, 2, index6[i])
+ else:
+ plt.subplot(3, 2, index6[i], sharex=ax0)
+
+ if i < 2:
+ plt.plot(t_range, diff_vel_lin[:, i], "k", linewidth=3)
+ plt.plot(t_range, diff_vel_nl[:, i], "b", linewidth=3)
+ plt.plot(t_range, diff_vel_planner[:, i], "r", linewidth=3)
+
+ elif i == 2 :
+ plt.plot(t_range, diff_pos_lin[:, i], "k", linewidth=3)
+ plt.plot(t_range, diff_pos_nl[:, i], "b", linewidth=3)
+ plt.plot(t_range, diff_pos_planner[:, i], "r", linewidth=3)
+
+
+ elif i == 3 or i == 4:
+ plt.plot(t_range, diff_rpy_lin[:, i-3], "k", linewidth=3)
+ plt.plot(t_range, diff_rpy_nl[:, i-3], "b", linewidth=3)
+ plt.plot(t_range, diff_rpy_planner[:, i-3], "r", linewidth=3)
+
+ else :
+
+ plt.plot(t_range, diff_ang_lin[:, i-3], "k", linewidth=3)
+ plt.plot(t_range, diff_ang_nl[:, i-3], "b", linewidth=3)
+ plt.plot(t_range, diff_ang_planner[:, i-3], "r", linewidth=3)
+
+ """N = 2000
+ y = np.convolve(self.mocap_b_w[:, i-3], np.ones(N)/N, mode='valid')
+ plt.plot(t_range[int(N/2)-1:-int(N/2)], y, linewidth=3, linestyle="--")"""
+
+ # plt.plot(t_range, self.log_dq[i, :], "g", linewidth=2)
+ # plt.plot(t_range[:-2], self.log_dx_invkin[i, :-2], "g", linewidth=2)
+ # plt.plot(t_range[:-2], self.log_dx_ref_invkin[i, :-2], "violet", linewidth=2, linestyle="--")
+ plt.legend(["LIN", "NL", "PLANNER"], prop={'size': 8})
+ plt.ylabel(lgd[i])
+plt.suptitle("Measured & Reference linear and angular velocities (ABS ERROR) ")
+
+
+
+# Diff with reference
+diff_pos_lin = (mocap_pos_mpc_lin[:,:] - planner_xref[:, :3, 1] )
+diff_pos_nl = (mocap_pos_mpc_nl[:,:] - planner_xref[:, :3, 1] )
+diff_pos_planner = (mocap_pos_mpc_planner[:,:] - planner_xref[:, :3, 1] )
+
+diff_rpy_lin = (mocap_RPY_mpc_lin[:,:] - planner_xref[:, 3:6, 1] )
+diff_rpy_nl = (mocap_RPY_mpc_nl[:,:] - planner_xref[:, 3:6, 1] )
+diff_rpy_planner = (mocap_RPY_mpc_planner[:,:] - planner_xref[:, 3:6, 1] )
+
+diff_vel_lin = (mocap_h_v_mpc_lin[:,:] - joy_v_ref[:,:3])
+diff_vel_nl = (mocap_h_v_mpc_nl[:,:] - joy_v_ref[:,:3])
+diff_vel_planner = (mocap_h_v_mpc_planner[:,:] - joy_v_ref[:,:3])
+
+diff_ang_lin = (mocap_b_w_mpc_lin[:,:] - joy_v_ref[:,3:6])
+diff_ang_nl = (mocap_b_w_mpc_nl[:,:] - joy_v_ref[:,3:6])
+diff_ang_planner = (mocap_b_w_mpc_planner[:,:] - joy_v_ref[:,3:6])
+
+
+# Max measures
+max_pos_lin = abs(np.max(mocap_pos_mpc_lin, axis = 0))
+max_pos_nl = abs(np.max(mocap_pos_mpc_nl, axis = 0))
+max_pos_planner = abs(np.max(mocap_pos_mpc_planner, axis = 0))
+
+max_rpy_lin = abs(np.max(mocap_RPY_mpc_lin, axis = 0))
+max_rpy_nl = abs(np.max(mocap_RPY_mpc_nl, axis = 0))
+max_rpy_planner = abs(np.max(mocap_RPY_mpc_planner, axis = 0))
+
+max_vel_lin = abs(np.max(mocap_h_v_mpc_lin, axis = 0))
+max_vel_nl = abs(np.max(mocap_h_v_mpc_nl, axis = 0))
+max_vel_planner = abs(np.max(mocap_h_v_mpc_planner, axis = 0))
+
+max_ang_lin = abs(np.max(mocap_b_w_mpc_lin, axis = 0))
+max_ang_nl = abs(np.max(mocap_b_w_mpc_nl, axis = 0))
+max_ang_planner = abs(np.max(mocap_b_w_mpc_planner, axis = 0))
+
+# RMSE normalized
+RMSE_pos_lin = np.sqrt((diff_pos_lin**2).mean(axis=0)) / max_pos_lin
+RMSE_pos_nl = np.sqrt((diff_pos_nl**2).mean(axis=0)) / max_pos_lin
+RMSE_pos_planner = np.sqrt((diff_pos_planner**2).mean(axis=0)) / max_pos_lin
+
+RMSE_rpy_lin = np.sqrt((diff_rpy_lin**2).mean(axis=0)) / max_rpy_lin
+RMSE_rpy_nl = np.sqrt((diff_rpy_nl**2).mean(axis=0)) / max_rpy_lin
+RMSE_rpy_planner = np.sqrt((diff_rpy_planner**2).mean(axis=0)) / max_rpy_lin
+
+RMSE_vel_lin = np.sqrt((diff_vel_lin**2).mean(axis=0)) / max_vel_lin
+RMSE_vel_nl = np.sqrt((diff_vel_nl**2).mean(axis=0)) / max_vel_lin
+RMSE_vel_planner = np.sqrt((diff_vel_planner**2).mean(axis=0)) / max_vel_lin
+
+RMSE_ang_lin = np.sqrt((diff_ang_lin**2).mean(axis=0)) / max_ang_lin
+RMSE_ang_nl = np.sqrt((diff_ang_nl**2).mean(axis=0)) / max_ang_lin
+RMSE_ang_planner = np.sqrt((diff_ang_planner**2).mean(axis=0)) / max_ang_lin
+
+print("NORMALIZED RMSE : sqrt( (measures - ref**2).mean() ) / measure_max ")
+
+print('RMSE Velocity : [Vx, Vy] ')
+print(RMSE_vel_lin[:2], ' : LINEAR')
+print(RMSE_vel_nl[:2], ' : NON LINEAR')
+print(RMSE_vel_planner[:2], ' : PLANNER')
+
+print("\n\n")
+print('RMSE POSITION : [Z] ')
+print(RMSE_pos_lin[2], ' : LINEAR')
+print(RMSE_pos_nl[2], ' : NON LINEAR')
+print(RMSE_pos_planner[2], ' : PLANNER')
+
+print("\n\n")
+print('RMSE Roll / Pitch : ')
+print(RMSE_rpy_lin[:2], ' : LINEAR')
+print(RMSE_rpy_nl[:2], ' : NON LINEAR')
+print(RMSE_rpy_planner[:2], ' : PLANNER')
+
+print("\n\n")
+print('RMSE Yaw Velocity : ')
+print(RMSE_ang_lin[2], ' : LINEAR')
+print(RMSE_ang_nl[2], ' : NON LINEAR')
+print(RMSE_ang_planner[2], ' : PLANNER')
+
+
+
+
+
+ # Display all graphs and wait
+plt.show(block=True)
\ No newline at end of file
--
GitLab