'''This class will log 1d array in Nd matrix from device and qualisys object'''
from multiprocessing import set_forkserver_preload
import pickle
import numpy as np
from datetime import datetime as datetime
from time import time
import pinocchio as pin
class LoggerControl():
def __init__(self, dt, N0_gait, type_MPC=None, joystick=None, estimator=None, loop=None, gait=None, statePlanner=None,
footstepPlanner=None, footTrajectoryGenerator=None, logSize=60e3, ringBuffer=False):
self.ringBuffer = ringBuffer
logSize = np.int(logSize)
self.logSize = logSize
self.i = 0
self.dt = dt
if type_MPC is not None:
self.type_MPC = int(type_MPC)
else:
self.type_MPC = 0
# Allocate the data:
# Joystick
self.joy_v_ref = np.zeros([logSize, 6]) # reference velocity of the joystick
# Estimator
self.esti_feet_status = np.zeros([logSize, 4]) # input feet status (contact or not)
self.esti_feet_goals = np.zeros([logSize, 3, 4]) # input feet goals (desired on the ground)
self.esti_q_filt = np.zeros([logSize, 19]) # estimated state of the robot (complementary filter)
self.esti_q_up = np.zeros([logSize, 18]) # state of the robot in the ideal world
self.esti_v_filt = np.zeros([logSize, 18]) # estimated velocity of the robot (b frame)
self.esti_v_filt_bis = np.zeros([logSize, 18]) # estimated velocity of the robot (b frame, windowed)
self.esti_v_up = np.zeros([logSize, 18]) # estimated velocity of the robot in the ideal world (h frame)
self.esti_v_ref = np.zeros([logSize, 6]) # joystick reference velocity (h frame)
self.esti_v_secu = np.zeros([logSize, 12]) # filtered actuators velocity for security checks
self.esti_a_ref = np.zeros([logSize, 6]) # joystick reference acceleration (finite difference of v_ref)
# h_v, h_v_windowed, yaw_estim, oRb, oRh, hRb, oTh can be reconstructed based on what is logged
self.esti_FK_lin_vel = np.zeros([logSize, 3]) # estimated velocity of the base with FK
self.esti_FK_xyz = np.zeros([logSize, 3]) # estimated position of the base with FK
self.esti_xyz_mean_feet = np.zeros([logSize, 3]) # average of feet goals
self.esti_filt_lin_vel = np.zeros([logSize, 3]) # estimated velocity of the base before low pass filter
self.esti_HP_x = np.zeros([logSize, 3]) # x input of the velocity complementary filter
self.esti_HP_dx = np.zeros([logSize, 3]) # dx input of the velocity complementary filter
self.esti_HP_alpha = np.zeros([logSize, 3]) # alpha parameter of the velocity complementary filter
self.esti_HP_filt_x = np.zeros([logSize, 3]) # filtered output of the velocity complementary filter
self.esti_LP_x = np.zeros([logSize, 3]) # x input of the position complementary filter
self.esti_LP_dx = np.zeros([logSize, 3]) # dx input of the position complementary filter
self.esti_LP_alpha = np.zeros([logSize, 3]) # alpha parameter of the position complementary filter
self.esti_LP_filt_x = np.zeros([logSize, 3]) # filtered output of the position complementary filter
# Loop
self.loop_o_q = np.zeros([logSize, 18]) # state of the robot in the ideal world
self.loop_o_v = np.zeros([logSize, 18]) # estimated velocity of the robot in the ideal world (h frame)
self.loop_h_v = np.zeros([logSize, 18]) # estimated velocity in horizontal frame
self.loop_h_v_windowed = np.zeros([logSize, 6]) # estimated velocity in horizontal frame (windowed)
self.loop_t_filter = np.zeros([logSize]) # time taken by the estimator
self.loop_t_planner = np.zeros([logSize]) # time taken by the planning
self.loop_t_mpc = np.zeros([logSize]) # time taken by the mcp
self.loop_t_wbc = np.zeros([logSize]) # time taken by the whole body control
self.loop_t_loop = np.zeros([logSize]) # time taken by the whole loop (without interface)
self.loop_t_loop_if = np.zeros([logSize]) # time taken by the whole loop (with interface)
self.loop_q_filt_mpc = np.zeros([logSize, 6]) # state in ideal world filtered by 1st order low pass
self.loop_h_v_filt_mpc = np.zeros([logSize, 6]) # vel in h frame filtered by 1st order low pass
self.loop_vref_filt_mpc = np.zeros([logSize, 6]) # ref vel in h frame filtered by 1st order low pass
# Gait
self.planner_gait = np.zeros([logSize, N0_gait, 4]) # Gait sequence
self.planner_is_static = np.zeros([logSize]) # if the planner is in static mode or not
# State planner
self.planner_xref = np.zeros([logSize, 12, 1+statePlanner.getNSteps()]) # Reference trajectory
# Footstep planner
self.planner_fsteps = np.zeros([logSize, gait.getCurrentGait().shape[0], 12]) # Reference footsteps position
self.planner_target_fsteps = np.zeros([logSize, 3, 4]) # For each foot, next target on the ground
# o_target_foosteps can be reconstructed using yaw_ideal and x y ideal (top of q_up)
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_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
# References given to the wbc can be retrieved applying a rotation hRb @ oRh.transpose()
# and a translation oTh + np.array([[0.0], [0.0], [self.h_ref]]) to the position
# Model Predictive Control
# output vector of the MPC (next state + reference contact force)
if loop.type_MPC == 3:
self.mpc_x_f = np.zeros([logSize, 32, statePlanner.getNSteps()])
else:
self.mpc_x_f = np.zeros([logSize, 24, statePlanner.getNSteps()])
self.mpc_solving_duration = np.zeros([logSize])
# Whole body control
self.wbc_P = np.zeros([logSize, 12]) # proportionnal gains of the PD+
self.wbc_D = np.zeros([logSize, 12]) # derivative gains of the PD+
self.wbc_q_des = np.zeros([logSize, 12]) # desired position of actuators
self.wbc_v_des = np.zeros([logSize, 12]) # desired velocity of actuators
self.wbc_FF = np.zeros([logSize, 12]) # gains for the feedforward torques
self.wbc_tau_ff = np.zeros([logSize, 12]) # feedforward torques computed by the WBC
self.wbc_ddq_IK = np.zeros([logSize, 18]) # joint accelerations computed by the IK
self.wbc_f_ctc = np.zeros([logSize, 12]) # contact forces computed by the WBC
self.wbc_ddq_QP = np.zeros([logSize, 18]) # joint accelerations computed by the QP
self.wbc_feet_pos = np.zeros([logSize, 3, 4]) # current feet positions according to WBC
self.wbc_feet_pos_target = np.zeros([logSize, 3, 4]) # current feet positions targets for WBC
self.wbc_feet_err = np.zeros([logSize, 3, 4]) # error between feet positions and their reference
self.wbc_feet_vel = np.zeros([logSize, 3, 4]) # current feet velocities according to WBC
self.wbc_feet_vel_target = np.zeros([logSize, 3, 4]) # current feet velocities targets for WBC
self.wbc_feet_acc_target = np.zeros([logSize, 3, 4]) # current feet accelerations targets for WBC
self.wbc_tasks_acc = np.zeros([logSize, 30]) # acceleration of tasks in InvKin
self.wbc_tasks_vel = np.zeros([logSize, 30]) # velocities of tasks in InvKin
self.wbc_tasks_err = np.zeros([logSize, 30]) # position error of tasks in InvKin
# Timestamps
self.tstamps = np.zeros(logSize)
def sample(self, joystick, estimator, loop, gait, statePlanner, footstepPlanner, footTrajectoryGenerator, wbc, dT_whole):
if (self.i >= self.logSize):
if self.ringBuffer:
self.i = 0
else:
return
# Logging from joystick
self.joy_v_ref[self.i] = joystick.v_ref[:, 0]
# Logging from estimator
self.esti_feet_status[self.i] = estimator.getFeetStatus()
self.esti_feet_goals[self.i] = estimator.getFeetGoals()
self.esti_q_filt[self.i] = estimator.getQFilt()
self.esti_q_up[self.i] = self.estimator.getQUpdated()
self.esti_v_filt[self.i] = estimator.getVFilt()
self.esti_v_filt_bis[self.i] = estimator.getVFiltBis()
self.esti_v_up[self.i] = self.estimator.getVUpdated()
self.esti_v_ref[self.i] = self.estimator.getVRef()
self.esti_v_secu[self.i] = estimator.getVSecu()
self.esti_a_ref[self.i] = self.estimator.getARef()
self.esti_FK_lin_vel[self.i] = estimator.getFKLinVel()
self.esti_FK_xyz[self.i] = estimator.getFKXYZ()
self.esti_xyz_mean_feet[self.i] = estimator.getXYZMeanFeet()
self.esti_filt_lin_vel[self.i] = estimator.getFiltLinVel()
self.esti_HP_x[self.i] = estimator.getFilterVelX()
self.esti_HP_dx[self.i] = estimator.getFilterVelDX()
self.esti_HP_alpha[self.i] = estimator.getFilterVelAlpha()
self.esti_HP_filt_x[self.i] = estimator.getFilterVelFiltX()
self.esti_LP_x[self.i] = estimator.getFilterPosX()
self.esti_LP_dx[self.i] = estimator.getFilterPosDX()
self.esti_LP_alpha[self.i] = estimator.getFilterPosAlpha()
self.esti_LP_filt_x[self.i] = estimator.getFilterPosFiltX()
# Logging from the main loop
self.loop_o_q[self.i] = loop.q[:, 0]
self.loop_o_v[self.i] = loop.v[:, 0]
self.loop_h_v[self.i] = loop.h_v[:, 0]
self.loop_h_v_windowed[self.i] = loop.h_v_windowed[:, 0]
self.loop_t_filter[self.i] = loop.t_filter
self.loop_t_planner[self.i] = loop.t_planner
self.loop_t_mpc[self.i] = loop.t_mpc
self.loop_t_wbc[self.i] = loop.t_wbc
self.loop_t_loop[self.i] = loop.t_loop
self.loop_t_loop_if[self.i] = dT_whole
self.loop_q_filt_mpc[self.i] = loop.q_filt_mpc[:6, 0]
self.loop_h_v_filt_mpc[self.i] = loop.h_v_filt_mpc[:, 0]
self.loop_vref_filt_mpc[self.i] = loop.vref_filt_mpc[:, 0]
# Logging from the planner
self.planner_xref[self.i] = statePlanner.getReferenceStates()
self.planner_fsteps[self.i] = footstepPlanner.getFootsteps()
self.planner_target_fsteps[self.i] = footstepPlanner.getTargetFootsteps()
self.planner_gait[self.i] = gait.getCurrentGait()
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
# Logging from model predictive control
self.mpc_x_f[self.i] = loop.x_f_mpc
self.mpc_solving_duration[self.i] = loop.mpc_wrapper.t_mpc_solving_duration
# Logging from whole body control
self.wbc_P[self.i] = loop.result.P
self.wbc_D[self.i] = loop.result.D
self.wbc_q_des[self.i] = loop.result.q_des
self.wbc_v_des[self.i] = loop.result.v_des
self.wbc_FF[self.i] = loop.result.FF
self.wbc_tau_ff[self.i] = loop.result.tau_ff
self.wbc_ddq_IK[self.i] = wbc.ddq_cmd
self.wbc_f_ctc[self.i] = wbc.f_with_delta
self.wbc_ddq_QP[self.i] = wbc.ddq_with_delta
self.wbc_feet_pos[self.i] = wbc.feet_pos
self.wbc_feet_pos_target[self.i] = wbc.feet_pos_target
self.wbc_feet_err[self.i] = wbc.feet_err
self.wbc_feet_vel[self.i] = wbc.feet_vel
self.wbc_feet_vel_target[self.i] = wbc.feet_vel_target
self.wbc_feet_acc_target[self.i] = wbc.feet_acc_target
self.wbc_tasks_acc[self.i] = wbc.getTasksAcc()
self.wbc_tasks_vel[self.i] = wbc.getTasksVel()
self.wbc_tasks_err[self.i] = wbc.getTasksErr()
# Logging timestamp
self.tstamps[self.i] = time()
self.i += 1
def processMocap(self, N, loggerSensors):
self.mocap_pos = np.zeros([N, 3])
self.mocap_h_v = np.zeros([N, 3])
self.mocap_b_w = np.zeros([N, 3])
self.mocap_RPY = np.zeros([N, 3])
for i in range(N):
self.mocap_RPY[i] = pin.rpy.matrixToRpy(pin.Quaternion(loggerSensors.mocapOrientationQuat[i]).toRotationMatrix())
# Robot world to Mocap initial translationa and rotation
mTo = np.array([loggerSensors.mocapPosition[0, 0], loggerSensors.mocapPosition[0, 1], 0.02])
mRo = pin.rpy.rpyToMatrix(0.0, 0.0, self.mocap_RPY[0, 2])
for i in range(N):
oRb = loggerSensors.mocapOrientationMat9[i]
oRh = pin.rpy.rpyToMatrix(0.0, 0.0, self.mocap_RPY[i, 2] - self.mocap_RPY[0, 2])
self.mocap_h_v[i] = (oRh.transpose() @ mRo.transpose() @ loggerSensors.mocapVelocity[i].reshape((3, 1))).ravel()
self.mocap_b_w[i] = (oRb.transpose() @ loggerSensors.mocapAngularVelocity[i].reshape((3, 1))).ravel()
self.mocap_pos[i] = (mRo.transpose() @ (loggerSensors.mocapPosition[i, :] - mTo).reshape((3, 1))).ravel()
def plotAll(self, loggerSensors):
from matplotlib import pyplot as plt
N = self.tstamps.shape[0]
t_range = np.array([k*self.dt for k in range(N)])
self.processMocap(N, loggerSensors)
index6 = [1, 3, 5, 2, 4, 6]
index12 = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
# Reconstruct pos and vel of feet in base frame to compare them with the
# ones desired by the foot trajectory generator and whole-body control
from example_robot_data.robots_loader import Solo12Loader
Solo12Loader.free_flyer = True
solo12 = Solo12Loader().robot
FL_FOOT_ID = solo12.model.getFrameId('FL_FOOT')
FR_FOOT_ID = solo12.model.getFrameId('FR_FOOT')
HL_FOOT_ID = solo12.model.getFrameId('HL_FOOT')
HR_FOOT_ID = solo12.model.getFrameId('HR_FOOT')
foot_ids = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
q = np.zeros((19, 1))
dq = np.zeros((18, 1))
pin.computeAllTerms(solo12.model, solo12.data, q, np.zeros((18, 1)))
feet_pos = np.zeros([self.esti_q_filt.shape[0], 3, 4])
feet_vel = np.zeros([self.esti_q_filt.shape[0], 3, 4])
for i in range(self.esti_q_filt.shape[0]):
q[:3, 0] = self.loop_q_filt_mpc[i, :3]
q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.loop_q_filt_mpc[i, 3:6])).coeffs()
q[7:, 0] = self.loop_o_q[i, 6:]
dq[6:, 0] = self.loop_o_v[i, 6:]
pin.forwardKinematics(solo12.model, solo12.data, q, dq)
pin.updateFramePlacements(solo12.model, solo12.data)
for j, idx in enumerate(foot_ids):
feet_pos[i, :, j] = solo12.data.oMf[int(idx)].translation
feet_vel[i, :, j] = pin.getFrameVelocity(solo12.model, solo12.data, int(idx), pin.LOCAL_WORLD_ALIGNED).linear
####
# Measured & Reference feet positions (base frame)
####
lgd_X = ["FL", "FR", "HL", "HR"]
lgd_Y = ["Pos X", "Pos Y", "Pos Z"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, index12[i])
else:
plt.subplot(3, 4, index12[i], sharex=ax0)
plt.plot(t_range, self.wbc_feet_pos[:, i % 3, np.int(i/3)], color='b', linewidth=3, marker='')
plt.plot(t_range, self.wbc_feet_pos_target[:, i % 3, np.int(i/3)], color='r', linewidth=3, marker='')
plt.plot(t_range, feet_pos[:, i % 3, np.int(i/3)], color='rebeccapurple', linewidth=3, marker='')
if (i % 3) == 2:
mini = np.min(self.wbc_feet_pos[:, i % 3, np.int(i/3)])
maxi = np.max(self.wbc_feet_pos[:, i % 3, np.int(i/3)])
plt.plot(t_range, self.planner_gait[:, 0, np.int(
i/3)] * (maxi - mini) + mini, color='k', linewidth=3, marker='')
plt.legend([lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+" WBC",
lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+" Ref",
lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+" Robot", "Contact state"], prop={'size': 8})
self.custom_suptitle("Feet positions (base frame)")
####
# Measured & Reference feet velocities (base frame)
####
lgd_X = ["FL", "FR", "HL", "HR"]
lgd_Y = ["Vel X", "Vel Y", "Vel Z"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, index12[i])
else:
plt.subplot(3, 4, index12[i], sharex=ax0)
plt.plot(t_range, self.wbc_feet_vel[:, i % 3, np.int(i/3)], color='b', linewidth=3, marker='')
plt.plot(t_range, self.wbc_feet_vel_target[:, i % 3, np.int(i/3)], color='r', linewidth=3, marker='')
plt.plot(t_range, feet_vel[:, i % 3, np.int(i/3)], color='rebeccapurple', linewidth=3, marker='')
plt.legend([lgd_Y[i % 3] + " WBC" + lgd_X[np.int(i/3)],
lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)] + " Ref",
lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)] + " Robot"], prop={'size': 8})
self.custom_suptitle("Feet velocities (base frame)")
####
# Reference feet accelerations (base frame)
####
lgd_X = ["FL", "FR", "HL", "HR"]
lgd_Y = ["Acc X", "Acc Y", "Acc Z"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, index12[i])
else:
plt.subplot(3, 4, index12[i], sharex=ax0)
plt.plot(t_range, self.wbc_feet_acc_target[:, i % 3, np.int(i/3)], color='r', linewidth=3, marker='')
plt.legend([lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+" Ref"], prop={'size': 8})
self.custom_suptitle("Feet accelerations (base frame)")
####
# Measured & Reference position and orientation (ideal world frame)
####
lgd = ["Pos X", "Pos Y", "Pos Z", "Roll", "Pitch", "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 in [0, 1, 5]:
plt.plot(t_range, self.loop_o_q[:, i], "b", linewidth=3)
else:
plt.plot(t_range, self.planner_xref[:, i, 0], "b", linewidth=2)
if i < 3:
plt.plot(t_range, self.mocap_pos[:, i], "k", linewidth=3)
else:
plt.plot(t_range, self.mocap_RPY[:, i-3], "k", linewidth=3)
if i in [0, 1, 5]:
plt.plot(t_range, self.loop_o_q[:, i], "r", linewidth=3)
else:
plt.plot(t_range, self.planner_xref[:, i, 1], "r", linewidth=3)
plt.legend(["Robot state", "Ground truth", "Robot reference state"], prop={'size': 8})
plt.ylabel(lgd[i])
self.custom_suptitle("Position and orientation")
####
# Measured & Reference linear and angular velocities (horizontal frame)
####
lgd = ["Linear vel X", "Linear vel Y", "Linear vel Z",
"Angular 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)
plt.plot(t_range, self.loop_h_v[:, i], "b", linewidth=2)
if i < 3:
plt.plot(t_range, self.mocap_h_v[:, i], "k", linewidth=3)
plt.plot(t_range, self.loop_h_v_filt_mpc[:, i], linewidth=3, color="forestgreen")
plt.plot(t_range, self.loop_h_v_windowed[:, i], linewidth=3, color="rebeccapurple")
else:
plt.plot(t_range, self.mocap_b_w[:, i-3], "k", linewidth=3)
plt.plot(t_range, self.joy_v_ref[:, i], "r", linewidth=3)
if i < 3:
plt.legend(["State", "Ground truth",
"State (LP 15Hz)", "State (windowed)", "Ref state"], prop={'size': 8})
else:
plt.legend(["State", "Ground truth", "Ref state"], prop={'size': 8})
plt.ylabel(lgd[i])
if i == 0:
plt.ylim([-0.05, 1.25])
self.custom_suptitle("Linear and angular velocities")
print("RMSE: ", np.sqrt(((self.joy_v_ref[:-1000, 0] - self.mocap_h_v[:-1000, 0])**2).mean()))
# Analysis of the footstep locations (current and future) with a slider to move along time
# self.slider_predicted_footholds()
####
# Analysis of the footholds locations during the whole experiment
####
"""f_c = ["r", "b", "forestgreen", "rebeccapurple"]
quat = np.zeros((4, 1))
steps = np.zeros((12, 1))
o_step = np.zeros((3, 1))
plt.figure()
plt.plot(self.loop_o_q[:, 0], self.loop_o_q[:, 1], linewidth=2, color="k")
for i in range(self.planner_fsteps.shape[0]):
fsteps = self.planner_fsteps[i]
RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[0, 3:7]).toRotationMatrix())
quat[:, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(np.array([0.0, 0.0, RPY[2]]))).coeffs()
oRh = pin.Quaternion(quat).toRotationMatrix()
for j in range(4):
#if np.any(fsteps[k, (j*3):((j+1)*3)]) and not np.array_equal(steps[(j*3):((j+1)*3), 0],
# fsteps[k, (j*3):((j+1)*3)]):
# steps[(j*3):((j+1)*3), 0] = fsteps[k, (j*3):((j+1)*3)]
# o_step[:, 0:1] = oRh @ steps[(j*3):((j+1)*3), 0:1] + self.loop_o_q[i:(i+1), 0:3].transpose()
o_step[:, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q[i:(i+1), 0:3].transpose()
plt.plot(o_step[0, 0], o_step[1, 0], linestyle=None, linewidth=1, marker="o", color=f_c[j])"""
####
# FF torques & FB torques & Sent torques & Meas torques
####
lgd1 = ["HAA", "HFE", "Knee"]
lgd2 = ["FL", "FR", "HL", "HR"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, index12[i])
else:
plt.subplot(3, 4, index12[i], sharex=ax0)
tau_fb = self.wbc_P[:, i] * (self.wbc_q_des[:, i] - self.loop_o_q[:, 6+i]) + \
self.wbc_D[:, i] * (self.wbc_v_des[:, i] - self.loop_o_v[:, 6+i])
h1, = plt.plot(t_range, self.wbc_FF[:, i] * self.wbc_tau_ff[:, i], "r", linewidth=3)
h2, = plt.plot(t_range, tau_fb, "b", linewidth=3)
h3, = plt.plot(t_range, self.wbc_FF[:, i] * self.wbc_tau_ff[:, i] + tau_fb, "g", linewidth=3)
h4, = plt.plot(t_range[:-1], loggerSensors.torquesFromCurrentMeasurment[1:, i],
"violet", linewidth=3, linestyle="--")
plt.xlabel("Time [s]")
plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [Nm]")
tmp = lgd1[i % 3]+" "+lgd2[int(i/3)]
plt.legend([h1, h2, h3, h4], ["FF "+tmp, "FB "+tmp, "PD+ "+tmp, "Meas "+tmp], prop={'size': 8})
plt.ylim([-8.0, 8.0])
self.custom_suptitle("Torques")
####
# Contact forces (MPC command) & WBC QP output
####
lgd1 = ["Ctct force X", "Ctct force Y", "Ctct force Z"]
lgd2 = ["FL", "FR", "HL", "HR"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, index12[i])
else:
plt.subplot(3, 4, index12[i], sharex=ax0)
h1, = plt.plot(t_range, self.mpc_x_f[:, 12+i, 0], "r", linewidth=3)
h2, = plt.plot(t_range, self.wbc_f_ctc[:, i], "b", linewidth=3, linestyle="--")
plt.xlabel("Time [s]")
plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [N]")
plt.legend([h1, h2], ["MPC " + lgd1[i % 3]+" "+lgd2[int(i/3)],
"WBC " + lgd1[i % 3]+" "+lgd2[int(i/3)]], prop={'size': 8})
if (i % 3) == 2:
plt.ylim([-0.0, 26.0])
else:
plt.ylim([-26.0, 26.0])
self.custom_suptitle("Contact forces (MPC command) & WBC QP output")
####
# Desired & Measured actuator positions
####
lgd1 = ["HAA", "HFE", "Knee"]
lgd2 = ["FL", "FR", "HL", "HR"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, index12[i])
else:
plt.subplot(3, 4, index12[i], sharex=ax0)
h1, = plt.plot(t_range, self.wbc_q_des[:, i], color='r', linewidth=3)
h2, = plt.plot(t_range, self.loop_o_q[:, 6+i], color='b', linewidth=3)
plt.xlabel("Time [s]")
plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [rad]")
plt.legend([h1, h2], ["Ref "+lgd1[i % 3]+" "+lgd2[int(i/3)],
lgd1[i % 3]+" "+lgd2[int(i/3)]], prop={'size': 8})
self.custom_suptitle("Actuator positions")
####
# Desired & Measured actuator velocity
####
lgd1 = ["HAA", "HFE", "Knee"]
lgd2 = ["FL", "FR", "HL", "HR"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, index12[i])
else:
plt.subplot(3, 4, index12[i], sharex=ax0)
h1, = plt.plot(t_range, self.wbc_v_des[:, i], color='r', linewidth=3)
h2, = plt.plot(t_range, self.loop_o_v[:, 6+i], color='b', linewidth=3)
plt.xlabel("Time [s]")
plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [rad]")
plt.legend([h1, h2], ["Ref "+lgd1[i % 3]+" "+lgd2[int(i/3)],
lgd1[i % 3]+" "+lgd2[int(i/3)]], prop={'size': 8})
self.custom_suptitle("Actuator velocities")
####
# Evolution of trajectories in position and orientation computed by the MPC
####
"""
log_t_pred = np.array([k*self.dt*10 for k in range(self.mpc_x_f.shape[2])])
log_t_ref = np.array([k*self.dt*10 for k in range(self.planner_xref.shape[2])])
titles = ["X", "Y", "Z", "Roll", "Pitch", "Yaw"]
step = 1000
plt.figure()
for j in range(6):
plt.subplot(3, 2, index6[j])
c = [[i/(self.mpc_x_f.shape[0]+5), 0.0, i/(self.mpc_x_f.shape[0]+5)]
for i in range(0, self.mpc_x_f.shape[0], step)]
for i in range(0, self.mpc_x_f.shape[0], step):
h1, = plt.plot(log_t_pred+(i+10)*self.dt,
self.mpc_x_f[i, j, :], "b", linewidth=2, color=c[int(i/step)])
h2, = plt.plot(log_t_ref+i*self.dt,
self.planner_xref[i, j, :], linestyle="--", marker='x', color="g", linewidth=2)
#h3, = plt.plot(np.array([k*self.dt for k in range(self.mpc_x_f.shape[0])]),
# self.planner_xref[:, j, 0], linestyle=None, marker='x', color="r", linewidth=1)
plt.xlabel("Time [s]")
plt.legend([h1, h2, h3], ["Output trajectory of MPC",
"Input trajectory of planner"]) #, "Actual robot trajectory"])
plt.title("Predicted trajectory for " + titles[j])
self.custom_suptitle("Analysis of trajectories in position and orientation computed by the MPC")
"""
####
# Evolution of trajectories of linear and angular velocities computed by the MPC
####
"""
plt.figure()
for j in range(6):
plt.subplot(3, 2, index6[j])
c = [[i/(self.mpc_x_f.shape[0]+5), 0.0, i/(self.mpc_x_f.shape[0]+5)]
for i in range(0, self.mpc_x_f.shape[0], step)]
for i in range(0, self.mpc_x_f.shape[0], step):
h1, = plt.plot(log_t_pred+(i+10)*self.dt,
self.mpc_x_f[i, j+6, :], "b", linewidth=2, color=c[int(i/step)])
h2, = plt.plot(log_t_ref+i*self.dt,
self.planner_xref[i, j+6, :], linestyle="--", marker='x', color="g", linewidth=2)
h3, = plt.plot(np.array([k*self.dt for k in range(self.mpc_x_f.shape[0])]),
self.planner_xref[:, j+6, 0], linestyle=None, marker='x', color="r", linewidth=1)
plt.xlabel("Time [s]")
plt.legend([h1, h2, h3], ["Output trajectory of MPC",
"Input trajectory of planner", "Actual robot trajectory"])
plt.title("Predicted trajectory for velocity in " + titles[j])
self.custom_suptitle("Analysis of trajectories of linear and angular velocities computed by the MPC")
"""
####
# Evolution of contact force trajectories
####
"""
step = 1000
lgd1 = ["Ctct force X", "Ctct force Y", "Ctct force Z"]
lgd2 = ["FL", "FR", "HL", "HR"]
plt.figure()
for i in range(4):
if i == 0:
ax0 = plt.subplot(1, 4, i+1)
else:
plt.subplot(1, 4, i+1, sharex=ax0)
for k in range(0, self.mpc_x_f.shape[0], step):
h2, = plt.plot(log_t_pred+k*self.dt, self.mpc_x_f[k, 12+(3*i+2), :], linestyle="--", marker='x', linewidth=2)
h1, = plt.plot(t_range, self.mpc_x_f[:, 12+(3*i+2), 0], "r", linewidth=3)
# h3, = plt.plot(t_range, self.wbc_f_ctc[:, i], "b", linewidth=3, linestyle="--")
plt.plot(t_range, self.esti_feet_status[:, i], "k", linestyle="--")
plt.xlabel("Time [s]")
plt.ylabel(lgd2[i]+" [N]")
plt.legend([h1, h2], ["MPC "+lgd2[i],
"MPC "+lgd2[i]+" trajectory"])
plt.ylim([-1.0, 26.0])
self.custom_suptitle("Contact forces trajectories & Actual forces trajectories")
"""
####
# Analysis of the complementary filter behaviour
####
"""
clr = ["b", "darkred", "forestgreen"]
# Velocity complementary filter
lgd_Y = ["dx", "ddx", "alpha dx", "dx_out", "dy", "ddy", "alpha dy", "dy_out", "dz", "ddz", "alpha dz", "dz_out"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, i+1)
else:
plt.subplot(3, 4, i+1, sharex=ax0)
if i % 4 == 0:
plt.plot(t_range, self.esti_HP_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # x input of the velocity complementary filter
elif i % 4 == 1:
plt.plot(t_range, self.esti_HP_dx[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # dx input of the velocity complementary filter
elif i % 4 == 2:
plt.plot(t_range, self.esti_HP_alpha[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # alpha parameter of the velocity complementary filter
else:
plt.plot(t_range, self.esti_HP_filt_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # filtered output of the velocity complementary filter
plt.legend([lgd_Y[i]], prop={'size': 8})
self.custom_suptitle("Evolution of the quantities of the velocity complementary filter")
# Position complementary filter
lgd_Y = ["x", "dx", "alpha x", "x_out", "y", "dy", "alpha y", "y_out", "z", "dz", "alpha z", "z_out"]
plt.figure()
for i in range(12):
if i == 0:
ax0 = plt.subplot(3, 4, i+1)
else:
plt.subplot(3, 4, i+1, sharex=ax0)
if i % 4 == 0:
plt.plot(t_range, self.esti_LP_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # x input of the position complementary filter
elif i % 4 == 1:
plt.plot(t_range, self.esti_LP_dx[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # dx input of the position complementary filter
elif i % 4 == 2:
plt.plot(t_range, self.esti_LP_alpha[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # alpha parameter of the position complementary filter
else:
plt.plot(t_range, self.esti_LP_filt_x[:, int(i/4)], color=clr[int(i/4)], linewidth=3, marker='') # filtered output of the position complementary filter
plt.legend([lgd_Y[i]], prop={'size': 8})
self.custom_suptitle("Evolution of the quantities of the position complementary filter")
"""
####
# Power supply profile
####
plt.figure()
for i in range(3):
if i == 0:
ax0 = plt.subplot(3, 1, i+1)
else:
plt.subplot(3, 1, i+1, sharex=ax0)
if i == 0:
plt.plot(t_range, loggerSensors.current[:], linewidth=2)
plt.ylabel("Bus current [A]")
elif i == 1:
plt.plot(t_range, loggerSensors.voltage[:], linewidth=2)
plt.ylabel("Bus voltage [V]")
else:
plt.plot(t_range, loggerSensors.energy[:], linewidth=2)
plt.ylabel("Bus energy [J]")
plt.xlabel("Time [s]")
####
# Estimated computation time for each step of the control architecture
####
plt.figure()
plt.plot(t_range, self.loop_t_filter, 'r+')
plt.plot(t_range, self.loop_t_planner, 'g+')
plt.plot(t_range, self.loop_t_mpc, 'b+')
plt.plot(t_range, self.loop_t_wbc, '+', color="violet")
plt.plot(t_range, self.loop_t_loop, 'k+')
plt.plot(t_range, self.loop_t_loop_if, '+', color="rebeccapurple")
plt.legend(["Estimator", "Planner", "MPC", "WBC", "Control loop", "Whole loop"])
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")
self.custom_suptitle("Computation time of each block")
# Plot estimated solving time of the model prediction control
fig = plt.figure()
plt.plot(t_range[35:], self.mpc_solving_duration[35:], 'k+')
plt.legend(["Solving duration"])
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")
self.custom_suptitle("MPC solving time")
####
# Step in system time at each loop
####
plt.figure()
plt.plot(np.diff(self.tstamps))
plt.legend(["System time step"])
plt.xlabel("Loop []")
plt.ylabel("Time [s]")
self.custom_suptitle("System time step between 2 sucessive loops")
####
# Comparison of raw and filtered quantities (15 Hz and windowed)
####
"""
lgd = ["Position X", "Position Y", "Position Z", "Position Roll", "Position Pitch", "Position 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)
plt.plot(t_range, self.loop_o_q[:, i], linewidth=3)
plt.plot(t_range, self.loop_q_filt_mpc[:, i], linewidth=3)
plt.legend(["Estimated", "Estimated 15Hz filt"], prop={'size': 8})
plt.ylabel(lgd[i])
self.custom_suptitle("Comparison between position quantities before and after 15Hz low-pass filtering")
lgd = ["Linear vel X", "Linear vel Y", "Linear vel Z",
"Angular 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)
plt.plot(t_range, self.loop_h_v[:, i], linewidth=3)
plt.plot(t_range, self.loop_h_v_windowed[:, i], linewidth=3)
plt.plot(t_range, self.loop_h_v_filt_mpc[:, i], linewidth=3)
plt.plot(t_range, self.loop_vref_filt_mpc[:, i], linewidth=3)
plt.legend(["Estimated", "Estimated 3Hz windowed", "Estimated 15Hz filt",
"Estimated 15Hz filt 3Hz windowed", "Reference 15Hz filt"], prop={'size': 8})
plt.ylabel(lgd[i])
self.custom_suptitle("Comparison between velocity quantities before and after 15Hz low-pass filtering")
"""
###############################
# Display all graphs and wait #
###############################
plt.show(block=True)
def custom_suptitle(self, name):
from matplotlib import pyplot as plt
fig = plt.gcf()
fig.suptitle(name)
fig.canvas.manager.set_window_title(name)
def saveAll(self, loggerSensors, fileName="data"):
date_str = datetime.now().strftime('_%Y_%m_%d_%H_%M')
np.savez_compressed(fileName + date_str + "_" + str(self.type_MPC) + ".npz",
joy_v_ref=self.joy_v_ref,
esti_feet_status=self.esti_feet_status,
esti_feet_goals=self.esti_feet_goals,
esti_q_filt=self.esti_q_filt,
esti_q_up=self.esti_q_up,
esti_v_filt=self.esti_v_filt,
esti_v_filt_bis=self.esti_v_filt_bis,
esti_v_up=self.esti_v_up,
esti_v_ref=self.esti_v_ref,
esti_v_secu=self.esti_v_secu,
esti_a_ref=self.esti_a_ref,
esti_FK_lin_vel=self.esti_FK_lin_vel,
esti_FK_xyz=self.esti_FK_xyz,
esti_xyz_mean_feet=self.esti_xyz_mean_feet,
esti_filt_lin_vel=self.esti_filt_lin_vel,
esti_HP_x=self.esti_HP_x,
esti_HP_dx=self.esti_HP_dx,
esti_HP_alpha=self.esti_HP_alpha,
esti_HP_filt_x=self.esti_HP_filt_x,
esti_LP_x=self.esti_LP_x,
esti_LP_dx=self.esti_LP_dx,
esti_LP_alpha=self.esti_LP_alpha,
esti_LP_filt_x=self.esti_LP_filt_x,
loop_o_q=self.loop_o_q,
loop_o_v=self.loop_o_v,
loop_h_v=self.loop_h_v,
loop_h_v_windowed=self.loop_h_v_windowed,
loop_t_filter=self.loop_t_filter,
loop_t_planner=self.loop_t_planner,
loop_t_mpc=self.loop_t_mpc,
loop_t_wbc=self.loop_t_wbc,
loop_t_loop=self.loop_t_loop,
loop_t_loop_if=self.loop_t_loop_if,
loop_q_filt_mpc=self.loop_q_filt_mpc,
loop_h_v_filt_mpc=self.loop_h_v_filt_mpc,
loop_vref_filt_mpc=self.loop_vref_filt_mpc,
planner_xref=self.planner_xref,
planner_fsteps=self.planner_fsteps,
planner_target_fsteps=self.planner_target_fsteps,
planner_gait=self.planner_gait,
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,
mpc_x_f=self.mpc_x_f,
mpc_solving_duration=self.mpc_solving_duration,
wbc_P=self.wbc_P,
wbc_D=self.wbc_D,
wbc_q_des=self.wbc_q_des,
wbc_v_des=self.wbc_v_des,
wbc_FF=self.wbc_FF,
wbc_tau_ff=self.wbc_tau_ff,
wbc_ddq_IK=self.wbc_ddq_IK,
wbc_f_ctc=self.wbc_f_ctc,
wbc_ddq_QP=self.wbc_ddq_QP,
wbc_feet_pos=self.wbc_feet_pos,
wbc_feet_pos_target=self.wbc_feet_pos_target,
wbc_feet_err=self.wbc_feet_err,
wbc_feet_vel=self.wbc_feet_vel,
wbc_feet_vel_target=self.wbc_feet_vel_target,
wbc_feet_acc_target=self.wbc_feet_acc_target,
wbc_tasks_acc=self.wbc_tasks_acc,
wbc_tasks_vel=self.wbc_tasks_vel,
wbc_tasks_err=self.wbc_tasks_err,
tstamps=self.tstamps,
q_mes=loggerSensors.q_mes,
v_mes=loggerSensors.v_mes,
baseOrientation=loggerSensors.baseOrientation,
baseOrientationQuat=loggerSensors.baseOrientationQuat,
baseAngularVelocity=loggerSensors.baseAngularVelocity,
baseLinearAcceleration=loggerSensors.baseLinearAcceleration,
baseAccelerometer=loggerSensors.baseAccelerometer,
torquesFromCurrentMeasurment=loggerSensors.torquesFromCurrentMeasurment,
mocapPosition=loggerSensors.mocapPosition,
mocapVelocity=loggerSensors.mocapVelocity,
mocapAngularVelocity=loggerSensors.mocapAngularVelocity,
mocapOrientationMat9=loggerSensors.mocapOrientationMat9,
mocapOrientationQuat=loggerSensors.mocapOrientationQuat,
current=loggerSensors.current,
voltage=loggerSensors.voltage,
energy=loggerSensors.energy,
)
def loadAll(self, loggerSensors, fileName=None):
if fileName is None:
import glob
fileName = np.sort(glob.glob('data_2021_*.npz'))[-1] # Most recent file
data = np.load(fileName, allow_pickle = True)
# Load LoggerControl arrays
self.joy_v_ref = data["joy_v_ref"]
self.logSize = self.joy_v_ref.shape[0]
self.esti_feet_status = data["esti_feet_status"]
self.esti_feet_goals = data["esti_feet_goals"]
self.esti_q_filt = data["esti_q_filt"]
self.esti_q_up = data["esti_q_up"]
self.esti_v_filt = data["esti_v_filt"]
self.esti_v_filt_bis = data["esti_v_filt_bis"]
self.esti_v_up = data["esti_v_up"]
self.esti_v_ref = data["esti_v_ref"]
self.esti_v_secu = data["esti_v_secu"]
self.esti_a_ref = data["esti_a_ref"]
self.esti_FK_lin_vel = data["esti_FK_lin_vel"]
self.esti_FK_xyz = data["esti_FK_xyz"]
self.esti_xyz_mean_feet = data["esti_xyz_mean_feet"]
self.esti_filt_lin_vel = data["esti_filt_lin_vel"]
self.esti_HP_x = data["esti_HP_x"]
self.esti_HP_dx = data["esti_HP_dx"]
self.esti_HP_alpha = data["esti_HP_alpha"]
self.esti_HP_filt_x = data["esti_HP_filt_x"]
self.esti_LP_x = data["esti_LP_x"]
self.esti_LP_dx = data["esti_LP_dx"]
self.esti_LP_alpha = data["esti_LP_alpha"]
self.esti_LP_filt_x = data["esti_LP_filt_x"]
self.loop_o_q = data["loop_o_q"]
self.loop_o_v = data["loop_o_v"]
self.loop_h_v = data["loop_h_v"]
self.loop_h_v_windowed = data["loop_h_v_windowed"]
self.loop_t_filter = data["loop_t_filter"]
self.loop_t_planner = data["loop_t_planner"]
self.loop_t_mpc = data["loop_t_mpc"]
self.loop_t_wbc = data["loop_t_wbc"]
self.loop_t_loop = data["loop_t_loop"]
self.loop_t_loop_if = data["loop_t_loop_if"]
self.loop_q_filt_mpc = data["loop_q_filt_mpc"]
self.loop_h_v_filt_mpc = data["loop_h_v_filt_mpc"]
self.loop_vref_filt_mpc = data["loop_vref_filt_mpc"]
self.planner_xref = data["planner_xref"]
self.planner_fsteps = data["planner_fsteps"]
self.planner_target_fsteps = data["planner_target_fsteps"]
self.planner_gait = data["planner_gait"]
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"]
self.mpc_x_f = data["mpc_x_f"]
self.mpc_solving_duration = data["mpc_solving_duration"]
self.wbc_P = data["wbc_P"]
self.wbc_D = data["wbc_D"]
self.wbc_q_des = data["wbc_q_des"]
self.wbc_v_des = data["wbc_v_des"]
self.wbc_FF = data["wbc_FF"]
self.wbc_tau_ff = data["wbc_tau_ff"]
self.wbc_ddq_IK = data["wbc_ddq_IK"]
self.wbc_f_ctc = data["wbc_f_ctc"]
self.wbc_ddq_QP = data["wbc_ddq_QP"]
self.wbc_feet_pos = data["wbc_feet_pos"]
self.wbc_feet_pos_target = data["wbc_feet_pos_target"]
self.wbc_feet_err = data["wbc_feet_err"]
self.wbc_feet_vel = data["wbc_feet_vel"]
self.wbc_feet_vel_target = data["wbc_feet_vel_target"]
self.wbc_feet_acc_target = data["wbc_feet_acc_target"]
self.wbc_tasks_acc = data["wbc_tasks_acc"]
self.wbc_tasks_vel = data["wbc_tasks_vel"]
self.wbc_tasks_err = data["wbc_tasks_err"]
self.tstamps = data["tstamps"]
# Load LoggerSensors arrays
loggerSensors.q_mes = data["q_mes"]
loggerSensors.v_mes = data["v_mes"]
loggerSensors.baseOrientation = data["baseOrientation"]
loggerSensors.baseOrientationQuat = data["baseOrientationQuat"]
loggerSensors.baseAngularVelocity = data["baseAngularVelocity"]
loggerSensors.baseLinearAcceleration = data["baseLinearAcceleration"]
loggerSensors.baseAccelerometer = data["baseAccelerometer"]
loggerSensors.torquesFromCurrentMeasurment = data["torquesFromCurrentMeasurment"]
loggerSensors.mocapPosition = data["mocapPosition"]
loggerSensors.mocapVelocity = data["mocapVelocity"]
loggerSensors.mocapAngularVelocity = data["mocapAngularVelocity"]
loggerSensors.mocapOrientationMat9 = data["mocapOrientationMat9"]
loggerSensors.mocapOrientationQuat = data["mocapOrientationQuat"]
loggerSensors.logSize = loggerSensors.q_mes.shape[0]
loggerSensors.current = data["current"]
loggerSensors.voltage = data["voltage"]
loggerSensors.energy = data["energy"]
def slider_predicted_trajectory(self):
from matplotlib import pyplot as plt
from matplotlib.widgets import Slider, Button
# The parametrized function to be plotted
def f(t, time):
return np.sin(2 * np.pi * t) + time
index6 = [1, 3, 5, 2, 4, 6]
log_t_pred = np.array([(k+1)*self.dt*10 for k in range(self.mpc_x_f.shape[2])])
log_t_ref = np.array([k*self.dt*10 for k in range(self.planner_xref.shape[2])])
trange = np.max([np.max(log_t_pred), np.max(log_t_ref)])
h1s = []
h2s = []
axs = []
h1s_vel = []
h2s_vel = []
axs_vel = []
# Define initial parameters
init_time = 0.0
# Create the figure and the line that we will manipulate
fig = plt.figure()
ax = plt.gca()
for j in range(6):
ax = plt.subplot(3, 2, index6[j])
h1, = plt.plot(log_t_pred, self.mpc_x_f[0, j, :], "b", linewidth=2)
h2, = plt.plot(log_t_ref, self.planner_xref[0, j, :], linestyle="--", marker='x', color="g", linewidth=2)
h3, = plt.plot(np.array([k*self.dt for k in range(self.mpc_x_f.shape[0])]),
self.planner_xref[:, j, 0], linestyle=None, marker='x', color="r", linewidth=1)
axs.append(ax)
h1s.append(h1)
h2s.append(h2)
#ax.set_xlabel('Time [s]')
axcolor = 'lightgoldenrodyellow'
#ax.margins(x=0)
# Make a horizontal slider to control the time.
axtime = plt.axes([0.25, 0.03, 0.65, 0.03], facecolor=axcolor)
time_slider = Slider(
ax=axtime,
label='Time [s]',
valmin=0.0,
valmax=self.logSize*self.dt,
valinit=init_time,
)
# Create the figure and the line that we will manipulate (for velocities)
fig_vel = plt.figure()
ax = plt.gca()
for j in range(6):
ax = plt.subplot(3, 2, index6[j])
h1, = plt.plot(log_t_pred, self.mpc_x_f[0, j, :], "b", linewidth=2)
h2, = plt.plot(log_t_ref, self.planner_xref[0, j, :], linestyle="--", marker='x', color="g", linewidth=2)
h3, = plt.plot(np.array([k*self.dt for k in range(self.mpc_x_f.shape[0])]),
self.planner_xref[:, j+6, 0], linestyle=None, marker='x', color="r", linewidth=1)
axs_vel.append(ax)
h1s_vel.append(h1)
h2s_vel.append(h2)
#axcolor = 'lightgoldenrodyellow'
#ax.margins(x=0)
# Make a horizontal slider to control the time.
axtime_vel = plt.axes([0.25, 0.03, 0.65, 0.03], facecolor=axcolor)
time_slider_vel = Slider(
ax=axtime_vel,
label='Time [s]',
valmin=0.0,
valmax=self.logSize*self.dt,
valinit=init_time,
)
# The function to be called anytime a slider's value changes
def update(val, recursive=False):
time_slider.val = np.round(val / (self.dt*10), decimals=0) * (self.dt*10)
rounded = int(np.round(time_slider.val / self.dt, decimals=0))
for j in range(6):
h1s[j].set_xdata(log_t_pred + time_slider.val)
h2s[j].set_xdata(log_t_ref + time_slider.val)
y1 = self.mpc_x_f[rounded, j, :] - self.planner_xref[rounded, j, 1:]
y2 = self.planner_xref[rounded, j, :] - self.planner_xref[rounded, j, :]
h1s[j].set_ydata(y1)
h2s[j].set_ydata(y2)
axs[j].set_xlim([time_slider.val - self.dt * 3, time_slider.val+trange+self.dt * 3])
ymin = np.min([np.min(y1), np.min(y2)])
ymax = np.max([np.max(y1), np.max(y2)])
axs[j].set_ylim([ymin - 0.05 * (ymax - ymin), ymax + 0.05 * (ymax - ymin)])
fig.canvas.draw_idle()
if not recursive:
update_vel(time_slider.val, True)
def update_vel(val, recursive=False):
time_slider_vel.val = np.round(val / (self.dt*10), decimals=0) * (self.dt*10)
rounded = int(np.round(time_slider_vel.val / self.dt, decimals=0))
for j in range(6):
h1s_vel[j].set_xdata(log_t_pred + time_slider.val)
h2s_vel[j].set_xdata(log_t_ref + time_slider.val)
y1 = self.mpc_x_f[rounded, j+6, :]
y2 = self.planner_xref[rounded, j+6, :]
h1s_vel[j].set_ydata(y1)
h2s_vel[j].set_ydata(y2)
axs_vel[j].set_xlim([time_slider.val - self.dt * 3, time_slider.val+trange+self.dt * 3])
ymin = np.min([np.min(y1), np.min(y2)])
ymax = np.max([np.max(y1), np.max(y2)])
axs_vel[j].set_ylim([ymin - 0.05 * (ymax - ymin), ymax + 0.05 * (ymax - ymin)])
fig_vel.canvas.draw_idle()
if not recursive:
update(time_slider_vel.val, True)
# register the update function with each slider
time_slider.on_changed(update)
time_slider_vel.on_changed(update)
plt.show()
def slider_predicted_footholds(self):
from matplotlib import pyplot as plt
from matplotlib.widgets import Slider, Button
import utils_mpc
import pinocchio as pin
self.planner_fsteps
# Define initial parameters
init_time = 0.0
# Create the figure and the line that we will manipulate
fig = plt.figure()
ax = plt.gca()
h1s = []
f_c = ["r", "b", "forestgreen", "rebeccapurple"]
quat = np.zeros((4, 1))
fsteps = self.planner_fsteps[0]
o_step = np.zeros((3*int(fsteps.shape[0]), 1))
RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[0, 3:7]).toRotationMatrix())
quat[:, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(np.array([0.0, 0.0, RPY[2]]))).coeffs()
oRh = pin.Quaternion(quat).toRotationMatrix()
for j in range(4):
o_step[0:3, 0:1] = oRh @ fsteps[0:1, (j*3):((j+1)*3)].transpose() + self.loop_o_q[0:1, 0:3].transpose()
h1, = plt.plot(o_step[0::3, 0], o_step[1::3, 0], linestyle=None, linewidth=0, marker="o", color=f_c[j])
h1s.append(h1)
axcolor = 'lightgoldenrodyellow'
# Make a horizontal slider to control the time.
axtime = plt.axes([0.25, 0.03, 0.65, 0.03], facecolor=axcolor)
time_slider = Slider(
ax=axtime,
label='Time [s]',
valmin=0.0,
valmax=self.logSize*self.dt,
valinit=init_time,
)
ax.set_xlim([-0.3, 0.5])
ax.set_ylim([-0.3, 0.5])
# The function to be called anytime a slider's value changes
def update(val):
time_slider.val = np.round(val / (self.dt*10), decimals=0) * (self.dt*10)
rounded = int(np.round(time_slider.val / self.dt, decimals=0))
fsteps = self.planner_fsteps[rounded]
o_step = np.zeros((3*int(fsteps.shape[0]), 1))
RPY = pin.rpy.matrixToRpy(pin.Quaternion(self.loop_o_q[rounded, 3:7]).toRotationMatrix())
quat[:, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(np.array([0.0, 0.0, RPY[2]]))).coeffs()
oRh = pin.Quaternion(quat).toRotationMatrix()
for j in range(4):
for k in range(int(fsteps.shape[0])):
o_step[(3*k):(3*(k+1)), 0:1] = oRh @ fsteps[(k):(k+1), (j*3):((j+1)*3)].transpose() + self.loop_o_q[rounded:(rounded+1), 0:3].transpose()
h1s[j].set_xdata(o_step[0::3, 0].copy())
h1s[j].set_ydata(o_step[1::3, 0].copy())
fig.canvas.draw_idle()
# register the update function with each slider
time_slider.on_changed(update)
plt.show()
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/odri/data_2021_10_07_17_40.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, fileName= file_name)
# Call all ploting functions
logger.plotAll(loggerSensors)
# logger.slider_predicted_trajectory()