diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 1cd85f214b27edb3200dde054e86bfb864577bbc..3d93e3ab80cca80b8878b9e301c2b058dff0fdbc 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -31,8 +31,53 @@ install(TARGETS ${PY_NAME} DESTINATION ${${PY_NAME}_INSTALL_DIR})
set(${PY_NAME}_PYTHON
__init__.py
+ Controller.py
+ main_solo12_control.py
+ main_solo12_replay.py
+ MPC_Wrapper.py
+ )
+
+set(${PY_NAME}_CROCODDYL_PYTHON
+ __init__.py
+ MPC_crocoddyl.py
+ MPC_crocoddyl_2.py
+ MPC_crocoddyl_planner.py
+ )
+
+set(${PY_NAME}_SOLO3D_PYTHON
+ __init__.py
+ heightmap_generator.py
+ heightmap_tools.py
+ pyb_environment_3D.py
+ SurfacePlanner.py
+ SurfacePlannerWrapper.py
+ utils.py
+ )
+
+set(${PY_NAME}_TOOLS_PYTHON
+ ForceMonitor.py
+ gamepadClient.py
+ __init__.py
+ LoggerControl.py
+ LoggerSensors.py
+ place_com_with_invkin.py
+ PyBulletSimulator.py
+ qualisysClient.py
+ utils_mpc.py
)
foreach(python ${${PY_NAME}_PYTHON})
PYTHON_INSTALL_ON_SITE(${PY_NAME} ${python})
-endforeach()
\ No newline at end of file
+endforeach()
+
+foreach(python ${${PY_NAME}_CROCODDYL_PYTHON})
+ PYTHON_INSTALL_ON_SITE(${PY_NAME}/Crocoddyl ${python})
+endforeach()
+
+foreach(python ${${PY_NAME}_SOLO3D_PYTHON})
+ PYTHON_INSTALL_ON_SITE(${PY_NAME}/solo3D ${python})
+endforeach()
+
+foreach(python ${${PY_NAME}_tools_PYTHON})
+ PYTHON_INSTALL_ON_SITE(${PY_NAME}/tools ${python})
+endforeach()
diff --git a/scripts/Controller.py b/python/quadruped_reactive_walking/Controller.py
similarity index 63%
rename from scripts/Controller.py
rename to python/quadruped_reactive_walking/Controller.py
index af012dc435afeb53601956c74f98baf7fd825f1f..ac6e2a8ad1a286a64212c4f6800df677f54570ea 100644
--- a/scripts/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -1,13 +1,12 @@
-import numpy as np
import time
-import pybullet as pyb
+import numpy as np
import pinocchio as pin
-import quadruped_reactive_walking as qrw
+import pybullet as pyb
-import MPC_Wrapper
-from tools.utils_mpc import init_robot
-from solo3D.utils import quaternionToRPY
+from . import MPC_Wrapper, quadruped_reactive_walking as qrw
+from .solo3D.utils import quaternionToRPY
+from .tools.utils_mpc import init_robot
class Result:
@@ -70,7 +69,6 @@ class dummyDevice:
class Controller:
-
def __init__(self, params, q_init, t):
"""Function that runs a simulation scenario based on a reference velocity profile, an environment and
various parameters to define the gait
@@ -85,7 +83,9 @@ class Controller:
# Parameters definition #
########################################################################
- self.q_security = np.array([1.2, 2.1, 3.14, 1.2, 2.1, 3.14, 1.2, 2.1, 3.14, 1.2, 2.1, 3.14])
+ self.q_security = np.array(
+ [1.2, 2.1, 3.14, 1.2, 2.1, 3.14, 1.2, 2.1, 3.14, 1.2, 2.1, 3.14]
+ )
# Init joint torques to correct shape
self.jointTorques = np.zeros((12, 1))
@@ -95,7 +95,9 @@ class Controller:
# Disable perfect estimator if we are not in simulation
if not params.SIMULATION:
- params.perfectEstimator = False # Cannot use perfect estimator if we are running on real robot
+ params.perfectEstimator = (
+ False # Cannot use perfect estimator if we are running on real robot
+ )
# Initialisation of the solo model/data and of the Gepetto viewer
self.solo = init_robot(q_init, params)
@@ -145,11 +147,12 @@ class Controller:
if params.solo3D:
from solo3D.SurfacePlannerWrapper import Surface_planner_wrapper
+
if self.SIMULATION:
from solo3D.pyb_environment_3D import PybEnvironment3D
self.enable_multiprocessing_mip = params.enable_multiprocessing_mip
- self.offset_perfect_estimator = 0.
+ self.offset_perfect_estimator = 0.0
self.update_mip = False
if self.solo3D:
self.surfacePlanner = Surface_planner_wrapper(params)
@@ -158,24 +161,43 @@ class Controller:
self.statePlanner.initialize(params)
self.footstepPlanner = qrw.FootstepPlannerQP()
- self.footstepPlanner.initialize(params, self.gait, self.surfacePlanner.floor_surface)
+ self.footstepPlanner.initialize(
+ params, self.gait, self.surfacePlanner.floor_surface
+ )
# Trajectory Generator Bezier
x_margin_max_ = 0.06 # margin inside convex surfaces [m].
- t_margin_ = 0.3 # 100*t_margin_% of the curve around critical point. range: [0, 1]
+ t_margin_ = (
+ 0.3 # 100*t_margin_% of the curve around critical point. range: [0, 1]
+ )
z_margin_ = 0.06 # 100*z_margin_% of the curve after the critical point. range: [0, 1]
- N_sample = 8 # Number of sample in the least square optimisation for Bezier coeffs
+ N_sample = (
+ 8 # Number of sample in the least square optimisation for Bezier coeffs
+ )
N_sample_ineq = 10 # Number of sample while browsing the curve
degree = 7 # Degree of the Bezier curve
self.footTrajectoryGenerator = qrw.FootTrajectoryGeneratorBezier()
- self.footTrajectoryGenerator.initialize(params, self.gait, self.surfacePlanner.floor_surface,
- x_margin_max_, t_margin_, z_margin_, N_sample, N_sample_ineq,
- degree)
+ self.footTrajectoryGenerator.initialize(
+ params,
+ self.gait,
+ self.surfacePlanner.floor_surface,
+ x_margin_max_,
+ t_margin_,
+ z_margin_,
+ N_sample,
+ N_sample_ineq,
+ degree,
+ )
if self.SIMULATION:
- self.pybEnvironment3D = PybEnvironment3D(params, self.gait, self.statePlanner, self.footstepPlanner,
- self.footTrajectoryGenerator)
+ self.pybEnvironment3D = PybEnvironment3D(
+ params,
+ self.gait,
+ self.statePlanner,
+ self.footstepPlanner,
+ self.footTrajectoryGenerator,
+ )
else:
self.statePlanner = qrw.StatePlanner()
@@ -267,11 +289,14 @@ class Controller:
b_baseVel_perfect = device.b_baseVel
elif self.solo3D and qc != None:
if self.k <= 1:
- self.initial_pos = [0., 0., -0.046]
- self.initial_matrix = pin.rpy.rpyToMatrix(0., 0., 0.).transpose()
+ self.initial_pos = [0.0, 0.0, -0.046]
+ self.initial_matrix = pin.rpy.rpyToMatrix(0.0, 0.0, 0.0).transpose()
q_perfect[:3] = self.initial_matrix @ (qc.getPosition() - self.initial_pos)
q_perfect[3:] = quaternionToRPY(qc.getOrientationQuat())[:, 0]
- b_baseVel_perfect[:] = (qc.getOrientationMat9().reshape((3, 3)).transpose() @ qc.getVelocity().reshape((3, 1))).ravel()
+ b_baseVel_perfect[:] = (
+ qc.getOrientationMat9().reshape((3, 3)).transpose()
+ @ qc.getVelocity().reshape((3, 1))
+ ).ravel()
if np.isnan(np.sum(q_perfect)):
print("Error: nan values in perfect position of the robot")
@@ -290,14 +315,17 @@ class Controller:
self.last_b_vel = b_baseVel_perfect
self.n_nan = 0
- self.estimator.run(self.gait.matrix,
- self.footTrajectoryGenerator.get_foot_position(),
- device.imu.linear_acceleration,
- device.imu.gyroscope,
- device.imu.attitude_euler,
- device.joints.positions,
- device.joints.velocities,
- q_perfect, b_baseVel_perfect)
+ self.estimator.run(
+ self.gait.matrix,
+ self.footTrajectoryGenerator.get_foot_position(),
+ device.imu.linear_acceleration,
+ device.imu.gyroscope,
+ device.imu.attitude_euler,
+ device.joints.positions,
+ device.joints.velocities,
+ q_perfect,
+ b_baseVel_perfect,
+ )
# Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
self.estimator.update_reference_state(self.joystick.get_v_ref())
@@ -326,7 +354,7 @@ class Controller:
if self.solo3D:
oTh_3d = np.zeros((3, 1))
oTh_3d[:2, 0] = self.q_filter[:2, 0]
- oRh_3d = pin.rpy.rpyToMatrix(0., 0., self.q_filter[5, 0])
+ oRh_3d = pin.rpy.rpyToMatrix(0.0, 0.0, self.q_filter[5, 0])
t_filter = time.time()
self.t_filter = t_filter - t_start
@@ -336,19 +364,32 @@ class Controller:
self.update_mip = self.k % self.k_mpc == 0 and self.gait.is_new_step()
if self.solo3D:
if self.update_mip:
- self.statePlanner.update_surface(self.q_filter[:6, :1], self.vref_filt_mpc[:6, :1])
+ self.statePlanner.update_surface(
+ self.q_filter[:6, :1], self.vref_filt_mpc[:6, :1]
+ )
if self.surfacePlanner.initialized:
self.error = self.surfacePlanner.get_latest_results()
- self.footstepPlanner.update_surfaces(self.surfacePlanner.potential_surfaces, self.surfacePlanner.selected_surfaces,
- self.surfacePlanner.mip_success, self.surfacePlanner.mip_iteration)
-
- self.o_targetFootstep = self.footstepPlanner.update_footsteps(self.k % self.k_mpc == 0 and self.k != 0,
- int(self.k_mpc - self.k % self.k_mpc),
- self.q_filter[:, 0], self.h_v_windowed[:6, :1].copy(),
- self.v_ref[:6, :1])
-
- self.statePlanner.compute_reference_states(self.q_filter[:6, :1], self.h_v_filt_mpc[:6, :1].copy(), self.vref_filt_mpc[:6, :1])
+ self.footstepPlanner.update_surfaces(
+ self.surfacePlanner.potential_surfaces,
+ self.surfacePlanner.selected_surfaces,
+ self.surfacePlanner.mip_success,
+ self.surfacePlanner.mip_iteration,
+ )
+
+ self.o_targetFootstep = self.footstepPlanner.update_footsteps(
+ self.k % self.k_mpc == 0 and self.k != 0,
+ int(self.k_mpc - self.k % self.k_mpc),
+ self.q_filter[:, 0],
+ self.h_v_windowed[:6, :1].copy(),
+ self.v_ref[:6, :1],
+ )
+
+ self.statePlanner.compute_reference_states(
+ self.q_filter[:6, :1],
+ self.h_v_filt_mpc[:6, :1].copy(),
+ self.vref_filt_mpc[:6, :1],
+ )
xref = self.statePlanner.get_reference_states()
fsteps = self.footstepPlanner.get_footsteps()
@@ -356,10 +397,17 @@ class Controller:
if self.update_mip and self.solo3D:
configs = self.statePlanner.get_configurations().transpose()
- self.surfacePlanner.run(configs, gait_matrix, self.o_targetFootstep, self.vref_filt_mpc[:3, 0].copy())
+ self.surfacePlanner.run(
+ configs,
+ gait_matrix,
+ self.o_targetFootstep,
+ self.vref_filt_mpc[:3, 0].copy(),
+ )
self.surfacePlanner.initialized = True
if not self.enable_multiprocessing_mip and self.SIMULATION:
- self.pybEnvironment3D.update_target_SL1M(self.surfacePlanner.all_feet_pos_syn)
+ self.pybEnvironment3D.update_target_SL1M(
+ self.surfacePlanner.all_feet_pos_syn
+ )
t_planner = time.time()
self.t_planner = t_planner - t_filter
@@ -370,14 +418,25 @@ 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, gait_matrix, l_targetFootstep, oRh, oTh,
- self.footTrajectoryGenerator.get_foot_position(),
- self.footTrajectoryGenerator.get_foot_velocity(),
- self.footTrajectoryGenerator.get_foot_acceleration(),
- self.footTrajectoryGenerator.get_foot_jerk(),
- self.footTrajectoryGenerator.get_phase_durations() - self.footTrajectoryGenerator.get_elapsed_durations())
+ self.mpc_wrapper.solve(
+ self.k,
+ xref,
+ fsteps,
+ gait_matrix,
+ l_targetFootstep,
+ oRh,
+ oTh,
+ self.footTrajectoryGenerator.get_foot_position(),
+ self.footTrajectoryGenerator.get_foot_velocity(),
+ self.footTrajectoryGenerator.get_foot_acceleration(),
+ self.footTrajectoryGenerator.get_foot_jerk(),
+ self.footTrajectoryGenerator.get_phase_durations()
+ - self.footTrajectoryGenerator.get_elapsed_durations(),
+ )
else:
- self.mpc_wrapper.solve(self.k, xref, fsteps, gait_matrix, np.zeros((3, 4)))
+ self.mpc_wrapper.solve(
+ self.k, xref, fsteps, gait_matrix, np.zeros((3, 4))
+ )
except ValueError:
print("MPC Problem")
self.x_f_mpc, self.mpc_cost = self.mpc_wrapper.get_latest_result()
@@ -392,11 +451,18 @@ class Controller:
id = 0
while gait_matrix[id, foot] == 0:
id += 1
- self.o_targetFootstep[:2, foot] = self.x_f_mpc[24 + 2*foot:24+2*foot+2, id+1]
+ self.o_targetFootstep[:2, foot] = self.x_f_mpc[
+ 24 + 2 * foot : 24 + 2 * foot + 2, id + 1
+ ]
# Update pos, vel and acc references for feet
if self.solo3D:
- self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep, self.surfacePlanner.selected_surfaces, self.q_filter)
+ self.footTrajectoryGenerator.update(
+ self.k,
+ self.o_targetFootstep,
+ self.surfacePlanner.selected_surfaces,
+ self.q_filter,
+ )
else:
self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep)
@@ -424,34 +490,64 @@ class Controller:
self.q_wbc[6:, 0] = self.wbcWrapper.qdes[:]
# Update velocity vector for wbc
- self.dq_wbc[:6, 0] = self.estimator.get_v_estimate()[:6] # Â Velocities in base frame (not horizontal frame!)
- self.dq_wbc[6:, 0] = self.wbcWrapper.vdes[:] # with reference angular velocities of previous loop
+ self.dq_wbc[:6, 0] = self.estimator.get_v_estimate()[
+ :6
+ ] # Â Velocities in base frame (not horizontal frame!)
+ self.dq_wbc[6:, 0] = self.wbcWrapper.vdes[
+ :
+ ] # with reference angular velocities of previous loop
# Feet command position, velocity and acceleration in base frame
if self.solo3D: # Use estimated base frame
- self.feet_a_cmd = self.footTrajectoryGenerator.get_foot_acceleration_base_frame(
- oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_v_cmd = self.footTrajectoryGenerator.get_foot_velocity_base_frame(
- oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_p_cmd = self.footTrajectoryGenerator.get_foot_position_base_frame(
- oRh_3d.transpose(), oTh_3d + np.array([[0.0], [0.0], [xref[2, 1]]]))
+ self.feet_a_cmd = (
+ self.footTrajectoryGenerator.get_foot_acceleration_base_frame(
+ oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1))
+ )
+ )
+ self.feet_v_cmd = (
+ self.footTrajectoryGenerator.get_foot_velocity_base_frame(
+ oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1))
+ )
+ )
+ self.feet_p_cmd = (
+ self.footTrajectoryGenerator.get_foot_position_base_frame(
+ oRh_3d.transpose(),
+ oTh_3d + np.array([[0.0], [0.0], [xref[2, 1]]]),
+ )
+ )
else: # Use ideal base frame
- self.feet_a_cmd = self.footTrajectoryGenerator.get_foot_acceleration_base_frame(
- hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_v_cmd = self.footTrajectoryGenerator.get_foot_velocity_base_frame(
- hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_p_cmd = self.footTrajectoryGenerator.get_foot_position_base_frame(
- hRb @ oRh.transpose(), oTh + np.array([[0.0], [0.0], [self.h_ref]]))
-
- self.xgoals[6:, 0] = self.vref_filt_mpc[:, 0] # Velocities (in horizontal frame!)
+ self.feet_a_cmd = (
+ self.footTrajectoryGenerator.get_foot_acceleration_base_frame(
+ hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1))
+ )
+ )
+ self.feet_v_cmd = (
+ self.footTrajectoryGenerator.get_foot_velocity_base_frame(
+ hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1))
+ )
+ )
+ self.feet_p_cmd = (
+ self.footTrajectoryGenerator.get_foot_position_base_frame(
+ hRb @ oRh.transpose(),
+ oTh + np.array([[0.0], [0.0], [self.h_ref]]),
+ )
+ )
+
+ self.xgoals[6:, 0] = self.vref_filt_mpc[
+ :, 0
+ ] # Velocities (in horizontal frame!)
# Run InvKin + WBC QP
- self.wbcWrapper.compute(self.q_wbc, self.dq_wbc,
- (self.x_f_mpc[12:24, 0:1]).copy(), np.array([gait_matrix[0, :]]),
- self.feet_p_cmd,
- self.feet_v_cmd,
- self.feet_a_cmd,
- self.xgoals)
+ self.wbcWrapper.compute(
+ self.q_wbc,
+ self.dq_wbc,
+ (self.x_f_mpc[12:24, 0:1]).copy(),
+ np.array([gait_matrix[0, :]]),
+ self.feet_p_cmd,
+ self.feet_v_cmd,
+ self.feet_a_cmd,
+ self.xgoals,
+ )
# Quantities sent to the control board
self.result.P = np.array(self.Kp_main.tolist() * 4)
self.result.D = np.array(self.Kd_main.tolist() * 4)
@@ -467,7 +563,9 @@ class Controller:
# Display robot in Gepetto corba viewer
if self.enable_corba_viewer and (self.k % 5 == 0):
self.q_display[:3, 0] = self.q_wbc[:3, 0]
- self.q_display[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.q_wbc[3:6, 0])).coeffs()
+ self.q_display[3:7, 0] = pin.Quaternion(
+ pin.rpy.rpyToMatrix(self.q_wbc[3:6, 0])
+ ).coeffs()
self.q_display[7:, 0] = self.q_wbc[6:, 0]
self.solo.display(self.q_display)
@@ -493,11 +591,19 @@ class Controller:
def pyb_camera(self, device, yaw):
"""
- Update position of PyBullet camera on the robot position to do as if it was attached to the robot
+ Update position of PyBullet camera on the robot position to do as if it was attached to the robot
"""
if self.k > 10 and self.enable_pyb_GUI:
- pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=45, cameraPitch=-39.9,
- cameraTargetPosition=[device.dummyHeight[0], device.dummyHeight[1], 0.0])
+ pyb.resetDebugVisualizerCamera(
+ cameraDistance=0.6,
+ cameraYaw=45,
+ cameraPitch=-39.9,
+ cameraTargetPosition=[
+ device.dummyHeight[0],
+ device.dummyHeight[1],
+ 0.0,
+ ],
+ )
def pyb_debug(self, device, fsteps, gait_matrix, xref):
@@ -507,63 +613,111 @@ class Controller:
oRh_pyb = pin.rpy.rpyToMatrix(0.0, 0.0, device.imu.attitude_euler[2])
for i in range(4):
if not self.solo3D:
- pos = oRh_pyb @ self.feet_p_cmd[:, i:(i+1)] + oTh_pyb
- pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids_deb[i], pos[:, 0].tolist(), [0, 0, 0, 1])
+ pos = oRh_pyb @ self.feet_p_cmd[:, i : (i + 1)] + oTh_pyb
+ pyb.resetBasePositionAndOrientation(
+ device.pyb_sim.ftps_Ids_deb[i], pos[:, 0].tolist(), [0, 0, 0, 1]
+ )
else:
pos = self.o_targetFootstep[:, i]
- pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids_deb[i], pos, [0, 0, 0, 1])
+ pyb.resetBasePositionAndOrientation(
+ device.pyb_sim.ftps_Ids_deb[i], pos, [0, 0, 0, 1]
+ )
# Display desired footstep positions as blue spheres
for i in range(4):
j = 0
cpt = 1
status = gait_matrix[0, i]
- while cpt < gait_matrix.shape[0] and j < device.pyb_sim.ftps_Ids.shape[1]:
+ while (
+ cpt < gait_matrix.shape[0] and j < device.pyb_sim.ftps_Ids.shape[1]
+ ):
while cpt < gait_matrix.shape[0] and gait_matrix[cpt, i] == status:
cpt += 1
if cpt < gait_matrix.shape[0]:
status = gait_matrix[cpt, i]
if status:
- pos = oRh_pyb @ fsteps[cpt, (3*i):(3*(i+1))].reshape((-1, 1)) + oTh_pyb - np.array([[0.0], [0.0], [oTh_pyb[2, 0]]])
+ pos = (
+ oRh_pyb
+ @ fsteps[cpt, (3 * i) : (3 * (i + 1))].reshape((-1, 1))
+ + oTh_pyb
+ - np.array([[0.0], [0.0], [oTh_pyb[2, 0]]])
+ )
pyb.resetBasePositionAndOrientation(
- device.pyb_sim.ftps_Ids[i, j], pos[:, 0].tolist(), [0, 0, 0, 1])
+ device.pyb_sim.ftps_Ids[i, j],
+ pos[:, 0].tolist(),
+ [0, 0, 0, 1],
+ )
else:
- pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids[i, j], [0.0, 0.0, -0.1], [0, 0, 0, 1])
+ pyb.resetBasePositionAndOrientation(
+ device.pyb_sim.ftps_Ids[i, j],
+ [0.0, 0.0, -0.1],
+ [0, 0, 0, 1],
+ )
j += 1
# Hide unused spheres underground
for k in range(j, device.pyb_sim.ftps_Ids.shape[1]):
- pyb.resetBasePositionAndOrientation(device.pyb_sim.ftps_Ids[i, k], [0.0, 0.0, -0.1], [0, 0, 0, 1])
+ pyb.resetBasePositionAndOrientation(
+ device.pyb_sim.ftps_Ids[i, k], [0.0, 0.0, -0.1], [0, 0, 0, 1]
+ )
# Display reference trajectory
xref_rot = np.zeros((3, xref.shape[1]))
for i in range(xref.shape[1]):
- xref_rot[:, i:(i+1)] = oRh_pyb @ xref[:3, i:(i+1)] + oTh_pyb + np.array([[0.0], [0.0], [0.05 - self.h_ref]])
+ xref_rot[:, i : (i + 1)] = (
+ oRh_pyb @ xref[:3, i : (i + 1)]
+ + oTh_pyb
+ + np.array([[0.0], [0.0], [0.05 - self.h_ref]])
+ )
if len(device.pyb_sim.lineId_red) == 0:
- for i in range(xref.shape[1]-1):
- device.pyb_sim.lineId_red.append(pyb.addUserDebugLine(
- xref_rot[:3, i].tolist(), xref_rot[:3, i+1].tolist(), lineColorRGB=[1.0, 0.0, 0.0], lineWidth=8))
+ for i in range(xref.shape[1] - 1):
+ device.pyb_sim.lineId_red.append(
+ pyb.addUserDebugLine(
+ xref_rot[:3, i].tolist(),
+ xref_rot[:3, i + 1].tolist(),
+ lineColorRGB=[1.0, 0.0, 0.0],
+ lineWidth=8,
+ )
+ )
else:
- for i in range(xref.shape[1]-1):
- device.pyb_sim.lineId_red[i] = pyb.addUserDebugLine(xref_rot[:3, i].tolist(), xref_rot[:3, i+1].tolist(),
- lineColorRGB=[1.0, 0.0, 0.0], lineWidth=8,
- replaceItemUniqueId=device.pyb_sim.lineId_red[i])
+ for i in range(xref.shape[1] - 1):
+ device.pyb_sim.lineId_red[i] = pyb.addUserDebugLine(
+ xref_rot[:3, i].tolist(),
+ xref_rot[:3, i + 1].tolist(),
+ lineColorRGB=[1.0, 0.0, 0.0],
+ lineWidth=8,
+ replaceItemUniqueId=device.pyb_sim.lineId_red[i],
+ )
# Display predicted trajectory
x_f_mpc_rot = np.zeros((3, self.x_f_mpc.shape[1]))
for i in range(self.x_f_mpc.shape[1]):
- x_f_mpc_rot[:, i:(i+1)] = oRh_pyb @ self.x_f_mpc[:3, i:(i+1)] + oTh_pyb + np.array([[0.0], [0.0], [0.05 - self.h_ref]])
+ x_f_mpc_rot[:, i : (i + 1)] = (
+ oRh_pyb @ self.x_f_mpc[:3, i : (i + 1)]
+ + oTh_pyb
+ + np.array([[0.0], [0.0], [0.05 - self.h_ref]])
+ )
if len(device.pyb_sim.lineId_blue) == 0:
- for i in range(self.x_f_mpc.shape[1]-1):
- device.pyb_sim.lineId_blue.append(pyb.addUserDebugLine(
- x_f_mpc_rot[:3, i].tolist(), x_f_mpc_rot[:3, i+1].tolist(), lineColorRGB=[0.0, 0.0, 1.0], lineWidth=8))
+ for i in range(self.x_f_mpc.shape[1] - 1):
+ device.pyb_sim.lineId_blue.append(
+ pyb.addUserDebugLine(
+ x_f_mpc_rot[:3, i].tolist(),
+ x_f_mpc_rot[:3, i + 1].tolist(),
+ lineColorRGB=[0.0, 0.0, 1.0],
+ lineWidth=8,
+ )
+ )
else:
- for i in range(self.x_f_mpc.shape[1]-1):
- device.pyb_sim.lineId_blue[i] = pyb.addUserDebugLine(x_f_mpc_rot[:3, i].tolist(), x_f_mpc_rot[:3, i+1].tolist(),
- lineColorRGB=[0.0, 0.0, 1.0], lineWidth=8,
- replaceItemUniqueId=device.pyb_sim.lineId_blue[i])
+ for i in range(self.x_f_mpc.shape[1] - 1):
+ device.pyb_sim.lineId_blue[i] = pyb.addUserDebugLine(
+ x_f_mpc_rot[:3, i].tolist(),
+ x_f_mpc_rot[:3, i + 1].tolist(),
+ lineColorRGB=[0.0, 0.0, 1.0],
+ lineWidth=8,
+ replaceItemUniqueId=device.pyb_sim.lineId_blue[i],
+ )
def security_check(self):
"""
@@ -576,10 +730,10 @@ class Controller:
print(self.estimator.get_q_estimate()[7:])
print(np.abs(self.estimator.get_q_estimate()[7:]) > self.q_security)
self.error = True
- elif (np.abs(self.estimator.get_v_security()) > 100.).any():
+ elif (np.abs(self.estimator.get_v_security()) > 100.0).any():
print("-- VELOCITY TOO HIGH ERROR --")
print(self.estimator.get_v_security())
- print(np.abs(self.estimator.get_v_security()) > 100.)
+ print(np.abs(self.estimator.get_v_security()) > 100.0)
self.error = True
elif (np.abs(self.wbcWrapper.tau_ff) > 8.0).any():
print("-- FEEDFORWARD TORQUES TOO HIGH ERROR --")
@@ -601,29 +755,41 @@ class Controller:
"""
Clamp the result
"""
- hip_max = 120. * np.pi / 180.
- knee_min = 5. * np.pi / 180.
+ hip_max = 120.0 * np.pi / 180.0
+ knee_min = 5.0 * np.pi / 180.0
for i in range(4):
if self.clamp(self.result.q_des[3 * i + 1], -hip_max, hip_max):
print("Clamping hip n " + str(i))
self.error = set_error
- if self.q_init[3 * i + 2] >= 0. and self.clamp(self.result.q_des[3 * i + 2], knee_min):
+ if self.q_init[3 * i + 2] >= 0.0 and self.clamp(
+ self.result.q_des[3 * i + 2], knee_min
+ ):
print("Clamping knee n " + str(i))
self.error = set_error
- elif self.q_init[3 * i + 2] <= 0. and self.clamp(self.result.q_des[3 * i + 2], max_value=-knee_min):
+ elif self.q_init[3 * i + 2] <= 0.0 and self.clamp(
+ self.result.q_des[3 * i + 2], max_value=-knee_min
+ ):
print("Clamping knee n " + str(i))
self.error = set_error
for i in range(12):
- if self.clamp(self.result.q_des[i], device.joints.positions[i] - 4., device.joints.positions[i] + 4.):
+ if self.clamp(
+ self.result.q_des[i],
+ device.joints.positions[i] - 4.0,
+ device.joints.positions[i] + 4.0,
+ ):
print("Clamping position difference of motor n " + str(i))
self.error = set_error
- if self.clamp(self.result.v_des[i], device.joints.velocities[i] - 100., device.joints.velocities[i] + 100.):
+ if self.clamp(
+ self.result.v_des[i],
+ device.joints.velocities[i] - 100.0,
+ device.joints.velocities[i] + 100.0,
+ ):
print("Clamping velocity of motor n " + str(i))
self.error = set_error
- if self.clamp(self.result.tau_ff[i], -8., 8.):
+ if self.clamp(self.result.tau_ff[i], -8.0, 8.0):
print("Clamping torque of motor n " + str(i))
self.error = set_error
diff --git a/scripts/Crocoddyl/MPC_crocoddyl.py b/python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl.py
similarity index 81%
rename from scripts/Crocoddyl/MPC_crocoddyl.py
rename to python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl.py
index 8adb7fdbadf06b07839c23ce92c4f903a5a4f3d2..024e2a1ae6f28798a862d8d85f18e4ac4aed1162 100644
--- a/scripts/Crocoddyl/MPC_crocoddyl.py
+++ b/python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl.py
@@ -3,9 +3,8 @@
import crocoddyl
import numpy as np
import quadruped_walkgen as quadruped_walkgen
-import pinocchio as pin
-np.set_printoptions(formatter={'float': lambda x: "{0:0.7f}".format(x)}, linewidth=450)
+np.set_printoptions(formatter={"float": lambda x: "{0:0.7f}".format(x)}, linewidth=450)
class MPC_crocoddyl:
@@ -24,7 +23,9 @@ class MPC_crocoddyl:
self.dt = params.dt_mpc # Time step of the solver
self.n_nodes = int(params.gait.shape[0]) # Number of nodes
self.mass = params.mass # Mass of the robot
- 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 ody frame
# Friction coefficient
if inner:
@@ -59,7 +60,7 @@ class MPC_crocoddyl:
self.min_fz = 0.2 # Minimum normal force (N)
self.max_fz = 25 # Maximum normal force (N)
- self.shoulderWeights = 0. # Weight on the shoulder term :
+ self.shoulderWeights = 0.0 # Weight on the shoulder term :
self.shoulder_hlim = 0.235 # shoulder maximum height
# Integration scheme
@@ -81,15 +82,25 @@ class MPC_crocoddyl:
# Action models
self.ListAction = []
if linearModel:
- self.ListAction = [quadruped_walkgen.ActionModelQuadruped(self.offset_com) for _ in range(self.n_nodes)]
+ self.ListAction = [
+ quadruped_walkgen.ActionModelQuadruped(self.offset_com)
+ for _ in range(self.n_nodes)
+ ]
self.terminalModel = quadruped_walkgen.ActionModelQuadruped(self.offset_com)
else:
- self.ListAction = [quadruped_walkgen.ActionModelQuadrupedNonLinear(self.offset_com) for _ in range(self.n_nodes)]
- self.terminalModel = quadruped_walkgen.ActionModelQuadrupedNonLinear(self.offset_com)
+ self.ListAction = [
+ quadruped_walkgen.ActionModelQuadrupedNonLinear(self.offset_com)
+ for _ in range(self.n_nodes)
+ ]
+ self.terminalModel = quadruped_walkgen.ActionModelQuadrupedNonLinear(
+ self.offset_com
+ )
self.updateActionModels()
# Shooting problem
- self.problem = crocoddyl.ShootingProblem(np.zeros(12), self.ListAction, self.terminalModel)
+ self.problem = crocoddyl.ShootingProblem(
+ np.zeros(12), self.ListAction, self.terminalModel
+ )
# DDP Solver
self.ddp = crocoddyl.SolverDDP(self.problem)
@@ -115,24 +126,30 @@ class MPC_crocoddyl:
# Construction of the gait matrix representing the feet in contact with the ground
self.index = self.n_nodes
- self.gait[:self.index, :] = 1.0 - (self.fsteps[:self.index, 0::3] == 0.0)
- self.gait[self.index:, :] = 0.0
+ self.gait[: self.index, :] = 1.0 - (self.fsteps[: self.index, 0::3] == 0.0)
+ self.gait[self.index :, :] = 0.0
# Iterate over all phases of the gait
# The first column of xref correspond to the current state
for j in range(self.index):
# Update model
- self.ListAction[j].updateModel(np.reshape(self.fsteps[j, :], (3, 4), order='F'), xref[:, j],
- self.gait[j, :])
+ self.ListAction[j].updateModel(
+ np.reshape(self.fsteps[j, :], (3, 4), order="F"),
+ xref[:, j],
+ self.gait[j, :],
+ )
# Update model of the terminal model
- self.terminalModel.updateModel(np.reshape(self.fsteps[self.index - 1, :], (3, 4), order='F'), xref[:, -1],
- self.gait[self.index - 1, :])
+ self.terminalModel.updateModel(
+ np.reshape(self.fsteps[self.index - 1, :], (3, 4), order="F"),
+ xref[:, -1],
+ self.gait[self.index - 1, :],
+ )
return 0
def solve(self, k, xref, fsteps):
- """ Solve the MPC problem
+ """Solve the MPC problem
Args:
k : Iteration
@@ -152,7 +169,10 @@ class MPC_crocoddyl:
if self.warm_start and k != 0:
self.u_init = self.ddp.us[1:].tolist()
- self.u_init.append(np.repeat(self.gait[self.index - 1, :], 3) * np.array(4 * [0.5, 0.5, 5.]))
+ self.u_init.append(
+ np.repeat(self.gait[self.index - 1, :], 3)
+ * np.array(4 * [0.5, 0.5, 5.0])
+ )
self.x_init = self.ddp.xs[2:].tolist()
self.x_init.insert(0, self.xref[:, 0])
@@ -190,7 +210,7 @@ class MPC_crocoddyl:
return np.array(self.ddp.us)[:, :].transpose()[:, :]
def initializeActionModel(self, model, terminal=False):
- """ Initialize an action model with the parameters"""
+ """Initialize an action model with the parameters"""
# Model parameters
model.dt = self.dt
model.mass = self.mass
@@ -203,7 +223,7 @@ class MPC_crocoddyl:
model.stateWeights = self.stateWeights
if terminal:
model.forceWeights = np.zeros(12)
- model.frictionWeights = 0.
+ model.frictionWeights = 0.0
else:
model.max_fz = self.max_fz
model.forceWeights = self.forceWeights
diff --git a/python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl_2.py b/python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl_2.py
new file mode 100644
index 0000000000000000000000000000000000000000..50eefe8908b78f819b7f935288ff74c52e37032f
--- /dev/null
+++ b/python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl_2.py
@@ -0,0 +1,627 @@
+# coding: utf8
+
+import crocoddyl
+import numpy as np
+import quadruped_walkgen
+
+
+class MPC_crocoddyl_2:
+ """
+ Wrapper class for the MPC problem to call the ddp solver and
+ retrieve the results.
+
+ Args:
+ dt (float): time step of the MPC
+ T_mpc (float): Duration of the prediction horizon
+ mu (float): Friction coefficient
+ inner(bool): Inside or outside approximation of the friction cone
+ linearModel(bool) : Approximation in the cross product by using desired state
+ """
+
+ def __init__(
+ self,
+ dt=0.02,
+ T_mpc=0.32,
+ mu=1,
+ inner=True,
+ linearModel=True,
+ n_period=1,
+ dt_tsid=0.001,
+ ):
+
+ # Time step of the solver
+ self.dt = dt
+
+ # Period of the MPC
+ self.T_mpc = T_mpc
+
+ # Mass of the robot
+ self.mass = 2.50000279
+
+ # linearModel :
+ self.linearModel = linearModel
+
+ # Inertia matrix of the robot in body frame
+ # self.gI = np.diag([0.00578574, 0.01938108, 0.02476124])
+ self.gI = np.array(
+ [
+ [3.09249e-2, -8.00101e-7, 1.865287e-5],
+ [-8.00101e-7, 5.106100e-2, 1.245813e-4],
+ [1.865287e-5, 1.245813e-4, 6.939757e-2],
+ ]
+ )
+
+ # Friction coefficient
+ if inner:
+ self.mu = (1 / np.sqrt(2)) * mu
+ else:
+ self.mu = mu
+
+ # Integration model :
+ # Implicit : V+ = V + B*U ; P+ = P + dt*V+ = P+ + dt*V + dt*B*U
+ # Explicit : V+ = V + B*U ; P+ = P + dt*V
+ self.implicit_integration = True
+
+ # Gain from OSQP MPC
+ self.w_x = np.sqrt(0.5)
+ self.w_y = np.sqrt(0.5)
+ self.w_z = np.sqrt(2.0)
+ self.w_roll = np.sqrt(0.11)
+ self.w_pitch = np.sqrt(0.11)
+ self.w_yaw = np.sqrt(0.11)
+ self.w_vx = np.sqrt(2.0 * np.sqrt(0.5))
+ self.w_vy = np.sqrt(2.0 * np.sqrt(0.5))
+ self.w_vz = np.sqrt(2.0 * np.sqrt(2.0))
+ self.w_vroll = np.sqrt(0.05 * np.sqrt(0.11))
+ self.w_vpitch = np.sqrt(0.05 * np.sqrt(0.11))
+ self.w_vyaw = np.sqrt(0.05 * np.sqrt(0.11))
+
+ self.stateWeight = 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,
+ ]
+ )
+
+ # Weight Vector : Force Norm
+ self.forceWeights = np.array(4 * [0.01, 0.01, 0.01])
+ self.relative_forces = True
+ # self.forceWeights = np.zeros(12)
+
+ # Weight Vector : Friction cone cost
+ self.frictionWeights = 1.0
+
+ # Max iteration ddp solver
+ self.max_iteration = 10
+
+ # Warm Start for the solver
+ self.warm_start = True
+
+ # Minimum normal force (N)
+ self.min_fz = 0.2
+ self.max_fz = 25
+
+ # Gait matrix
+ self.gait = np.zeros((20, 5))
+ self.index = 0
+
+ # Weight on the shoulder term :
+ self.shoulderWeights = 2.0
+ self.shoulder_hlim = 0.225
+
+ # Position of the feet
+ self.fsteps = np.full((20, 13), np.nan)
+
+ # List of the actionModel
+ self.ListAction = []
+
+ # dt TSID
+ self.dt_tsid = dt_tsid
+ self.initial_node = 5
+ self.k_mpc = int(self.dt / self.dt_tsid)
+ self.xref = np.zeros(
+ (12, int(self.T_mpc / self.dt) * n_period - 1 + self.initial_node)
+ )
+ self.n_period = n_period
+ self.dt_vector = None
+
+ # Initialisation of the List model using ActionQuadrupedModel()
+ # The same model cannot be used [model]*(T_mpc/dt) because the dynamic
+ # model changes for each nodes.
+ for j in range(self.initial_node):
+ if linearModel:
+ model = quadruped_walkgen.ActionModelQuadruped()
+ else:
+ model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
+
+ # Model parameters
+ if j == self.initial_node - 1:
+ model.dt = self.dt - self.dt_tsid * (self.initial_node - 1)
+ else:
+ model.dt = self.dt_tsid
+ model.mass = self.mass
+ model.gI = self.gI
+ model.mu = self.mu
+ model.min_fz = self.min_fz
+ model.max_fz = self.max_fz
+
+ # Weights vectors
+ model.stateWeights = self.stateWeight
+ model.forceWeights = self.forceWeights
+ model.frictionWeights = self.frictionWeights
+ # shoulder term :
+ model.shoulderWeights = self.shoulderWeights
+ model.shoulder_hlim = self.shoulder_hlim
+
+ # Relative forces
+ model.relative_forces = self.relative_forces
+
+ # Integration
+ model.implicit_integration = self.implicit_integration
+
+ # Add model to the list of model
+ self.ListAction.append(model)
+
+ # -1 because the first node has been replaced by the 5
+ for i in range(int(self.T_mpc / self.dt) * n_period - 1):
+
+ if linearModel:
+ model = quadruped_walkgen.ActionModelQuadruped()
+ else:
+ model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
+
+ # Model parameters
+ model.dt = self.dt
+ model.mass = self.mass
+ model.gI = self.gI
+ model.mu = self.mu
+ model.min_fz = self.min_fz
+ model.max_fz = self.max_fz
+
+ # Weights vectors
+ model.stateWeights = self.stateWeight
+ model.forceWeights = self.forceWeights
+ model.frictionWeights = self.frictionWeights
+ # shoulder term :
+ model.shoulderWeights = self.shoulderWeights
+ model.shoulder_hlim = self.shoulder_hlim
+
+ # Relative forces
+ model.relative_forces = self.relative_forces
+
+ # Integration
+ model.implicit_integration = self.implicit_integration
+
+ # Add model to the list of model
+ self.ListAction.append(model)
+
+ # Terminal Node
+ if linearModel:
+ self.terminalModel = quadruped_walkgen.ActionModelQuadruped()
+ else:
+ self.terminalModel = quadruped_walkgen.ActionModelQuadrupedNonLinear()
+
+ # Model parameters of terminal node
+ self.terminalModel.dt = self.dt
+ self.terminalModel.mass = self.mass
+ self.terminalModel.gI = self.gI
+ self.terminalModel.mu = self.mu
+ self.terminalModel.min_fz = self.min_fz
+ self.terminalModel.shoulderWeights = self.shoulderWeights
+ self.terminalModel.shoulder_hlim = self.shoulder_hlim
+
+ # Weights vectors of terminal node
+ self.terminalModel.stateWeights = self.stateWeight
+ self.terminalModel.forceWeights = np.zeros(12)
+ self.terminalModel.frictionWeights = 0.0
+
+ # Relative forces ; useless here since no command
+ self.terminalModel.relative_forces = self.relative_forces
+
+ # Integration
+ self.terminalModel.implicit_integration = self.implicit_integration
+
+ # Shooting problem
+ self.problem = crocoddyl.ShootingProblem(
+ np.zeros(12), self.ListAction, self.terminalModel
+ )
+
+ # DDP Solver
+ self.ddp = crocoddyl.SolverDDP(self.problem)
+
+ # Warm start
+ self.x_init = []
+ self.u_init = []
+
+ def new_model(self):
+ if self.linearModel:
+ model = quadruped_walkgen.ActionModelQuadruped()
+ else:
+ model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
+
+ # Model parameters
+ model.dt = self.dt
+ model.mass = self.mass
+ model.gI = self.gI
+ model.mu = self.mu
+ model.min_fz = self.min_fz
+ model.max_fz = self.max_fz
+
+ # Weights vectors
+ model.stateWeights = self.stateWeight
+ model.forceWeights = self.forceWeights
+ model.frictionWeights = self.frictionWeights
+ # shoulder term :
+ model.shoulderWeights = self.shoulderWeights
+ model.shoulder_hlim = self.shoulder_hlim
+
+ # Relative forces
+ model.relative_forces = self.relative_forces
+
+ # Integration
+ model.implicit_integration = self.implicit_integration
+
+ return model
+
+ def updateProblem(self, k, fsteps, xref_, lC, abg, lV, lW, v_ref, h_ref=0.2027682):
+ """Update the dynamic of the model list according to the predicted position of the feet,
+ and the desired state.
+
+ Args:
+ fsteps (6x13): Position of the feet in local frame
+ xref (12x17): Desired state vector for the whole gait cycle
+ (the initial state is the first column)
+ """
+ # Update position of the feet
+ self.fsteps[:, :] = fsteps[:, :]
+
+ # Construction of the gait matrix representing the feet in contact with the ground
+ self.index = next(
+ (idx for idx, val in np.ndenumerate(self.fsteps[:, 0]) if val == 0.0), 0.0
+ )[0]
+ self.gait[:, 0] = self.fsteps[:, 0]
+ self.gait[: self.index, 1:] = 1.0 - (
+ np.isnan(self.fsteps[: self.index, 1::3])
+ | (self.fsteps[: self.index, 1::3] == 0.0)
+ )
+ # Replace NaN values by zeroes
+ self.fsteps[np.isnan(self.fsteps)] = 0.0
+
+ # 5 first nodes
+ k_remaining = self.k_mpc - k % self.k_mpc
+
+ N_total = int(self.T_mpc / self.dt) * self.n_period - 1 + self.initial_node
+
+ if k_remaining >= self.initial_node:
+ nb_total = (
+ int(self.T_mpc / self.dt) * self.n_period - 1 + self.initial_node + 1
+ )
+ self.dt_vector = np.zeros(nb_total)
+ dt_list = np.zeros(nb_total)
+ else:
+ nb_total = (
+ int(self.T_mpc / self.dt) * self.n_period - 2 + self.initial_node + 1
+ )
+ self.dt_vector = np.zeros(nb_total)
+ dt_list = np.zeros(nb_total)
+
+ self.xref = np.zeros((12, nb_total))
+
+ # Xref
+ # Update the current state
+ self.xref[0:3, 0:1] = lC
+ self.xref[3:6, 0:1] = abg
+ self.xref[6:9, 0:1] = lV
+ self.xref[9:12, 0:1] = lW
+
+ # Update initial state of the problem
+ self.problem.x0 = self.xref[:, 0]
+
+ self.xref[2, 1:] = h_ref
+
+ for i in range(1, nb_total):
+ if i < self.initial_node:
+ dt_list[i] = self.dt_tsid
+ self.dt_vector[i] = self.dt_vector[i - 1] + self.dt_tsid
+ elif i == self.initial_node:
+ dt_list[i] = self.dt_tsid * k_remaining - self.dt_tsid * (
+ self.initial_node - 1
+ )
+ self.dt_vector[i] = (
+ self.dt_vector[i - 1]
+ + self.dt_tsid * k_remaining
+ - self.dt_tsid * (self.initial_node - 1)
+ )
+
+ else:
+ dt_list[i] = self.dt
+ self.dt_vector[i] = self.dt_vector[i - 1] + self.dt
+
+ # Update x and y velocities taking into account the rotation of the base over the prediction horizon
+ yaw1 = self.dt_vector[1 : self.initial_node] * v_ref[5, 0]
+ self.xref[6, 1 : self.initial_node] = v_ref[0, 0] * np.cos(yaw1) - v_ref[
+ 1, 0
+ ] * np.sin(yaw1)
+ self.xref[7, 1 : self.initial_node] = v_ref[0, 0] * np.sin(yaw1) + v_ref[
+ 1, 0
+ ] * np.cos(yaw1)
+
+ # with dt catch up
+ # 1st node at 0.02
+ yaw2 = self.dt_vector[self.initial_node] * v_ref[5, 0]
+ self.xref[6, self.initial_node] = v_ref[0, 0] * np.cos(yaw2) - v_ref[
+ 1, 0
+ ] * np.sin(yaw2)
+ self.xref[7, self.initial_node] = v_ref[0, 0] * np.sin(yaw2) + v_ref[
+ 1, 0
+ ] * np.cos(yaw2)
+
+ yaw3 = self.dt_vector[self.initial_node + 1 :] * v_ref[5, 0]
+ self.xref[6, self.initial_node + 1 :] = v_ref[0, 0] * np.cos(yaw3) - v_ref[
+ 1, 0
+ ] * np.sin(yaw3)
+ self.xref[7, self.initial_node + 1 :] = v_ref[0, 0] * np.sin(yaw3) + v_ref[
+ 1, 0
+ ] * np.cos(yaw3)
+
+ # Update x and y depending on x and y velocities (cumulative sum)
+ for id in range(1, len(dt_list)):
+ self.xref[0, id] = self.xref[0, id - 1] + dt_list[id] * self.xref[6, id]
+ self.xref[1, id] = self.xref[1, id - 1] + dt_list[id] * self.xref[7, id]
+
+ # Start from position of the CoM in local frame
+ # self.xref[0, 1:] += lC[0, 0]
+ # self.xref[1, 1:] += lC[1, 0]
+
+ # No need to update Z velocity as the reference is always 0
+ # No need to update roll and roll velocity as the reference is always 0 for those
+ # No need to update pitch and pitch velocity as the reference is always 0 for those
+ # Update yaw and yaw velocity
+ self.xref[5, 1:] = v_ref[5, 0] * self.dt_vector[1:]
+ self.xref[11, 1:] = v_ref[5, 0]
+
+ i = 0
+
+ if k_remaining >= self.initial_node:
+ if len(self.ListAction) != N_total:
+ model = self.new_model()
+ self.ListAction.append(model)
+
+ for i in range(self.initial_node):
+ if i == self.initial_node - 1:
+ self.ListAction[
+ i
+ ].dt = self.dt_tsid * k_remaining - self.dt_tsid * (
+ self.initial_node - 1
+ )
+ else:
+ self.ListAction[i].dt = self.dt_tsid
+
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[0, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[0, 1:],
+ )
+
+ else:
+ if len(self.ListAction) != N_total - 1:
+ self.ListAction.pop(-1)
+
+ for i in range(k_remaining):
+ self.ListAction[i].dt = self.dt_tsid
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[0, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[0, 1:],
+ )
+
+ for i in range(k_remaining, k_remaining + self.initial_node - k_remaining):
+ if i == k_remaining + self.initial_node - k_remaining - 1:
+ self.ListAction[i].dt = self.dt - self.dt_tsid * (
+ self.initial_node - k_remaining - 1
+ )
+ else:
+ self.ListAction[i].dt = self.dt_tsid
+
+ if self.gait[0, 0] > 1:
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[0, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[0, 1:],
+ )
+ else:
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[1, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[1, 1:],
+ )
+
+ k_cum = self.initial_node
+ L = []
+
+ # Iterate over the 1st phase of the gait
+ if k_remaining >= self.initial_node: # 1st node removed
+ if self.gait[0, 0] > 1:
+ for i in range(k_cum, k_cum + np.int(self.gait[0, 0] - 1)):
+ # Update model
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[0, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[0, 1:],
+ )
+ k_cum += np.int(self.gait[0, 0] - 1)
+ j_init = 1
+ else:
+ j_init = 1
+
+ else: # the 2 first nodes to remove
+ if self.gait[0, 0] > 2:
+ for i in range(k_cum, k_cum + np.int(self.gait[0, 0] - 2)):
+ # Update model
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[0, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[0, 1:],
+ )
+ k_cum += np.int(self.gait[0, 0] - 2)
+ j_init = 1
+ else:
+ if self.gait[0, 0] == 2:
+ j_init = 1
+ else: # 1 1001
+ if self.gait[1, 0] == 1: # 1 1001 | 1 1111
+ j_init = 2
+ else: # 1 1111 | 7 1001
+ for i in range(k_cum, k_cum + np.int(self.gait[1, 0] - 1)):
+ # Update model
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[1, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[1, 1:],
+ )
+ k_cum += np.int(self.gait[1, 0] - 1)
+ j_init = 2
+
+ j = j_init
+ # Iterate over all phases of the gait
+ # The first column of xref correspond to the current state
+ # print(len(self.ListAction))
+ while self.gait[j, 0] != 0:
+ for i in range(k_cum, k_cum + np.int(self.gait[j, 0])):
+ # Update model
+ # print(i)
+ self.ListAction[i].updateModel(
+ np.reshape(self.fsteps[j, 1:], (3, 4), order="F"),
+ self.xref[:, i],
+ self.gait[j, 1:],
+ )
+
+ k_cum += np.int(self.gait[j, 0])
+ j += 1
+
+ # Update model of the terminal model
+ self.terminalModel.updateModel(
+ np.reshape(self.fsteps[j - 1, 1:], (3, 4), order="F"),
+ self.xref[:, -1],
+ self.gait[j - 1, 1:],
+ )
+
+ # Shooting problem
+ self.problem = crocoddyl.ShootingProblem(
+ np.zeros(12), self.ListAction, self.terminalModel
+ )
+ # Update initial state of the problem
+ self.problem.x0 = self.xref[:, 0]
+
+ # DDP Solver
+ self.ddp = crocoddyl.SolverDDP(self.problem)
+
+ return 0
+
+ def solve(self, k, fstep_planner):
+ """Solve the MPC problem
+
+ Args:
+ k : Iteration
+ fstep_planner : Object that includes the feet predicted position and the desired state vector
+ """
+
+ # Update the dynamic depending on the predicted feet position
+ self.updateProblem(k, fstep_planner.fsteps, fstep_planner.xref)
+
+ self.x_init.clear()
+ self.u_init.clear()
+
+ # Warm start : set candidate state and input vector
+ if self.warm_start and k != 0:
+
+ self.u_init = self.ddp.us[1:]
+ self.u_init.append(
+ np.repeat(self.gait[self.index - 1, 1:], 3)
+ * np.array(4 * [0.5, 0.5, 5.0])
+ )
+
+ self.x_init = self.ddp.xs[2:]
+ self.x_init.insert(0, fstep_planner.xref[:, 0])
+ self.x_init.append(self.ddp.xs[-1])
+
+ self.ddp.solve(self.x_init, self.u_init, self.max_iteration)
+
+ return 0
+
+ def get_latest_result(self):
+ """Returns the desired contact forces that have been computed by the last iteration of the MPC
+ Args:
+ """
+ return np.reshape(np.asarray(self.ddp.us[0]), (12,))
+
+ def get_xrobot(self):
+ """Returns the state vectors predicted by the mpc throughout the time horizon, the initial column is deleted as it corresponds
+ initial state vector
+ Args:
+ """
+
+ return np.array(self.ddp.xs)[1:, :].transpose()
+
+ def get_fpredicted(self):
+ """Returns the force vectors command predicted by the mpc throughout the time horizon,
+ Args:
+ """
+
+ return np.array(self.ddp.us)[:, :].transpose()[:, :]
+
+ def updateActionModel(self):
+ """Update the quadruped model with the new weights or model parameters. Useful to try new weights without modify this class"""
+
+ for elt in self.ListAction:
+ elt.dt = self.dt
+ elt.mass = self.mass
+ elt.gI = self.gI
+ elt.mu = self.mu
+ elt.min_fz = self.min_fz
+ elt.max_fz = self.max_fz
+
+ # Weights vectors
+ elt.stateWeights = self.stateWeight
+ elt.forceWeights = self.forceWeights
+ elt.frictionWeights = self.frictionWeights
+
+ # shoulder term :
+ elt.shoulderWeights = self.shoulderWeights
+ elt.shoulder_hlim = self.shoulder_hlim
+
+ elt.relative_forces = self.relative_forces
+
+ # Integration
+ elt.implicit_integration = self.implicit_integration
+
+ # Model parameters of terminal node
+ self.terminalModel.dt = self.dt
+ self.terminalModel.mass = self.mass
+ self.terminalModel.gI = self.gI
+ self.terminalModel.mu = self.mu
+ self.terminalModel.min_fz = self.min_fz
+
+ # Weights vectors of terminal node
+ self.terminalModel.stateWeights = self.stateWeight
+ self.terminalModel.forceWeights = np.zeros(12)
+ self.terminalModel.frictionWeights = 0.0
+
+ # shoulder term :
+ self.terminalModel.shoulderWeights = self.shoulderWeights
+ self.terminalModel.shoulder_hlim = self.shoulder_hlim
+
+ # Integration
+ self.terminalModel.implicit_integration = self.implicit_integration
+
+ return 0
diff --git a/scripts/Crocoddyl/MPC_crocoddyl_planner.py b/python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl_planner.py
similarity index 66%
rename from scripts/Crocoddyl/MPC_crocoddyl_planner.py
rename to python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl_planner.py
index 395506c6a68b1a7a791af8a16974a1953bbe8527..4bf95dcaee94c94283f94a8f66b552e8c69b167f 100644
--- a/scripts/Crocoddyl/MPC_crocoddyl_planner.py
+++ b/python/quadruped_reactive_walking/Crocoddyl/MPC_crocoddyl_planner.py
@@ -3,10 +3,9 @@
import crocoddyl
import numpy as np
import quadruped_walkgen as quadruped_walkgen
-import pinocchio as pin
-class MPC_crocoddyl_planner():
+class MPC_crocoddyl_planner:
"""Wrapper class for the MPC problem to call the ddp solver and
retrieve the results.
@@ -24,7 +23,9 @@ class MPC_crocoddyl_planner():
self.n_nodes = int(params.gait.shape[0]) # 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 body frame
+ self.gI = np.array(params.I_mat.tolist()).reshape(
+ (3, 3)
+ ) # Inertia matrix in body frame
# Friction coefficient
if inner:
@@ -57,44 +58,56 @@ class MPC_crocoddyl_planner():
self.stateWeights = state_tmp
# self.stateWeights = np.sqrt(params.osqp_w_states)
- self.forceWeights = np.tile(np.sqrt(params.osqp_w_forces), 4)
+ self.forceWeights = np.tile(np.sqrt(params.osqp_w_forces), 4)
# 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.full(8, 0.005) # Weight vector : Stop the optimisation (end flying phase)
- 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.frictionWeights = 1.0 # Weight vector : Friction cone penalisation
+ self.heuristicWeights = 0.0 * np.array(
+ 4 * [0.03, 0.04]
+ ) # Weight vector : Heuristic term
+ self.stepWeights = 0.0 * np.full(
+ 8, 0.005
+ ) # Weight vector : Norm of step command (distance between steps)
+ self.stopWeights = 0.0 * np.full(
+ 8, 0.005
+ ) # Weight vector : Stop the optimisation (end flying phase)
+ self.shoulderContactWeight = 1.5 * np.full(
+ 4, 1.0
+ ) # 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/Maximal normal force(N) and relative_forces activation
- self.min_fz = 0.
- self.max_fz = 25.
+ self.min_fz = 0.0
+ self.max_fz = 25.0
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_limit = np.array(
+ [60.0, 60.0]
+ ) # 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.
+ self.dx_new_phase = 0.0
+ self.dy_new_phase = 0.0
# Velocity penalty
self.is_vel_activated = True
- self.vel_limit = np.array([5., 5.]) # Velocity to activate the penalisation
- self.vel_weight = 1.
+ self.vel_limit = np.array([5.0, 5.0]) # Velocity to activate the penalisation
+ self.vel_weight = 1.0
# Jerk penalty
self.is_jerk_activated = True
self.jerk_weight = 1e-7
self.jerk_alpha = 21 # Define the slope of the cost, not used
- self.jerk_beta = 0. # Define the slope of the cost, not used
+ self.jerk_beta = 0.0 # Define the slope of the cost, not used
# Offset CoM
self.offset_com = np.array(params.CoM_offset).reshape((-1, 1))
@@ -111,35 +124,59 @@ class MPC_crocoddyl_planner():
self.problem = None # Shooting problem
self.ddp = None # ddp solver
- self.Xs = np.zeros((20, int(self.n_nodes / self.dt_mpc))) # Xs results without the actionStepModel
+ self.Xs = np.zeros(
+ (20, int(self.n_nodes / self.dt_mpc))
+ ) # Xs results without the actionStepModel
# Initial foot location (local frame, X,Y plan)
- self.shoulders = [0.18, 0.14695, 0.18, -0.14695, -0.21, 0.14695, -0.21, -0.14695]
+ self.shoulders = [
+ 0.18,
+ 0.14695,
+ 0.18,
+ -0.14695,
+ -0.21,
+ 0.14695,
+ -0.21,
+ -0.14695,
+ ]
self.xref = np.full((12, self.n_nodes + 1), np.nan)
# Index to stop the feet optimisation
# Row index in the gait matrix when the optimisation of the feet should be stopped
self.index_lock_time = int(params.lock_time / params.dt_mpc)
- self.index_stop_optimisation = [] # List of index to reset the stopWeights to 0 after optimisation
+ self.index_stop_optimisation = (
+ []
+ ) # List of index to reset the stopWeights to 0 after optimisation
# Usefull to optimise around the previous optimisation
- self.flying_foot = 4 * [False] # Bool corresponding to the current flying foot (gait[0,foot_id] == 0)
- self.flying_foot_nodes = np.zeros(4) # The number of nodes in the next phase of flight
+ self.flying_foot = 4 * [
+ False
+ ] # Bool corresponding to the current flying foot (gait[0,foot_id] == 0)
+ self.flying_foot_nodes = np.zeros(
+ 4
+ ) # The number of nodes in the next phase of flight
self.flying_max_nodes = int(
- params.T_gait / (2 * params.dt_mpc)) # TODO : get the maximum number of nodes from the gait_planner
+ params.T_gait / (2 * params.dt_mpc)
+ ) # TODO : get the maximum number of nodes from the gait_planner
# Usefull to setup the shoulder-to-contact cost (no cost for the initial stance phase)
- self.stance_foot = 4 * [False] # Bool corresponding to the current flying foot (gait[0,foot_id] == 0)
- self.stance_foot_nodes = np.zeros(4) # The number of nodes in the next phase of flight
- self.index_inital_stance_phase = [] # List of index to reset the stopWeights to 0 after optimisation
+ self.stance_foot = 4 * [
+ False
+ ] # Bool corresponding to the current flying foot (gait[0,foot_id] == 0)
+ self.stance_foot_nodes = np.zeros(
+ 4
+ ) # The number of nodes in the next phase of flight
+ self.index_inital_stance_phase = (
+ []
+ ) # List of index to reset the stopWeights to 0 after optimisation
# Initialize the lists of models
self.initialize_models()
def initialize_models(self):
- ''' Reset the two lists of augmented and step-by-step models, to avoid recreating them at each loop.
+ """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.
- '''
+ """
self.models_augmented = []
self.models_step = []
@@ -155,11 +192,26 @@ class MPC_crocoddyl_planner():
self.models_step.append(model)
# Terminal node
- self.terminal_model = quadruped_walkgen.ActionModelQuadrupedAugmented(self.offset_com)
+ self.terminal_model = quadruped_walkgen.ActionModelQuadrupedAugmented(
+ self.offset_com
+ )
self.update_model_augmented(self.terminal_model, terminal=True)
- def solve(self, k, xref, footsteps, l_stop, position, velocity, acceleration, jerk, oRh, oTh, dt_flying):
- """ 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 (int) : Iteration
@@ -177,8 +229,19 @@ class MPC_crocoddyl_planner():
self.xref[:, :] = xref
self.xref[0:3, :] += self.offset_com
- self.updateProblem(k, self.xref, footsteps, l_stop, position, velocity, acceleration, jerk, oRh, oTh,
- dt_flying)
+ 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)
# Reset to 0 the stopWeights for next optimisation (already 0.)
@@ -186,9 +249,24 @@ class MPC_crocoddyl_planner():
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, position, velocity, acceleration, jerk, oRh, oTh, dt_flying):
+ self.models_augmented[
+ index_stance
+ ].shoulderContactWeight = self.shoulderContactWeight
+
+ def updateProblem(
+ self,
+ k,
+ xref,
+ footsteps,
+ l_stop,
+ position,
+ velocity,
+ acceleration,
+ jerk,
+ oRh,
+ oTh,
+ dt_flying,
+ ):
"""
Update the models of the nodes according to parameters of the simulations.
@@ -209,8 +287,9 @@ class MPC_crocoddyl_planner():
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]]
+ 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()
@@ -228,20 +307,28 @@ class MPC_crocoddyl_planner():
for index_foot, is_flying in enumerate(self.flying_foot):
if is_flying:
# Position optimized at the previous control cycle
- stopping_needed[index_foot] = self.flying_foot_nodes[index_foot] != self.flying_max_nodes
+ stopping_needed[index_foot] = (
+ self.flying_foot_nodes[index_foot] != self.flying_max_nodes
+ )
# Augmented model, first node, j = 0
- self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[0, :], (3, 4), order='F'), l_stop,
- xref[:, 0], self.gait[0, :])
+ self.models_augmented[index_augmented].updateModel(
+ np.reshape(footsteps[0, :], (3, 4), order="F"),
+ l_stop,
+ xref[:, 0],
+ self.gait[0, :],
+ )
shoulder_weight = self.shoulderContactWeight.copy()
for foot in range(4):
if self.stance_foot[foot] and j < self.stance_foot_nodes[foot]:
- shoulder_weight[foot] = 0.
+ shoulder_weight[foot] = 0.0
if np.sum(shoulder_weight != self.shoulderContactWeight) > 0:
# The shoulder-to-contact weight has been modified, needs to be added to the list
- self.models_augmented[index_augmented].shoulderContactWeight = shoulder_weight
+ self.models_augmented[
+ index_augmented
+ ].shoulderContactWeight = shoulder_weight
self.index_inital_stance_phase.append(index_augmented)
self.action_models.append(self.models_augmented[index_augmented])
@@ -250,7 +337,9 @@ class MPC_crocoddyl_planner():
# 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, :])]))
+ np.repeat(self.gait[0, :], 3)
+ * np.array(4 * [0.0, 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
@@ -259,7 +348,9 @@ class MPC_crocoddyl_planner():
# TODO Adapt this [1,1,1,1] --> [1,0,0,1] True here but not step model to add
if np.any(self.gait[j, :] - self.gait[j - 1, :]):
- flying_feet = np.where(self.gait[j - 1, :] == 0)[0] # Current flying feet
+ 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
@@ -273,8 +364,12 @@ class MPC_crocoddyl_planner():
# ------ <- 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.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:
@@ -290,18 +385,26 @@ class MPC_crocoddyl_planner():
# 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
+ ].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
+ 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
+ ].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.
+ self.models_step[index_step].jerk_weight = 0.0
# Update is_first_step bool
is_first_step = False
@@ -314,14 +417,27 @@ class MPC_crocoddyl_planner():
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, :], position, velocity,
- acceleration, jerk, oRh, oTh, dt_)
+ self.models_step[index_step].updateModel(
+ np.reshape(footsteps[j, :], (3, 4), order="F"),
+ 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
- self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
- l_stop, xref[:, j], self.gait[j, :])
+ self.models_augmented[index_augmented].updateModel(
+ np.reshape(footsteps[j, :], (3, 4), order="F"),
+ l_stop,
+ xref[:, j],
+ self.gait[j, :],
+ )
# Activation of the cost to stop the optimisation around l_stop (position locked by the footstepGenerator)
# if j < self.index_lock_time:
@@ -331,12 +447,21 @@ class MPC_crocoddyl_planner():
feet_ground = np.where(self.gait[j, :] == 1)[0]
activation_cost = False
for foot in range(4):
- if (stopping_needed[foot]) and (self.flying_foot[foot]) and (foot in feet_ground) and (
- j < int(self.flying_foot_nodes[foot] + self.flying_max_nodes)):
+ if (
+ (stopping_needed[foot])
+ and (self.flying_foot[foot])
+ and (foot in feet_ground)
+ and (
+ j
+ < int(self.flying_foot_nodes[foot] + self.flying_max_nodes)
+ )
+ ):
coeff_activated = np.zeros(8)
- coeff_activated[2 * foot:2 * foot + 2] = np.array([1, 1])
- self.models_augmented[index_augmented].stopWeights = self.models_augmented[
- index_augmented].stopWeights + coeff_activated * self.stopWeights
+ coeff_activated[2 * foot : 2 * foot + 2] = np.array([1, 1])
+ self.models_augmented[index_augmented].stopWeights = (
+ self.models_augmented[index_augmented].stopWeights
+ + coeff_activated * self.stopWeights
+ )
activation_cost = True
if activation_cost:
@@ -345,10 +470,12 @@ class MPC_crocoddyl_planner():
shoulder_weight = self.shoulderContactWeight.copy()
for foot in range(4):
if self.stance_foot[foot] and j < self.stance_foot_nodes[foot]:
- shoulder_weight[foot] = 0.
+ shoulder_weight[foot] = 0.0
if np.sum(shoulder_weight != self.shoulderContactWeight) > 0:
# The shoulder-to-contact weight has been modified, needs to be added to the list
- self.models_augmented[index_augmented].shoulderContactWeight = shoulder_weight
+ self.models_augmented[
+ index_augmented
+ ].shoulderContactWeight = shoulder_weight
self.index_inital_stance_phase.append(index_augmented)
self.action_models.append(self.models_augmented[index_augmented])
@@ -360,22 +487,37 @@ class MPC_crocoddyl_planner():
self.u_init.append(np.zeros(8))
self.x_init.append(np.concatenate([xref[:, j], p0]))
self.u_init.append(
- np.repeat(self.gait[j, :], 3) * np.array(4 * [0., 0., 2.5 * 9.81 / np.sum(self.gait[j, :])]))
+ np.repeat(self.gait[j, :], 3)
+ * np.array(4 * [0.0, 0.0, 2.5 * 9.81 / np.sum(self.gait[j, :])])
+ )
else:
- self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
- l_stop, xref[:, j], self.gait[j, :])
+ self.models_augmented[index_augmented].updateModel(
+ np.reshape(footsteps[j, :], (3, 4), order="F"),
+ l_stop,
+ xref[:, j],
+ self.gait[j, :],
+ )
self.action_models.append(self.models_augmented[index_augmented])
feet_ground = np.where(self.gait[j, :] == 1)[0]
activation_cost = False
for foot in range(4):
- if (stopping_needed[foot]) and (self.flying_foot[foot]) and (foot in feet_ground) and (
- j < int(self.flying_foot_nodes[foot] + self.flying_max_nodes)):
+ if (
+ (stopping_needed[foot])
+ and (self.flying_foot[foot])
+ and (foot in feet_ground)
+ and (
+ j
+ < int(self.flying_foot_nodes[foot] + self.flying_max_nodes)
+ )
+ ):
coeff_activated = np.zeros(8)
- coeff_activated[2 * foot:2 * foot + 2] = np.array([1, 1])
- self.models_augmented[index_augmented].stopWeights = self.models_augmented[
- index_augmented].stopWeights + coeff_activated * self.stopWeights
+ coeff_activated[2 * foot : 2 * foot + 2] = np.array([1, 1])
+ self.models_augmented[index_augmented].stopWeights = (
+ self.models_augmented[index_augmented].stopWeights
+ + coeff_activated * self.stopWeights
+ )
activation_cost = True
if activation_cost:
@@ -384,36 +526,46 @@ class MPC_crocoddyl_planner():
shoulder_weight = self.shoulderContactWeight.copy()
for foot in range(4):
if self.stance_foot[foot] and j < self.stance_foot_nodes[foot]:
- shoulder_weight[foot] = 0.
+ shoulder_weight[foot] = 0.0
if np.sum(shoulder_weight != self.shoulderContactWeight) > 0:
# The shoulder-to-contact weight has been modified, needs to be added to the list
- self.models_augmented[index_augmented].shoulderContactWeight = shoulder_weight
+ self.models_augmented[
+ index_augmented
+ ].shoulderContactWeight = shoulder_weight
self.index_inital_stance_phase.append(index_augmented)
index_augmented += 1
# Warm-start
self.x_init.append(np.concatenate([xref[:, j], p0]))
self.u_init.append(
- np.repeat(self.gait[j, :], 3) * np.array(4 * [0., 0., 2.5 * 9.81 / np.sum(self.gait[j, :])]))
+ np.repeat(self.gait[j, :], 3)
+ * np.array(4 * [0.0, 0.0, 2.5 * 9.81 / np.sum(self.gait[j, :])])
+ )
# Update row matrix
j += 1
# Update terminal model
- self.terminal_model.updateModel(np.reshape(footsteps[j - 1, :], (3, 4), order='F'), l_stop, xref[:, -1],
- self.gait[j - 1, :])
+ self.terminal_model.updateModel(
+ np.reshape(footsteps[j - 1, :], (3, 4), order="F"),
+ l_stop,
+ xref[:, -1],
+ self.gait[j - 1, :],
+ )
# Warm-start
self.x_init.append(np.concatenate([xref[:, -1], p0]))
- self.problem = crocoddyl.ShootingProblem(np.zeros(20), self.action_models, self.terminal_model)
+ self.problem = crocoddyl.ShootingProblem(
+ np.zeros(20), self.action_models, self.terminal_model
+ )
self.problem.x0 = np.concatenate([xref[:, 0], p0])
self.ddp = crocoddyl.SolverDDP(self.problem)
def get_latest_result(self, oRh, oTh):
- """
+ """
Return the desired contact forces that have been computed by the last iteration of the MPC
- Args :
+ Args :
- q ( Array 7x1 ) : pos, quaternion orientation
"""
# print("\n\n")
@@ -422,13 +574,23 @@ class MPC_crocoddyl_planner():
for i in range(len(self.action_models)):
if self.action_models[i].__class__.__name__ != "ActionModelQuadrupedStep":
if index >= self.n_nodes:
- raise ValueError("Too many action model considering the current MPC prediction horizon")
- result[:12, index] = self.ddp.xs[i + 1][:12] # First node correspond to current state
+ raise ValueError(
+ "Too many action model considering the current MPC prediction horizon"
+ )
+ result[:12, index] = self.ddp.xs[i + 1][
+ :12
+ ] # First node correspond to current state
result[:3, index] -= self.offset_com.ravel()
result[12:24, index] = self.ddp.us[i]
- result[24:, index] = (oRh[:2, :2] @ (self.ddp.xs[i + 1][12:].reshape(
- (2, 4), order="F")) + oTh[:2]).reshape((8), order="F")
- if i > 0 and self.action_models[i - 1].__class__.__name__ == "ActionModelQuadrupedStep":
+ result[24:, index] = (
+ oRh[:2, :2] @ (self.ddp.xs[i + 1][12:].reshape((2, 4), order="F"))
+ + oTh[:2]
+ ).reshape((8), order="F")
+ if (
+ i > 0
+ and self.action_models[i - 1].__class__.__name__
+ == "ActionModelQuadrupedStep"
+ ):
# print(self.ddp.xs[i + 1][12:].reshape((2, 4), order="F"))
pass
index += 1
@@ -436,7 +598,7 @@ class MPC_crocoddyl_planner():
return result
def update_model_augmented(self, model, terminal=False):
- """
+ """
Set intern parameters for augmented model type
"""
# Model parameters
@@ -458,7 +620,7 @@ class MPC_crocoddyl_planner():
if terminal:
self.terminal_model.forceWeights = np.zeros(12)
- self.terminal_model.frictionWeights = 0.
+ self.terminal_model.frictionWeights = 0.0
self.terminal_model.heuristicWeights = np.zeros(8)
else:
model.frictionWeights = self.frictionWeights
@@ -483,7 +645,7 @@ class MPC_crocoddyl_planner():
model.set_sample_feet_traj(self.n_sampling)
def compute_gait_matrix(self, footsteps):
- """
+ """
Recontruct the gait based on the computed footstepsC
Args:
footsteps : current and predicted position of the feet
@@ -498,7 +660,9 @@ 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
+ 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:
@@ -522,15 +686,16 @@ class MPC_crocoddyl_planner():
def exponential_cost(self, t):
"""
- Returns weight_jerk * exp(alpha(t-beta)) - 1
+ Returns weight_jerk * exp(alpha(t-beta)) - 1
Args:
- t (float) : time in s
+ t (float) : time in s
"""
return self.jerk_weight * (np.exp(self.jerk_alpha * (t - self.jerk_beta)) - 1)
+
if __name__ == "__main__":
- """ Plot the exponential cost used to penalize the jerk difference withe the
+ """Plot the exponential cost used to penalize the jerk difference withe the
previous control cycle
"""
@@ -540,13 +705,11 @@ if __name__ == "__main__":
params = qrw.Params()
mpc = MPC_crocoddyl_planner(params)
mpc.jerk_alpha = 20 # Define the slope of the cost, not used
- mpc.jerk_beta = 0. # Define the slope of the cost, not used
+ mpc.jerk_beta = 0.0 # Define the slope of the cost, not used
plt.figure()
n_points = 100
T = np.linspace(1e-6, 0.23, n_points)
X = [mpc.exponential_cost(0.24 - t) for t in T]
- plt.plot(T,X)
- plt.show(block = True)
-
-
+ plt.plot(T, X)
+ plt.show(block=True)
diff --git a/python/quadruped_reactive_walking/Crocoddyl/__init__.py b/python/quadruped_reactive_walking/Crocoddyl/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/scripts/MPC_Wrapper.py b/python/quadruped_reactive_walking/MPC_Wrapper.py
similarity index 62%
rename from scripts/MPC_Wrapper.py
rename to python/quadruped_reactive_walking/MPC_Wrapper.py
index 320b27c8385e4f04047ff064b85073dbdd81970a..cfe7abf4e98bdb70949ddc147941aca3fa619a06 100644
--- a/scripts/MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/MPC_Wrapper.py
@@ -1,13 +1,13 @@
-import numpy as np
-from multiprocessing import Process, Value, Array
-from time import time
-from enum import Enum
import ctypes
from ctypes import Structure
+from enum import Enum
+from multiprocessing import Process, Value, Array
+from time import time
-import Crocoddyl.MPC_crocoddyl as MPC_crocoddyl
-import Crocoddyl.MPC_crocoddyl_planner as MPC_crocoddyl_planner
-import quadruped_reactive_walking as qrw
+import numpy as np
+
+from . import quadruped_reactive_walking as qrw
+from .Crocoddyl import MPC_crocoddyl, MPC_crocoddyl_planner
class MPC_type(Enum):
@@ -19,29 +19,35 @@ class MPC_type(Enum):
class DataInCtype(Structure):
- ''' Ctype data structure for the shared memory between processes.
- '''
- params = qrw.Params() # Object that holds all controller parameters
- mpc_type = MPC_type(params.type_MPC) # MPC type
+ """Ctype data structure for the shared memory between processes."""
+
+ params = qrw.Params() # Object that holds all controller parameters
+ mpc_type = MPC_type(params.type_MPC) # MPC type
n_steps = np.int(params.gait.shape[0]) # Colomn size for xref (12 x n_steps)
- N_gait = int(params.gait.shape[0]) # Row size for fsteps (N_gait x 12), from utils_mpc.py
+ N_gait = int(
+ params.gait.shape[0]
+ ) # 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)]
+ _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)]
+ _fields_ = [
+ ("k", ctypes.c_int64),
+ ("xref", ctypes.c_double * 12 * (n_steps + 1)),
+ ("fsteps", ctypes.c_double * 12 * N_gait),
+ ]
class Dummy:
@@ -80,7 +86,7 @@ class MPC_Wrapper:
self.t_mpc_solving_duration = 0.0
# Number of WBC steps for 1 step of the MPC
- self.k_mpc = int(params.dt_mpc/params.dt_wbc)
+ self.k_mpc = int(params.dt_mpc / params.dt_wbc)
self.dt = params.dt_mpc
self.n_steps = np.int(params.gait.shape[0])
@@ -93,44 +99,74 @@ class MPC_Wrapper:
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)
- self.cost = Value('d', 0.)
+ self.newData = Value("b", False)
+ self.newResult = Value("b", False)
+ self.cost = Value("d", 0.0)
self.dataIn = Value(DataInCtype)
if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
- self.dataOut = Array('d', [0] * 32 * (np.int(self.n_steps)))
+ self.dataOut = Array("d", [0] * 32 * (np.int(self.n_steps)))
else:
- self.dataOut = Array('d', [0] * 24 * (np.int(self.n_steps)))
+ 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)
+ self.running = Value("b", True)
else:
# Create the new version of the MPC solver object
if self.mpc_type == MPC_type.OSQP: # OSQP MPC
- self.mpc = qrw.MPC(params) # self.dt, self.n_steps, self.T_gait, self.N_gait)
+ self.mpc = qrw.MPC(
+ params
+ ) # self.dt, self.n_steps, self.T_gait, self.N_gait)
elif self.mpc_type == MPC_type.CROCODDYL_LINEAR: # Crocoddyl MPC Linear
- self.mpc = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=True)
- elif self.mpc_type == MPC_type.CROCODDYL_NON_LINEAR: # Crocoddyl MPC Non-Linear
- self.mpc = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=False)
- elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Crocoddyl MPC Non-Linear with footsteps optimization
- self.mpc = MPC_crocoddyl_planner.MPC_crocoddyl_planner(params, mu=0.9, inner=False)
+ self.mpc = MPC_crocoddyl.MPC_crocoddyl(
+ params, mu=0.9, inner=False, linearModel=True
+ )
+ elif (
+ self.mpc_type == MPC_type.CROCODDYL_NON_LINEAR
+ ): # Crocoddyl MPC Non-Linear
+ self.mpc = MPC_crocoddyl.MPC_crocoddyl(
+ params, mu=0.9, inner=False, linearModel=False
+ )
+ elif (
+ self.mpc_type == MPC_type.CROCODDYL_PLANNER
+ ): # Crocoddyl MPC Non-Linear with footsteps optimization
+ self.mpc = MPC_crocoddyl_planner.MPC_crocoddyl_planner(
+ params, mu=0.9, inner=False
+ )
else:
print("Unknown MPC type, using crocoddyl linear")
self.type = MPC_type.CROCODDYL_LINEAR
- self.mpc = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=True)
+ self.mpc = MPC_crocoddyl.MPC_crocoddyl(
+ params, mu=0.9, inner=False, linearModel=True
+ )
# Setup initial result for the first iteration of the main control loop
x_init = np.zeros(12)
x_init[0:6] = q_init[0:6, 0].copy()
- if self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Need more space to store optimized footsteps
+ if (
+ self.mpc_type == MPC_type.CROCODDYL_PLANNER
+ ): # Need more space to store optimized footsteps
self.last_available_result = np.zeros((32, (np.int(self.n_steps))))
else:
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)))
- self.last_cost = 0.
-
- 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)):
+ self.last_available_result[:24, 0] = np.hstack(
+ (x_init, np.array([0.0, 0.0, 8.0] * 4))
+ )
+ self.last_cost = 0.0
+
+ 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
@@ -145,13 +181,41 @@ class MPC_Wrapper:
self.t_mpc_solving_start = time()
if self.multiprocessing: # Run in parallel process
- self.run_MPC_asynchronous(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
+ 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_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
+ 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
- self.last_available_result[12:24, 0] = self.last_available_result[12:24, 1].copy()
+ if np.allclose(
+ gait[0, :], self.gait_next
+ ): # If we're still doing what was planned the last time MPC was solved
+ self.last_available_result[12:24, 0] = self.last_available_result[
+ 12:24, 1
+ ].copy()
else: # Otherwise use a default contact force command till we get the actual result of the MPC for this new sequence
F = 9.81 * self.mass / np.sum(gait[0, :])
self.last_available_result[12:24:3, 0] = 0.0
@@ -170,7 +234,7 @@ class MPC_Wrapper:
If a new result is available, return the new result. Otherwise return the old result again.
"""
- if (self.not_first_iter):
+ if self.not_first_iter:
if self.multiprocessing:
if self.newResult.value:
self.newResult.value = False
@@ -189,7 +253,20 @@ class MPC_Wrapper:
self.not_first_iter = True
return self.last_available_result, self.last_cost
- def run_MPC_synchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
+ 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:
@@ -205,9 +282,23 @@ class MPC_Wrapper:
if self.mpc_type == MPC_type.OSQP:
# OSQP MPC
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
+ 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_fsteps_target, position, velocity, acceleration, jerk, oRh, oTh, dt_flying)
+ 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())
@@ -221,7 +312,20 @@ class MPC_Wrapper:
if self.mpc_type == MPC_type.OSQP:
self.last_cost = self.mpc.retrieve_cost()
- def run_MPC_asynchronous(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
+ 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:
@@ -233,13 +337,33 @@ class MPC_Wrapper:
"""
# If this is the first iteration, creation of the parallel process
- if (k == 0):
- p = Process(target=self.create_MPC_asynchronous, args=(
- self.newData, self.newResult, self.dataIn, self.dataOut, self.running))
+ if k == 0:
+ p = Process(
+ target=self.create_MPC_asynchronous,
+ args=(
+ self.newData,
+ self.newResult,
+ self.dataIn,
+ self.dataOut,
+ self.running,
+ ),
+ )
p.start()
# Stacking data to send them to the parallel process
- self.compress_dataIn(k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying)
+ self.compress_dataIn(
+ k,
+ xref,
+ fsteps,
+ l_fsteps_target,
+ oRh,
+ oTh,
+ position,
+ velocity,
+ acceleration,
+ jerk,
+ dt_flying,
+ )
self.newData.value = True
return 0
@@ -268,30 +392,66 @@ class MPC_Wrapper:
if self.mpc_type != MPC_type.CROCODDYL_PLANNER:
k, xref, fsteps = self.decompress_dataIn(dataIn)
else:
- k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying = self.decompress_dataIn(dataIn)
+ (
+ 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:
# loop_mpc = MPC.MPC(self.dt, self.n_steps, self.T_gait)
if self.mpc_type == MPC_type.OSQP:
loop_mpc = qrw.MPC(self.params)
- elif self.mpc_type == MPC_type.CROCODDYL_LINEAR: # Crocoddyl MPC Linear
- loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=True)
- elif self.mpc_type == MPC_type.CROCODDYL_NON_LINEAR: # Crocoddyl MPC Non-Linear
- loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=False)
- elif self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Crocoddyl MPC Non-Linear with footsteps optimization
- loop_mpc = MPC_crocoddyl_planner.MPC_crocoddyl_planner(self.params, mu=0.9, inner=False)
+ elif (
+ self.mpc_type == MPC_type.CROCODDYL_LINEAR
+ ): # Crocoddyl MPC Linear
+ loop_mpc = MPC_crocoddyl.MPC_crocoddyl(
+ self.params, mu=0.9, inner=False, linearModel=True
+ )
+ elif (
+ self.mpc_type == MPC_type.CROCODDYL_NON_LINEAR
+ ): # Crocoddyl MPC Non-Linear
+ loop_mpc = MPC_crocoddyl.MPC_crocoddyl(
+ self.params, mu=0.9, inner=False, linearModel=False
+ )
+ elif (
+ self.mpc_type == MPC_type.CROCODDYL_PLANNER
+ ): # Crocoddyl MPC Non-Linear with footsteps optimization
+ loop_mpc = MPC_crocoddyl_planner.MPC_crocoddyl_planner(
+ self.params, mu=0.9, inner=False
+ )
else: # Using linear model
- loop_mpc = MPC_crocoddyl.MPC_crocoddyl(self.params, mu=0.9, inner=False, linearModel=True)
+ loop_mpc = MPC_crocoddyl.MPC_crocoddyl(
+ self.params, mu=0.9, inner=False, linearModel=True
+ )
# Run the asynchronous MPC with the data that as been retrieved
if self.mpc_type == MPC_type.OSQP:
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_fsteps_target.copy(),
- position.copy(), velocity.copy(), acceleration.copy(), jerk.copy(),
- oRh.copy(), oTh.copy(), dt_flying.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())
@@ -300,9 +460,11 @@ class MPC_Wrapper:
# print((loop_mpc.get_latest_result()).shape)
if self.mpc_type == MPC_type.CROCODDYL_PLANNER:
- self.dataOut[:] = loop_mpc.get_latest_result(oRh, oTh).ravel(order='F')
+ self.dataOut[:] = loop_mpc.get_latest_result(oRh, oTh).ravel(
+ order="F"
+ )
else:
- self.dataOut[:] = loop_mpc.get_latest_result().ravel(order='F')
+ self.dataOut[:] = loop_mpc.get_latest_result().ravel(order="F")
if self.mpc_type == MPC_type.OSQP:
self.cost.value = loop_mpc.retrieve_cost()
@@ -312,7 +474,20 @@ class MPC_Wrapper:
return 0
- def compress_dataIn(self, k, xref, fsteps, l_fsteps_target, oRh, oTh, position, velocity, acceleration, jerk, dt_flying):
+ 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
@@ -329,21 +504,33 @@ class MPC_Wrapper:
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.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.acceleration).reshape((3, 4))[
+ :, :
+ ] = acceleration
np.frombuffer(self.dataIn.jerk).reshape((3, 4))[:, :] = jerk
np.frombuffer(self.dataIn.dt_flying).reshape(4)[:] = dt_flying
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
+ 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 C-type structure that belongs to the shared memory to retrieve data from the main control
@@ -356,9 +543,11 @@ class MPC_Wrapper:
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))
+ 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))
+ 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))
@@ -367,23 +556,36 @@ class MPC_Wrapper:
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
+ 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))
+ xref = np.frombuffer(self.dataIn.xref).reshape((12, self.n_steps + 1))
fsteps = np.frombuffer(self.dataIn.fsteps).reshape((self.N_gait, 12))
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
- """
+ """Return the result of the asynchronous MPC (desired contact forces) that is stored in the shared memory"""
- if self.mpc_type == MPC_type.CROCODDYL_PLANNER: # Need more space to store optimized footsteps
- return np.array(self.dataOut[:]).reshape((32, -1), order='F')
+ if (
+ self.mpc_type == MPC_type.CROCODDYL_PLANNER
+ ): # Need more space to store optimized footsteps
+ return np.array(self.dataOut[:]).reshape((32, -1), order="F")
else:
- return np.array(self.dataOut[:]).reshape((24, -1), order='F')
+ return np.array(self.dataOut[:]).reshape((24, -1), order="F")
def roll_asynchronous(self, fsteps):
"""Move one step further in the gait cycle. Since the output of the asynchronous MPC is retrieved by
@@ -405,23 +607,29 @@ class MPC_Wrapper:
self.fsteps_future = fsteps.copy()
# Index of the first empty line
- index = next((idx for idx, val in np.ndenumerate(self.fsteps_future[:, 0]) if val == 0.0), 0.0)[0]
+ index = next(
+ (
+ idx
+ for idx, val in np.ndenumerate(self.fsteps_future[:, 0])
+ if val == 0.0
+ ),
+ 0.0,
+ )[0]
# Create a new phase if needed or increase the last one by 1 step
- self.fsteps_future[index-1, 0] += 1.0
+ self.fsteps_future[index - 1, 0] += 1.0
# Decrease the current phase by 1 step and delete it if it has ended
if self.fsteps_future[0, 0] > 1.0:
self.fsteps_future[0, 0] -= 1.0
else:
self.fsteps_future = np.roll(self.fsteps_future, -1, axis=0)
- self.fsteps_future[-1, :] = np.zeros((13, ))
+ self.fsteps_future[-1, :] = np.zeros((13,))
return 0
def stop_parallel_loop(self):
- """Stop the infinite loop in the parallel process to properly close the simulation
- """
+ """Stop the infinite loop in the parallel process to properly close the simulation"""
self.running.value = False
diff --git a/scripts/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
similarity index 78%
rename from scripts/main_solo12_control.py
rename to python/quadruped_reactive_walking/main_solo12_control.py
index 1450b12c7371b13dacfdb00a2a2b2c2aeb76b647..cfc88dca7b60c360de7003f44f742b0f670519c6 100644
--- a/scripts/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -1,12 +1,12 @@
-import numpy as np
import threading
import time
-from Controller import Controller
-from tools.LoggerSensors import LoggerSensors
-from tools.LoggerControl import LoggerControl
+import numpy as np
-import quadruped_reactive_walking as qrw
+from . import quadruped_reactive_walking as qrw
+from .Controller import Controller
+from .tools.LoggerControl import LoggerControl
+from .tools.LoggerSensors import LoggerSensors
params = qrw.Params() # Object that holds all controller parameters
@@ -18,8 +18,8 @@ else:
def get_input():
- """
- Thread to get the input
+ """
+ Thread to get the input
"""
input()
@@ -35,7 +35,7 @@ def put_on_the_floor(device, q_init):
"""
print("PUT ON THE FLOOR.")
- Kp_pos = 6.
+ Kp_pos = 6.0
Kd_pos = 0.3
device.joints.set_position_gains(Kp_pos * np.ones(12))
@@ -55,7 +55,9 @@ def put_on_the_floor(device, q_init):
# Slow increase till 1/4th of mass is supported by each foot
duration_increase = 2.0 # in seconds
steps = int(duration_increase / params.dt_wbc)
- tau_ff = np.array([0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, -0.022, -0.5, 0.0, -0.022, -0.5])
+ tau_ff = np.array(
+ [0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, -0.022, -0.5, 0.0, -0.022, -0.5]
+ )
# tau_ff = np.array([0.0, 0.022, 0.5, 0.0, 0.022, 0.5, 0.0, 0.025, 0.575, 0.0, 0.025, 0.575])
for i in range(steps):
@@ -105,9 +107,9 @@ def damp_control(device, nb_motors):
# Send command to the robot
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
if (t % 1) < 5e-5:
- print('IMU attitude:', device.imu.attitude_euler)
- print('joint pos :', device.joints.positions)
- print('joint vel :', device.joints.velocities)
+ print("IMU attitude:", device.imu.attitude_euler)
+ print("joint pos :", device.joints.positions)
+ print("joint vel :", device.joints.velocities)
device.robot_interface.PrintStats()
t += params.dt_wbc
@@ -127,14 +129,20 @@ def control_loop(des_vel_analysis=None):
q_init = np.array(params.q_init.tolist()) # Default position after calibration
if params.SIMULATION and (des_vel_analysis is not None):
- print("Analysis: %1.1f %1.1f %1.1f" % (des_vel_analysis[0], des_vel_analysis[1], des_vel_analysis[5]))
+ print(
+ "Analysis: %1.1f %1.1f %1.1f"
+ % (des_vel_analysis[0], des_vel_analysis[1], des_vel_analysis[5])
+ )
acceleration_rate = 0.1 # m/s^2
steady_state_duration = 3 # s
- N_analysis = int(np.max(np.abs(des_vel_analysis)) / acceleration_rate / params.dt_wbc) + 1
+ N_analysis = (
+ int(np.max(np.abs(des_vel_analysis)) / acceleration_rate / params.dt_wbc)
+ + 1
+ )
N_steady = int(steady_state_duration / params.dt_wbc)
params.N_SIMULATION = N_analysis + N_steady
- controller = Controller(params, q_init, 0.)
+ controller = Controller(params, q_init, 0.0)
if params.SIMULATION and (des_vel_analysis is not None):
controller.joystick.update_for_analysis(des_vel_analysis, N_analysis, N_steady)
@@ -147,12 +155,20 @@ def control_loop(des_vel_analysis=None):
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if params.LOGGING or params.PLOTTING:
- loggerSensors = LoggerSensors(device, qualisys=qc, logSize=params.N_SIMULATION-3)
- loggerControl = LoggerControl(params, logSize=params.N_SIMULATION-3)
+ loggerSensors = LoggerSensors(
+ device, qualisys=qc, logSize=params.N_SIMULATION - 3
+ )
+ loggerControl = LoggerControl(params, logSize=params.N_SIMULATION - 3)
if params.SIMULATION:
- device.Init(calibrateEncoders=True, q_init=q_init, envID=params.envID,
- use_flat_plane=params.use_flat_plane, enable_pyb_GUI=params.enable_pyb_GUI, dt=params.dt_wbc)
+ device.Init(
+ calibrateEncoders=True,
+ q_init=q_init,
+ envID=params.envID,
+ use_flat_plane=params.use_flat_plane,
+ enable_pyb_GUI=params.enable_pyb_GUI,
+ dt=params.dt_wbc,
+ )
# import ForceMonitor
# myForceMonitor = ForceMonitor.ForceMonitor(device.pyb_sim.robotId, device.pyb_sim.planeId)
else: # Initialize the communication and the session.
@@ -162,9 +178,9 @@ def control_loop(des_vel_analysis=None):
put_on_the_floor(device, q_init)
# CONTROL LOOP ***************************************************
-
+
t = 0.0
- t_max = (params.N_SIMULATION-2) * params.dt_wbc
+ t_max = (params.N_SIMULATION - 2) * params.dt_wbc
t_log_whole = np.zeros((params.N_SIMULATION))
k_log_whole = 0
@@ -176,7 +192,7 @@ def control_loop(des_vel_analysis=None):
log_JcTf = np.zeros((params.N_SIMULATION, 6))
log_Mddq_out = np.zeros((params.N_SIMULATION, 6))
log_JcTf_out = np.zeros((params.N_SIMULATION, 6))
- while ((not device.is_timeout) and (t < t_max) and (not controller.error)):
+ while (not device.is_timeout) and (t < t_max) and (not controller.error):
t_start_whole = time.time()
device.parse_sensor_data()
@@ -191,7 +207,9 @@ def control_loop(des_vel_analysis=None):
device.joints.set_velocity_gains(controller.result.D)
device.joints.set_desired_positions(controller.result.q_des)
device.joints.set_desired_velocities(controller.result.v_des)
- device.joints.set_torques(controller.result.FF * controller.result.tau_ff.ravel())
+ device.joints.set_torques(
+ controller.result.FF * controller.result.tau_ff.ravel()
+ )
log_Mddq[k_log_whole] = controller.wbcWrapper.Mddq
log_NLE[k_log_whole] = controller.wbcWrapper.NLE
@@ -202,16 +220,25 @@ def control_loop(des_vel_analysis=None):
# Call logger
if params.LOGGING or params.PLOTTING:
loggerSensors.sample(device, qc)
- loggerControl.sample(controller.joystick, controller.estimator,
- controller, controller.gait, controller.statePlanner,
- controller.footstepPlanner, controller.footTrajectoryGenerator,
- controller.wbcWrapper, dT_whole)
+ loggerControl.sample(
+ controller.joystick,
+ controller.estimator,
+ controller,
+ controller.gait,
+ controller.statePlanner,
+ controller.footstepPlanner,
+ controller.footTrajectoryGenerator,
+ controller.wbcWrapper,
+ dT_whole,
+ )
t_end_whole = time.time()
# myForceMonitor.display_contact_forces()
- device.send_command_and_wait_end_of_cycle(params.dt_wbc) # Send command to the robot
+ device.send_command_and_wait_end_of_cycle(
+ params.dt_wbc
+ ) # Send command to the robot
t += params.dt_wbc # Increment loop time
dT_whole = T_whole
@@ -222,7 +249,7 @@ def control_loop(des_vel_analysis=None):
k_log_whole += 1
# ****************************************************************
- finished = (t >= t_max)
+ finished = t >= t_max
damp_control(device, 12)
if params.enable_multiprocessing:
@@ -241,7 +268,9 @@ def control_loop(des_vel_analysis=None):
if device.is_timeout:
print("Masterboard timeout detected.")
- print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.")
+ print(
+ "Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
+ )
if params.LOGGING:
loggerControl.saveAll(loggerSensors, "/home/odri/git/fanny/logs/data")
diff --git a/scripts/main_solo12_replay.py b/python/quadruped_reactive_walking/main_solo12_replay.py
similarity index 80%
rename from scripts/main_solo12_replay.py
rename to python/quadruped_reactive_walking/main_solo12_replay.py
index 73dccc57e12a70e2f2f277e6650f8390b7dad2c9..e3d75349a5c8f36aa787de3890f85565bd90ad9b 100644
--- a/scripts/main_solo12_replay.py
+++ b/python/quadruped_reactive_walking/main_solo12_replay.py
@@ -1,11 +1,12 @@
import os
import threading
-import numpy as np
-import argparse
-from tools.LoggerSensors import LoggerSensors
-import quadruped_reactive_walking as qrw
import time
+import numpy as np
+
+from . import quadruped_reactive_walking as qrw
+from .tools.LoggerSensors import LoggerSensors
+
params = qrw.Params() # Object that holds all controller parameters
if params.SIMULATION:
@@ -29,7 +30,7 @@ def put_on_the_floor(device, q_init):
"""
print("PUT ON THE FLOOR.")
- Kp_pos = 3.
+ Kp_pos = 3.0
Kd_pos = 0.3
device.joints.set_position_gains(Kp_pos * np.ones(12))
@@ -75,9 +76,9 @@ def damp_control(device, nb_motors):
# Send command to the robot
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
if (t % 1) < 5e-5:
- print('IMU attitude:', device.imu.attitude_euler)
- print('joint pos :', device.joints.positions)
- print('joint vel :', device.joints.velocities)
+ print("IMU attitude:", device.imu.attitude_euler)
+ print("joint pos :", device.joints.positions)
+ print("joint vel :", device.joints.velocities)
device.robot_interface.PrintStats()
t += params.dt_wbc
@@ -94,9 +95,9 @@ def control_loop(des_vel_analysis=None):
# replay = np.load("/home/odri/git/abonnefoy/Motion/Logs/push_up_position.npz")
replay = np.load("/home/odri/git/abonnefoy/Motion/Logs/full_push_up.npz")
- replay_q = replay['q'][7:, 1:].transpose().copy()
- replay_v = replay['v'][6:, 1:].transpose().copy()
- replay_tau = replay['tau'].transpose().copy()
+ replay_q = replay["q"][7:, 1:].transpose().copy()
+ replay_v = replay["v"][6:, 1:].transpose().copy()
+ replay_tau = replay["tau"].transpose().copy()
params.N_SIMULATION = replay_q.shape[0]
N = replay_q.shape[0]
replay_P = 6.0 * np.ones((N, 12)) # replay["P"]
@@ -125,12 +126,20 @@ def control_loop(des_vel_analysis=None):
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if params.LOGGING or params.PLOTTING:
- loggerSensors = LoggerSensors(device, qualisys=qc, logSize=params.N_SIMULATION-3)
+ loggerSensors = LoggerSensors(
+ device, qualisys=qc, logSize=params.N_SIMULATION - 3
+ )
# Initiate communication with the device and calibrate encoders
if params.SIMULATION:
- device.Init(calibrateEncoders=True, q_init=q_init, envID=params.envID,
- use_flat_plane=params.use_flat_plane, enable_pyb_GUI=params.enable_pyb_GUI, dt=params.dt_wbc)
+ device.Init(
+ calibrateEncoders=True,
+ q_init=q_init,
+ envID=params.envID,
+ use_flat_plane=params.use_flat_plane,
+ enable_pyb_GUI=params.enable_pyb_GUI,
+ dt=params.dt_wbc,
+ )
else:
# Initialize the communication and the session.
device.initialize(q_init[:])
@@ -143,22 +152,27 @@ def control_loop(des_vel_analysis=None):
# CONTROL LOOP ***************************************************
t = 0.0
- t_max = (params.N_SIMULATION-2) * params.dt_wbc
+ t_max = (params.N_SIMULATION - 2) * params.dt_wbc
k_log_whole = 0
T_whole = time.time()
dT_whole = 0.0
np.set_printoptions(precision=2, linewidth=300)
- while ((not device.is_timeout) and (t < t_max)):
+ while (not device.is_timeout) and (t < t_max):
# Update sensor data (IMU, encoders, Motion capture)
device.parse_sensor_data()
# Check that the initial position of actuators is not too far from the
# desired position of actuators to avoid breaking the robot
- if (t <= 10 * params.dt_wbc):
- if np.max(np.abs(replay_q[k_log_whole, :] - device.joints.positions)) > 0.15:
- print("DIFFERENCE: ", replay_q[k_log_whole, :] - device.joints.positions)
+ if t <= 10 * params.dt_wbc:
+ if (
+ np.max(np.abs(replay_q[k_log_whole, :] - device.joints.positions))
+ > 0.15
+ ):
+ print(
+ "DIFFERENCE: ", replay_q[k_log_whole, :] - device.joints.positions
+ )
print("q_des: ", replay_q[k_log_whole, :])
print("q_mes: ", device.joints.positions)
break
@@ -186,15 +200,17 @@ def control_loop(des_vel_analysis=None):
# ****************************************************************
- finished = (t >= t_max)
+ finished = t >= t_max
damp_control(device, 12)
-
+
device.joints.set_torques(np.zeros(12))
device.send_command_and_wait_end_of_cycle(params.dt_wbc)
if device.is_timeout:
print("Masterboard timeout detected.")
- print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.")
+ print(
+ "Either the masterboard has been shut down or there has been a connection issue with the cable/wifi."
+ )
if params.LOGGING:
loggerSensors.saveAll()
@@ -207,7 +223,9 @@ def control_loop(des_vel_analysis=None):
if __name__ == "__main__":
- os.nice(-20) # Â Set the process to highest priority (from -20 highest to +20 lowest)
+ os.nice(
+ -20
+ ) # Â Set the process to highest priority (from -20 highest to +20 lowest)
f, v = control_loop() # , np.array([1.5, 0.0, 0.0, 0.0, 0.0, 0.0]))
print("End of script")
print(f, v)
diff --git a/scripts/solo3D/SurfacePlanner.py b/python/quadruped_reactive_walking/solo3D/SurfacePlanner.py
similarity index 71%
rename from scripts/solo3D/SurfacePlanner.py
rename to python/quadruped_reactive_walking/solo3D/SurfacePlanner.py
index 9e7d8bf37149e1da5cb3a6574819d2095a584fd7..6286832071354b4a224e46459b7f51b6397ade47 100644
--- a/scripts/solo3D/SurfacePlanner.py
+++ b/python/quadruped_reactive_walking/solo3D/SurfacePlanner.py
@@ -1,26 +1,27 @@
-import pinocchio as pin
-import numpy as np
-import os
import copy
+import os
+import pinocchio as pin
+import numpy as np
from sl1m.problem_definition import Problem
from sl1m.generic_solver import solve_MIP
-
from solo_rbprm.solo_abstract import Robot as SoloAbstract
-
from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver.rbprm.tools.surfaces_from_path import getAllSurfacesDict
-from solo3D.utils import getAllSurfacesDict_inner
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import ViewerFactory
+from .utils import getAllSurfacesDict_inner
+
# --------------------------------- PROBLEM DEFINITION ---------------------------------------------------------------
-paths = [os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/com_inequalities/feet_quasi_flat/",
- os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/relative_effector_positions/"]
-limbs = ['FLleg', 'FRleg', 'HLleg', 'HRleg']
-others = ['FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT']
-rom_names = ['solo_LFleg_rom', 'solo_RFleg_rom', 'solo_LHleg_rom', 'solo_RHleg_rom']
+paths = [
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/com_inequalities/feet_quasi_flat/",
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/relative_effector_positions/",
+]
+limbs = ["FLleg", "FRleg", "HLleg", "HRleg"]
+others = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"]
+rom_names = ["solo_LFleg_rom", "solo_RFleg_rom", "solo_LHleg_rom", "solo_RHleg_rom"]
class SurfacePlanner:
@@ -35,24 +36,32 @@ class SurfacePlanner:
self.plot = False
self.use_heuristique = params.use_heuristic
- self.step_duration = params.T_gait/2
+ self.step_duration = params.T_gait / 2
self.shoulders = np.reshape(params.footsteps_under_shoulders, (3, 4), order="F")
self.solo_abstract = SoloAbstract()
- self.solo_abstract.setJointBounds("root_joint", [-5., 5., -5., 5., 0.241, 1.5])
+ self.solo_abstract.setJointBounds(
+ "root_joint", [-5.0, 5.0, -5.0, 5.0, 0.241, 1.5]
+ )
self.solo_abstract.boundSO3([-3.14, 3.14, -0.01, 0.01, -0.01, 0.01])
self.solo_abstract.setFilter(rom_names)
for limb in rom_names:
- self.solo_abstract.setAffordanceFilter(limb, ['Support'])
+ self.solo_abstract.setAffordanceFilter(limb, ["Support"])
self.ps = ProblemSolver(self.solo_abstract)
self.vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
- self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+ self.afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005])
- self.afftool.loadObstacleModel(os.environ["SOLO3D_ENV_DIR"] + params.environment_URDF, "environment", self.vf)
+ self.afftool.loadObstacleModel(
+ os.environ["SOLO3D_ENV_DIR"] + params.environment_URDF,
+ "environment",
+ self.vf,
+ )
self.ps.selectPathValidation("RbprmPathValidation", 0.05)
- self.all_surfaces = getAllSurfacesDict_inner(getAllSurfacesDict(self.afftool), margin=0.02)
+ self.all_surfaces = getAllSurfacesDict_inner(
+ getAllSurfacesDict(self.afftool), margin=0.02
+ )
self.potential_surfaces = []
@@ -99,11 +108,17 @@ class SurfacePlanner:
for phase in self.pb.phaseData:
for foot in range(4):
if foot in phase.moving:
- rpy = pin.rpy.matrixToRpy(pin.Quaternion(configs[phase.id][3:7].copy()).toRotationMatrix())
- hRb = pin.rpy.rpyToMatrix(np.array([rpy[0], rpy[1], 0.]))
- wRh = pin.rpy.rpyToMatrix(np.array([0., 0., rpy[2]]))
- heuristic = wRh @ (0.5 * self.step_duration * copy.deepcopy(h_v_ref)) + wRh @ hRb @ copy.deepcopy(self.shoulders[:, foot])
- effector_positions[foot].append(np.array(configs[phase.id][:2] + heuristic[:2]))
+ rpy = pin.rpy.matrixToRpy(
+ pin.Quaternion(configs[phase.id][3:7].copy()).toRotationMatrix()
+ )
+ hRb = pin.rpy.rpyToMatrix(np.array([rpy[0], rpy[1], 0.0]))
+ wRh = pin.rpy.rpyToMatrix(np.array([0.0, 0.0, rpy[2]]))
+ heuristic = wRh @ (
+ 0.5 * self.step_duration * copy.deepcopy(h_v_ref)
+ ) + wRh @ hRb @ copy.deepcopy(self.shoulders[:, foot])
+ effector_positions[foot].append(
+ np.array(configs[phase.id][:2] + heuristic[:2])
+ )
else:
effector_positions[foot].append(None)
@@ -111,14 +126,16 @@ class SurfacePlanner:
def compute_shoulder_positions(self, configs):
"""
- Compute the shoulder positions
+ Compute the shoulder positions
:param configs the list of configurations
"""
shoulder_positions = np.zeros((4, self.pb.n_phases, 3))
for phase in self.pb.phaseData:
for foot in phase.moving:
R = pin.Quaternion(configs[phase.id][3:7]).toRotationMatrix()
- shoulder_positions[foot][phase.id] = R @ self.shoulders[:, foot] + configs[phase.id][:3]
+ shoulder_positions[foot][phase.id] = (
+ R @ self.shoulders[:, foot] + configs[phase.id][:3]
+ )
return shoulder_positions
def get_potential_surfaces(self, configs, gait):
@@ -132,14 +149,20 @@ class SurfacePlanner:
empty_list = False
for id, config in enumerate(configs):
stance_feet = np.nonzero(gait[id % len(gait)] == 1)[0]
- previous_swing_feet = np.nonzero(gait[(id-1) % len(gait)] == 0)[0]
- moving_feet = stance_feet[np.in1d(stance_feet, previous_swing_feet, assume_unique=True)]
+ previous_swing_feet = np.nonzero(gait[(id - 1) % len(gait)] == 0)[0]
+ moving_feet = stance_feet[
+ np.in1d(stance_feet, previous_swing_feet, assume_unique=True)
+ ]
roms = np.array(rom_names)[moving_feet]
foot_surfaces = []
for rom in roms:
surfaces = []
- surfaces_names = self.solo_abstract.clientRbprm.rbprm.getCollidingObstacleAtConfig(config.tolist(), rom)
+ surfaces_names = (
+ self.solo_abstract.clientRbprm.rbprm.getCollidingObstacleAtConfig(
+ config.tolist(), rom
+ )
+ )
for name in surfaces_names:
surfaces.append(self.all_surfaces[name][0])
@@ -210,34 +233,62 @@ class SurfacePlanner:
vertices, inequalities, _ = self.retrieve_surfaces(surfaces)
return vertices, inequalities, None, None, False
- self.pb.generate_problem(R, surfaces, gait, initial_contacts, c0=None, com=False)
+ self.pb.generate_problem(
+ R, surfaces, gait, initial_contacts, c0=None, com=False
+ )
shoulder_positions = self.compute_shoulder_positions(configs)
if self.use_heuristique:
effector_positions = self.compute_effector_positions(configs, h_v_ref)
- costs = {"effector_positions_xy": [1., effector_positions], "effector_positions_3D": [0.1, shoulder_positions]}
+ costs = {
+ "effector_positions_xy": [1.0, effector_positions],
+ "effector_positions_3D": [0.1, shoulder_positions],
+ }
else:
step_length = self.compute_step_length(h_v_ref[:2])
- costs = {"step_length": [1.0, step_length], "effector_positions_3D": [0.1, shoulder_positions]}
+ costs = {
+ "step_length": [1.0, step_length],
+ "effector_positions_3D": [0.1, shoulder_positions],
+ }
result = solve_MIP(self.pb, costs=costs, com=False)
if result.success:
if self.plot:
import matplotlib.pyplot as plt
import sl1m.tools.plot_tools as plot
- ax = plot.draw_whole_scene(self.all_surfaces)
- plot.plot_planner_result(result.all_feet_pos, effector_positions=effector_positions, coms=configs, ax=ax, show=True)
- vertices, inequalities, indices = self.retrieve_surfaces(surfaces, result.surface_indices)
+ ax = plot.draw_whole_scene(self.all_surfaces)
+ plot.plot_planner_result(
+ result.all_feet_pos,
+ effector_positions=effector_positions,
+ coms=configs,
+ ax=ax,
+ show=True,
+ )
+
+ vertices, inequalities, indices = self.retrieve_surfaces(
+ surfaces, result.surface_indices
+ )
return vertices, inequalities, indices, result.all_feet_pos, True
if self.plot:
import matplotlib.pyplot as plt
import sl1m.tools.plot_tools as plot
+
ax = plot.draw_whole_scene(self.all_surfaces)
plot.plot_initial_contacts(initial_contacts, ax=ax)
- ax.scatter([c[0] for c in configs], [c[1] for c in configs], [c[2] for c in configs], marker='o', linewidth=5)
- ax.plot([c[0] for c in configs], [c[1] for c in configs], [c[2] for c in configs])
+ ax.scatter(
+ [c[0] for c in configs],
+ [c[1] for c in configs],
+ [c[2] for c in configs],
+ marker="o",
+ linewidth=5,
+ )
+ ax.plot(
+ [c[0] for c in configs],
+ [c[1] for c in configs],
+ [c[2] for c in configs],
+ )
plt.show()
print("The MIP problem did NOT converge")
diff --git a/scripts/solo3D/SurfacePlannerWrapper.py b/python/quadruped_reactive_walking/solo3D/SurfacePlannerWrapper.py
similarity index 69%
rename from scripts/solo3D/SurfacePlannerWrapper.py
rename to python/quadruped_reactive_walking/solo3D/SurfacePlannerWrapper.py
index 14dd62ba20304024fb99f674ea0180534fb3751f..811bed38dd78d818cff0eea0c512c032ce1129c1 100644
--- a/scripts/solo3D/SurfacePlannerWrapper.py
+++ b/python/quadruped_reactive_walking/solo3D/SurfacePlannerWrapper.py
@@ -1,22 +1,24 @@
-from solo3D.SurfacePlanner import SurfacePlanner
-
from multiprocessing import Process
from multiprocessing.sharedctypes import Value
import ctypes
import time
import copy
-import quadruped_reactive_walking as qrw
import numpy as np
+from .. import quadruped_reactive_walking as qrw
+from .SurfacePlanner import SurfacePlanner
+
N_VERTICES_MAX = 4
N_ROWS = N_VERTICES_MAX + 2
N_SURFACE_MAX = 10
N_POTENTIAL_SURFACE = 7
N_FEET = 4
+
class SurfaceDataCtype(ctypes.Structure):
- ''' ctype data structure for the shared memory between processes, for surfaces
+ """
+ ctype data structure for the shared memory between processes, for surfaces
Ax <= b
If normal as inequalities : A = [A , n , -n] , b = [b , d + eps, -d-eps]
Normal as equality : A = [A , n] , b = [b , d]
@@ -26,36 +28,52 @@ class SurfaceDataCtype(ctypes.Structure):
[x2,y2,z2], ...]
on = Bool if the surface is used or not
- TODO : if more than 4 vertices, add a variable for the number of vertice to reshape the appropriate buffer
- '''
- _fields_ = [('A', ctypes.c_double * 3 * N_ROWS), ('b', ctypes.c_double * N_ROWS),
- ('vertices', ctypes.c_double * 3 * N_VERTICES_MAX), ('on', ctypes.c_bool)]
+ """
+
+ _fields_ = [
+ ("A", ctypes.c_double * 3 * N_ROWS),
+ ("b", ctypes.c_double * N_ROWS),
+ ("vertices", ctypes.c_double * 3 * N_VERTICES_MAX),
+ ("on", ctypes.c_bool),
+ ]
class DataOutCtype(ctypes.Structure):
- ''' ctype data structure for the shared memory between processes
+ """
+ ctype data structure for the shared memory between processes
Data Out, list of potential and the selected surfaces given by the MIP
Potential surfaces are used if the MIP has not converged
- '''
+ """
+
params = qrw.Params()
- _fields_ = [('potentialSurfaces', SurfaceDataCtype * N_POTENTIAL_SURFACE * N_FEET),
- ('selectedSurfaces', SurfaceDataCtype * N_FEET), ('all_feet', ctypes.c_double * 12 * params.number_steps),
- ('success', ctypes.c_bool), ('t_mip', ctypes.c_float)]
+ _fields_ = [
+ ("potentialSurfaces", SurfaceDataCtype * N_POTENTIAL_SURFACE * N_FEET),
+ ("selectedSurfaces", SurfaceDataCtype * N_FEET),
+ ("all_feet", ctypes.c_double * 12 * params.number_steps),
+ ("success", ctypes.c_bool),
+ ("t_mip", ctypes.c_float),
+ ]
class DataInCtype(ctypes.Structure):
- ''' ctype data structure for the shared memory between processes
+ """
+ ctype data structure for the shared memory between processes
TODO : if more than 4 vertices, add a variable for the number of vertice to reshape the appropriate buffer
- '''
+ """
+
params = qrw.Params()
- _fields_ = [('gait', ctypes.c_int64 * 4 * int(params.gait.shape[0])), ('configs', ctypes.c_double * 7 * params.number_steps),
- ('h_v_ref', ctypes.c_double * 3), ('contacts', ctypes.c_double * 12)]
+ _fields_ = [
+ ("gait", ctypes.c_int64 * 4 * int(params.gait.shape[0])),
+ ("configs", ctypes.c_double * 7 * params.number_steps),
+ ("h_v_ref", ctypes.c_double * 3),
+ ("contacts", ctypes.c_double * 12),
+ ]
-class Surface_planner_wrapper():
- '''
+class Surface_planner_wrapper:
+ """
Wrapper for the class SurfacePlanner for the paralellisation
- '''
+ """
def __init__(self, params):
@@ -63,17 +81,26 @@ class Surface_planner_wrapper():
self.n_gait = int(params.gait.shape[0])
# Usefull for 1st iteration of QP
- A = [[-1.0000000, 0.0000000, 0.0000000], [0.0000000, -1.0000000, 0.0000000],
- [0.0000000, 1.0000000, 0.0000000], [1.0000000, 0.0000000, 0.0000000],
- [0.0000000, 0.0000000, 1.0000000], [0.0000000, 0.0000000, -1.0000000]]
+ A = [
+ [-1.0000000, 0.0000000, 0.0000000],
+ [0.0000000, -1.0000000, 0.0000000],
+ [0.0000000, 1.0000000, 0.0000000],
+ [1.0000000, 0.0000000, 0.0000000],
+ [0.0000000, 0.0000000, 1.0000000],
+ [0.0000000, 0.0000000, -1.0000000],
+ ]
b = [1.3946447, 0.9646447, 0.9646447, 0.5346446, 0.0000, 0.0000]
- vertices = [[-1.3946447276978748, 0.9646446609406726, 0.0], [-1.3946447276978748, -0.9646446609406726, 0.0],
- [0.5346445941834704, -0.9646446609406726, 0.0], [0.5346445941834704, 0.9646446609406726, 0.0]]
+ vertices = [
+ [-1.3946447276978748, 0.9646446609406726, 0.0],
+ [-1.3946447276978748, -0.9646446609406726, 0.0],
+ [0.5346445941834704, -0.9646446609406726, 0.0],
+ [0.5346445941834704, 0.9646446609406726, 0.0],
+ ]
self.floor_surface = qrw.Surface(np.array(A), np.array(b), np.array(vertices))
# Results used by controller
self.mip_success = False
- self.t_mip = 0.
+ self.t_mip = 0.0
self.mip_iteration = 0
self.potential_surfaces = qrw.SurfaceVectorVector()
self.selected_surfaces = qrw.SurfaceVector()
@@ -88,9 +115,9 @@ class Surface_planner_wrapper():
self.multiprocessing = params.enable_multiprocessing_mip
if self.multiprocessing:
- self.new_data = Value('b', False)
- self.new_result = Value('b', True)
- self.running = Value('b', True)
+ self.new_data = Value("b", False)
+ self.new_result = Value("b", True)
+ self.running = Value("b", True)
self.data_out = Value(DataOutCtype)
self.data_in = Value(DataInCtype)
p = Process(target=self.asynchronous_process)
@@ -101,20 +128,26 @@ class Surface_planner_wrapper():
self.initialized = False
def run(self, configs, gait_in, current_contacts, h_v_ref):
- '''
+ """
Either call synchronous or asynchronous planner
- '''
+ """
if self.multiprocessing:
self.run_asynchronous(configs, gait_in, current_contacts, h_v_ref)
else:
self.run_synchronous(configs, gait_in, current_contacts, h_v_ref)
def run_synchronous(self, configs, gait, current_contacts, h_v_ref):
- '''
+ """
Call the planner and store the result in syn variables
- '''
+ """
t_start = time.time()
- vertices, inequalities, indices, self.all_feet_pos_syn, success = self.planner.run(configs, gait, current_contacts, h_v_ref)
+ (
+ vertices,
+ inequalities,
+ indices,
+ self.all_feet_pos_syn,
+ success,
+ ) = self.planner.run(configs, gait, current_contacts, h_v_ref)
self.mip_iteration_syn += 1
self.mip_success_syn = success
@@ -129,13 +162,15 @@ class Surface_planner_wrapper():
if success:
for foot, foot_inequalities in enumerate(inequalities):
S, s = foot_inequalities[indices[foot]]
- self.selected_surfaces_syn.append(qrw.Surface(S, s, vertices[foot][indices[foot]].T))
+ self.selected_surfaces_syn.append(
+ qrw.Surface(S, s, vertices[foot][indices[foot]].T)
+ )
self.t_mip = time.time() - t_start
def run_asynchronous(self, configs, gait_in, current_contacts, h_v_ref_in):
- '''
- Compress the data and send them for asynchronous process
- '''
+ """
+ Compress the data and send them for asynchronous process
+ """
for i, config in enumerate(configs):
data_config = np.frombuffer(self.data_in.configs[i])
data_config[:] = config[:]
@@ -148,9 +183,9 @@ class Surface_planner_wrapper():
self.new_data.value = True
def asynchronous_process(self):
- '''
- Asynchronous process created during initialization
- '''
+ """
+ Asynchronous process created during initialization
+ """
planner = SurfacePlanner(self.params)
while self.running.value:
if self.new_data.value:
@@ -162,26 +197,32 @@ class Surface_planner_wrapper():
h_v_ref = np.frombuffer(self.data_in.h_v_ref).reshape((3))
contacts = np.frombuffer(self.data_in.contacts).reshape((3, 4))
- vertices, inequalities, indices, _, success = planner.run(configs, gait, contacts, h_v_ref)
+ vertices, inequalities, indices, _, success = planner.run(
+ configs, gait, contacts, h_v_ref
+ )
t = time.time() - t_start
self.compress_result(vertices, inequalities, indices, success, t)
self.new_result.value = True
def compress_result(self, vertices, inequalities, indices, success, t):
- '''
+ """
Store the planner result in data_out
- '''
+ """
self.data_out.success = success
self.data_out.t_mip = t
for foot, foot_inequalities in enumerate(inequalities):
i = 0
for i, (S, s) in enumerate(foot_inequalities):
- A = np.frombuffer(self.data_out.potentialSurfaces[foot][i].A).reshape((N_ROWS, 3))
+ A = np.frombuffer(self.data_out.potentialSurfaces[foot][i].A).reshape(
+ (N_ROWS, 3)
+ )
A[:, :] = S[:, :]
b = np.frombuffer(self.data_out.potentialSurfaces[foot][i].b)
b[:] = s[:]
- v = np.frombuffer(self.data_out.potentialSurfaces[foot][i].vertices).reshape((N_VERTICES_MAX, 3))
+ v = np.frombuffer(
+ self.data_out.potentialSurfaces[foot][i].vertices
+ ).reshape((N_VERTICES_MAX, 3))
v[:, :] = vertices[foot][i].T[:, :]
self.data_out.potentialSurfaces[foot][i].on = True
@@ -190,20 +231,24 @@ class Surface_planner_wrapper():
if success:
for foot, index in enumerate(indices):
- A = np.frombuffer(self.data_out.selectedSurfaces[foot].A).reshape((N_ROWS, 3))
+ A = np.frombuffer(self.data_out.selectedSurfaces[foot].A).reshape(
+ (N_ROWS, 3)
+ )
A[:, :] = inequalities[foot][index][0][:, :]
b = np.frombuffer(self.data_out.selectedSurfaces[foot].b)
b[:] = inequalities[foot][index][1][:]
- v = np.frombuffer(self.data_out.selectedSurfaces[foot].vertices).reshape((N_VERTICES_MAX, 3))
+ v = np.frombuffer(
+ self.data_out.selectedSurfaces[foot].vertices
+ ).reshape((N_VERTICES_MAX, 3))
v = vertices[foot][index].T[:, :]
self.data_out.selectedSurfaces[foot].on = True
def get_latest_results(self):
- '''
- Update latest results : 2 list
+ """
+ Update latest results : 2 list
- potential surfaces : used if MIP has not converged
- selected_surfaces : surfaces selected for the next phases
- '''
+ """
if self.multiprocessing:
if self.new_result.value:
self.new_result.value = False
@@ -217,13 +262,21 @@ class Surface_planner_wrapper():
potential_surfaces = qrw.SurfaceVector()
for s in foot_surfaces:
if s.on:
- potential_surfaces.append(qrw.Surface(np.array(s.A), np.array(s.b), np.array(s.vertices)))
+ potential_surfaces.append(
+ qrw.Surface(
+ np.array(s.A), np.array(s.b), np.array(s.vertices)
+ )
+ )
self.potential_surfaces.append(potential_surfaces)
self.selected_surfaces = qrw.SurfaceVector()
if self.data_out.success:
for s in self.data_out.selectedSurfaces:
- self.selected_surfaces.append(qrw.Surface(np.array(s.A), np.array(s.b), np.array(s.vertices)))
+ self.selected_surfaces.append(
+ qrw.Surface(
+ np.array(s.A), np.array(s.b), np.array(s.vertices)
+ )
+ )
else:
print("Error: No new MIP result available \n")
pass
diff --git a/python/quadruped_reactive_walking/solo3D/__init__.py b/python/quadruped_reactive_walking/solo3D/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/scripts/solo3D/heightmap_generator.py b/python/quadruped_reactive_walking/solo3D/heightmap_generator.py
similarity index 69%
rename from scripts/solo3D/heightmap_generator.py
rename to python/quadruped_reactive_walking/solo3D/heightmap_generator.py
index 3e49a4f4bb01f819e9bcbcf9c39ae5da18722a34..155bbfbc5f224b099d723d913ea285d5d4d5e676 100644
--- a/scripts/solo3D/heightmap_generator.py
+++ b/python/quadruped_reactive_walking/solo3D/heightmap_generator.py
@@ -1,17 +1,16 @@
-import matplotlib.pyplot as plt
import os
-import numpy as np
-
-from solo3D.heightmap_tools import Heightmap
-from solo3D.utils import getAllSurfacesDict_inner
+import matplotlib.pyplot as plt
+import numpy as np
from solo_rbprm.solo_abstract import Robot
-
from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver.rbprm.tools.surfaces_from_path import getAllSurfacesDict
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import ViewerFactory
-import quadruped_reactive_walking as qrw
+
+from .. import quadruped_reactive_walking as qrw
+from .heightmap_tools import Heightmap
+from .utils import getAllSurfacesDict_inner
# --------------------------------- PROBLEM DEFINITION ---------------------------------------------------------------
params = qrw.Params()
@@ -21,13 +20,24 @@ N_Y = 90
X_BOUNDS = [-0.3, 2.5]
Y_BOUNDS = [-0.8, 0.8]
-rom_names = ['solo_LFleg_rom', 'solo_RFleg_rom', 'solo_LHleg_rom', 'solo_RHleg_rom']
-others = ['FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT']
-LIMBS = ['solo_RHleg_rom', 'solo_LHleg_rom', 'solo_LFleg_rom', 'solo_RFleg_rom']
-paths = [os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/com_inequalities/feet_quasi_flat/",
- os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/relative_effector_positions/"]
-
-COLORS = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
+rom_names = ["solo_LFleg_rom", "solo_RFleg_rom", "solo_LHleg_rom", "solo_RHleg_rom"]
+others = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"]
+LIMBS = ["solo_RHleg_rom", "solo_LHleg_rom", "solo_LFleg_rom", "solo_RFleg_rom"]
+paths = [
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/com_inequalities/feet_quasi_flat/",
+ os.environ["INSTALL_HPP_DIR"] + "/solo-rbprm/relative_effector_positions/",
+]
+
+COLORS = [
+ "#1f77b4",
+ "#ff7f0e",
+ "#2ca02c",
+ "#d62728",
+ "#e377c2",
+ "#7f7f7f",
+ "#bcbd22",
+ "#17becf",
+]
# --------------------------------- METHODS ---------------------------------------------------------------
@@ -37,22 +47,24 @@ def init_afftool():
dictionary and all the affordance points
"""
robot = Robot()
- robot.setJointBounds("root_joint", [-5., 5., -5., 5., 0.241, 1.5])
+ robot.setJointBounds("root_joint", [-5.0, 5.0, -5.0, 5.0, 0.241, 1.5])
robot.boundSO3([-3.14, 3.14, -0.01, 0.01, -0.01, 0.01])
robot.setFilter(LIMBS)
for limb in LIMBS:
- robot.setAffordanceFilter(limb, ['Support'])
+ robot.setAffordanceFilter(limb, ["Support"])
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
afftool = AffordanceTool()
- afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
- afftool.loadObstacleModel(os.environ["SOLO3D_ENV_DIR"] + params.environment_URDF, "environment", vf)
+ afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005])
+ afftool.loadObstacleModel(
+ os.environ["SOLO3D_ENV_DIR"] + params.environment_URDF, "environment", vf
+ )
ps.selectPathValidation("RbprmPathValidation", 0.05)
return afftool
-def plot_surface(points, ax, color_id=0, alpha=1.):
+def plot_surface(points, ax, color_id=0, alpha=1.0):
"""
Plot a surface
"""
@@ -78,10 +90,11 @@ def draw_whole_scene(surface_dict, ax=None, title=None, color_id=5):
plot_surface(np.array(surface_dict[key][0]).T, ax, color_id)
return ax
+
# --------------------------------- MAIN ---------------------------------------------------------------
if __name__ == "__main__":
afftool = init_afftool()
- affordances = afftool.getAffordancePoints('Support')
+ affordances = afftool.getAffordancePoints("Support")
all_surfaces = getAllSurfacesDict(afftool)
new_surfaces = getAllSurfacesDict_inner(all_surfaces, 0.03)
@@ -93,4 +106,4 @@ if __name__ == "__main__":
heightmap.plot()
ax = draw_whole_scene(all_surfaces)
draw_whole_scene(new_surfaces, ax, color_id=0)
- plt.show(block=True)
\ No newline at end of file
+ plt.show(block=True)
diff --git a/scripts/solo3D/heightmap_tools.py b/python/quadruped_reactive_walking/solo3D/heightmap_tools.py
similarity index 88%
rename from scripts/solo3D/heightmap_tools.py
rename to python/quadruped_reactive_walking/solo3D/heightmap_tools.py
index 3f715206b2729ef8d081ad42f4bd7df19fa3a5b8..99006d3c5f7a56bc2b6568a000068ea5b32ab295 100644
--- a/scripts/solo3D/heightmap_tools.py
+++ b/python/quadruped_reactive_walking/solo3D/heightmap_tools.py
@@ -1,9 +1,19 @@
-import numpy as np
-import hppfcl
-import pickle
import ctypes
+import pickle
-COLORS = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
+import hppfcl
+import numpy as np
+
+COLORS = [
+ "#1f77b4",
+ "#ff7f0e",
+ "#2ca02c",
+ "#d62728",
+ "#e377c2",
+ "#7f7f7f",
+ "#bcbd22",
+ "#17becf",
+]
class MapHeader(ctypes.Structure):
@@ -18,7 +28,6 @@ class MapHeader(ctypes.Structure):
class Heightmap:
-
def __init__(self, n_x, n_y, x_lim, y_lim):
"""
:param n_x number of samples in x
@@ -35,7 +44,7 @@ class Heightmap:
self.z = np.zeros((n_x, n_y))
def save_pickle(self, filename):
- filehandler = open(filename, 'wb')
+ filehandler = open(filename, "wb")
pickle.dump(self, filehandler)
def save_binary(self, filename):
@@ -57,7 +66,7 @@ class Heightmap:
def build_from_fit(self, fit):
"""
Build the heightmap and return it
- For each slot in the grid create a vertical segment and check its collisions with the
+ For each slot in the grid create a vertical segment and check its collisions with the
affordances until one is found
:param affordances list of affordances
"""
@@ -68,15 +77,15 @@ class Heightmap:
def build(self, affordances):
"""
Build the heightmap and return it
- For each slot in the grid create a vertical segment and check its collisions with the
+ For each slot in the grid create a vertical segment and check its collisions with the
affordances until one is found
:param affordances list of affordances
"""
- last_z = 0.
+ last_z = 0.0
for i in range(self.n_x):
for j in range(self.n_y):
- p1 = np.array([self.x[i], self.y[j], -1.])
- p2 = np.array([self.x[i], self.y[j], 10.])
+ p1 = np.array([self.x[i], self.y[j], -1.0])
+ p2 = np.array([self.x[i], self.y[j], 10.0])
segment = np.array([p1, p2])
fcl_segment = convex(segment, [0, 1, 0])
@@ -87,7 +96,11 @@ class Heightmap:
for triangle_list in affordance:
triangle = [np.array(p) for p in triangle_list]
if intersect_line_triangle(segment, triangle):
- intersections.append(get_point_intersect_line_triangle(segment, triangle)[2])
+ intersections.append(
+ get_point_intersect_line_triangle(
+ segment, triangle
+ )[2]
+ )
if len(intersections) != 0:
self.z[i, j] = np.max(np.array(intersections))
@@ -95,7 +108,7 @@ class Heightmap:
else:
self.z[i, j] = last_z
- def plot(self, alpha=1., ax=None):
+ def plot(self, alpha=1.0, ax=None):
"""
Plot the heightmap
"""
@@ -105,16 +118,16 @@ class Heightmap:
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
i = 0
- if alpha != 1.:
+ if alpha != 1.0:
i = 1
- xv, yv = np.meshgrid(self.x, self.y, sparse=False, indexing='ij')
+ xv, yv = np.meshgrid(self.x, self.y, sparse=False, indexing="ij")
ax.plot_surface(xv, yv, self.z, color=COLORS[i], alpha=alpha)
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
- ax.set_zlim([np.min(self.z), np.max(self.z) + 1.])
+ ax.set_zlim([np.min(self.z), np.max(self.z) + 1.0])
return ax
@@ -147,7 +160,7 @@ def distance(object1, object2):
"""
Returns the distance between object1 and object2
"""
- guess = np.array([1., 0., 0.])
+ guess = np.array([1.0, 0.0, 0.0])
support_hint = np.array([0, 0], dtype=np.int32)
shape = hppfcl.MinkowskiDiff()
diff --git a/scripts/solo3D/pyb_environment_3D.py b/python/quadruped_reactive_walking/solo3D/pyb_environment_3D.py
similarity index 53%
rename from scripts/solo3D/pyb_environment_3D.py
rename to python/quadruped_reactive_walking/solo3D/pyb_environment_3D.py
index 4997cf92ef34c59e5053fcd5242828da25bad33e..8265fd69e8da904b6f7f6de85689db5c0383deb2 100644
--- a/scripts/solo3D/pyb_environment_3D.py
+++ b/python/quadruped_reactive_walking/solo3D/pyb_environment_3D.py
@@ -1,13 +1,16 @@
+import os
+
import numpy as np
import pybullet as pyb
import pybullet_data
-import os
-class PybEnvironment3D():
- ''' Class to vizualise the 3D environment and foot trajectory and in PyBullet simulation.
- '''
- def __init__(self, params, gait, statePlanner, footStepPlanner, footTrajectoryGenerator):
+class PybEnvironment3D:
+ """Class to vizualise the 3D environment and foot trajectory and in PyBullet simulation."""
+
+ def __init__(
+ self, params, gait, statePlanner, footStepPlanner, footTrajectoryGenerator
+ ):
"""
Store the solo3D class, used for trajectory vizualisation.
Args:
@@ -35,10 +38,10 @@ class PybEnvironment3D():
self.refresh = 1
def update(self, k):
- ''' Update position of the objects in pybullet environment.
+ """Update position of the objects in pybullet environment.
Args :
- k (int) : step of the simulation.
- '''
+ """
# On iteration 0, PyBullet env has not been started
if k == 1:
self.initializeEnv()
@@ -51,38 +54,43 @@ class PybEnvironment3D():
def updateCamera(self, k, device):
# Update position of PyBullet camera on the robot position to do as if it was attached to the robot
if k > 10 and self.enable_pyb_GUI:
- pyb.resetDebugVisualizerCamera(cameraDistance=0.95,
- cameraYaw=357,
- cameraPitch=-29,
- cameraTargetPosition=[0.6, 0.14, -0.22])
+ pyb.resetDebugVisualizerCamera(
+ cameraDistance=0.95,
+ cameraYaw=357,
+ cameraPitch=-29,
+ cameraTargetPosition=[0.6, 0.14, -0.22],
+ )
# pyb.resetDebugVisualizerCamera(cameraDistance=1., cameraYaw=357, cameraPitch=-28,
# cameraTargetPosition=[device.dummyHeight[0], device.dummyHeight[1], 0.0])
return 0
def update_target_SL1M(self, all_feet_pos):
- ''' Update position of the SL1M target
+ """Update position of the SL1M target
Args :
- all_feet_pos : list of optimized position such as : [[Foot 1 next_pos, None , Foot1 next_pos] , [Foot 2 next_pos, None , Foot2 next_pos] ]
- '''
+ """
for step in range(len(all_feet_pos[0])):
for foot in range(len(all_feet_pos)):
if all_feet_pos[foot][step] is None:
- pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[step, foot]),
- posObj=np.array([0., 0., -0.5]),
- ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+ pyb.resetBasePositionAndOrientation(
+ int(self.sl1m_Ids_target[step, foot]),
+ posObj=np.array([0.0, 0.0, -0.5]),
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]),
+ )
else:
- pyb.resetBasePositionAndOrientation(int(self.sl1m_Ids_target[step, foot]),
- posObj=all_feet_pos[foot][step],
- ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+ pyb.resetBasePositionAndOrientation(
+ int(self.sl1m_Ids_target[step, foot]),
+ posObj=all_feet_pos[foot][step],
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]),
+ )
return 0
def updateTargetTrajectory(self):
- ''' Update the target trajectory for current and next phases. Hide the unnecessary spheres.
- '''
+ """Update the target trajectory for current and next phases. Hide the unnecessary spheres."""
gait = self.gait.get_gait_matrix()
fsteps = self.footStepPlanner.get_footsteps()
@@ -97,7 +105,9 @@ class PybEnvironment3D():
for i in range(gait.shape[0]):
# footsteps = fsteps[i].reshape((3, 4), order="F")
if i > 0:
- if (1 - gait[i - 1, j]) * gait[i, j] > 0: # from flying phase to stance
+ if (1 - gait[i - 1, j]) * gait[
+ i, j
+ ] > 0: # from flying phase to stance
if c == 0:
# Current flying phase, using coeff store in Bezier curve class
t0 = self.footTrajectoryGenerator.t0s[j]
@@ -106,22 +116,33 @@ class PybEnvironment3D():
for id_t, t in enumerate(t_vector):
# Bezier trajectory
- pos = self.footTrajectoryGenerator.evaluate_bezier(j, 0, t)
+ pos = self.footTrajectoryGenerator.evaluate_bezier(
+ j, 0, t
+ )
# Polynomial Curve 5th order
# pos = self.footTrajectoryGenerator.evaluate_polynom(j, 0, t)
- pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, id_t]),
- posObj=pos,
- ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+ pyb.resetBasePositionAndOrientation(
+ int(self.trajectory_Ids[c, j, id_t]),
+ posObj=pos,
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]),
+ )
c += 1
else:
if gait[i, j] == 1:
# not hidden in the floor, traj
- if not pyb.getBasePositionAndOrientation(int(self.trajectory_Ids[0, j, 0]))[0][2] == -0.1:
+ if (
+ not pyb.getBasePositionAndOrientation(
+ int(self.trajectory_Ids[0, j, 0])
+ )[0][2]
+ == -0.1
+ ):
for t in range(self.n_points):
- pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[0, j, t]),
- posObj=np.array([0., 0., -0.1]),
- ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+ pyb.resetBasePositionAndOrientation(
+ int(self.trajectory_Ids[0, j, t]),
+ posObj=np.array([0.0, 0.0, -0.1]),
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]),
+ )
c += 1
@@ -131,20 +152,27 @@ class PybEnvironment3D():
while c < self.trajectory_Ids.shape[0]:
# not hidden in the floor, traj
- if not pyb.getBasePositionAndOrientation(int(self.trajectory_Ids[c, j, 0]))[0][2] == -0.1:
+ if (
+ not pyb.getBasePositionAndOrientation(
+ int(self.trajectory_Ids[c, j, 0])
+ )[0][2]
+ == -0.1
+ ):
for t in range(self.n_points):
- pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, t]),
- posObj=np.array([0., 0., -0.1]),
- ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
+ pyb.resetBasePositionAndOrientation(
+ int(self.trajectory_Ids[c, j, t]),
+ posObj=np.array([0.0, 0.0, -0.1]),
+ ornObj=np.array([0.0, 0.0, 0.0, 1.0]),
+ )
c += 1
return 0
def initializeEnv(self):
- '''
+ """
Load objects in pybullet simulation.
- '''
+ """
print("Loading PyBullet object ...")
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
@@ -157,48 +185,61 @@ class PybEnvironment3D():
# rgbaColor : [R , B , G , alpha opacity]
if i == 0:
- rgba = [0.41, 1., 0., 1.]
+ rgba = [0.41, 1.0, 0.0, 1.0]
else:
- rgba = [0.41, 1., 0., 0.5]
+ rgba = [0.41, 1.0, 0.0, 0.5]
mesh_scale = [0.0035, 0.0035, 0.0035]
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="sphere_smooth.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=rgba,
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=rgba,
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
for j in range(4):
for id_t in range(self.n_points):
- self.trajectory_Ids[i, j, id_t] = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.0, 0.0, -0.1],
- useMaximalCoordinates=True)
+ self.trajectory_Ids[i, j, id_t] = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.0, -0.1],
+ useMaximalCoordinates=True,
+ )
# Sphere Object for SLM
# 5 phases + init pos
for i in range(6):
- rgba_list = [[1., 0., 0., 1.], [1., 0., 1., 1.], [1., 1., 0., 1.], [0., 0., 1., 1.]]
+ rgba_list = [
+ [1.0, 0.0, 0.0, 1.0],
+ [1.0, 0.0, 1.0, 1.0],
+ [1.0, 1.0, 0.0, 1.0],
+ [0.0, 0.0, 1.0, 1.0],
+ ]
mesh_scale = [0.01, 0.01, 0.01]
for j in range(4):
rgba = rgba_list[j]
rgba[-1] = 1 - (1 / 9) * i
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="sphere_smooth.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=rgba,
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
-
- self.sl1m_Ids_target[i, j] = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.0, 0.0, -0.1],
- useMaximalCoordinates=True)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=rgba,
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
+
+ self.sl1m_Ids_target[i, j] = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.0, -0.1],
+ useMaximalCoordinates=True,
+ )
print("PyBullet object loaded")
diff --git a/scripts/solo3D/utils.py b/python/quadruped_reactive_walking/solo3D/utils.py
similarity index 73%
rename from scripts/solo3D/utils.py
rename to python/quadruped_reactive_walking/solo3D/utils.py
index 7abe0152253f0c475d4b71b1d19df4fd9b6421db..c0863ac1641773a627b6ae2bc0d8fbc31f9543c0 100644
--- a/scripts/solo3D/utils.py
+++ b/python/quadruped_reactive_walking/solo3D/utils.py
@@ -4,12 +4,12 @@ from scipy.spatial import ConvexHull
def EulerToQuaternion(roll_pitch_yaw):
roll, pitch, yaw = roll_pitch_yaw
- sr = np.sin(roll / 2.)
- cr = np.cos(roll / 2.)
- sp = np.sin(pitch / 2.)
- cp = np.cos(pitch / 2.)
- sy = np.sin(yaw / 2.)
- cy = np.cos(yaw / 2.)
+ sr = np.sin(roll / 2.0)
+ cr = np.cos(roll / 2.0)
+ sp = np.sin(pitch / 2.0)
+ cp = np.cos(pitch / 2.0)
+ sy = np.sin(yaw / 2.0)
+ cy = np.cos(yaw / 2.0)
qx = sr * cp * cy - cr * sp * sy
qy = cr * sp * cy + sr * cp * sy
qz = cr * cp * sy - sr * sp * cy
@@ -18,19 +18,25 @@ def EulerToQuaternion(roll_pitch_yaw):
def inertiaTranslation(inertia, vect, mass):
- ''' Translation of the inertia matrix using Parallel Axis theorem
+ """Translation of the inertia matrix using Parallel Axis theorem
Translation from frame expressed in G to frame expressed in O.
Args :
- inertia (Array 3x3): initial inertia matrix expressed in G
- vect (Array 3x) : translation vector to apply (OG) (!warning sign)
- - mass (float) : mass
- '''
+ - mass (float) : mass
+ """
inertia_o = inertia.copy()
# Diagonal coeff :
- inertia_o[0, 0] += mass * (vect[1]**2 + vect[2]**2) # Ixx_o = Ixx_g + m*(yg**2 + zg**2)
- inertia_o[1, 1] += mass * (vect[0]**2 + vect[2]**2) # Iyy_o = Iyy_g + m*(xg**2 + zg**2)
- inertia_o[2, 2] += mass * (vect[0]**2 + vect[1]**2) # Izz_o = Iyy_g + m*(xg**2 + zg**2)
+ inertia_o[0, 0] += mass * (
+ vect[1] ** 2 + vect[2] ** 2
+ ) # Ixx_o = Ixx_g + m*(yg**2 + zg**2)
+ inertia_o[1, 1] += mass * (
+ vect[0] ** 2 + vect[2] ** 2
+ ) # Iyy_o = Iyy_g + m*(xg**2 + zg**2)
+ inertia_o[2, 2] += mass * (
+ vect[0] ** 2 + vect[1] ** 2
+ ) # Izz_o = Iyy_g + m*(xg**2 + zg**2)
inertia_o[0, 1] += mass * vect[0] * vect[1] # Ixy_o = Ixy_g + m*xg*yg
inertia_o[0, 2] += mass * vect[0] * vect[2] # Ixz_o = Ixz_g + m*xg*zg
@@ -60,9 +66,9 @@ def quaternionToRPY(quat):
rotateYa0 = -2.0 * (qx * qz - qw * qy)
rotateY = 0.0
- if (rotateYa0 >= 1.0):
+ if rotateYa0 >= 1.0:
rotateY = np.pi / 2.0
- elif (rotateYa0 <= -1.0):
+ elif rotateYa0 <= -1.0:
rotateY = -np.pi / 2.0
else:
rotateY = np.arcsin(rotateYa0)
@@ -75,22 +81,25 @@ def quaternionToRPY(quat):
return np.array([[rotateX], [rotateY], [rotateZ]])
+
def getAllSurfacesDict_inner(all_surfaces, margin):
- '''
+ """
Computes the inner vertices of the given convex surface, with a margin.
Args :
- all_surfaces : Dictionary containing the surface vertices, normal and name.
- margin : (float) margin in m
Returns :
- New dictionnary with inner vertices
- '''
+ """
all_names = []
surfaces = []
- for name_surface in all_surfaces :
+ for name_surface in all_surfaces:
vertices = order(np.array(all_surfaces.get(name_surface)[0]))
- ineq_inner, ineq_inner_vect, normal = compute_inner_inequalities(vertices, margin)
- vertices_inner = compute_inner_vertices(vertices, ineq_inner, ineq_inner_vect )
+ ineq_inner, ineq_inner_vect, normal = compute_inner_inequalities(
+ vertices, margin
+ )
+ vertices_inner = compute_inner_vertices(vertices, ineq_inner, ineq_inner_vect)
# Save inner vertices
all_names.append(name_surface)
@@ -99,16 +108,15 @@ def getAllSurfacesDict_inner(all_surfaces, margin):
surfaces_dict = dict(zip(all_names, surfaces))
return surfaces_dict
+
def norm(sq):
- """ Computes b=norm
-"""
+ """Computes b=norm"""
cr = np.cross(sq[2] - sq[0], sq[1] - sq[0])
return np.abs(cr / np.linalg.norm(cr))
def order(vertices, method="convexHull"):
- """" Order the array of vertice in counterclock wise using convex Hull method
-"""
+ """ " Order the array of vertice in counterclock wise using convex Hull method"""
if len(vertices) <= 3:
return 0
v = np.unique(vertices, axis=0)
@@ -129,20 +137,22 @@ def order(vertices, method="convexHull"):
def compute_inner_inequalities(vertices, margin):
- """Compute surface inequalities from the vertices list with a margin, update self.ineq_inner,
- self.ineq_vect_inner
-ineq_iner X <= ineq_vect_inner
-the last row contains the equality vector
-Keyword arguments:
-Vertice of the surface = [[x1 ,y1 ,z1 ]
- [x2 ,y2 ,z2 ]
- ... ]]
- """
+ """Compute surface inequalities from the vertices list with a margin, update self.ineq_inner,
+ self.ineq_vect_inner
+ ineq_iner X <= ineq_vect_inner
+ the last row contains the equality vector
+ Keyword arguments:
+ Vertice of the surface = [[x1 ,y1 ,z1 ]
+ [x2 ,y2 ,z2 ]
+ ... ]]
+ """
nb_vert = vertices.shape[0]
# Computes normal surface
- S_normal = np.cross(vertices[0, :] - vertices[1, :], vertices[0, :] - vertices[2, :])
- if S_normal @ np.array([0., 0., 1.]) < 0.: # Check orientation of the normal
+ S_normal = np.cross(
+ vertices[0, :] - vertices[1, :], vertices[0, :] - vertices[2, :]
+ )
+ if S_normal @ np.array([0.0, 0.0, 1.0]) < 0.0: # Check orientation of the normal
S_normal = -S_normal
normal = S_normal / np.linalg.norm(S_normal)
@@ -151,7 +161,11 @@ Vertice of the surface = [[x1 ,y1 ,z1 ]
ineq_vect_inner = np.zeros((nb_vert + 1))
ineq_inner[-1, :] = normal
- ineq_vect_inner[-1] = -(-normal[0] * vertices[0, 0] - normal[1] * vertices[0, 1] - normal[2] * vertices[0, 2])
+ ineq_vect_inner[-1] = -(
+ -normal[0] * vertices[0, 0]
+ - normal[1] * vertices[0, 1]
+ - normal[2] * vertices[0, 2]
+ )
for i in range(nb_vert):
@@ -180,8 +194,8 @@ Vertice of the surface = [[x1 ,y1 ,z1 ]
return ineq_inner, ineq_vect_inner, normal
-def compute_inner_vertices( vertices, ineq_inner, ineq_vect_inner):
- """" Compute the list of vertice defining the inner surface :
+def compute_inner_vertices(vertices, ineq_inner, ineq_vect_inner):
+ """ " Compute the list of vertice defining the inner surface :
update self.vertices_inner = = [[x1 ,y1 ,z1 ] shape((nb vertice , 3))
[x2 ,y2 ,z2 ]
... ]]
@@ -215,19 +229,18 @@ def compute_inner_vertices( vertices, ineq_inner, ineq_vect_inner):
def plane_intersect(P1, P2):
- """ Get the intersection between 2 plan, return Point and direction
-:param P1,P2: Plan equalities
- np.array([a,b,c,d])
- ax + by + cz + d = 0
-Returns : 1 point and 1 direction vect of the line of intersection, np.arrays, shape (3,)
-"""
+ """Get the intersection between 2 plan, return Point and direction
+ :param P1,P2: Plan equalities
+ np.array([a,b,c,d])
+ ax + by + cz + d = 0
+ Returns : 1 point and 1 direction vect of the line of intersection, np.arrays, shape (3,)"""
P1_normal, P2_normal = P1[:3], P2[:3]
aXb_vec = np.cross(P1_normal, P2_normal)
A = np.array([P1_normal, P2_normal, aXb_vec])
- d = np.array([-P1[3], -P2[3], 0.]).reshape(3, 1)
+ d = np.array([-P1[3], -P2[3], 0.0]).reshape(3, 1)
# could add np.linalg.det(A) == 0 test to prevent linalg.solve throwing error
@@ -237,13 +250,12 @@ Returns : 1 point and 1 direction vect of the line of intersection, np.arrays, s
def LinePlaneCollision(P, A, B, epsilon=1e-6):
- """ Get the intersection point between 1 plane and 1 line
-:param P: Plane equality
- np.array([a,b,c,d])
- ax + by + cz + d = 0
-param A,B : 2 points defining the line np.arrays, shape(3,)
-Returns : 1 point, np.array, shape (3,)
-"""
+ """Get the intersection point between 1 plane and 1 line
+ :param P: Plane equality
+ np.array([a,b,c,d])
+ ax + by + cz + d = 0
+ param A,B : 2 points defining the line np.arrays, shape(3,)
+ Returns : 1 point, np.array, shape (3,)"""
plane_normal = P[:3]
if P[0] == 0:
if P[1] == 0:
@@ -251,7 +263,7 @@ Returns : 1 point, np.array, shape (3,)
else:
planePoint = np.array([0, -P[-1] / P[1], 0]) # a,c = 0 --> y = -d/b
else:
- planePoint = np.array([-P[-1] / P[0], 0., 0]) # b,c = 0 --> x = -d/a
+ planePoint = np.array([-P[-1] / P[0], 0.0, 0]) # b,c = 0 --> x = -d/a
rayDirection = A - B
ndotu = plane_normal.dot(rayDirection)
diff --git a/scripts/tools/ForceMonitor.py b/python/quadruped_reactive_walking/tools/ForceMonitor.py
similarity index 56%
rename from scripts/tools/ForceMonitor.py
rename to python/quadruped_reactive_walking/tools/ForceMonitor.py
index 36a4eb8d61ee032a835e7aacfc461f45125c0b03..1cec072c620adb45c980d5e1a780043810fdb4eb 100644
--- a/scripts/tools/ForceMonitor.py
+++ b/python/quadruped_reactive_walking/tools/ForceMonitor.py
@@ -1,7 +1,7 @@
import pybullet as pyb
-class ForceMonitor:
+class ForceMonitor:
def __init__(self, robotId, planeId):
self.lines = []
@@ -9,14 +9,14 @@ class ForceMonitor:
self.planeId = planeId
def getContactPoint(self, contactPoints):
- """ Sort contacts points as there should be only one contact per foot
- and sometimes PyBullet detect several of them. There is one contact
- with a non zero force and the others have a zero contact force
+ """Sort contacts points as there should be only one contact per foot
+ and sometimes PyBullet detect several of them. There is one contact
+ with a non zero force and the others have a zero contact force
"""
for i in range(0, len(contactPoints)):
# There may be several contact points for each foot but only one of them as a non zero normal force
- if (contactPoints[i][9] != 0):
+ if contactPoints[i][9] != 0:
return contactPoints[i]
# If it returns 0 then it means there is no contact point with a non zero normal force (should not happen)
@@ -25,10 +25,18 @@ class ForceMonitor:
def display_contact_forces(self):
# Info about contact points with the ground
- contactPoints_FL = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=3) # Front left foot
- contactPoints_FR = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=7) # Front right foot
- contactPoints_HL = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=11) # Hind left foot
- contactPoints_HR = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=15) # Hind right foot
+ contactPoints_FL = pyb.getContactPoints(
+ self.robotId, self.planeId, linkIndexA=3
+ ) # Front left foot
+ contactPoints_FR = pyb.getContactPoints(
+ self.robotId, self.planeId, linkIndexA=7
+ ) # Front right foot
+ contactPoints_HL = pyb.getContactPoints(
+ self.robotId, self.planeId, linkIndexA=11
+ ) # Hind left foot
+ contactPoints_HR = pyb.getContactPoints(
+ self.robotId, self.planeId, linkIndexA=15
+ ) # Hind right foot
# print(len(contactPoint_FL), len(contactPoint_FR), len(contactPoint_HL), len(contactPoint_HR))
# Sort contacts points to get only one contact per foot
@@ -47,12 +55,15 @@ class ForceMonitor:
f_tmp = [0.0] * 3
for contact in contactPoints:
if not isinstance(contact, int): # type(contact) != type(0):
- start = [contact[6][0], contact[6][1], contact[6][2]+0.04]
- end = [contact[6][0], contact[6][1], contact[6][2]+0.04]
+ start = [contact[6][0], contact[6][1], contact[6][2] + 0.04]
+ end = [contact[6][0], contact[6][1], contact[6][2] + 0.04]
K = 0.02
for i_direction in range(0, 3):
- f_tmp[i_direction] = (contact[9] * contact[7][i_direction] + contact[10] *
- contact[11][i_direction] + contact[12] * contact[13][i_direction])
+ f_tmp[i_direction] = (
+ contact[9] * contact[7][i_direction]
+ + contact[10] * contact[11][i_direction]
+ + contact[12] * contact[13][i_direction]
+ )
end[i_direction] += K * f_tmp[i_direction]
"""if contact[3] < 10:
@@ -65,17 +76,31 @@ class ForceMonitor:
f_z += f_tmp[2]
# print("Contact: ", -f_tmp[0], -f_tmp[1], -f_tmp[2])
- if (i_line+1) > len(self.lines): # If not enough existing lines in line storage a new item is created
- lineID = pyb.addUserDebugLine(start, end, lineColorRGB=[1.0, 0.0, 0.0], lineWidth=8)
+ if (i_line + 1) > len(
+ self.lines
+ ): # If not enough existing lines in line storage a new item is created
+ lineID = pyb.addUserDebugLine(
+ start, end, lineColorRGB=[1.0, 0.0, 0.0], lineWidth=8
+ )
self.lines.append(lineID)
else: # If there is already an existing line item we modify it (to avoid flickering)
- self.lines[i_line] = pyb.addUserDebugLine(start, end, lineColorRGB=[
- 1.0, 0.0, 0.0], lineWidth=8, replaceItemUniqueId=self.lines[i_line])
+ self.lines[i_line] = pyb.addUserDebugLine(
+ start,
+ end,
+ lineColorRGB=[1.0, 0.0, 0.0],
+ lineWidth=8,
+ replaceItemUniqueId=self.lines[i_line],
+ )
i_line += 1
# Should be around 21,5 (2.2 kg * 9.81 m^2/s)
# print("Total ground reaction force: (", -f_x, ", ", -f_y, ", ", -f_z, ")")
for i_zero in range(i_line, len(self.lines)):
- self.lines[i_zero] = pyb.addUserDebugLine([0.0, 0.0, 0.0], [0.0, 0.0, 0.0], lineColorRGB=[
- 1.0, 0.0, 0.0], lineWidth=8, replaceItemUniqueId=self.lines[i_zero])
+ self.lines[i_zero] = pyb.addUserDebugLine(
+ [0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0],
+ lineColorRGB=[1.0, 0.0, 0.0],
+ lineWidth=8,
+ replaceItemUniqueId=self.lines[i_zero],
+ )
diff --git a/scripts/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
similarity index 58%
rename from scripts/tools/LoggerControl.py
rename to python/quadruped_reactive_walking/tools/LoggerControl.py
index b92d0b3ad24a545e23a9d8f50e2317d3e3a7b6b6..3b54e254dc7d446bd3d81879493f423023785edb 100644
--- a/scripts/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -1,16 +1,20 @@
-'''This class will log 1d array in Nd matrix from device and qualisys object'''
-import numpy as np
-from datetime import datetime as datetime
+"""This class will log 1d array in Nd matrix from device and qualisys object"""
+from datetime import datetime
from time import time
+
+import numpy as np
import pinocchio as pin
-class LoggerControl():
- def __init__(self, params, logSize=60e3, ringBuffer=False, loading=False, fileName=None):
+class LoggerControl:
+ def __init__(
+ self, params, logSize=60e3, ringBuffer=False, loading=False, fileName=None
+ ):
if loading:
if fileName is None:
import glob
- fileName = np.sort(glob.glob('data_2021_*.npz'))[-1] # Most recent file
+
+ fileName = np.sort(glob.glob("data_2021_*.npz"))[-1] # Most recent file
logSize = self.data["planner_gait"].shape[0]
n_gait = self.data["planner_gait"].shape[1]
self.type_MPC = int(fileName[-5])
@@ -31,61 +35,125 @@ class LoggerControl():
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)
-
- 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_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)
+
+ 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
+ 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_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
+ 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, n_gait, 4]) # Gait sequence
- self.planner_is_static = np.zeros([logSize]) # if the planner is in static mode or not
+ 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, n_gait + 1]) # Reference trajectory
# Footstep planner
- self.planner_fsteps = np.zeros([logSize, n_gait, 12]) # Reference footsteps position
- self.planner_target_fsteps = np.zeros([logSize, 3, 4]) # For each foot, next target on the ground
+ self.planner_fsteps = np.zeros(
+ [logSize, n_gait, 12]
+ ) # Reference footsteps position
+ self.planner_target_fsteps = np.zeros(
+ [logSize, 3, 4]
+ ) # For each foot, next target on the ground
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
@@ -95,8 +163,8 @@ class LoggerControl():
self.mpc_x_f = np.zeros([logSize, 32, n_gait]) # Result of the MPC
else:
self.mpc_x_f = np.zeros([logSize, 24, n_gait]) # Result of the MPC
- self.mpc_solving_duration = np.zeros([logSize]) # Computation time of the MPC
- self.mpc_cost = np.zeros([logSize, 1]) # Cost of the mpc
+ self.mpc_solving_duration = np.zeros([logSize]) # Computation time of the MPC
+ self.mpc_cost = np.zeros([logSize, 1]) # Cost of the mpc
# Whole body control
self.wbc_P = np.zeros([logSize, 12]) # proportionnal gains of the PD+
@@ -104,31 +172,66 @@ class LoggerControl():
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_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_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
+ self.wbc_tasks_err = np.zeros(
+ [logSize, 30]
+ ) # position error of tasks in InvKin
# Timestamps
self.tstamps = np.zeros(logSize)
# Solo3d logs
if self.solo3d:
- self.update_mip = np.zeros([logSize, 1]) # Boolean to know if mip computation launched
- self.configs = np.zeros([logSize, 7, params.number_steps]) # Reference configs for surface planner
- self.initial_contacts = np.zeros([logSize, 3, 4]) # Initial contacts
- self.t_mip = np.zeros([logSize, 1]) # Surface planner computation time
-
- def sample(self, joystick, estimator, controller, gait, statePlanner, footstepPlanner, footTrajectoryGenerator, wbc, dT_whole):
+ self.update_mip = np.zeros(
+ [logSize, 1]
+ ) # Boolean to know if mip computation launched
+ self.configs = np.zeros(
+ [logSize, 7, params.number_steps]
+ ) # Reference configs for surface planner
+ self.initial_contacts = np.zeros([logSize, 3, 4]) # Initial contacts
+ self.t_mip = np.zeros([logSize, 1]) # Surface planner computation time
+
+ def sample(
+ self,
+ joystick,
+ estimator,
+ controller,
+ gait,
+ statePlanner,
+ footstepPlanner,
+ footTrajectoryGenerator,
+ wbc,
+ dT_whole,
+ ):
if self.i >= self.logSize:
if self.ringBuffer:
self.i = 0
@@ -194,7 +297,9 @@ class LoggerControl():
# Logging from model predictive control
self.mpc_x_f[self.i] = controller.x_f_mpc
- self.mpc_solving_duration[self.i] = controller.mpc_wrapper.t_mpc_solving_duration
+ self.mpc_solving_duration[
+ self.i
+ ] = controller.mpc_wrapper.t_mpc_solving_duration
self.mpc_cost[self.i] = controller.mpc_cost
# Logging from whole body control
@@ -236,37 +341,63 @@ class LoggerControl():
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())
+ 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])
+ 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()
+ 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 plotTimes(self):
"""
Estimated computation time for each step of the control architecture
"""
from matplotlib import pyplot as plt
- t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+
+ t_range = np.array([k * self.dt for k in range(self.tstamps.shape[0])])
plt.figure()
- plt.plot(t_range, self.t_mip, '+', color="gold")
- 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(["SurfacePlanner", "Estimator", "Planner", "MPC", "WBC", "Control loop", "Whole loop"])
+ plt.plot(t_range, self.t_mip, "+", color="gold")
+ 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(
+ [
+ "SurfacePlanner",
+ "Estimator",
+ "Planner",
+ "MPC",
+ "WBC",
+ "Control loop",
+ "Whole loop",
+ ]
+ )
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")
self.custom_suptitle("Computation time of each block")
@@ -277,10 +408,10 @@ class LoggerControl():
"""
from matplotlib import pyplot as plt
- t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+ t_range = np.array([k * self.dt for k in range(self.tstamps.shape[0])])
fig = plt.figure()
- plt.plot(t_range[100:], self.t_mip[100:], 'k+')
+ plt.plot(t_range[100:], self.t_mip[100:], "k+")
plt.legend(["Solving duration"])
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")
@@ -292,10 +423,10 @@ class LoggerControl():
"""
from matplotlib import pyplot as plt
- t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+ t_range = np.array([k * self.dt for k in range(self.tstamps.shape[0])])
fig = plt.figure()
- plt.plot(t_range[100:], self.mpc_cost[100:], 'k+')
+ plt.plot(t_range[100:], self.mpc_cost[100:], "k+")
plt.legend(["MPC cost"])
plt.xlabel("Time [s]")
plt.ylabel("Cost value")
@@ -306,17 +437,18 @@ class LoggerControl():
Plot estimated solving time of the model prediction control
"""
from matplotlib import pyplot as plt
- t_range = np.array([k*self.dt for k in range(self.tstamps.shape[0])])
+
+ t_range = np.array([k * self.dt for k in range(self.tstamps.shape[0])])
fig = plt.figure()
- plt.plot(t_range[35:], self.mpc_solving_duration[35:], 'k+')
+ 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")
def plotStepTime(self):
- """"
+ """ "
Step in system time at each loop
"""
from matplotlib import pyplot as plt
@@ -329,29 +461,30 @@ class LoggerControl():
self.custom_suptitle("System time step between 2 sucessive loops")
def plotAllGraphs(self, loggerSensors):
- """"
+ """ "
Step in system time at each loop
"""
from matplotlib import pyplot as plt
N = self.tstamps.shape[0]
- t_range = np.array([k*self.dt for k in range(N)])
+ 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
+ # 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')
+ 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))
@@ -360,14 +493,18 @@ class LoggerControl():
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[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
+ feet_vel[i, :, j] = pin.getFrameVelocity(
+ solo12.model, solo12.data, int(idx), pin.LOCAL_WORLD_ALIGNED
+ ).linear
####
# Measured & Reference feet positions (base frame)
@@ -381,17 +518,46 @@ class LoggerControl():
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='')
+ 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})
+ 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)")
####
@@ -405,12 +571,35 @@ class LoggerControl():
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})
+ 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)")
####
@@ -424,8 +613,16 @@ class LoggerControl():
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})
+ 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)")
####
@@ -446,20 +643,29 @@ class LoggerControl():
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)
+ 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.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"]
+ 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:
@@ -470,22 +676,45 @@ class LoggerControl():
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")
+ 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.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})
+ 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.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()))
+ print(
+ "RMSE: ",
+ np.sqrt(
+ ((self.joy_v_ref[:-1000, 0] - self.mocap_h_v[:-1000, 0]) ** 2).mean()
+ ),
+ )
####
# FF torques & FB torques & Sent torques & Meas torques
@@ -498,17 +727,34 @@ class LoggerControl():
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="--")
+ 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.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")
@@ -523,12 +769,20 @@ class LoggerControl():
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="--")
+ (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})
+ 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:
@@ -546,12 +800,18 @@ class LoggerControl():
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)
+ (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})
+ 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")
####
@@ -565,12 +825,18 @@ class LoggerControl():
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)
+ (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})
+ 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")
####
@@ -672,7 +938,7 @@ class LoggerControl():
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")
@@ -692,7 +958,7 @@ class LoggerControl():
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")
"""
@@ -703,9 +969,9 @@ class LoggerControl():
plt.figure()
for i in range(3):
if i == 0:
- ax0 = plt.subplot(3, 1, i+1)
+ ax0 = plt.subplot(3, 1, i + 1)
else:
- plt.subplot(3, 1, i+1, sharex=ax0)
+ plt.subplot(3, 1, i + 1, sharex=ax0)
if i == 0:
plt.plot(t_range, loggerSensors.current[:], linewidth=2)
@@ -734,111 +1000,100 @@ class LoggerControl():
fig.canvas.manager.set_window_title(name)
def saveAll(self, loggerSensors, fileName="data"):
- date_str = datetime.now().strftime('_%Y_%m_%d_%H_%M')
+ date_str = datetime.now().strftime("_%Y_%m_%d_%H_%M")
name = fileName + date_str + "_" + str(self.type_MPC) + ".npz"
- np.savez_compressed(name,
-
- 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,
-
- update_mip=self.update_mip,
- configs=self.configs,
- initial_contacts=self.initial_contacts,
- t_mip=self.t_mip,
- mpc_cost=self.mpc_cost,
-
- 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,
- )
+ np.savez_compressed(
+ name,
+ 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,
+ update_mip=self.update_mip,
+ configs=self.configs,
+ initial_contacts=self.initial_contacts,
+ t_mip=self.t_mip,
+ mpc_cost=self.mpc_cost,
+ 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,
+ )
print("Log saved in " + name)
def loadAll(self, loggerSensors):
@@ -941,7 +1196,9 @@ class LoggerControl():
loggerSensors.baseAngularVelocity = self.data["baseAngularVelocity"]
loggerSensors.baseLinearAcceleration = self.data["baseLinearAcceleration"]
loggerSensors.baseAccelerometer = self.data["baseAccelerometer"]
- loggerSensors.torquesFromCurrentMeasurment = self.data["torquesFromCurrentMeasurment"]
+ loggerSensors.torquesFromCurrentMeasurment = self.data[
+ "torquesFromCurrentMeasurment"
+ ]
loggerSensors.mocapPosition = self.data["mocapPosition"]
loggerSensors.mocapVelocity = self.data["mocapVelocity"]
loggerSensors.mocapAngularVelocity = self.data["mocapAngularVelocity"]
@@ -962,8 +1219,12 @@ class LoggerControl():
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])])
+ 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 = []
@@ -980,25 +1241,38 @@ class LoggerControl():
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)
+ (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.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]',
+ label="Time [s]",
valmin=0.0,
- valmax=self.logSize*self.dt,
+ valmax=self.logSize * self.dt,
valinit=init_time,
)
@@ -1007,30 +1281,45 @@ class LoggerControl():
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)
+ (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'
+ # 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]',
+ label="Time [s]",
valmin=0.0,
- valmax=self.logSize*self.dt,
+ 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)
+ 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)
@@ -1039,28 +1328,44 @@ class LoggerControl():
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])
+ 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)])
+ 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)
+ 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, :]
+ 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])
+ 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)])
+ 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)
@@ -1092,24 +1397,38 @@ class LoggerControl():
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()
+ 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])
+ 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'
+ 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]',
+ label="Time [s]",
valmin=0.0,
- valmax=self.logSize*self.dt,
+ valmax=self.logSize * self.dt,
valinit=init_time,
)
@@ -1118,16 +1437,25 @@ class LoggerControl():
# 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)
+ 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()
+ 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()
+ 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()
diff --git a/scripts/tools/LoggerSensors.py b/python/quadruped_reactive_walking/tools/LoggerSensors.py
similarity index 69%
rename from scripts/tools/LoggerSensors.py
rename to python/quadruped_reactive_walking/tools/LoggerSensors.py
index 44976985d30c956e3e28e0d170b2779e5054d7dd..3d760c0e4bf2ee2a3250674fb23b4bbd46f40c41 100644
--- a/scripts/tools/LoggerSensors.py
+++ b/python/quadruped_reactive_walking/tools/LoggerSensors.py
@@ -1,10 +1,12 @@
-'''This class will log 1d array in Nd matrix from device and qualisys object'''
-import numpy as np
-from datetime import datetime as datetime
+"""This class will log 1d array in Nd matrix from device and qualisys object"""
+
+from datetime import datetime
from time import time
+import numpy as np
+
-class LoggerSensors():
+class LoggerSensors:
def __init__(self, device=None, qualisys=None, logSize=60e3, ringBuffer=False):
self.ringBuffer = ringBuffer
logSize = np.int(logSize)
@@ -37,7 +39,7 @@ class LoggerSensors():
self.tstamps = np.zeros(logSize)
def sample(self, device, qualisys=None):
- if (self.i >= self.logSize):
+ if self.i >= self.logSize:
if self.ringBuffer:
self.i = 0
else:
@@ -76,23 +78,25 @@ class LoggerSensors():
self.i += 1
def saveAll(self, fileName="dataSensors"):
- date_str = datetime.now().strftime('_%Y_%m_%d_%H_%M')
+ date_str = datetime.now().strftime("_%Y_%m_%d_%H_%M")
- np.savez_compressed(fileName + date_str + ".npz",
- q_mes=self.q_mes,
- v_mes=self.v_mes,
- baseOrientation=self.baseOrientation,
- baseOrientationQuat=self.baseOrientationQuat,
- baseAngularVelocity=self.baseAngularVelocity,
- baseLinearAcceleration=self.baseLinearAcceleration,
- baseAccelerometer=self.baseAccelerometer,
- torquesFromCurrentMeasurment=self.torquesFromCurrentMeasurment,
- current=self.current,
- voltage=self.voltage,
- energy=self.energy,
- mocapPosition=self.mocapPosition,
- mocapVelocity=self.mocapVelocity,
- mocapAngularVelocity=self.mocapAngularVelocity,
- mocapOrientationMat9=self.mocapOrientationMat9,
- mocapOrientationQuat=self.mocapOrientationQuat,
- tstamps=self.tstamps)
+ np.savez_compressed(
+ fileName + date_str + ".npz",
+ q_mes=self.q_mes,
+ v_mes=self.v_mes,
+ baseOrientation=self.baseOrientation,
+ baseOrientationQuat=self.baseOrientationQuat,
+ baseAngularVelocity=self.baseAngularVelocity,
+ baseLinearAcceleration=self.baseLinearAcceleration,
+ baseAccelerometer=self.baseAccelerometer,
+ torquesFromCurrentMeasurment=self.torquesFromCurrentMeasurment,
+ current=self.current,
+ voltage=self.voltage,
+ energy=self.energy,
+ mocapPosition=self.mocapPosition,
+ mocapVelocity=self.mocapVelocity,
+ mocapAngularVelocity=self.mocapAngularVelocity,
+ mocapOrientationMat9=self.mocapOrientationMat9,
+ mocapOrientationQuat=self.mocapOrientationQuat,
+ tstamps=self.tstamps,
+ )
diff --git a/scripts/tools/PyBulletSimulator.py b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
similarity index 59%
rename from scripts/tools/PyBulletSimulator.py
rename to python/quadruped_reactive_walking/tools/PyBulletSimulator.py
index cdad9b695bb8b8e42d4c487f9dad292b3c2da717..52e550f66b4862db239f029b86961b941ab8b7fd 100644
--- a/scripts/tools/PyBulletSimulator.py
+++ b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
@@ -1,8 +1,9 @@
+import time as time
+import sys
+
import numpy as np
import pybullet as pyb # Pybullet server
import pybullet_data
-import time as time
-import sys
import pinocchio as pin
from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR
@@ -37,8 +38,8 @@ class pybullet_simulator:
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
- #Â Roll and Pitch angle of the ground
- p_roll = 0.0 / 57.3 #Â Roll angle of ground
+ # Roll and Pitch angle of the ground
+ p_roll = 0.0 / 57.3 # Roll angle of ground
p_pitch = 0.0 / 57.3 # Pitch angle of ground
# Either use a flat ground or a rough terrain
@@ -47,35 +48,53 @@ class pybullet_simulator:
self.planeIdbis = pyb.loadURDF("plane.urdf") # Flat plane
# Tune position and orientation of plane
- pyb.resetBasePositionAndOrientation(self.planeId, [0, 0, 0.0], pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs())
- pyb.resetBasePositionAndOrientation(self.planeIdbis, [200.0, 0, -100.0 * np.sin(p_pitch)], pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs())
+ pyb.resetBasePositionAndOrientation(
+ self.planeId,
+ [0, 0, 0.0],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
+ pyb.resetBasePositionAndOrientation(
+ self.planeIdbis,
+ [200.0, 0, -100.0 * np.sin(p_pitch)],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
else:
import random
+
random.seed(41)
# p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
heightPerturbationRange = 0.05
- numHeightfieldRows = 256*2
- numHeightfieldColumns = 256*2
- heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns
+ numHeightfieldRows = 256 * 2
+ numHeightfieldColumns = 256 * 2
+ heightfieldData = [0] * numHeightfieldRows * numHeightfieldColumns
height_prev = 0.0
- for j in range(int(numHeightfieldColumns/2)):
- for i in range(int(numHeightfieldRows/2)):
+ for j in range(int(numHeightfieldColumns / 2)):
+ for i in range(int(numHeightfieldRows / 2)):
# uniform distribution
height = random.uniform(0, heightPerturbationRange)
# height = 0.25*np.sin(2*np.pi*(i-128)/46) # sinus pattern
- heightfieldData[2*i+2*j * numHeightfieldRows] = (height + height_prev) * 0.5
- heightfieldData[2*i+1+2*j*numHeightfieldRows] = height
- heightfieldData[2*i+(2*j+1) * numHeightfieldRows] = (height + height_prev) * 0.5
- heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height
+ heightfieldData[2 * i + 2 * j * numHeightfieldRows] = (
+ height + height_prev
+ ) * 0.5
+ heightfieldData[2 * i + 1 + 2 * j * numHeightfieldRows] = height
+ heightfieldData[2 * i + (2 * j + 1) * numHeightfieldRows] = (
+ height + height_prev
+ ) * 0.5
+ heightfieldData[
+ 2 * i + 1 + (2 * j + 1) * numHeightfieldRows
+ ] = height
height_prev = height
# Create the collision shape based on the height field
- terrainShape = pyb.createCollisionShape(shapeType=pyb.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1],
- heightfieldTextureScaling=(numHeightfieldRows-1)/2,
- heightfieldData=heightfieldData,
- numHeightfieldRows=numHeightfieldRows,
- numHeightfieldColumns=numHeightfieldColumns)
+ terrainShape = pyb.createCollisionShape(
+ shapeType=pyb.GEOM_HEIGHTFIELD,
+ meshScale=[0.05, 0.05, 1],
+ heightfieldTextureScaling=(numHeightfieldRows - 1) / 2,
+ heightfieldData=heightfieldData,
+ numHeightfieldRows=numHeightfieldRows,
+ numHeightfieldColumns=numHeightfieldColumns,
+ )
self.planeId = pyb.createMultiBody(0, terrainShape)
pyb.resetBasePositionAndOrientation(self.planeId, [0, 0, 0], [0, 0, 0, 1])
pyb.changeVisualShape(self.planeId, -1, rgbaColor=[1, 1, 1, 1])
@@ -90,95 +109,119 @@ class pybullet_simulator:
# Create the red steps to act as small perturbations
mesh_scale = [1.0, 0.1, 0.02]
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="cube.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=[1.0, 0.0, 0.0, 1.0],
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
-
- collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
- fileName="cube.obj",
- collisionFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="cube.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=[1.0, 0.0, 0.0, 1.0],
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
+
+ collisionShapeId = pyb.createCollisionShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="cube.obj",
+ collisionFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
for i in range(4):
- tmpId = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseCollisionShapeIndex=collisionShapeId,
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.0, 0.5+0.2*i, 0.01],
- useMaximalCoordinates=True)
+ tmpId = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.5 + 0.2 * i, 0.01],
+ useMaximalCoordinates=True,
+ )
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
- tmpId = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseCollisionShapeIndex=collisionShapeId,
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.5, 0.5+0.2*4, 0.01],
- useMaximalCoordinates=True)
+ tmpId = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.5, 0.5 + 0.2 * 4, 0.01],
+ useMaximalCoordinates=True,
+ )
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
- tmpId = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseCollisionShapeIndex=collisionShapeId,
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.5, 0.5+0.2*5, 0.01],
- useMaximalCoordinates=True)
+ tmpId = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.5, 0.5 + 0.2 * 5, 0.01],
+ useMaximalCoordinates=True,
+ )
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
# Create the green steps to act as bigger perturbations
mesh_scale = [0.2, 0.1, 0.01]
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="cube.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=[0.0, 1.0, 0.0, 1.0],
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
-
- collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
- fileName="cube.obj",
- collisionFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="cube.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=[0.0, 1.0, 0.0, 1.0],
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
+
+ collisionShapeId = pyb.createCollisionShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="cube.obj",
+ collisionFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
for i in range(3):
- tmpId = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseCollisionShapeIndex=collisionShapeId,
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.15 * (-1)**i, 0.9+0.2*i, 0.025],
- useMaximalCoordinates=True)
+ tmpId = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.15 * (-1) ** i, 0.9 + 0.2 * i, 0.025],
+ useMaximalCoordinates=True,
+ )
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
# Create sphere obstacles that will be thrown toward the quadruped
mesh_scale = [0.1, 0.1, 0.1]
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="sphere_smooth.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=[1.0, 0.0, 0.0, 1.0],
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
-
- collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
- fileName="sphere_smooth.obj",
- collisionFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
-
- self.sphereId1 = pyb.createMultiBody(baseMass=0.4,
- baseInertialFramePosition=[0, 0, 0],
- baseCollisionShapeIndex=collisionShapeId,
- baseVisualShapeIndex=visualShapeId,
- basePosition=[-0.6, 0.9, 0.1],
- useMaximalCoordinates=True)
-
- self.sphereId2 = pyb.createMultiBody(baseMass=0.4,
- baseInertialFramePosition=[0, 0, 0],
- baseCollisionShapeIndex=collisionShapeId,
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.6, 1.1, 0.1],
- useMaximalCoordinates=True)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=[1.0, 0.0, 0.0, 1.0],
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
+
+ collisionShapeId = pyb.createCollisionShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ collisionFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
+
+ self.sphereId1 = pyb.createMultiBody(
+ baseMass=0.4,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[-0.6, 0.9, 0.1],
+ useMaximalCoordinates=True,
+ )
+
+ self.sphereId2 = pyb.createMultiBody(
+ baseMass=0.4,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.6, 1.1, 0.1],
+ useMaximalCoordinates=True,
+ )
# Flag to launch the two spheres in the environment toward the robot
self.flag_sphere1 = True
@@ -186,45 +229,52 @@ class pybullet_simulator:
# Create blue spheres without collision box for debug purpose
mesh_scale = [0.015, 0.015, 0.015]
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="sphere_smooth.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=[0.0, 0.0, 1.0, 1.0],
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=[0.0, 0.0, 1.0, 1.0],
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
self.ftps_Ids = np.zeros((4, 5), dtype=np.int)
for i in range(4):
for j in range(5):
- self.ftps_Ids[i, j] = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.0, 0.0, -0.1],
- useMaximalCoordinates=True)
+ self.ftps_Ids[i, j] = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.0, -0.1],
+ useMaximalCoordinates=True,
+ )
# Create green spheres without collision box for debug purpose
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="sphere_smooth.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=[0.0, 1.0, 0.0, 1.0],
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="sphere_smooth.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=[0.0, 1.0, 0.0, 1.0],
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
self.ftps_Ids_deb = [0] * 4
for i in range(4):
- self.ftps_Ids_deb[i] = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.0, 0.0, -0.1],
- useMaximalCoordinates=True)
+ self.ftps_Ids_deb[i] = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.0, 0.0, -0.1],
+ useMaximalCoordinates=True,
+ )
# Create a red line for debug purpose
self.lineId_red = []
# Create a blue line for debug purpose
self.lineId_blue = []
-
"""cubeStartPos = [0.0, 0.45, 0.0]
cubeStartOrientation = pyb.getQuaternionFromEuler([0, 0, 0])
self.cubeId = pyb.loadURDF("cube_small.urdf",
@@ -238,24 +288,35 @@ class pybullet_simulator:
# Load Quadruped robot
robotStartPos = [0, 0, 0.0]
robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0]) # -np.pi/2
- pyb.setAdditionalSearchPath(EXAMPLE_ROBOT_DATA_MODEL_DIR + "/solo_description/robots")
+ pyb.setAdditionalSearchPath(
+ EXAMPLE_ROBOT_DATA_MODEL_DIR + "/solo_description/robots"
+ )
self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation)
# Disable default motor control for revolute joints
self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
- pyb.setJointMotorControlArray(self.robotId, jointIndices=self.revoluteJointIndices,
- controlMode=pyb.VELOCITY_CONTROL,
- targetVelocities=[0.0 for m in self.revoluteJointIndices],
- forces=[0.0 for m in self.revoluteJointIndices])
+ pyb.setJointMotorControlArray(
+ self.robotId,
+ jointIndices=self.revoluteJointIndices,
+ controlMode=pyb.VELOCITY_CONTROL,
+ targetVelocities=[0.0 for m in self.revoluteJointIndices],
+ forces=[0.0 for m in self.revoluteJointIndices],
+ )
# Initialize the robot in a specific configuration
self.q_init = np.array([q_init]).transpose()
- pyb.resetJointStatesMultiDof(self.robotId, self.revoluteJointIndices, self.q_init) # q0[7:])
+ pyb.resetJointStatesMultiDof(
+ self.robotId, self.revoluteJointIndices, self.q_init
+ ) # q0[7:])
# Enable torque control for revolute joints
jointTorques = [0.0 for m in self.revoluteJointIndices]
- pyb.setJointMotorControlArray(self.robotId, self.revoluteJointIndices,
- controlMode=pyb.TORQUE_CONTROL, forces=jointTorques)
+ pyb.setJointMotorControlArray(
+ self.robotId,
+ self.revoluteJointIndices,
+ controlMode=pyb.TORQUE_CONTROL,
+ forces=jointTorques,
+ )
# Get position of feet in world frame with base at (0, 0, 0)
feetLinksID = [3, 7, 11, 15]
@@ -272,13 +333,27 @@ class pybullet_simulator:
i += 1
# Set base at (0, 0, -z_min) so that the lowest foot is at z = 0
- pyb.resetBasePositionAndOrientation(self.robotId, [0.0, 0.0, -z_min], pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs())
+ pyb.resetBasePositionAndOrientation(
+ self.robotId,
+ [0.0, 0.0, -z_min],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
# Progressively raise the base to achieve proper contact (take into account radius of the foot)
- while (pyb.getClosestPoints(self.robotId, self.planeId, distance=0.005,
- linkIndexA=feetLinksID[i_min]))[0][8] < -0.001:
+ while (
+ pyb.getClosestPoints(
+ self.robotId,
+ self.planeId,
+ distance=0.005,
+ linkIndexA=feetLinksID[i_min],
+ )
+ )[0][8] < -0.001:
z_min -= 0.001
- pyb.resetBasePositionAndOrientation(self.robotId, [0.0, 0.0, -z_min], pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs())
+ pyb.resetBasePositionAndOrientation(
+ self.robotId,
+ [0.0, 0.0, -z_min],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
# Fix the base in the world frame
# pyb.createConstraint(self.robotId, -1, -1, -1, pyb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, robotStartPos[2]])
@@ -287,8 +362,12 @@ class pybullet_simulator:
pyb.setTimeStep(dt)
# Change camera position
- pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=45, cameraPitch=-39.9,
- cameraTargetPosition=[0.0, 0.0, robotStartPos[2]-0.2])
+ pyb.resetDebugVisualizerCamera(
+ cameraDistance=0.6,
+ cameraYaw=45,
+ cameraPitch=-39.9,
+ cameraTargetPosition=[0.0, 0.0, robotStartPos[2] - 0.2],
+ )
def check_pyb_env(self, k, envID, velID, qmes12):
"""Check the state of the robot to trigger events and update camera
@@ -344,32 +423,44 @@ class pybullet_simulator:
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
mesh_scale = [0.1, 0.1, 0.04]
- visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
- fileName="cube.obj",
- halfExtents=[0.5, 0.5, 0.1],
- rgbaColor=[0.0, 0.0, 1.0, 1.0],
- specularColor=[0.4, .4, 0],
- visualFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
-
- collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
- fileName="cube.obj",
- collisionFramePosition=[0.0, 0.0, 0.0],
- meshScale=mesh_scale)
-
- tmpId = pyb.createMultiBody(baseMass=0.0,
- baseInertialFramePosition=[0, 0, 0],
- baseCollisionShapeIndex=collisionShapeId,
- baseVisualShapeIndex=visualShapeId,
- basePosition=[0.19, 0.15005, 0.02],
- useMaximalCoordinates=True)
+ visualShapeId = pyb.createVisualShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="cube.obj",
+ halfExtents=[0.5, 0.5, 0.1],
+ rgbaColor=[0.0, 0.0, 1.0, 1.0],
+ specularColor=[0.4, 0.4, 0],
+ visualFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
+
+ collisionShapeId = pyb.createCollisionShape(
+ shapeType=pyb.GEOM_MESH,
+ fileName="cube.obj",
+ collisionFramePosition=[0.0, 0.0, 0.0],
+ meshScale=mesh_scale,
+ )
+
+ tmpId = pyb.createMultiBody(
+ baseMass=0.0,
+ baseInertialFramePosition=[0, 0, 0],
+ baseCollisionShapeIndex=collisionShapeId,
+ baseVisualShapeIndex=visualShapeId,
+ basePosition=[0.19, 0.15005, 0.02],
+ useMaximalCoordinates=True,
+ )
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
- pyb.resetBasePositionAndOrientation(self.robotId, [0, 0, 0.25], [0, 0, 0, 1])
+ pyb.resetBasePositionAndOrientation(
+ self.robotId, [0, 0, 0.25], [0, 0, 0, 1]
+ )
# Apply perturbations by directly applying external forces on the robot trunk
if velID == 4:
- self.apply_external_force(k, 4250, 500, np.array([0.0, 0.0, -3.0]), np.zeros((3,)))
- self.apply_external_force(k, 5250, 500, np.array([0.0, +3.0, 0.0]), np.zeros((3,)))
+ self.apply_external_force(
+ k, 4250, 500, np.array([0.0, 0.0, -3.0]), np.zeros((3,))
+ )
+ self.apply_external_force(
+ k, 5250, 500, np.array([0.0, +3.0, 0.0]), np.zeros((3,))
+ )
"""if velID == 0:
self.apply_external_force(k, 1000, 1000, np.array([0.0, 0.0, -6.0]), np.zeros((3,)))
self.apply_external_force(k, 2000, 1000, np.array([0.0, +12.0, 0.0]), np.zeros((3,)))"""
@@ -383,26 +474,42 @@ class pybullet_simulator:
RPY = pin.rpy.matrixToRpy(oMb_tmp.rotation)
# Update the PyBullet camera on the robot position to do as if it was attached to the robot
- pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=(0.0*RPY[2]*(180/3.1415)+45), cameraPitch=-39.9,
- cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0])
+ pyb.resetDebugVisualizerCamera(
+ cameraDistance=0.6,
+ cameraYaw=(0.0 * RPY[2] * (180 / 3.1415) + 45),
+ cameraPitch=-39.9,
+ cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0],
+ )
return 0
def retrieve_pyb_data(self):
- """Retrieve the position and orientation of the base in world frame as well as its linear and angular velocities
- """
+ """Retrieve the position and orientation of the base in world frame as well as its linear and angular velocities"""
# Retrieve data from the simulation
- self.jointStates = pyb.getJointStates(self.robotId, self.revoluteJointIndices) # State of all joints
- self.baseState = pyb.getBasePositionAndOrientation(self.robotId) # Position and orientation of the trunk
+ self.jointStates = pyb.getJointStates(
+ self.robotId, self.revoluteJointIndices
+ ) # State of all joints
+ self.baseState = pyb.getBasePositionAndOrientation(
+ self.robotId
+ ) # Position and orientation of the trunk
self.baseVel = pyb.getBaseVelocity(self.robotId) # Velocity of the trunk
# Joints configuration and velocity vector for free-flyer + 12 actuators
- self.qmes12 = np.vstack((np.array([self.baseState[0]]).T, np.array([self.baseState[1]]).T,
- np.array([[state[0] for state in self.jointStates]]).T))
- self.vmes12 = np.vstack((np.array([self.baseVel[0]]).T, np.array([self.baseVel[1]]).T,
- np.array([[state[1] for state in self.jointStates]]).T))
-
+ self.qmes12 = np.vstack(
+ (
+ np.array([self.baseState[0]]).T,
+ np.array([self.baseState[1]]).T,
+ np.array([[state[0] for state in self.jointStates]]).T,
+ )
+ )
+ self.vmes12 = np.vstack(
+ (
+ np.array([self.baseVel[0]]).T,
+ np.array([self.baseVel[1]]).T,
+ np.array([[state[1] for state in self.jointStates]]).T,
+ )
+ )
"""robotVirtualOrientation = pyb.getQuaternionFromEuler([0, 0, np.pi / 4])
self.qmes12[3:7, 0] = robotVirtualOrientation"""
@@ -428,7 +535,7 @@ class pybullet_simulator:
loc (3x array): position on the link where the force is applied
"""
- if ((k < start) or (k > (start+duration))):
+ if (k < start) or (k > (start + duration)):
return 0.0
"""if k == start:
print("Applying [", F[0], ", ", F[1], ", ", F[2], "]")"""
@@ -436,9 +543,9 @@ class pybullet_simulator:
ev = k - start
t1 = duration
A4 = 16 / (t1**4)
- A3 = - 2 * t1 * A4
+ A3 = -2 * t1 * A4
A2 = t1**2 * A4
- alpha = A2*ev**2 + A3*ev**3 + A4*ev**4
+ alpha = A2 * ev**2 + A3 * ev**3 + A4 * ev**4
self.applied_force[:] = alpha * F
@@ -470,8 +577,12 @@ class pybullet_simulator:
# PD settings
P = 1.0 * 3.0
- D = 0.05 * np.array([[1.0, 0.3, 0.3, 1.0, 0.3, 0.3,
- 1.0, 0.3, 0.3, 1.0, 0.3, 0.3]]).transpose()
+ D = (
+ 0.05
+ * np.array(
+ [[1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3, 1.0, 0.3, 0.3]]
+ ).transpose()
+ )
while True or np.max(np.abs(qtarget - qmes)) > 0.1:
@@ -483,12 +594,12 @@ class pybullet_simulator:
vmes[:, 0] = [state[1] for state in jointStates]
# Torque PD controller
- if (cpt * dt_traj < t1):
+ if cpt * dt_traj < t1:
ev = dt_traj * cpt
A3 = 2 * (qmes - qtarget) / t1**3
- A2 = (-3/2) * t1 * A3
- qdes = qmes + A2*(ev**2) + A3*(ev**3)
- vdes = 2*A2*ev + 3*A3*(ev**2)
+ A2 = (-3 / 2) * t1 * A3
+ qdes = qmes + A2 * (ev**2) + A3 * (ev**3)
+ vdes = 2 * A2 * ev + 3 * A3 * (ev**2)
jointTorques = P * (qdes - qmes) + D * (vdes - vmes)
# Saturation to limit the maximal torque
@@ -497,8 +608,12 @@ class pybullet_simulator:
jointTorques[jointTorques < -t_max] = -t_max
# Set control torque for all joints
- pyb.setJointMotorControlArray(self.robotId, self.revoluteJointIndices,
- controlMode=pyb.TORQUE_CONTROL, forces=jointTorques)
+ pyb.setJointMotorControlArray(
+ self.robotId,
+ self.revoluteJointIndices,
+ controlMode=pyb.TORQUE_CONTROL,
+ forces=jointTorques,
+ )
# Compute one step of simulation
pyb.stepSimulation()
@@ -510,7 +625,7 @@ class pybullet_simulator:
pass
-class Hardware():
+class Hardware:
"""Dummy class that simulates the Hardware class used to communicate with the real masterboard"""
def __init__(self):
@@ -537,17 +652,19 @@ class Hardware():
elif i == 2:
return self.yaw
-class IMU():
+
+class IMU:
"""Dummy class that simulates the IMU class used to communicate with the real masterboard"""
def __init__(self):
- self.linear_acceleration = np.zeros((3, ))
- self.accelerometer = np.zeros((3, ))
- self.gyroscope = np.zeros((3, ))
- self.attitude_euler = np.zeros((3, ))
+ self.linear_acceleration = np.zeros((3,))
+ self.accelerometer = np.zeros((3,))
+ self.gyroscope = np.zeros((3,))
+ self.attitude_euler = np.zeros((3,))
self.attitude_quaternion = np.array([0.0, 0.0, 0.0, 1.0])
-class Powerboard():
+
+class Powerboard:
"""Dummy class that simulates the Powerboard class used to communicate with the real masterboard"""
def __init__(self):
@@ -555,14 +672,15 @@ class Powerboard():
self.voltage = 0.0
self.energy = 0.0
-class Joints():
+
+class Joints:
"""Dummy class that simulates the Joints class used to communicate with the real masterboard"""
def __init__(self, parent_class):
self.parent = parent_class
- self.positions = np.zeros((12, ))
- self.velocities = np.zeros((12, ))
- self.measured_torques = np.zeros((12, ))
+ self.positions = np.zeros((12,))
+ self.velocities = np.zeros((12,))
+ self.measured_torques = np.zeros((12,))
def set_torques(self, torques):
"""Set desired joint torques
@@ -607,7 +725,8 @@ class Joints():
"""
self.parent.v_des = v_des
-class RobotInterface():
+
+class RobotInterface:
"""Dummy class that simulates the robot_interface class used to communicate with the real masterboard"""
def __init__(self):
@@ -616,7 +735,8 @@ class RobotInterface():
def PrintStats(self):
pass
-class PyBulletSimulator():
+
+class PyBulletSimulator:
"""Class that wraps a PyBullet simulation environment to seamlessly switch between the real robot or
simulation by having the same interface in both cases (calling the same functions/variables)
"""
@@ -645,7 +765,15 @@ class PyBulletSimulator():
self.v_des = np.zeros(12)
self.tau_ff = np.zeros(12)
- def Init(self, calibrateEncoders=False, q_init=None, envID=0, use_flat_plane=True, enable_pyb_GUI=False, dt=0.002):
+ def Init(
+ self,
+ calibrateEncoders=False,
+ q_init=None,
+ envID=0,
+ use_flat_plane=True,
+ enable_pyb_GUI=False,
+ dt=0.002,
+ ):
"""Initialize the PyBullet simultor with a given environment and a given state of the robot
Args:
@@ -658,7 +786,9 @@ class PyBulletSimulator():
"""
# Initialisation of the PyBullet simulator
- self.pyb_sim = pybullet_simulator(q_init, envID, use_flat_plane, enable_pyb_GUI, dt)
+ self.pyb_sim = pybullet_simulator(
+ q_init, envID, use_flat_plane, enable_pyb_GUI, dt
+ )
self.q_init = q_init
self.joints.positions[:] = q_init
self.dt = dt
@@ -673,16 +803,21 @@ class PyBulletSimulator():
left (3x0 array): left term of the cross product
right (3x0 array): right term of the cross product
"""
- return np.array([[left[1] * right[2] - left[2] * right[1]],
- [left[2] * right[0] - left[0] * right[2]],
- [left[0] * right[1] - left[1] * right[0]]])
+ return np.array(
+ [
+ [left[1] * right[2] - left[2] * right[1]],
+ [left[2] * right[0] - left[0] * right[2]],
+ [left[0] * right[1] - left[1] * right[0]],
+ ]
+ )
def parse_sensor_data(self):
- """Retrieve data about the robot from the simulation to mimic what the masterboard does
- """
+ """Retrieve data about the robot from the simulation to mimic what the masterboard does"""
# Position and velocity of actuators
- jointStates = pyb.getJointStates(self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints
+ jointStates = pyb.getJointStates(
+ self.pyb_sim.robotId, self.revoluteJointIndices
+ ) # State of all joints
self.joints.positions[:] = np.array([state[0] for state in jointStates])
self.joints.velocities[:] = np.array([state[1] for state in jointStates])
@@ -701,23 +836,35 @@ class PyBulletSimulator():
# Orientation of the base (quaternion)
self.imu.attitude_quaternion[:] = np.array(self.baseState[1])
- self.imu.attitude_euler[:] = pin.rpy.matrixToRpy(pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix())
+ self.imu.attitude_euler[:] = pin.rpy.matrixToRpy(
+ pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix()
+ )
self.rot_oMb = pin.Quaternion(self.imu.attitude_quaternion).toRotationMatrix()
self.oMb = pin.SE3(self.rot_oMb, np.array([self.dummyHeight]).transpose())
# Angular velocities of the base
- self.imu.gyroscope[:] = (self.oMb.rotation.transpose() @ np.array([self.baseVel[1]]).transpose()).ravel()
+ self.imu.gyroscope[:] = (
+ self.oMb.rotation.transpose() @ np.array([self.baseVel[1]]).transpose()
+ ).ravel()
# Linear Acceleration of the base
self.o_baseVel = np.array([self.baseVel[0]]).transpose()
self.b_baseVel = (self.oMb.rotation.transpose() @ self.o_baseVel).ravel()
- self.o_imuVel = self.o_baseVel + self.oMb.rotation @ self.cross3(np.array([0.1163, 0.0, 0.02]), self.imu.gyroscope[:])
+ self.o_imuVel = self.o_baseVel + self.oMb.rotation @ self.cross3(
+ np.array([0.1163, 0.0, 0.02]), self.imu.gyroscope[:]
+ )
- self.imu.linear_acceleration[:] = (self.oMb.rotation.transpose() @ (self.o_imuVel - self.prev_o_imuVel)).ravel() / self.dt
+ self.imu.linear_acceleration[:] = (
+ self.oMb.rotation.transpose() @ (self.o_imuVel - self.prev_o_imuVel)
+ ).ravel() / self.dt
self.prev_o_imuVel[:, 0:1] = self.o_imuVel
- self.imu.accelerometer[:] = self.imu.linear_acceleration + \
- (self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]])).ravel()
+ self.imu.accelerometer[:] = (
+ self.imu.linear_acceleration
+ + (
+ self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]])
+ ).ravel()
+ )
return
@@ -729,19 +876,27 @@ class PyBulletSimulator():
"""
# Position and velocity of actuators
- jointStates = pyb.getJointStates(self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints
+ jointStates = pyb.getJointStates(
+ self.pyb_sim.robotId, self.revoluteJointIndices
+ ) # State of all joints
self.joints.positions[:] = np.array([state[0] for state in jointStates])
self.joints.velocities[:] = np.array([state[1] for state in jointStates])
# Compute PD torques
- tau_pd = self.P * (self.q_des - self.joints.positions) + self.D * (self.v_des - self.joints.velocities)
+ tau_pd = self.P * (self.q_des - self.joints.positions) + self.D * (
+ self.v_des - self.joints.velocities
+ )
# Save desired torques in a storage array
self.jointTorques = tau_pd + self.tau_ff
# Set control torque for all joints
- pyb.setJointMotorControlArray(self.pyb_sim.robotId, self.pyb_sim.revoluteJointIndices,
- controlMode=pyb.TORQUE_CONTROL, forces=self.jointTorques)
+ pyb.setJointMotorControlArray(
+ self.pyb_sim.robotId,
+ self.pyb_sim.revoluteJointIndices,
+ controlMode=pyb.TORQUE_CONTROL,
+ forces=self.jointTorques,
+ )
# Apply arbitrary external forces to the robot base in the simulation
# self.pyb_sim.apply_external_force(self.cpt, 3000, 1000, np.array([0.0, +0.0, -20.0]), np.array([+0.0, +0.1, 0.0]))
diff --git a/python/quadruped_reactive_walking/tools/__init__.py b/python/quadruped_reactive_walking/tools/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/scripts/tools/gamepadClient.py b/python/quadruped_reactive_walking/tools/gamepadClient.py
similarity index 64%
rename from scripts/tools/gamepadClient.py
rename to python/quadruped_reactive_walking/tools/gamepadClient.py
index b55cf2aeac6ba5831d9bc76e816bade5d3b2632b..a17ff99577b65c86742401d363e2d6f78f6dcaf4 100644
--- a/scripts/tools/gamepadClient.py
+++ b/python/quadruped_reactive_walking/tools/gamepadClient.py
@@ -1,19 +1,21 @@
-'''
+"""
Simple python script to get Asyncronous gamepad inputs
Thomas FLAYOLS - LAAS CNRS
From https://github.com/thomasfla/solopython
Use:
To display data, run "python gamepadClient.py"
-'''
-import inputs
-import time
+"""
+
+from ctypes import c_double, c_bool
from multiprocessing import Process
from multiprocessing.sharedctypes import Value
-from ctypes import c_double, c_bool
+import time
+
+import inputs
-class GamepadClient():
+class GamepadClient:
def __init__(self):
self.running = Value(c_bool, lock=True)
self.startButton = Value(c_bool, lock=True)
@@ -42,44 +44,71 @@ class GamepadClient():
self.R1Button.value = False
self.L1Button.value = False
- args = (self.running, self.startButton, self.backButton,
- self.northButton, self.eastButton, self.southButton, self.westButton, self.leftJoystickX,
- self.leftJoystickY, self.rightJoystickX, self.rightJoystickY, self.R1Button, self.L1Button)
+ args = (
+ self.running,
+ self.startButton,
+ self.backButton,
+ self.northButton,
+ self.eastButton,
+ self.southButton,
+ self.westButton,
+ self.leftJoystickX,
+ self.leftJoystickY,
+ self.rightJoystickX,
+ self.rightJoystickY,
+ self.R1Button,
+ self.L1Button,
+ )
self.process = Process(target=self.run, args=args)
self.process.start()
time.sleep(0.2)
- def run(self, running, startButton, backButton, northButton, eastButton, southButton, westButton, leftJoystickX, leftJoystickY, rightJoystickX, rightJoystickY, R1Button, L1Button):
+ def run(
+ self,
+ running,
+ startButton,
+ backButton,
+ northButton,
+ eastButton,
+ southButton,
+ westButton,
+ leftJoystickX,
+ leftJoystickY,
+ rightJoystickX,
+ rightJoystickY,
+ R1Button,
+ L1Button,
+ ):
running.value = True
- while(running.value):
+ while running.value:
events = inputs.get_gamepad()
for event in events:
# print(event.ev_type, event.code, event.state)
- if event.ev_type == 'Absolute':
- if event.code == 'ABS_X':
+ if event.ev_type == "Absolute":
+ if event.code == "ABS_X":
leftJoystickX.value = event.state / 32768.0
- if event.code == 'ABS_Y':
+ if event.code == "ABS_Y":
leftJoystickY.value = event.state / 32768.0
- if event.code == 'ABS_RX':
+ if event.code == "ABS_RX":
rightJoystickX.value = event.state / 32768.0
- if event.code == 'ABS_RY':
+ if event.code == "ABS_RY":
rightJoystickY.value = event.state / 32768.0
- if (event.ev_type == 'Key'):
- if event.code == 'BTN_START':
+ if event.ev_type == "Key":
+ if event.code == "BTN_START":
startButton.value = event.state
- elif event.code == 'BTN_TR':
+ elif event.code == "BTN_TR":
R1Button.value = event.state
- elif event.code == 'BTN_TL':
+ elif event.code == "BTN_TL":
L1Button.value = event.state
- elif event.code == 'BTN_SELECT':
+ elif event.code == "BTN_SELECT":
backButton.value = event.state
- elif event.code == 'BTN_NORTH':
+ elif event.code == "BTN_NORTH":
northButton.value = event.state
- elif event.code == 'BTN_EAST':
+ elif event.code == "BTN_EAST":
eastButton.value = event.state
- elif event.code == 'BTN_SOUTH':
+ elif event.code == "BTN_SOUTH":
southButton.value = event.state
- elif event.code == 'BTN_WEST':
+ elif event.code == "BTN_WEST":
westButton.value = event.state
def stop(self):
@@ -95,10 +124,10 @@ if __name__ == "__main__":
print("LY = ", gp.leftJoystickY.value, end=" ; ")
print("RX = ", gp.rightJoystickX.value, end=" ; ")
print("RY = ", gp.rightJoystickY.value, end=" ; ")
- print("start = ",gp.startButton.value)
- print("back = ",gp.backButton.value)
- print("R1 = ",gp.R1Button.value)
- print("L1 = ",gp.L1Button.value)
+ print("start = ", gp.startButton.value)
+ print("back = ", gp.backButton.value)
+ print("R1 = ", gp.R1Button.value)
+ print("L1 = ", gp.L1Button.value)
time.sleep(0.1)
gp.stop()
diff --git a/scripts/tools/place_com_with_invkin.py b/python/quadruped_reactive_walking/tools/place_com_with_invkin.py
similarity index 54%
rename from scripts/tools/place_com_with_invkin.py
rename to python/quadruped_reactive_walking/tools/place_com_with_invkin.py
index a34d50e62cf452dd030952ef2a502fb714aa8003..f78e150bb1ae4bebba66aa6431a17dae63045e25 100644
--- a/scripts/tools/place_com_with_invkin.py
+++ b/python/quadruped_reactive_walking/tools/place_com_with_invkin.py
@@ -1,8 +1,8 @@
+from example_robot_data import load
import numpy as np
import pinocchio as pin
-from example_robot_data.robots_loader import Solo12Loader
-# Parameters of the InvkinÂ
+# Parameters of the Invkin
l = 0.1946 * 2
L = 0.14695 * 2
h = 0.18
@@ -10,17 +10,18 @@ q_init = [0.0, 0.7, -1.4, -0.0, 0.7, -1.4, 0.0, 0.7, -1.4, -0.0, 0.7, -1.4]
# Load robot model and data
# Initialisation of the Gepetto viewer
-Solo12Loader.free_flyer = True
-solo = Solo12Loader().robot
+solo = load("solo12")
q = solo.q0.reshape((-1, 1))
q[7:, 0] = q_init # Initial angular positions of actuators
# Get foot indexes
-BASE_ID = solo.model.getFrameId('base_link')
-foot_ids = [solo.model.getFrameId('FL_FOOT'),
- solo.model.getFrameId('FR_FOOT'),
- solo.model.getFrameId('HL_FOOT'),
- solo.model.getFrameId('HR_FOOT')]
+BASE_ID = solo.model.getFrameId("base_link")
+foot_ids = [
+ solo.model.getFrameId("FL_FOOT"),
+ solo.model.getFrameId("FR_FOOT"),
+ solo.model.getFrameId("HL_FOOT"),
+ solo.model.getFrameId("HR_FOOT"),
+]
# Init variables
Jf = np.zeros((12, 18))
@@ -28,28 +29,39 @@ posf = np.zeros((4, 3))
pos_ref = np.array([0.0, 0.0, h])
rot_ref = np.eye(3)
-posf_ref = np.array([[l * 0.5, l * 0.5, -l * 0.5, -l * 0.5],
- [L * 0.5, -L * 0.5, L * 0.5, -L * 0.5],
- [0.0, 0.0, 0.0, 0.0]])
+posf_ref = np.array(
+ [
+ [l * 0.5, l * 0.5, -l * 0.5, -l * 0.5],
+ [L * 0.5, -L * 0.5, L * 0.5, -L * 0.5],
+ [0.0, 0.0, 0.0, 0.0],
+ ]
+)
e_basispos = 1.0
while np.any(np.abs(e_basispos) > 0.001):
# Update model and data of the robot
pin.computeJointJacobians(solo.model, solo.data, q)
- pin.forwardKinematics(solo.model, solo.data, q, np.zeros(
- solo.model.nv), np.zeros(solo.model.nv))
+ pin.forwardKinematics(
+ solo.model, solo.data, q, np.zeros(solo.model.nv), np.zeros(solo.model.nv)
+ )
pin.updateFramePlacements(solo.model, solo.data)
# Get data required by IK with Pinocchio
pos = solo.com()
rot = solo.data.oMf[BASE_ID].rotation
- Jbasis = pin.getFrameJacobian(solo.model, solo.data, BASE_ID, pin.LOCAL_WORLD_ALIGNED)[:3]
- Jwbasis = pin.getFrameJacobian(solo.model, solo.data, BASE_ID, pin.LOCAL_WORLD_ALIGNED)[3:]
+ Jbasis = pin.getFrameJacobian(
+ solo.model, solo.data, BASE_ID, pin.LOCAL_WORLD_ALIGNED
+ )[:3]
+ Jwbasis = pin.getFrameJacobian(
+ solo.model, solo.data, BASE_ID, pin.LOCAL_WORLD_ALIGNED
+ )[3:]
for i_ee in range(4):
idx = int(foot_ids[i_ee])
posf[i_ee, :] = solo.data.oMf[idx].translation
- Jf[(3*i_ee):(3*(i_ee+1)), :] = pin.getFrameJacobian(solo.model, solo.data, idx, pin.LOCAL_WORLD_ALIGNED)[:3]
+ Jf[(3 * i_ee) : (3 * (i_ee + 1)), :] = pin.getFrameJacobian(
+ solo.model, solo.data, idx, pin.LOCAL_WORLD_ALIGNED
+ )[:3]
# Compute errors
e_basispos = pos_ref - pos
@@ -58,17 +70,18 @@ while np.any(np.abs(e_basispos) > 0.001):
for i in range(4):
pfeet_err.append(posf_ref[:, i] - posf[i, :])
- #Â Set up matrices
+ # Set up matrices
J = np.vstack([Jbasis, Jwbasis, Jf])
- x_err = np.concatenate([e_basispos, e_basisrot]+pfeet_err)
+ x_err = np.concatenate([e_basispos, e_basisrot] + pfeet_err)
# Loop
q = pin.integrate(solo.model, q, 0.01 * np.linalg.pinv(J) @ x_err)
# Compute final position of CoM
-#q[7:] = np.array([0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407])
-pin.forwardKinematics(solo.model, solo.data, q, np.zeros(
- solo.model.nv), np.zeros(solo.model.nv))
+# q[7:] = np.array([0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407])
+pin.forwardKinematics(
+ solo.model, solo.data, q, np.zeros(solo.model.nv), np.zeros(solo.model.nv)
+)
pos = solo.com()
for i_ee in range(4):
idx = int(foot_ids[i_ee])
@@ -79,7 +92,7 @@ print("Feet: ", posf)
print("Joints: ", q[7:])
solo.initViewer(loadModel=True)
-if ('viewer' in solo.viz.__dict__):
- solo.viewer.gui.addFloor('world/floor')
+if "viewer" in solo.viz.__dict__:
+ solo.viewer.gui.addFloor("world/floor")
solo.viewer.gui.setRefreshIsSynchronous(False)
solo.display(q)
diff --git a/scripts/tools/qualisysClient.py b/python/quadruped_reactive_walking/tools/qualisysClient.py
similarity index 59%
rename from scripts/tools/qualisysClient.py
rename to python/quadruped_reactive_walking/tools/qualisysClient.py
index 614da788156dc56b2a28995c47f3afd341adb2a1..3183f281d4f391debe7b103e4dd05d4127f0f3b8 100644
--- a/scripts/tools/qualisysClient.py
+++ b/python/quadruped_reactive_walking/tools/qualisysClient.py
@@ -1,21 +1,21 @@
-"""
+"""
This client connects to a Qualisys (motion capture) server with an asyncronous subprocess and expose 6d position and velocity of a given body
Thomas FLAYOLS - LAAS CNRS
"""
import asyncio
-from multiprocessing import Process, Lock
+from multiprocessing import Process
from multiprocessing.sharedctypes import Value, Array
from ctypes import c_double
+
import qtm
import numpy as np
-
import pinocchio
-from pinocchio.utils import se3ToXYZQUAT
from pinocchio.explog import log
+from pinocchio.utils import se3ToXYZQUAT
-class QualisysClient():
+class QualisysClient:
def __init__(self, ip="127.0.0.1", body_id=0):
# shared c_double array
self.shared_bodyPosition = Array(c_double, 3, lock=False)
@@ -24,10 +24,17 @@ class QualisysClient():
self.shared_bodyOrientationMat9 = Array(c_double, 9, lock=False)
self.shared_bodyAngularVelocity = Array(c_double, 3, lock=False)
self.shared_timestamp = Value(c_double, lock=False)
- #self.shared_timestamp = -1
- args = (ip, body_id, self.shared_bodyPosition, self.shared_bodyVelocity,
- self.shared_bodyOrientationQuat, self.shared_bodyOrientationMat9,
- self.shared_bodyAngularVelocity, self.shared_timestamp)
+ # self.shared_timestamp = -1
+ args = (
+ ip,
+ body_id,
+ self.shared_bodyPosition,
+ self.shared_bodyVelocity,
+ self.shared_bodyOrientationQuat,
+ self.shared_bodyOrientationMat9,
+ self.shared_bodyAngularVelocity,
+ self.shared_timestamp,
+ )
self.p = Process(target=self.qualisys_process, args=args)
self.p.start()
@@ -36,51 +43,107 @@ class QualisysClient():
self.p.join()
def getPosition(self):
- return np.array([self.shared_bodyPosition[0],
- self.shared_bodyPosition[1],
- self.shared_bodyPosition[2]])
+ return np.array(
+ [
+ self.shared_bodyPosition[0],
+ self.shared_bodyPosition[1],
+ self.shared_bodyPosition[2],
+ ]
+ )
def getVelocity(self):
- return np.array([self.shared_bodyVelocity[0],
- self.shared_bodyVelocity[1],
- self.shared_bodyVelocity[2]])
+ return np.array(
+ [
+ self.shared_bodyVelocity[0],
+ self.shared_bodyVelocity[1],
+ self.shared_bodyVelocity[2],
+ ]
+ )
def getAngularVelocity(self):
- return np.array([self.shared_bodyAngularVelocity[0],
- self.shared_bodyAngularVelocity[1],
- self.shared_bodyAngularVelocity[2]])
+ return np.array(
+ [
+ self.shared_bodyAngularVelocity[0],
+ self.shared_bodyAngularVelocity[1],
+ self.shared_bodyAngularVelocity[2],
+ ]
+ )
def getOrientationMat9(self):
- return np.array([[self.shared_bodyOrientationMat9[0], self.shared_bodyOrientationMat9[1], self.shared_bodyOrientationMat9[2]],
- [self.shared_bodyOrientationMat9[3], self.shared_bodyOrientationMat9[4],
- self.shared_bodyOrientationMat9[5]],
- [self.shared_bodyOrientationMat9[6], self.shared_bodyOrientationMat9[7], self.shared_bodyOrientationMat9[8]]])
+ return np.array(
+ [
+ [
+ self.shared_bodyOrientationMat9[0],
+ self.shared_bodyOrientationMat9[1],
+ self.shared_bodyOrientationMat9[2],
+ ],
+ [
+ self.shared_bodyOrientationMat9[3],
+ self.shared_bodyOrientationMat9[4],
+ self.shared_bodyOrientationMat9[5],
+ ],
+ [
+ self.shared_bodyOrientationMat9[6],
+ self.shared_bodyOrientationMat9[7],
+ self.shared_bodyOrientationMat9[8],
+ ],
+ ]
+ )
def getOrientationQuat(self):
- return np.array([self.shared_bodyOrientationQuat[0],
- self.shared_bodyOrientationQuat[1],
- self.shared_bodyOrientationQuat[2],
- self.shared_bodyOrientationQuat[3]])
-
- def qualisys_process(self, ip, body_id, shared_bodyPosition, shared_bodyVelocity,
- shared_bodyOrientationQuat, shared_bodyOrientationMat9,
- shared_bodyAngularVelocity, shared_timestamp):
+ return np.array(
+ [
+ self.shared_bodyOrientationQuat[0],
+ self.shared_bodyOrientationQuat[1],
+ self.shared_bodyOrientationQuat[2],
+ self.shared_bodyOrientationQuat[3],
+ ]
+ )
+
+ def qualisys_process(
+ self,
+ ip,
+ body_id,
+ shared_bodyPosition,
+ shared_bodyVelocity,
+ shared_bodyOrientationQuat,
+ shared_bodyOrientationMat9,
+ shared_bodyAngularVelocity,
+ shared_timestamp,
+ ):
print("Qualisys process!")
- ''' This will run on a different process'''
+ """ This will run on a different process"""
shared_timestamp.value = -1
def on_packet(packet):
- """ Callback function that is called everytime a data packet arrives from QTM """
+ """Callback function that is called everytime a data packet arrives from QTM"""
position = packet.get_6d()[1][body_id][0]
orientation = packet.get_6d()[1][body_id][1]
timestamp = packet.timestamp * 1e-6
# Get the last position and Rotation matrix from the shared memory.
last_position = np.array(
- [shared_bodyPosition[0], shared_bodyPosition[1], shared_bodyPosition[2]])
- last_rotation = np.array([[shared_bodyOrientationMat9[0], shared_bodyOrientationMat9[1], shared_bodyOrientationMat9[2]],
- [shared_bodyOrientationMat9[3], shared_bodyOrientationMat9[4], shared_bodyOrientationMat9[5]],
- [shared_bodyOrientationMat9[6], shared_bodyOrientationMat9[7], shared_bodyOrientationMat9[8]]])
+ [shared_bodyPosition[0], shared_bodyPosition[1], shared_bodyPosition[2]]
+ )
+ last_rotation = np.array(
+ [
+ [
+ shared_bodyOrientationMat9[0],
+ shared_bodyOrientationMat9[1],
+ shared_bodyOrientationMat9[2],
+ ],
+ [
+ shared_bodyOrientationMat9[3],
+ shared_bodyOrientationMat9[4],
+ shared_bodyOrientationMat9[5],
+ ],
+ [
+ shared_bodyOrientationMat9[6],
+ shared_bodyOrientationMat9[7],
+ shared_bodyOrientationMat9[8],
+ ],
+ ]
+ )
last_se3 = pinocchio.SE3(last_rotation, last_position)
# Get the new position and Rotation matrix from the motion capture.
@@ -109,7 +172,7 @@ class QualisysClient():
shared_bodyOrientationMat9[8] = orientation.matrix[8]
# Compute world velocity.
- if (shared_timestamp.value == -1):
+ if shared_timestamp.value == -1:
shared_bodyVelocity[0] = 0
shared_bodyVelocity[1] = 0
shared_bodyVelocity[2] = 0
@@ -118,10 +181,10 @@ class QualisysClient():
shared_bodyAngularVelocity[2] = 0.0
else:
dt = timestamp - shared_timestamp.value
- shared_bodyVelocity[0] = (position[0] - last_position[0])/dt
- shared_bodyVelocity[1] = (position[1] - last_position[1])/dt
- shared_bodyVelocity[2] = (position[2] - last_position[2])/dt
- bodyAngularVelocity = log(last_se3.rotation.T.dot(se3.rotation))/dt
+ shared_bodyVelocity[0] = (position[0] - last_position[0]) / dt
+ shared_bodyVelocity[1] = (position[1] - last_position[1]) / dt
+ shared_bodyVelocity[2] = (position[2] - last_position[2]) / dt
+ bodyAngularVelocity = log(last_se3.rotation.T.dot(se3.rotation)) / dt
shared_bodyAngularVelocity[0] = bodyAngularVelocity[0]
shared_bodyAngularVelocity[1] = bodyAngularVelocity[1]
shared_bodyAngularVelocity[2] = bodyAngularVelocity[2]
@@ -129,7 +192,7 @@ class QualisysClient():
shared_timestamp.value = timestamp
async def setup():
- """ Main function """
+ """Main function"""
connection = await qtm.connect(ip)
if connection is None:
print("no connection with qualisys!")
@@ -146,6 +209,7 @@ class QualisysClient():
def exampleOfUse():
import time
+
qc = QualisysClient(ip="140.93.16.160", body_id=0)
for i in range(300):
print(chr(27) + "[2J")
diff --git a/scripts/tools/utils_mpc.py b/python/quadruped_reactive_walking/tools/utils_mpc.py
similarity index 70%
rename from scripts/tools/utils_mpc.py
rename to python/quadruped_reactive_walking/tools/utils_mpc.py
index b0b82fd90a7121c692705db29f8a4d21a35076e2..6fa9dac11d02f7d7bc031987c06846050c098c24 100644
--- a/scripts/tools/utils_mpc.py
+++ b/python/quadruped_reactive_walking/tools/utils_mpc.py
@@ -1,5 +1,5 @@
+from example_robot_data import load
import numpy as np
-from example_robot_data.robots_loader import Solo12Loader
import pinocchio as pin
@@ -12,8 +12,7 @@ def init_robot(q_init, params):
"""
# Load robot model and data
# Initialisation of the Gepetto viewer
- Solo12Loader.free_flyer = True
- solo = Solo12Loader().robot
+ solo = load("solo12")
q = solo.q0.reshape((-1, 1))
# Initialisation of the position of footsteps to be under the shoulder
@@ -31,8 +30,8 @@ def init_robot(q_init, params):
if params.enable_corba_viewer:
solo.initViewer(loadModel=True)
- if ('viewer' in solo.viz.__dict__):
- solo.viewer.gui.addFloor('world/floor')
+ if "viewer" in solo.viz.__dict__:
+ solo.viewer.gui.addFloor("world/floor")
solo.viewer.gui.setRefreshIsSynchronous(False)
solo.display(q)
@@ -43,15 +42,19 @@ def init_robot(q_init, params):
# Initialisation of the position of footsteps
fsteps_init = np.zeros((3, 4))
- indexes = [solo.model.getFrameId('FL_FOOT'),
- solo.model.getFrameId('FR_FOOT'),
- solo.model.getFrameId('HL_FOOT'),
- solo.model.getFrameId('HR_FOOT')]
+ indexes = [
+ solo.model.getFrameId("FL_FOOT"),
+ solo.model.getFrameId("FR_FOOT"),
+ solo.model.getFrameId("HL_FOOT"),
+ solo.model.getFrameId("HR_FOOT"),
+ ]
for i in range(4):
fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation
h_init = 0.0
for i in range(4):
- h_tmp = (solo.data.oMf[1].translation - solo.data.oMf[indexes[i]].translation)[2]
+ h_tmp = (solo.data.oMf[1].translation - solo.data.oMf[indexes[i]].translation)[
+ 2
+ ]
if h_tmp > h_init:
h_init = h_tmp
@@ -66,15 +69,21 @@ def init_robot(q_init, params):
# Saving data
params.h_ref = h_init
- params.mass = solo.data.mass[0] # Mass of the whole urdf model (also = to Ycrb[1].mass)
- params.I_mat = solo.data.Ycrb[1].inertia.ravel().tolist() # Composite rigid body inertia in q_init position
+ params.mass = solo.data.mass[
+ 0
+ ] # Mass of the whole urdf model (also = to Ycrb[1].mass)
+ params.I_mat = (
+ solo.data.Ycrb[1].inertia.ravel().tolist()
+ ) # Composite rigid body inertia in q_init position
params.CoM_offset = (solo.data.com[0][:3] - q[0:3, 0]).tolist()
params.CoM_offset[1] = 0.0
for i in range(4):
for j in range(3):
- params.shoulders[3*i+j] = shoulders_init[j, i]
- params.footsteps_init[3*i+j] = fsteps_init[j, i]
- params.footsteps_under_shoulders[3*i+j] = fsteps_init[j, i] # Â Use initial feet pos as reference
+ params.shoulders[3 * i + j] = shoulders_init[j, i]
+ params.footsteps_init[3 * i + j] = fsteps_init[j, i]
+ params.footsteps_under_shoulders[3 * i + j] = fsteps_init[
+ j, i
+ ] # Â Use initial feet pos as reference
return solo
diff --git a/scripts/Crocoddyl/MPC_crocoddyl_2.py b/scripts/Crocoddyl/MPC_crocoddyl_2.py
deleted file mode 100644
index 08079a6289db7b7247303a50f63880a651d0058e..0000000000000000000000000000000000000000
--- a/scripts/Crocoddyl/MPC_crocoddyl_2.py
+++ /dev/null
@@ -1,542 +0,0 @@
-# coding: utf8
-
-import crocoddyl
-import numpy as np
-import quadruped_walkgen
-
-class MPC_crocoddyl_2:
- """Wrapper class for the MPC problem to call the ddp solver and
- retrieve the results.
-
- Args:
- dt (float): time step of the MPC
- T_mpc (float): Duration of the prediction horizon
- mu (float): Friction coefficient
- inner(bool): Inside or outside approximation of the friction cone
- linearModel(bool) : Approximation in the cross product by using desired state
- """
-
- def __init__(self, dt = 0.02 , T_mpc = 0.32 , mu = 1, inner = True , linearModel = True , n_period = 1 , dt_tsid = 0.001):
-
- # Time step of the solver
- self.dt = dt
-
- # Period of the MPC
- self.T_mpc = T_mpc
-
- # Mass of the robot
- self.mass = 2.50000279
-
- # linearModel :
- self.linearModel = linearModel
-
- # Inertia matrix of the robot in body frame
- # self.gI = np.diag([0.00578574, 0.01938108, 0.02476124])
- self.gI = np.array([[3.09249e-2, -8.00101e-7, 1.865287e-5],
- [-8.00101e-7, 5.106100e-2, 1.245813e-4],
- [1.865287e-5, 1.245813e-4, 6.939757e-2]])
-
- # Friction coefficient
- if inner :
- self.mu = (1/np.sqrt(2))*mu
- else:
- self.mu = mu
-
- # Integration model :
- # Implicit : V+ = V + B*U ; P+ = P + dt*V+ = P+ + dt*V + dt*B*U
- # Explicit : V+ = V + B*U ; P+ = P + dt*V
- self.implicit_integration = True
-
- # Gain from OSQP MPC
- self.w_x = np.sqrt(0.5)
- self.w_y = np.sqrt(0.5)
- self.w_z = np.sqrt(2.)
- self.w_roll = np.sqrt(0.11)
- self.w_pitch = np.sqrt(0.11)
- self.w_yaw = np.sqrt(0.11)
- self.w_vx = np.sqrt(2.*np.sqrt(0.5))
- self.w_vy = np.sqrt(2.*np.sqrt(0.5))
- self.w_vz = np.sqrt(2.*np.sqrt(2.))
- self.w_vroll = np.sqrt(0.05*np.sqrt(0.11))
- self.w_vpitch = np.sqrt(0.05*np.sqrt(0.11))
- self.w_vyaw = np.sqrt(0.05*np.sqrt(0.11))
-
- self.stateWeight = 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])
-
- # Weight Vector : Force Norm
- self.forceWeights = np.array(4*[0.01,0.01,0.01])
- self.relative_forces = True
- # self.forceWeights = np.zeros(12)
-
- # Weight Vector : Friction cone cost
- self.frictionWeights = 1.0
-
- # Max iteration ddp solver
- self.max_iteration = 10
-
- # Warm Start for the solver
- self.warm_start = True
-
- # Minimum normal force (N)
- self.min_fz = 0.2
- self.max_fz = 25
-
- # Gait matrix
- self.gait = np.zeros((20, 5))
- self.index = 0
-
- # Weight on the shoulder term :
- self.shoulderWeights = 2.
- self.shoulder_hlim = 0.225
-
- # Position of the feet
- self.fsteps = np.full((20, 13), np.nan)
-
- # List of the actionModel
- self.ListAction = []
-
- # dt TSID
- self.dt_tsid = dt_tsid
- self.initial_node = 5
- self.k_mpc = int(self.dt / self.dt_tsid)
- self.xref = np.zeros((12, int(self.T_mpc/self.dt )*n_period - 1 + self.initial_node) )
- self.n_period = n_period
- self.dt_vector = None
-
-
- # Initialisation of the List model using ActionQuadrupedModel()
- # The same model cannot be used [model]*(T_mpc/dt) because the dynamic
- # model changes for each nodes.
- for j in range(self.initial_node) :
- if linearModel :
- model = quadruped_walkgen.ActionModelQuadruped()
- else :
- model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
-
- # Model parameters
- if j == self.initial_node - 1 :
- model.dt = self.dt - self.dt_tsid*(self.initial_node - 1)
- else :
- model.dt = self.dt_tsid
- model.mass = self.mass
- model.gI = self.gI
- model.mu = self.mu
- model.min_fz = self.min_fz
- model.max_fz = self.max_fz
-
- # Weights vectors
- model.stateWeights = self.stateWeight
- model.forceWeights = self.forceWeights
- model.frictionWeights = self.frictionWeights
- #shoulder term :
- model.shoulderWeights = self.shoulderWeights
- model.shoulder_hlim = self.shoulder_hlim
-
- # Relative forces
- model.relative_forces = self.relative_forces
-
- #Integration
- model.implicit_integration = self.implicit_integration
-
- # Add model to the list of model
- self.ListAction.append(model)
-
-
- # -1 because the first node has been replaced by the 5
- for i in range(int(self.T_mpc/self.dt )*n_period - 1):
-
- if linearModel :
- model = quadruped_walkgen.ActionModelQuadruped()
- else :
- model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
-
- # Model parameters
- model.dt = self.dt
- model.mass = self.mass
- model.gI = self.gI
- model.mu = self.mu
- model.min_fz = self.min_fz
- model.max_fz = self.max_fz
-
- # Weights vectors
- model.stateWeights = self.stateWeight
- model.forceWeights = self.forceWeights
- model.frictionWeights = self.frictionWeights
- #shoulder term :
- model.shoulderWeights = self.shoulderWeights
- model.shoulder_hlim = self.shoulder_hlim
-
- # Relative forces
- model.relative_forces = self.relative_forces
-
- #Integration
- model.implicit_integration = self.implicit_integration
-
- # Add model to the list of model
- self.ListAction.append(model)
-
-
- # Terminal Node
- if linearModel :
- self.terminalModel = quadruped_walkgen.ActionModelQuadruped()
- else :
- self.terminalModel = quadruped_walkgen.ActionModelQuadrupedNonLinear()
-
- # Model parameters of terminal node
- self.terminalModel.dt = self.dt
- self.terminalModel.mass = self.mass
- self.terminalModel.gI = self.gI
- self.terminalModel.mu = self.mu
- self.terminalModel.min_fz = self.min_fz
- self.terminalModel.shoulderWeights = self.shoulderWeights
- self.terminalModel.shoulder_hlim = self.shoulder_hlim
-
- # Weights vectors of terminal node
- self.terminalModel.stateWeights = self.stateWeight
- self.terminalModel.forceWeights = np.zeros(12)
- self.terminalModel.frictionWeights = 0.
-
- # Relative forces ; useless here since no command
- self.terminalModel.relative_forces = self.relative_forces
-
- # Integration
- self.terminalModel.implicit_integration = self.implicit_integration
-
- # Shooting problem
- self.problem = crocoddyl.ShootingProblem(np.zeros(12), self.ListAction, self.terminalModel)
-
- # DDP Solver
- self.ddp = crocoddyl.SolverDDP(self.problem)
-
- # Warm start
- self.x_init = []
- self.u_init = []
-
- def new_model(self) :
- if self.linearModel :
- model = quadruped_walkgen.ActionModelQuadruped()
- else :
- model = quadruped_walkgen.ActionModelQuadrupedNonLinear()
-
- # Model parameters
- model.dt = self.dt
- model.mass = self.mass
- model.gI = self.gI
- model.mu = self.mu
- model.min_fz = self.min_fz
- model.max_fz = self.max_fz
-
- # Weights vectors
- model.stateWeights = self.stateWeight
- model.forceWeights = self.forceWeights
- model.frictionWeights = self.frictionWeights
- #shoulder term :
- model.shoulderWeights = self.shoulderWeights
- model.shoulder_hlim = self.shoulder_hlim
-
- # Relative forces
- model.relative_forces = self.relative_forces
-
- #Integration
- model.implicit_integration = self.implicit_integration
-
- return model
-
-
-
- def updateProblem(self,k,fsteps,xref_ , lC, abg, lV, lW, v_ref, h_ref=0.2027682):
- """Update the dynamic of the model list according to the predicted position of the feet,
- and the desired state.
-
- Args:
- fsteps (6x13): Position of the feet in local frame
- xref (12x17): Desired state vector for the whole gait cycle
- (the initial state is the first column)
- """
- # Update position of the feet
- self.fsteps[:,:] = fsteps[:,:]
-
-
-
- #Construction of the gait matrix representing the feet in contact with the ground
- self.index = next((idx for idx, val in np.ndenumerate(self.fsteps[:, 0]) if val==0.0), 0.0)[0]
- self.gait[:, 0] = self.fsteps[:, 0]
- self.gait[:self.index, 1:] = 1.0 - (np.isnan(self.fsteps[:self.index, 1::3]) | (self.fsteps[:self.index, 1::3] == 0.0))
- # Replace NaN values by zeroes
- self.fsteps[np.isnan(self.fsteps)] = 0.0
-
- # 5 first nodes
- k_remaining = self.k_mpc - k%self.k_mpc
-
- N_total = int(self.T_mpc/self.dt )*self.n_period - 1 + self.initial_node
-
- if k_remaining >= self.initial_node :
- nb_total = int(self.T_mpc/self.dt )*self.n_period - 1 + self.initial_node + 1
- self.dt_vector = np.zeros(nb_total)
- dt_list = np.zeros(nb_total)
- else :
- nb_total = int(self.T_mpc/self.dt )*self.n_period - 2 + self.initial_node + 1
- self.dt_vector = np.zeros(nb_total )
- dt_list = np.zeros(nb_total)
-
- self.xref = np.zeros((12,nb_total))
-
- # Xref
- # Update the current state
- self.xref[0:3, 0:1] = lC
- self.xref[3:6, 0:1] = abg
- self.xref[6:9, 0:1] = lV
- self.xref[9:12, 0:1] = lW
-
- # Update initial state of the problem
- self.problem.x0 = self.xref[:,0]
-
- self.xref[2, 1:] = h_ref
-
-
- for i in range(1,nb_total) :
- if i < self.initial_node :
- dt_list[i] = self.dt_tsid
- self.dt_vector[i] = self.dt_vector[i-1] + self.dt_tsid
- elif i == self.initial_node :
- dt_list[i] = self.dt_tsid*k_remaining - self.dt_tsid*(self.initial_node - 1)
- self.dt_vector[i] = self.dt_vector[i-1] + self.dt_tsid*k_remaining - self.dt_tsid*(self.initial_node - 1)
-
- else :
- dt_list[i] = self.dt
- self.dt_vector[i] = self.dt_vector[i-1] + self.dt
-
-
-
- # Update x and y velocities taking into account the rotation of the base over the prediction horizon
- yaw1 = self.dt_vector[1:self.initial_node] * v_ref[5, 0]
- self.xref[6, 1:self.initial_node] = v_ref[0, 0] * np.cos(yaw1) - v_ref[1, 0] * np.sin(yaw1)
- self.xref[7, 1:self.initial_node] = v_ref[0, 0] * np.sin(yaw1) + v_ref[1, 0] * np.cos(yaw1)
-
- # with dt catch up
- # 1st node at 0.02
- yaw2 = self.dt_vector[self.initial_node] * v_ref[5, 0]
- self.xref[6, self.initial_node] = v_ref[0, 0] * np.cos(yaw2) - v_ref[1, 0] * np.sin(yaw2)
- self.xref[7, self.initial_node] = v_ref[0, 0] * np.sin(yaw2) + v_ref[1, 0] * np.cos(yaw2)
-
- yaw3 = self.dt_vector[self.initial_node + 1 :] * v_ref[5, 0]
- self.xref[6, self.initial_node+1:] = v_ref[0, 0] * np.cos(yaw3) - v_ref[1, 0] * np.sin(yaw3)
- self.xref[7, self.initial_node+1:] = v_ref[0, 0] * np.sin(yaw3) + v_ref[1, 0] * np.cos(yaw3)
-
- # Update x and y depending on x and y velocities (cumulative sum)
- for id in range(1,len(dt_list)) :
- self.xref[0, id] = self.xref[0, id-1] + dt_list[id] * self.xref[6, id]
- self.xref[1, id] = self.xref[1, id-1] + dt_list[id] * self.xref[7, id]
-
- # Start from position of the CoM in local frame
- # self.xref[0, 1:] += lC[0, 0]
- # self.xref[1, 1:] += lC[1, 0]
-
- # No need to update Z velocity as the reference is always 0
- # No need to update roll and roll velocity as the reference is always 0 for those
- # No need to update pitch and pitch velocity as the reference is always 0 for those
- # Update yaw and yaw velocity
- self.xref[5, 1:] = v_ref[5, 0] * self.dt_vector[1:]
- self.xref[11, 1:] = v_ref[5, 0]
-
- i = 0
-
-
- if k_remaining >= self.initial_node :
- if len(self.ListAction) != N_total :
- model = self.new_model()
- self.ListAction.append(model)
-
- for i in range(self.initial_node ) :
- if i == self.initial_node - 1 :
- self.ListAction[i].dt = self.dt_tsid*k_remaining - self.dt_tsid*(self.initial_node - 1)
- else :
- self.ListAction[i].dt = self.dt_tsid
-
- self.ListAction[i].updateModel(np.reshape(self.fsteps[0, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[0, 1:])
-
-
- else :
- if len(self.ListAction) != N_total - 1 :
- self.ListAction.pop(-1)
-
- for i in range(k_remaining) :
- self.ListAction[i].dt = self.dt_tsid
- self.ListAction[i].updateModel(np.reshape(self.fsteps[0, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[0, 1:])
-
- for i in range(k_remaining , k_remaining + self.initial_node - k_remaining) :
- if i == k_remaining + self.initial_node - k_remaining - 1 :
- self.ListAction[i].dt = self.dt - self.dt_tsid*(self.initial_node - k_remaining - 1)
- else :
- self.ListAction[i].dt = self.dt_tsid
-
- if self.gait[0,0] > 1 :
- self.ListAction[i].updateModel(np.reshape(self.fsteps[0, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[0, 1:])
- else :
- self.ListAction[i].updateModel(np.reshape(self.fsteps[1, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[1, 1:])
-
- k_cum = self.initial_node
- L = []
-
-
- # Iterate over the 1st phase of the gait
- if k_remaining >= self.initial_node : #1st node removed
- if self.gait[0,0] > 1 :
- for i in range(k_cum, k_cum+np.int(self.gait[0, 0] - 1)):
- # Update model
- self.ListAction[i].updateModel(np.reshape(self.fsteps[0, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[0, 1:])
- k_cum += np.int(self.gait[0, 0] - 1)
- j_init = 1
- else :
- j_init = 1
-
- else : # the 2 first nodes to remove
- if self.gait[0,0] > 2 :
- for i in range(k_cum, k_cum+np.int(self.gait[0, 0]-2)):
- # Update model
- self.ListAction[i].updateModel(np.reshape(self.fsteps[0, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[0, 1:])
- k_cum += np.int(self.gait[0, 0] - 2)
- j_init = 1
- else :
- if self.gait[0,0] == 2 :
- j_init = 1
- else : # 1 1001
- if self.gait[1,0] == 1 : # 1 1001 | 1 1111
- j_init = 2
- else : # 1 1111 | 7 1001
- for i in range(k_cum, k_cum+np.int(self.gait[1, 0]-1)):
- # Update model
- self.ListAction[i].updateModel(np.reshape(self.fsteps[1, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[1, 1:])
- k_cum += np.int(self.gait[1, 0] - 1)
- j_init = 2
-
-
- j = j_init
- # Iterate over all phases of the gait
- # The first column of xref correspond to the current state
- # print(len(self.ListAction))
- while (self.gait[j, 0] != 0):
- for i in range(k_cum, k_cum+np.int(self.gait[j, 0])):
- # Update model
- # print(i)
- self.ListAction[i].updateModel(np.reshape(self.fsteps[j, 1:], (3, 4), order='F') , self.xref[:, i] , self.gait[j, 1:])
-
- k_cum += np.int(self.gait[j, 0])
- j += 1
-
- # Update model of the terminal model
- self.terminalModel.updateModel(np.reshape(self.fsteps[j-1, 1:], (3, 4), order='F') , self.xref[:,-1] , self.gait[j-1, 1:])
-
- # Shooting problem
- self.problem = crocoddyl.ShootingProblem(np.zeros(12), self.ListAction, self.terminalModel)
- # Update initial state of the problem
- self.problem.x0 = self.xref[:,0]
-
- # DDP Solver
- self.ddp = crocoddyl.SolverDDP(self.problem)
-
- return 0
-
-
-
- def solve(self, k, fstep_planner):
- """ Solve the MPC problem
-
- Args:
- k : Iteration
- fstep_planner : Object that includes the feet predicted position and the desired state vector
- """
-
- # Update the dynamic depending on the predicted feet position
- self.updateProblem(k,fstep_planner.fsteps , fstep_planner.xref )
-
- self.x_init.clear()
- self.u_init.clear()
-
- # Warm start : set candidate state and input vector
- if self.warm_start and k != 0:
-
- self.u_init = self.ddp.us[1:]
- self.u_init.append(np.repeat(self.gait[self.index-1,1:],3)*np.array(4*[0.5,0.5,5.]))
-
-
- self.x_init = self.ddp.xs[2:]
- self.x_init.insert(0,fstep_planner.xref[:,0])
- self.x_init.append(self.ddp.xs[-1])
-
- self.ddp.solve(self.x_init , self.u_init, self.max_iteration )
-
- return 0
-
- def get_latest_result(self):
- """Returns the desired contact forces that have been computed by the last iteration of the MPC
- Args:
- """
- return np.reshape(np.asarray(self.ddp.us[0]) , (12,))
-
- def get_xrobot(self):
- """Returns the state vectors predicted by the mpc throughout the time horizon, the initial column is deleted as it corresponds
- initial state vector
- Args:
- """
-
- return np.array(self.ddp.xs)[1:,:].transpose()
-
- def get_fpredicted(self):
- """Returns the force vectors command predicted by the mpc throughout the time horizon,
- Args:
- """
-
- return np.array(self.ddp.us)[:,:].transpose()[:,:]
-
- def updateActionModel(self) :
- """Update the quadruped model with the new weights or model parameters. Useful to try new weights without modify this class
- """
-
- for elt in self.ListAction:
- elt.dt = self.dt
- elt.mass = self.mass
- elt.gI = self.gI
- elt.mu = self.mu
- elt.min_fz = self.min_fz
- elt.max_fz = self.max_fz
-
- # Weights vectors
- elt.stateWeights = self.stateWeight
- elt.forceWeights = self.forceWeights
- elt.frictionWeights = self.frictionWeights
-
- #shoulder term :
- elt.shoulderWeights = self.shoulderWeights
- elt.shoulder_hlim = self.shoulder_hlim
-
- elt.relative_forces = self.relative_forces
-
- #Integration
- elt.implicit_integration = self.implicit_integration
-
- # Model parameters of terminal node
- self.terminalModel.dt = self.dt
- self.terminalModel.mass = self.mass
- self.terminalModel.gI = self.gI
- self.terminalModel.mu = self.mu
- self.terminalModel.min_fz = self.min_fz
-
- # Weights vectors of terminal node
- self.terminalModel.stateWeights = self.stateWeight
- self.terminalModel.forceWeights = np.zeros(12)
- self.terminalModel.frictionWeights = 0.
-
- #shoulder term :
- self.terminalModel.shoulderWeights = self.shoulderWeights
- self.terminalModel.shoulder_hlim = self.shoulder_hlim
-
- #Integration
- self.terminalModel.implicit_integration = self.implicit_integration
-
- return 0
-
-
-
-