diff --git a/Controller.py b/Controller.py
index 06647184d5caac426ccefb917881d8d135078655..27f156b9441d6d0c7402741091ba215607e6721b 100644
--- a/Controller.py
+++ b/Controller.py
@@ -4,17 +4,17 @@ import numpy as np
import utils_mpc
import time
-# from TSID_Debug_controller_four_legs_fb_vel import controller
from QP_WBC import controller
-import processing as proc
import MPC_Wrapper
import pybullet as pyb
from Planner import Planner
import pinocchio as pin
-from Planner import EulerToQuaternion
class Result:
+ """Object to store the result of the control loop
+ It contains what is sent to the robot (gains, desired positions and velocities,
+ feedforward torques)"""
def __init__(self):
@@ -26,6 +26,7 @@ class Result:
class dummyHardware:
+ """Fake hardware for initialisation purpose"""
def __init__(self):
@@ -37,6 +38,7 @@ class dummyHardware:
class dummyDevice:
+ """Fake device for initialisation purpose"""
def __init__(self):
@@ -93,9 +95,8 @@ class Controller:
self.enable_gepetto_viewer = False
# Create Joystick, FootstepPlanner, Logger and Interface objects
- self.joystick, self.fstep_planner, self.logger, self.interface, self.estimator = utils_mpc.init_objects(
- dt_tsid, dt_mpc, N_SIMULATION, k_mpc, n_periods, T_gait, type_MPC, on_solo8,
- predefined_vel)
+ self.joystick, self.logger, self.estimator = utils_mpc.init_objects(
+ dt_tsid, dt_mpc, N_SIMULATION, k_mpc, n_periods, T_gait, type_MPC, predefined_vel)
# Enable/Disable hybrid control
self.enable_hybrid_control = True
@@ -113,33 +114,17 @@ class Controller:
# Wrapper that makes the link with the solver that you want to use for the MPC
# First argument to True to have PA's MPC, to False to have Thomas's MPC
self.enable_multiprocessing = True
- self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(type_MPC, dt_mpc, self.fstep_planner.n_steps,
- k_mpc, self.fstep_planner.T_gait, self.q, self.enable_multiprocessing)
-
- ########################################################################
- # Gepetto viewer #
- ########################################################################
+ self.mpc_wrapper = MPC_Wrapper.MPC_Wrapper(type_MPC, dt_mpc, np.int(n_periods*T_gait/dt_mpc),
+ k_mpc, T_gait, self.q, self.enable_multiprocessing)
# Initialisation of the Gepetto viewer
self.solo = utils_mpc.init_viewer(self.enable_gepetto_viewer)
- ########################################################################
- # PyBullet #
- ########################################################################
-
- # Initialisation of the PyBullet simulator
- # self.pyb_sim = utils_mpc.pybullet_simulator(envID, use_flat_plane, enable_pyb_GUI, dt=dt_tsid)
-
- # Force monitor to display contact forces in PyBullet with red lines
+ # ForceMonitor to display contact forces in PyBullet with red lines
# import ForceMonitor
# myForceMonitor = ForceMonitor.ForceMonitor(pyb_sim.robotId, pyb_sim.planeId)
- ########################################################################
- # Simulator #
- ########################################################################
-
# Define the default controller
- # self.myController = controller(q_init, int(N_SIMULATION), dt_tsid, k_mpc, n_periods, T_gait, on_solo8)
self.myController = controller(dt_tsid, int(N_SIMULATION))
self.envID = envID
@@ -184,6 +169,11 @@ class Controller:
self.compute(dDevice)
def compute(self, device):
+ """Run one iteration of the main control loop
+
+ Args:
+ device (object): Interface with the masterboard or the simulation
+ """
tic = time.time()
@@ -191,7 +181,8 @@ class Controller:
self.joystick.update_v_ref(self.k, self.velID)
# Process state estimator
- self.estimator.run_filter(self.k, self.planner.gait[0, 1:], device, self.planner.goals, self.planner.gait[0, 0])
+ self.estimator.run_filter(self.k, self.planner.gait[0, 1:],
+ device, self.planner.goals, self.planner.gait[0, 0])
t_filter = time.time()
self.t_list_filter[self.k] = t_filter - tic
@@ -202,18 +193,18 @@ class Controller:
oMb = pin.SE3(pin.Quaternion(self.q[3:7, 0:1]), self.q[0:3, 0:1])
self.v[0:3, 0:1] = oMb.rotation @ self.estimator.v_filt[0:3, 0:1]
self.v[3:6, 0:1] = oMb.rotation @ self.estimator.v_filt[3:6, 0:1]
- self.v[6:, 0] = self.estimator.v_filt[6:, 0]
+ self.v[6:, 0] = self.estimator.v_filt[6:, 0]
# Update estimated position of the robot
self.v_estim[0:3, 0:1] = oMb.rotation.transpose() @ self.joystick.v_ref[0:3, 0:1]
self.v_estim[3:6, 0:1] = oMb.rotation.transpose() @ self.joystick.v_ref[3:6, 0:1]
if not self.planner.is_static:
self.q_estim[:, 0] = pin.integrate(self.solo.model,
- self.q, self.v_estim * self.myController.dt)
+ self.q, self.v_estim * self.myController.dt)
self.yaw_estim = (utils_mpc.quaternionToRPY(self.q_estim[3:7, 0]))[2, 0]
else:
self.planner.q_static[:, 0] = pin.integrate(self.solo.model,
- self.planner.q_static, self.v_estim * self.myController.dt)
+ self.planner.q_static, self.v_estim * self.myController.dt)
self.planner.RPY_static[:, 0:1] = utils_mpc.quaternionToRPY(self.planner.q_static[3:7, 0])
else:
self.yaw_estim = 0.0
@@ -228,7 +219,7 @@ class Controller:
pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=45, cameraPitch=-39.9,
cameraTargetPosition=[device.dummyHeight[0], device.dummyHeight[1], 0.0])
- #Â Update reference height
+ # Update reference height
"""if self.k < 1000:
self.planner.h_ref += 0.00005"""
@@ -258,7 +249,7 @@ class Controller:
t_mpc = time.time()
self.t_list_mpc[self.k] = t_mpc - t_planner
- # Target state for the inverse kinematics
+ # Target state for the whole body control
if not self.planner.is_static:
self.x_f_mpc[0] = self.q_estim[0, 0]
self.x_f_mpc[1] = self.q_estim[1, 0]
@@ -338,99 +329,3 @@ class Controller:
self.k += 1
return 0.0
-
- ####################
- # END OF MAIN LOOP #
- ####################
-
- def launch_simu(self, device):
- """# Default position after calibration
- q_init = np.array([0.0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8, 1.6])
-
- class dummyDevice:
-
- def __init__(self):
-
- pass
-
- dDevice = dummyDevice()
- dDevice.q_mes = q_init
- dDevice.v_mes = np.zeros(12)
- dDevice.baseLinearAcceleration = np.zeros(3)
- dDevice.baseAngularVelocity = np.zeros(3)
- dDevice.baseOrientation = np.array([0.0, 0.0, 0.0, 1.0])
- tau = self.compute(dDevice)"""
-
- tic = time.time()
-
- for k in range(1, int(self.N_SIMULATION)):
-
- device.UpdateMeasurment()
-
- tau = self.compute(device)
-
- # device.SetDesiredJointTorque(tau)
- device.SetKp(self.result.P)
- device.SetKd(self.result.D)
- device.SetQdes(self.result.q_des)
- device.SetVdes(self.result.v_des)
- # device.SetTauFF(self.result.tau_ff)
-
- device.SendCommand(WaitEndOfCycle=True)
-
- print("###")
- print("TSID: ", self.myController.qtsid.ravel())
- print("TAU: ", tau.ravel())
-
- if self.enable_pyb_GUI:
- # Update the PyBullet camera on the robot position to do as if it was attached to the robot
- pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=45, cameraPitch=-39.9,
- cameraTargetPosition=[self.estimator.q_filt[0, 0],
- self.estimator.q_filt[1, 0], 0.0])
-
- # Process PyBullet
- # proc.process_pybullet(self.pyb_sim, self.k, self.envID, self.velID, tau)
-
- # Call logger object to log various parameters
- # logger.call_log_functions(k, pyb_sim, joystick, fstep_planner, interface, mpc_wrapper, myController,
- # False, pyb_sim.robotId, pyb_sim.planeId, solo)
- # logger.log_state(k, pyb_sim, joystick, interface, mpc_wrapper, solo)
- # logger.log_forces(k, interface, myController, pyb_sim.robotId, pyb_sim.planeId)
- # logger.log_footsteps(k, interface, myController)
- # logger.log_fstep_planner(k, fstep_planner)
- # logger.log_tracking_foot(k, myController, solo)
-
- tac = time.time()
- print("Average computation time of one iteration: ",
- (tac-tic)/self.N_SIMULATION)
- print("Computation duration: ", tac-tic)
- print("Simulated duration: ", self.N_SIMULATION*self.dt_tsid)
- print("Max loop time: ", np.max(self.t_list_loop[10:]))
-
- # Close the parallel process if it is running
- if self.enable_multiprocessing:
- print("Stopping parallel process")
- self.mpc_wrapper.stop_parallel_loop()
-
- print("END")
-
- # Plot processing duration of each step of the control loop
- """
- import matplotlib.pylab as plt
- plt.figure()
- plt.plot(t_list_filter[1:], '+', color="orange")
- plt.plot(t_list_states[1:], 'r+')
- plt.plot(t_list_fsteps[1:], 'g+')
- plt.plot(t_list_mpc[1:], 'b+')
- plt.plot(t_list_tsid[1:], '+', color="violet")
- plt.plot(t_list_loop[1:], 'k+')
- plt.title("Time for state update + footstep planner + MPC communication + Inv Dyn + PD+")
- plt.show(block=True)"""
-
- # Disconnect the PyBullet server (also close the GUI)
- pyb.disconnect()
-
- # Plot graphs of the state estimator
- # estimator.plot_graphs()
-
- return self.logger
diff --git a/MPC_Wrapper.py b/MPC_Wrapper.py
index 41acab59b787c2e302ff602b367b9258025425e7..bf6137678877a89c4d70eccc4b46b468dedcd943 100644
--- a/MPC_Wrapper.py
+++ b/MPC_Wrapper.py
@@ -8,11 +8,12 @@ from utils_mpc import quaternionToRPY
class Dummy:
+ """Dummy class to store variables"""
def __init__(self):
- self.xref = None
- self.fsteps = None
+ self.xref = None # Desired trajectory
+ self.fsteps = None # Desired location of footsteps
pass
@@ -27,6 +28,7 @@ class MPC_Wrapper:
n_steps (int): Number of time steps in one gait cycle
k_mpc (int): Number of inv dyn time step for one iteration of the MPC
T_gait (float): Duration of one period of gait
+ q_init (array): the default position of the robot
multiprocessing (bool): Enable/Disable running the MPC with another process
"""
@@ -44,7 +46,7 @@ class MPC_Wrapper:
self.mpc_type = mpc_type
self.multiprocessing = multiprocessing
- if multiprocessing:
+ if multiprocessing: # Setup variables in the shared memory
self.newData = Value('b', False)
self.newResult = Value('b', False)
self.dataIn = Array('d', [0.0] * (1 + (np.int(self.n_steps)+1) * 12 + 13*20))
@@ -59,6 +61,7 @@ class MPC_Wrapper:
self.mpc = MPC_crocoddyl.MPC_crocoddyl(
dt=dt, T_mpc=T_gait, mu=0.9, inner=False, linearModel=True, n_period=int((dt * n_steps)/T_gait))
+ # Setup initial result for the first iteration of the main control loop
x_init = np.zeros(12)
x_init[0:3] = q_init[0:3, 0]
x_init[3:6] = quaternionToRPY(q_init[3:7, 0]).ravel()
@@ -73,9 +76,9 @@ class MPC_Wrapper:
fstep_planner (object): FootstepPlanner object of the control loop
"""
- if self.multiprocessing:
+ if self.multiprocessing: # Run in parallel process
self.run_MPC_asynchronous(k, fstep_planner)
- else:
+ else: # Run in the same process than main loop
self.run_MPC_synchronous(k, fstep_planner)
return 0
@@ -94,7 +97,6 @@ class MPC_Wrapper:
return self.last_available_result
else:
return self.last_available_result
- # raise ValueError("Error: something went wrong with the MPC, result not available.")
else:
# Directly retrieve desired contact force of the synchronous MPC object
return self.f_applied
@@ -114,32 +116,15 @@ class MPC_Wrapper:
# Run the MPC to get the reference forces and the next predicted state
# Result is stored in mpc.f_applied, mpc.q_next, mpc.v_next
- """print(dt, n_steps, k, T_gait)
- print(np.round(interface.lC.ravel(), decimals=2))
- print(np.round(interface.abg.ravel(), decimals=2))
- print(np.round(interface.lV.ravel(), decimals=2))
- print(np.round(interface.lW.ravel(), decimals=2))
- print(interface.l_feet.ravel())
- print(joystick.v_ref.ravel())
- print(fstep_planner.fsteps)"""
-
if self.mpc_type:
# OSQP MPC
- # Replace NaN values by 0.0
+ # Replace NaN values by 0.0 (shared memory cannot handle np.nan)
fstep_planner.fsteps_mpc[np.isnan(fstep_planner.fsteps_mpc)] = 0.0
self.mpc.run(np.int(k), fstep_planner.xref.copy(), fstep_planner.fsteps_mpc.copy())
else:
# Crocoddyl MPC
self.mpc.solve(k, fstep_planner)
- """tmp_lC = interface.lC.copy()
- tmp_lC[2, 0] += dt * interface.lV[2, 0]
- tmp_abg = interface.abg + dt * interface.lW
- tmp_abg[2, 0] = 0.0
- tmp_lfeet = interface.l_feet - dt * interface.lV
- tmp_xref = fstep_planner.xref.copy()
- tmp_xref """
-
# Output of the MPC
self.f_applied = self.mpc.get_latest_result()
@@ -157,18 +142,8 @@ class MPC_Wrapper:
self.newData, self.newResult, self.dataIn, self.dataOut, self.running))
p.start()
- # print("Setting Data")
+ # Stacking data to send them to the parallel process
self.compress_dataIn(k, fstep_planner)
- """print("Sending")
- print(dt, n_steps, k, T_gait)
- print(interface.lC.ravel())
- print(interface.abg.ravel())
- print(interface.lV.ravel())
- print(interface.lW.ravel())
- print(interface.l_feet.ravel())
- print(joystick.v_ref.ravel())
- print(fstep_planner.fsteps)"""
-
self.newData.value = True
return 0
@@ -196,34 +171,11 @@ class MPC_Wrapper:
# Retrieve data thanks to the decompression function and reshape it
kf, xref_1dim, fsteps_1dim = self.decompress_dataIn(dataIn)
- # print("Receiving")
- """dt = dt[0]
- nsteps = np.int(nsteps[0])
- k = k[0]
- T_gait = T_gait[0]
- lC = np.reshape(lC, (3, 1))
- abg = np.reshape(abg, (3, 1))
- lV = np.reshape(lV, (3, 1))
- lW = np.reshape(lW, (3, 1))
- l_feet = np.reshape(l_feet, (3, 4))
- xref = np.reshape(xref, (12, nsteps+1))
- x0 = np.reshape(x0, (12, 1))
- v_ref = np.reshape(v_ref, (6, 1))
- fsteps = np.reshape(fsteps, (20, 13))"""
-
+ # Reshaping 1-dimensional data
k = int(kf[0])
xref = np.reshape(xref_1dim, (12, self.n_steps+1))
fsteps = np.reshape(fsteps_1dim, (20, 13))
- """print(dt, nsteps, k, T_gait)
- print(lC.ravel())
- print(abg.ravel())
- print(lV.ravel())
- print(lW.ravel())
- print(l_feet.ravel())
- print(v_ref.ravel())
- print(fsteps)"""
-
# 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)
@@ -242,8 +194,7 @@ class MPC_Wrapper:
dummy_fstep_planner.fsteps = fsteps
loop_mpc.solve(k, dummy_fstep_planner)
- # Store the result (predicted trajectory + desired forces) in the shared memory
- # print("X_F: ", loop_mpc.get_latest_result().tolist())
+ # Store the result (predicted state + desired forces) in the shared memory
self.dataOut[:] = loop_mpc.get_latest_result().tolist()
# Set shared variable to true to signal that a new result is available
@@ -260,8 +211,6 @@ class MPC_Wrapper:
fstep_planner (object): FootstepPlanner object of the control loop
"""
- # print("Compressing dataIn")
-
# Replace NaN values by 0.0 to be stored in C-type arrays
fstep_planner.fsteps[np.isnan(fstep_planner.fsteps)] = 0.0
@@ -279,8 +228,6 @@ class MPC_Wrapper:
dataIn (Array): shared array that contains the data the asynchronous MPC will use as inputs
"""
- # print("Decompressing dataIn")
-
# Sizes of the different variables that are stored in the C-type array
sizes = [0, 1, (np.int(self.n_steps)+1) * 12, 13*20]
csizes = np.cumsum(sizes)
diff --git a/Planner.py b/Planner.py
index 1cd7c650e649b9c2587eeb5b26d0eac5e9675d99..538bf0776e1c45d1dcda1a7adbe375af1a765c64 100644
--- a/Planner.py
+++ b/Planner.py
@@ -765,20 +765,6 @@ class Planner:
[left[0] * right[1] - left[1] * right[0]]])
-def EulerToQuaternion(roll_pitch_yaw):
- roll, pitch, yaw = roll_pitch_yaw
- sr = math.sin(roll/2.)
- cr = math.cos(roll/2.)
- sp = math.sin(pitch/2.)
- cp = math.cos(pitch/2.)
- sy = math.sin(yaw/2.)
- cy = math.cos(yaw/2.)
- qx = sr * cp * cy - cr * sp * sy
- qy = cr * sp * cy + sr * cp * sy
- qz = cr * cp * sy - sr * sp * cy
- qw = cr * cp * cy + sr * sp * sy
- return [qx, qy, qz, qw]
-
def test_planner():
@@ -885,7 +871,7 @@ def test_planner():
# Following the mpc reference trajectory perfectly
q[0:3, 0] = planner.xref[0:3, 1].copy() # np.array(pin.integrate(model, q, b_v * dt))
- q[3:7, 0] = EulerToQuaternion(planner.xref[3:6, 1])
+ q[3:7, 0] = utils_mpc.EulerToQuaternion(planner.xref[3:6, 1])
v[0:3, 0] = planner.xref[6:9, 1].copy()
v[3:6, 0] = planner.xref[9:12, 1].copy()
k += 1
@@ -1010,7 +996,7 @@ def test_planner_mpc():
# Run planner
"""if k == 100:
- q[3:7, 0] = EulerToQuaternion([0.2, 0.0, 0.0])"""
+ q[3:7, 0] = utils_mpc.EulerToQuaternion([0.2, 0.0, 0.0])"""
planner.run_planner(k, k_mpc, q[0:7, 0:1], v[0:6, 0:1], joystick.v_ref)
# Logging output of foot trajectory generator
@@ -1042,12 +1028,12 @@ def test_planner_mpc():
# Following the mpc reference trajectory perfectly
"""q[0:3, 0] = planner.xref[0:3, 1].copy() # np.array(pin.integrate(model, q, b_v * dt))
- q[3:7, 0] = EulerToQuaternion(planner.xref[3:6, 1])
+ q[3:7, 0] = utils_mpc.EulerToQuaternion(planner.xref[3:6, 1])
v[0:3, 0] = planner.xref[6:9, 1].copy()
v[3:6, 0] = planner.xref[9:12, 1].copy()"""
q[0:3, 0] = x_f_mpc[0:3]
- q[3:7, 0] = EulerToQuaternion(x_f_mpc[3:6])
+ q[3:7, 0] = utils_mpc.EulerToQuaternion(x_f_mpc[3:6])
v[0:3, 0] = x_f_mpc[6:9]
v[3:6, 0] = x_f_mpc[9:12]
k += 1
@@ -1229,7 +1215,7 @@ def test_planner_mpc_tsid():
for i in range(planner.xref.shape[1]):
planner.xref[0:3, i] = oMl.inverse() * planner.xref[0:3, i:(i+1)]
- planner.xref[3:6, i] = pin.rpy.matrixToRpy(oMl.rotation.transpose() @ pin.Quaternion(np.array([EulerToQuaternion(planner.xref[3:6, i])]).T).toRotationMatrix())
+ planner.xref[3:6, i] = pin.rpy.matrixToRpy(oMl.rotation.transpose() @ pin.Quaternion(np.array([utils_mpc.EulerToQuaternion(planner.xref[3:6, i])]).T).toRotationMatrix())
planner.xref[6:9, i:(i+1)] = oMl.rotation.transpose() @ planner.xref[6:9, i:(i+1)]
planner.xref[9:12, i:(i+1)] = oMl.rotation.transpose() @ planner.xref[9:12, i:(i+1)]
@@ -1252,7 +1238,7 @@ def test_planner_mpc_tsid():
x_f_mpc = mpc_wrapper.get_latest_result()
"""b_x_f_mpc = x_f_mpc.copy()
b_x_f_mpc[0:3] = (oMl * np.array([x_f_mpc[0:3]]).transpose()).ravel()
- b_x_f_mpc[3:6] = pin.rpy.matrixToRpy(oMl.rotation @ pin.Quaternion(np.array([EulerToQuaternion(x_f_mpc[3:6])]).T).toRotationMatrix())
+ b_x_f_mpc[3:6] = pin.rpy.matrixToRpy(oMl.rotation @ pin.Quaternion(np.array([utils_mpc.EulerToQuaternion(x_f_mpc[3:6])]).T).toRotationMatrix())
b_x_f_mpc[6:9] = (oMl.rotation @ np.array([x_f_mpc[6:9]]).transpose()).ravel()
b_x_f_mpc[9:12] = (oMl.rotation @ np.array([x_f_mpc[9:12]]).transpose()).ravel()
for i in range(4):
@@ -1281,7 +1267,7 @@ def test_planner_mpc_tsid():
plt.show(block=True)"""
"""q[0:3, 0] = x_f_mpc[0:3]
- q[3:7, 0] = EulerToQuaternion(x_f_mpc[3:6])
+ q[3:7, 0] = utils_mpc.EulerToQuaternion(x_f_mpc[3:6])
v[0:3, 0] = x_f_mpc[6:9]
v[3:6, 0] = x_f_mpc[9:12]"""
@@ -1368,11 +1354,11 @@ def test_planner_mpc_tsid():
# Following the mpc reference trajectory perfectly
"""q[0:3, 0] = planner.xref[0:3, 1].copy() # np.array(pin.integrate(model, q, b_v * dt))
- q[3:7, 0] = EulerToQuaternion(planner.xref[3:6, 1])
+ q[3:7, 0] = utils_mpc.EulerToQuaternion(planner.xref[3:6, 1])
v[0:3, 0] = planner.xref[6:9, 1].copy()
v[3:6, 0] = planner.xref[9:12, 1].copy()"""
"""q[0:3, 0] = x_f_mpc[0:3]
- q[3:7, 0] = EulerToQuaternion(x_f_mpc[3:6])
+ q[3:7, 0] = utils_mpc.EulerToQuaternion(x_f_mpc[3:6])
v[0:3, 0] = x_f_mpc[6:9]
v[3:6, 0] = x_f_mpc[9:12]"""
@@ -1401,7 +1387,7 @@ def test_planner_mpc_tsid():
# if k == 1000:
# oMb = pin.SE3(pin.utils.rotate('z', 3.1415/2), np.array([0.0, 0.0, 0.0]))
- # q[3:7, 0] = np.array([0, 0, 0.7009093, 0.7132504]) #pin.Quaternion(EulerToQuaternion(pin.rpy.matrixToRpy(oMb.rotation @ pin.Quaternion(q[3:7, 0:1]).toRotationMatrix())))
+ # q[3:7, 0] = np.array([0, 0, 0.7009093, 0.7132504]) #pin.Quaternion(utils_mpc.EulerToQuaternion(pin.rpy.matrixToRpy(oMb.rotation @ pin.Quaternion(q[3:7, 0:1]).toRotationMatrix())))
while (time.time() - t_start) < 0.002:
pass
diff --git a/PyBulletSimulator.py b/PyBulletSimulator.py
index e46b1767ab8d62ffce1a56aa4af1d8f357e8f072..74688139d72964262c14f50a349e24d981c1e143 100644
--- a/PyBulletSimulator.py
+++ b/PyBulletSimulator.py
@@ -1,8 +1,18 @@
+import numpy as np
+import pybullet as pyb # Pybullet server
+import pybullet_data
+import time as time
+import sys
+import pinocchio as pin
+from utils_mpc import quaternionToRPY
+
+
class pybullet_simulator:
"""Wrapper for the PyBullet simulator to initialize the simulation, interact with it
and use various utility functions
Args:
+ q_init (array): the default position of the robot
envID (int): identifier of the current environment to be able to handle different scenarios
use_flat_plane (bool): to use either a flat ground or a rough ground
enable_pyb_GUI (bool): to display PyBullet GUI or not
@@ -462,6 +472,7 @@ class pybullet_simulator:
class Hardware():
+ """Dummy class that simulates the Hardware class used to communicate with the real masterboard"""
def __init__(self):
@@ -489,6 +500,9 @@ class Hardware():
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)
+ """
def __init__(self):
@@ -516,6 +530,16 @@ class PyBulletSimulator():
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):
+ """Initialize the PyBullet simultor with a given environment and a given state of the robot
+
+ Args:
+ calibrateEncoders (bool): dummy variable, not used for simulation but used for real robot
+ q_init (12 x 0 array): initial angular positions of the joints of the robot
+ envID (int): which environment should be loaded in the simulation
+ use_flat_plane (bool): to use either a flat ground or a rough ground
+ enable_pyb_GUI (bool): to display PyBullet GUI or not
+ dt (float): time step of the simulation
+ """
# Initialisation of the PyBullet simulator
self.pyb_sim = pybullet_simulator(q_init, envID, use_flat_plane, enable_pyb_GUI, dt)
@@ -526,6 +550,8 @@ class PyBulletSimulator():
return
def UpdateMeasurment(self):
+ """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
@@ -567,23 +593,48 @@ class PyBulletSimulator():
return
def SetDesiredJointTorque(self, torques):
+ """Set desired joint torques
+ Args:
+ torques (12 x 0): desired articular feedforward torques
+ """
# Save desired torques in a storage array
self.tau_ff = torques.copy()
return
def SetDesiredJointPDgains(self, P, D):
+ """Set desired PD gains for articular low level control
+
+ Args:
+ P (12 x 0 array): desired position gains
+ D (12 x 0 array): desired velocity gains
+ """
self.P = P
self.D = D
def SetDesiredJointPosition(self, q_des):
+ """Set desired joint positions
+
+ Args:
+ q_des (12 x 0 array): desired articular positions
+ """
self.q_des = q_des
def SetDesiredJointVelocity(self, v_des):
+ """Set desired joint velocities
+
+ Args:
+ v_des (12 x 0 array): desired articular velocities
+ """
self.v_des = v_des
def SendCommand(self, WaitEndOfCycle=True):
+ """Send control commands to the robot
+
+ Args:
+ WaitEndOfCycle (bool): wait to have simulation time = real time
+ """
# Compute PD torques
tau_pd = self.P * (self.q_des - self.q_mes) + self.D * (self.v_des - self.v_mes)
@@ -591,12 +642,11 @@ class PyBulletSimulator():
# Save desired torques in a storage array
self.jointTorques = tau_pd + self.tau_ff
- # print("FF+PD: ", self.jointTorques.ravel())
-
# Set control torque for all joints
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, 1000, 1000, np.array([0.0, +8.0, 0.0]), np.zeros((3,)))
# self.pyb_sim.apply_external_force(self.cpt, 4250, 500, np.array([+5.0, 0.0, 0.0]), np.zeros((3,)))
# self.pyb_sim.apply_external_force(self.cpt, 5250, 500, np.array([0.0, +5.0, 0.0]), np.zeros((3,)))
@@ -615,6 +665,8 @@ class PyBulletSimulator():
return
def Print(self):
+ """Print simulation parameters in the console"""
+
np.set_printoptions(precision=2)
# print(chr(27) + "[2J")
print("#######")
@@ -627,6 +679,7 @@ class PyBulletSimulator():
sys.stdout.flush()
def Stop(self):
+ """Stop the simulation environment"""
# Disconnect the PyBullet server (also close the GUI)
pyb.disconnect()
diff --git a/QP_WBC.py b/QP_WBC.py
index d6431fc879bb31a9ac9fb4f7d1b73948aece796d..4b11432373878615da5452ef13cf7edfdf03d20e 100644
--- a/QP_WBC.py
+++ b/QP_WBC.py
@@ -30,6 +30,7 @@ class controller():
# Logging
self.k_log = 0
self.log_feet_pos = np.zeros((3, 4, N_SIMULATION))
+ self.log_feet_err = np.zeros((3, 4, N_SIMULATION))
self.log_feet_vel = np.zeros((3, 4, N_SIMULATION))
self.log_feet_pos_target = np.zeros((3, 4, N_SIMULATION))
self.log_feet_vel_target = np.zeros((3, 4, N_SIMULATION))
@@ -74,8 +75,9 @@ class controller():
# Log position, velocity and acceleration references for the feet
indexes = [10, 18, 26, 34]
for i in range(4):
- self.log_feet_pos[:, i, self.k_log] = self.invKin.robot.data.oMf[indexes[i]].translation
- self.log_feet_vel[:, i, self.k_log] = pin.getFrameVelocity(self.invKin.robot.model, self.invKin.robot.data,
+ self.log_feet_pos[:, i, self.k_log] = self.invKin.rdata.oMf[indexes[i]].translation
+ self.log_feet_err[:, i, self.k_log] = self.invKin.pfeet_err[i]
+ self.log_feet_vel[:, i, self.k_log] = pin.getFrameVelocity(self.invKin.rmodel, self.invKin.rdata,
indexes[i], pin.LOCAL_WORLD_ALIGNED).linear
self.log_feet_pos_target[:, :, self.k_log] = planner.goals[:, :] # + np.array([[0.0, 0.0, q[2, 0] - planner.h_ref]]).T
self.log_feet_vel_target[:, :, self.k_log] = planner.vgoals[:, :]
@@ -163,9 +165,10 @@ class controller():
plt.subplot(3, 4, index12[i], sharex=ax0)
plt.plot(t_range, self.log_feet_pos[i % 3, np.int(i/3), :], color='b', linewidth=3, marker='')
+ plt.plot(t_range, self.log_feet_err[i % 3, np.int(i/3), :], color='g', linewidth=3, marker='')
plt.plot(t_range, self.log_feet_pos_target[i % 3, np.int(i/3), :], color='r', linewidth=3, marker='')
plt.plot(t_range, self.log_contacts[np.int(i/3), :] * np.max(self.log_feet_pos[i % 3, np.int(i/3), :]), color='k', linewidth=3, marker='')
- plt.legend([lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+"", lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+" Ref", "Contact state"])
+ plt.legend([lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+"", "error", lgd_Y[i % 3] + " " + lgd_X[np.int(i/3)]+" Ref", "Contact state"])
plt.suptitle("Reference positions of feet (world frame)")
lgd_X = ["FL", "FR", "HL", "HR"]
diff --git a/libexample_adder.so b/libexample_adder.so
index 9ed140fc1a10e24c9bb2e73649c5cef8507c1b04..62a554a96cfb0b74bcbd36c63d0052d80711cbc7 100755
Binary files a/libexample_adder.so and b/libexample_adder.so differ
diff --git a/solo12InvKin.py b/solo12InvKin.py
index 89f6eeea33e7c83990677b2fe2b7852d6836d7a7..1cb2fcf71b47b69be39167f25a9d0cd53a213485 100644
--- a/solo12InvKin.py
+++ b/solo12InvKin.py
@@ -99,7 +99,7 @@ class Solo12InvKin:
# FEET
Jfeet = []
afeet = []
- pfeet_err = []
+ self.pfeet_err = []
vfeet_ref = []
pin.forwardKinematics(self.rmodel, self.rdata, q, dq, np.zeros(self.rmodel.nv))
pin.updateFramePlacements(self.rmodel, self.rdata)
@@ -125,7 +125,7 @@ class Solo12InvKin:
Jfeet.append(J1)
afeet.append(acc1)
- pfeet_err.append(e1)
+ self.pfeet_err.append(e1)
vfeet_ref.append(vref)
# BASE POSITION
@@ -170,7 +170,7 @@ class Solo12InvKin:
J = np.vstack([Jbasis, Jwbasis]+Jfeet)
acc = np.concatenate([accbasis, accwbasis]+afeet)
- x_err = np.concatenate([e_basispos, e_basisrot]+pfeet_err)
+ x_err = np.concatenate([e_basispos, e_basisrot]+self.pfeet_err)
dx_ref = np.concatenate([self.base_linearvelocity_ref, self.base_angularvelocity_ref]+vfeet_ref)
diff --git a/utils_mpc.py b/utils_mpc.py
index 91d7aba1269f4c9f00e8938065916c358a652bad..b4bc00ed65d6a5088ca3cc99bda566178536b8dc 100644
--- a/utils_mpc.py
+++ b/utils_mpc.py
@@ -1,66 +1,22 @@
import math
import numpy as np
-import robots_loader # Gepetto viewer
+from example_robot_data import load
import Joystick
-import FootstepPlanner
import Logger
-import Interface
import Estimator
-
-import pybullet as pyb # Pybullet server
-import pybullet_data
import pinocchio as pin
-import time as time
-import sys
-
-##########################
-# ROTATION MATRIX TO RPY #
-##########################
-
-# Taken from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
-
-# Checks if a matrix is a valid rotation matrix.
-
-
-def isRotationMatrix(R):
- Rt = np.transpose(R)
- shouldBeIdentity = np.dot(Rt, R)
- Id = np.identity(3, dtype=R.dtype)
- n = np.linalg.norm(Id - shouldBeIdentity)
- return n < 1e-6
-
-
-# Calculates rotation matrix to euler angles
-# The result is the same as MATLAB except the order
-# of the euler angles ( x and z are swapped ).
-def rotationMatrixToEulerAngles(R):
-
- assert(isRotationMatrix(R))
-
- sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
-
- singular = sy < 1e-6
- if not singular:
- x = math.atan2(R[2, 1], R[2, 2])
- y = math.atan2(-R[2, 0], sy)
- z = math.atan2(R[1, 0], R[0, 0])
- else:
- x = math.atan2(-R[1, 2], R[1, 1])
- y = math.atan2(-R[2, 0], sy)
- z = 0
-
- return np.array([x, y, z])
-
-###########################################################
-# Roll Pitch Yaw (3 x 1) to Quaternion function (4 x 1) Â #
-###########################################################
+######################################
+# RPY / Quaternion / Rotation matrix #
+######################################
def getQuaternion(rpy):
+ """Roll Pitch Yaw (3 x 1) to Quaternion (4 x 1)"""
+
c = np.cos(rpy*0.5)
s = np.sin(rpy*0.5)
cy = c[2, 0]
@@ -77,12 +33,10 @@ def getQuaternion(rpy):
return np.array([[x, y, z, w]]).transpose()
-#################################
-# Quarternion to Roll Pitch Yaw #
-#################################
-
def quaternionToRPY(quat):
+ """Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1)"""
+
qx = quat[0]
qy = quat[1]
qz = quat[2]
@@ -113,6 +67,22 @@ def quaternionToRPY(quat):
return np.array([[rotateX], [rotateY], [rotateZ]])
+def EulerToQuaternion(roll_pitch_yaw):
+ """Roll Pitch Yaw to Quaternion"""
+
+ roll, pitch, yaw = roll_pitch_yaw
+ sr = math.sin(roll/2.)
+ cr = math.cos(roll/2.)
+ sp = math.sin(pitch/2.)
+ cp = math.cos(pitch/2.)
+ sy = math.sin(yaw/2.)
+ cy = math.cos(yaw/2.)
+ qx = sr * cp * cy - cr * sp * sy
+ qy = cr * sp * cy + sr * cp * sy
+ qz = cr * cp * sy - sr * sp * cy
+ qw = cr * cp * cy + sr * sp * sy
+ return [qx, qy, qz, qw]
+
##################
# Initialisation #
##################
@@ -125,30 +95,26 @@ def init_viewer(enable_viewer):
enable_viewer (bool): if the Gepetto viewer is enabled or not
"""
- # loadSolo(False) to load solo12
- # loadSolo(True) to load solo8
- solo = robots_loader.loadSolo(False)
+ # Load robot model and data
+ solo = load('solo12')
+ # Initialisation of the Gepetto viewer
if enable_viewer:
solo.initDisplay(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)
- """offset = np.zeros((19, 1))
- offset[5, 0] = 0.7071067811865475
- offset[6, 0] = 0.7071067811865475 - 1.0
- temp = solo.q0 + offset"""
solo.display(solo.q0)
- pin.centerOfMass(solo.model, solo.data, solo.q0, np.zeros((18, 1)))
- pin.updateFramePlacements(solo.model, solo.data)
- pin.crba(solo.model, solo.data, solo.q0)
+ # Initialisation of model quantities
+ pin.centerOfMass(solo.model, solo.data, solo.q0, np.zeros((18, 1)))
+ pin.updateFramePlacements(solo.model, solo.data)
+ pin.crba(solo.model, solo.data, solo.q0)
return solo
-def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, n_periods, T_gait, type_MPC, on_solo8,
- predefined):
+def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, n_periods, T_gait, type_MPC, predefined):
"""Create several objects that are used in the control loop
Args:
@@ -159,26 +125,19 @@ def init_objects(dt_tsid, dt_mpc, k_max_loop, k_mpc, n_periods, T_gait, type_MPC
n_periods (int): number of gait periods in the prediction horizon
T_gait (float): duration of one gait period
type_MPC (bool): which MPC you want to use (PA's or Thomas')
- on_solo8 (bool): whether we are working on solo8 or not
predefined (bool): if we are using a predefined reference velocity (True) or a joystick (False)
"""
# Create Joystick object
joystick = Joystick.Joystick(predefined)
- # Create footstep planner object
- fstep_planner = FootstepPlanner.FootstepPlanner(dt_mpc, n_periods, T_gait, on_solo8)
-
# Create logger object
logger = Logger.Logger(k_max_loop, dt_tsid, dt_mpc, k_mpc, n_periods, T_gait, type_MPC)
- # Create Interface object
- interface = Interface.Interface()
-
# Create Estimator object
estimator = Estimator.Estimator(dt_tsid, k_max_loop)
- return joystick, fstep_planner, logger, interface, estimator
+ return joystick, logger, estimator
def display_all(solo, k, sequencer, fstep_planner, ftraj_gen, mpc):
@@ -217,641 +176,3 @@ def getSkew(a):
a -- the column vector
"""
return np.array([[0, -a[2], a[1]], [a[2], 0, -a[0]], [-a[1], a[0], 0]], dtype=a.dtype)
-
-########################################################################
-# PyBullet #
-########################################################################
-
-
-class pybullet_simulator:
- """Wrapper for the PyBullet simulator to initialize the simulation, interact with it
- and use various utility functions
-
- Args:
- envID (int): identifier of the current environment to be able to handle different scenarios
- use_flat_plane (bool): to use either a flat ground or a rough ground
- enable_pyb_GUI (bool): to display PyBullet GUI or not
- dt (float): time step of the inverse dynamics
- """
-
- def __init__(self, q_init, envID, use_flat_plane, enable_pyb_GUI, dt=0.001):
-
- self.applied_force = np.zeros(3)
-
- # Start the client for PyBullet
- if enable_pyb_GUI:
- pyb.connect(pyb.GUI)
- else:
- pyb.connect(pyb.DIRECT)
- # p.GUI for graphical version
- # p.DIRECT for non-graphical version
-
- # Load horizontal plane
- pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
-
- # Either use a flat ground or a rough terrain
- if use_flat_plane:
- self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
- self.planeIdbis = pyb.loadURDF("plane.urdf") # Flat plane
- pyb.resetBasePositionAndOrientation(self.planeIdbis, [20.0, 0, 0], [0, 0, 0, 1])
- 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
- height_prev = 0.0
- 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
- 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)
- 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])
-
- if envID == 1:
-
- # Add stairs with platform and bridge
- self.stairsId = pyb.loadURDF("../../../../../git/paleziart/solopython/mpctsid/bauzil_stairs.urdf") # ,
- """basePosition=[-1.25, 3.5, -0.1],
- baseOrientation=pyb.getQuaternionFromEuler([0.0, 0.0, 3.1415]))"""
- pyb.changeDynamics(self.stairsId, -1, lateralFriction=1.0)
-
- # 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)
- 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)
- 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)
- 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)
- 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)
-
- 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)
- 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)
-
- # Flag to launch the two spheres in the environment toward the robot
- self.flag_sphere1 = True
- self.flag_sphere2 = True
-
- # 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)
-
- 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)
-
- # 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)
- 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)
-
- """cubeStartPos = [0.0, 0.45, 0.0]
- cubeStartOrientation = pyb.getQuaternionFromEuler([0, 0, 0])
- self.cubeId = pyb.loadURDF("cube_small.urdf",
- cubeStartPos, cubeStartOrientation)
- pyb.changeDynamics(self.cubeId, -1, mass=0.5)
- print("Mass of cube:", pyb.getDynamicsInfo(self.cubeId, -1)[0])"""
-
- # Set the gravity
- pyb.setGravity(0, 0, -9.81)
-
- # Load Quadruped robot
- robotStartPos = [0, 0, 0.235+0.0045] # +0.2]
- robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0]) # -np.pi/2
- pyb.setAdditionalSearchPath("/opt/openrobots/share/example-robot-data/robots/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])
-
- # 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:])
-
- # 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)
-
- # 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]])
-
- # Set time step for the simulation
- 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])
-
- def check_pyb_env(self, k, envID, velID, qmes12):
- """Check the state of the robot to trigger events and update camera
-
- Args:
- k (int): Number of inv dynamics iterations since the start of the simulation
- envID (int): Identifier of the current environment to be able to handle different scenarios
- velID (int): Identifier of the current velocity profile to be able to handle different scenarios
- qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
-
- """
-
- # If spheres are loaded
- if envID == 1:
- # Check if the robot is in front of the first sphere to trigger it
- if self.flag_sphere1 and (qmes12[1, 0] >= 0.9):
- pyb.resetBaseVelocity(self.sphereId1, linearVelocity=[2.5, 0.0, 2.0])
- self.flag_sphere1 = False
-
- # Check if the robot is in front of the second sphere to trigger it
- if self.flag_sphere2 and (qmes12[1, 0] >= 1.1):
- pyb.resetBaseVelocity(self.sphereId2, linearVelocity=[-2.5, 0.0, 2.0])
- self.flag_sphere2 = False
-
- # Create the red steps to act as small perturbations
- """if k == 10:
- pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
-
- mesh_scale = [2.0, 2.0, 0.3]
- 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.0, 0.0, 0.15],
- useMaximalCoordinates=True)
- pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
-
- pyb.resetBasePositionAndOrientation(self.robotId, [0, 0, 0.5], [0, 0, 0, 1])"""
- if k == 10:
- 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)
- pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
- 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,)))
- """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,)))"""
-
- # Update the PyBullet camera on the robot position to do as if it was attached to the robot
- """pyb.resetDebugVisualizerCamera(cameraDistance=0.75, cameraYaw=+50, cameraPitch=-35,
- cameraTargetPosition=[qmes12[0, 0], qmes12[1, 0] + 0.0, 0.0])"""
-
- # Get the orientation of the robot to change the orientation of the camera with the rotation of the robot
- oMb_tmp = pin.SE3(pin.Quaternion(qmes12[3:7]), np.array([0.0, 0.0, 0.0]))
- 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]) # qmes12[2, 0]-0.15])
-
- 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 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.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))
-
- """robotVirtualOrientation = pyb.getQuaternionFromEuler([0, 0, np.pi / 4])
- self.qmes12[3:7, 0] = robotVirtualOrientation"""
-
- # Add uncertainty to feedback from PyBullet to mimic imperfect measurements
- """tmp = np.array([pyb.getQuaternionFromEuler(pin.rpy.matrixToRpy(
- pin.Quaternion(self.qmes12[3:7, 0:1]).toRotationMatrix())
- + np.random.normal(0, 0.03, (3,)))])
- self.qmes12[3:7, 0] = tmp[0, :]
- self.vmes12[0:6, 0] += np.random.normal(0, 0.01, (6,))"""
-
- return 0
-
- def apply_external_force(self, k, start, duration, F, M):
- """Apply an external force/momentum to the robot
- 4-th order polynomial: zero force and force velocity at start and end
- (bell-like force trajectory)
-
- Args:
- k (int): numero of the current iteration of the simulation
- start (int): numero of the iteration for which the force should start to be applied
- duration (int): number of iterations the force should last
- F (3x array): components of the force in PyBullet world frame
- M (3x array): components of the force momentum in PyBullet world frame
- """
-
- if ((k < start) or (k > (start+duration))):
- return 0.0
- """if k == start:
- print("Applying [", F[0], ", ", F[1], ", ", F[2], "]")"""
-
- ev = k - start
- t1 = duration
- A4 = 16 / (t1**4)
- A3 = - 2 * t1 * A4
- A2 = t1**2 * A4
- alpha = A2*ev**2 + A3*ev**3 + A4*ev**4
-
- self.applied_force[:] = alpha * F
-
- pyb.applyExternalForce(self.robotId, -1, alpha * F, alpha*M, pyb.LINK_FRAME)
-
- return 0.0
-
- def get_to_default_position(self, qtarget):
- """Controler that tries to get the robot back to a default angular positions
- of its 12 actuators using polynomials to generate trajectories and a PD controller
- to make the actuators follow them
-
- Args:
- qtarget (12x1 array): the target position for the 12 actuators
- """
-
- qmes = np.zeros((12, 1))
- vmes = np.zeros((12, 1))
-
- # Retrieve angular position and velocity of actuators
- jointStates = pyb.getJointStates(self.robotId, self.revoluteJointIndices)
- qmes[:, 0] = [state[0] for state in jointStates]
- vmes[:, 0] = [state[1] for state in jointStates]
-
- # Create trajectory
- dt_traj = 0.0020
- t1 = 4.0 # seconds
- cpt = 0
-
- # 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()
-
- while True or np.max(np.abs(qtarget - qmes)) > 0.1:
-
- time_loop = time.time()
-
- # Retrieve angular position and velocity of actuators
- jointStates = pyb.getJointStates(self.robotId, self.revoluteJointIndices)
- qmes[:, 0] = [state[0] for state in jointStates]
- vmes[:, 0] = [state[1] for state in jointStates]
-
- # Torque PD controller
- 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)
- jointTorques = P * (qdes - qmes) + D * (vdes - vmes)
-
- # Saturation to limit the maximal torque
- t_max = 2.5
- jointTorques[jointTorques > t_max] = t_max
- jointTorques[jointTorques < -t_max] = -t_max
-
- # Set control torque for all joints
- pyb.setJointMotorControlArray(self.robotId, self.revoluteJointIndices,
- controlMode=pyb.TORQUE_CONTROL, forces=jointTorques)
-
- # Compute one step of simulation
- pyb.stepSimulation()
-
- # Increment loop counter
- cpt += 1
-
- while (time.time() - time_loop) < dt_traj:
- pass
-
-
-class Hardware():
-
- def __init__(self):
-
- self.is_timeout = False
-
- self.roll = 0.0
- self.pitch = 0.0
- self.yaw = 0.0
-
- def IsTimeout(self):
-
- return self.is_timeout
-
- def Stop(self):
-
- return 0
-
- def imu_data_attitude(self, i):
- if i == 0:
- return self.roll
- elif i == 1:
- return self.pitch
- elif i == 2:
- return self.yaw
-
-
-class PyBulletSimulator():
-
- def __init__(self):
-
- self.cpt = 0
- self.nb_motors = 12
- self.jointTorques = np.zeros(self.nb_motors)
- self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
- self.hardware = Hardware()
-
- # Measured data
- self.q_mes = np.zeros(12)
- self.v_mes = np.zeros(12)
- self.torquesFromCurrentMeasurment = np.zeros(12)
- self.baseAngularVelocity = np.zeros(3)
- self.baseOrientation = np.zeros(4)
- self.baseLinearAcceleration = np.zeros(3)
- self.baseAccelerometer = np.zeros(3)
- self.prev_b_baseVel = np.zeros(3)
-
- # PD+ quantities
- self.P = 0.0
- self.D = 0.0
- self.q_des = np.zeros(12)
- 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):
-
- # Initialisation of the PyBullet simulator
- self.pyb_sim = pybullet_simulator(q_init, envID, use_flat_plane, enable_pyb_GUI, dt)
- self.q_init = q_init
- self.dt = dt
- self.time_loop = time.time()
-
- return
-
- def UpdateMeasurment(self):
-
- # Position and velocity of actuators
- jointStates = pyb.getJointStates(self.pyb_sim.robotId, self.revoluteJointIndices) # State of all joints
- self.q_mes[:] = np.array([state[0] for state in jointStates])
- self.v_mes[:] = np.array([state[1] for state in jointStates])
-
- # Measured torques
- self.torquesFromCurrentMeasurment[:] = self.jointTorques[:].ravel()
-
- # Position and orientation of the trunk (PyBullet world frame)
- self.baseState = pyb.getBasePositionAndOrientation(self.pyb_sim.robotId)
- self.dummyHeight = np.array(self.baseState[0])
- self.dummyHeight[2] = 0.20
- self.dummyPos = np.array(self.baseState[0])
-
- # Linear and angular velocity of the trunk (PyBullet world frame)
- self.baseVel = pyb.getBaseVelocity(self.pyb_sim.robotId)
- # print("baseVel: ", np.array([self.baseVel[0]]))
-
- # Orientation of the base (quaternion)
- self.baseOrientation[:] = np.array(self.baseState[1])
- RPY = quaternionToRPY(self.baseOrientation)
- self.hardware.roll = RPY[0]
- self.hardware.pitch = RPY[1]
- self.hardware.yaw = RPY[2]
- self.rot_oMb = pin.Quaternion(np.array([self.baseState[1]]).transpose()).toRotationMatrix()
- self.oMb = pin.SE3(self.rot_oMb, np.array([self.dummyHeight]).transpose())
-
- # Angular velocities of the base
- self.baseAngularVelocity[:] = (self.oMb.rotation.transpose() @ np.array([self.baseVel[1]]).transpose()).ravel()
-
- # Linear Acceleration of the base
- self.b_baseVel = (self.oMb.rotation.transpose() @ np.array([self.baseVel[0]]).transpose()).ravel()
- self.baseLinearAcceleration[:] = (self.b_baseVel.ravel() - self.prev_b_baseVel) / self.dt
- self.prev_b_baseVel[:] = self.b_baseVel.ravel()
- self.baseAccelerometer[:] = self.baseLinearAcceleration[:] + \
- (self.oMb.rotation.transpose() @ np.array([[0.0], [0.0], [-9.81]])).ravel()
-
- return
-
- def SetDesiredJointTorque(self, torques):
-
- # Save desired torques in a storage array
- self.tau_ff = torques.copy()
-
- return
-
- def SetDesiredJointPDgains(self, P, D):
- self.P = P
- self.D = D
-
- def SetDesiredJointPosition(self, q_des):
- self.q_des = q_des
-
- def SetDesiredJointVelocity(self, v_des):
- self.v_des = v_des
-
- def SendCommand(self, WaitEndOfCycle=True):
-
- # Compute PD torques
- tau_pd = self.P * (self.q_des - self.q_mes) + self.D * (self.v_des - self.v_mes)
-
- # Save desired torques in a storage array
- self.jointTorques = tau_pd + self.tau_ff
-
- # print("FF+PD: ", self.jointTorques.ravel())
-
- # Set control torque for all joints
- pyb.setJointMotorControlArray(self.pyb_sim.robotId, self.pyb_sim.revoluteJointIndices,
- controlMode=pyb.TORQUE_CONTROL, forces=self.jointTorques)
-
- # self.pyb_sim.apply_external_force(self.cpt, 1000, 1000, np.array([0.0, +8.0, 0.0]), np.zeros((3,)))
- # self.pyb_sim.apply_external_force(self.cpt, 4250, 500, np.array([+5.0, 0.0, 0.0]), np.zeros((3,)))
- # self.pyb_sim.apply_external_force(self.cpt, 5250, 500, np.array([0.0, +5.0, 0.0]), np.zeros((3,)))
-
- # Compute one step of simulation
- pyb.stepSimulation()
-
- # Wait to have simulation time = real time
- if WaitEndOfCycle:
- while (time.time() - self.time_loop) < self.dt:
- pass
- self.cpt += 1
-
- self.time_loop = time.time()
-
- return
-
- def Print(self):
- np.set_printoptions(precision=2)
- # print(chr(27) + "[2J")
- print("#######")
- print("q_mes = ", self.q_mes)
- print("v_mes = ", self.v_mes)
- print("torques = ", self.torquesFromCurrentMeasurment)
- print("orientation = ", self.baseOrientation)
- print("lin acc = ", self.baseLinearAcceleration)
- print("ang vel = ", self.baseAngularVelocity)
- sys.stdout.flush()
-
- def Stop(self):
-
- # Disconnect the PyBullet server (also close the GUI)
- pyb.disconnect()