From 4a2e7391b5df95abc30115ce5cad09b6365ea2ed Mon Sep 17 00:00:00 2001
From: Pierre-Alexandre Leziart <paleziart@laas.fr>
Date: Tue, 8 Sep 2020 14:38:39 +0200
Subject: [PATCH] Add velocity profiles for two new scenarios + Remove
reference velocity limiter
---
Estimator.py | 11 +++++++----
Joystick.py | 26 ++++++++++++++++++++++++--
main.py | 19 ++++++++++++-------
processing.py | 13 +++++++------
utils.py | 9 ++++++++-
5 files changed, 58 insertions(+), 20 deletions(-)
diff --git a/Estimator.py b/Estimator.py
index 28d9059f..cbfa7e1b 100644
--- a/Estimator.py
+++ b/Estimator.py
@@ -18,7 +18,7 @@ class Estimator:
self.dt = dt
# Cut frequency (fc should be < than 1/dt)
- self.fc = 400
+ self.fc = 500
# Filter coefficient (0 < alpha < 1)
self.alpha = self.dt * self.fc
@@ -89,9 +89,12 @@ class Estimator:
self.cheat_lin_pos = np.array(baseState[0])
# Linear acceleration of the trunk (PyBullet base frame)
- tmp = (self.oMb.rotation.transpose() @ np.array([baseVel[0]]).transpose()).ravel()
- self.IMU_lin_acc[:] = (tmp.ravel() - self.prev) / self.dt
- self.prev[:] = tmp.ravel()
+ self.b_baseVel = (self.oMb.rotation.transpose() @ np.array([baseVel[0]]).transpose()).ravel()
+ self.IMU_lin_acc[:] = (self.b_baseVel.ravel() - self.prev) / self.dt
+ """ + (self.k_log / 10000) * np.ones((3, )) \
+ + np.sin(2 * np.pi * self.k_log / 1000) \
+ + np.random.normal(loc=0.0, scale=0.05 * np.max(np.abs(self.IMU_lin_acc[:])), size=(3,))"""
+ self.prev[:] = self.b_baseVel.ravel()
# Angular velocity of the trunk (PyBullet base frame)
self.IMU_ang_vel[:] = (self.oMb.rotation.transpose() @ np.array([baseVel[1]]).transpose()).ravel()
diff --git a/Joystick.py b/Joystick.py
index 730cb7ab..f1986516 100644
--- a/Joystick.py
+++ b/Joystick.py
@@ -121,10 +121,32 @@ class Joystick:
"""if k_loop == self.k_mpc*16*3:
self.v_ref = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T"""
+ if velID == 5:
+ if (k_loop == 0):
+ self.k_switch = np.array([0, 1000, 6000, 8000, 12000])
+ self.v_switch = np.array([[0.0, 0.0, 0.3, 0.3, 0.3],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.4, 0.4]])
+ self.handle_v_switch(k_loop)
+
+ if velID == 4:
+ if (k_loop == 0):
+ self.k_switch = np.array([0, 1000, 3000, 7000, 9000, 30000])
+ self.v_switch = np.array([[0.0, 0.0, 0.6, 0.6, 0.6, 0.6],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.4, 0.4]])
+ self.handle_v_switch(k_loop)
+
if velID == 3:
if (k_loop == 0):
- self.k_switch = np.array([0, 1000, 2000, 7000, 9000, 15000])
- self.v_switch = np.array([[0.0, 0.0, 0.0, 0.3, 0.3, 0.3],
+ self.k_switch = np.array([0, 1000, 2000, 7000, 26000, 30000])
+ self.v_switch = np.array([[0.0, 0.0, 0.0, 0.3, 0.3, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
diff --git a/main.py b/main.py
index 297fcab6..37961a56 100644
--- a/main.py
+++ b/main.py
@@ -377,12 +377,13 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
t_list_loop[k] = time.time() - time_loop
# Process PyBullet
- proc.process_pybullet(pyb_sim, k, envID, jointTorques)
+ proc.process_pybullet(pyb_sim, k, envID, velID, jointTorques)
# 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_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)
@@ -420,7 +421,7 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
pyb.disconnect()
- NN = estimator.log_v_est.shape[2]
+ """NN = estimator.log_v_est.shape[2]
avg = np.zeros((3, NN))
for m in range(NN):
tmp_cpt = 0
@@ -434,7 +435,10 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
plt.figure()
for i in range(3):
- plt.subplot(3, 1, i+1)
+ if i == 0:
+ ax0 = plt.subplot(3, 1, i+1)
+ else:
+ plt.subplot(3, 1, i+1, sharex=ax0)
for j in range(4):
plt.plot(estimator.log_v_est[i, j, :], linewidth=3)
# plt.plot(-myController.log_Fv1F[i, j, :], linewidth=3, linestyle="--")
@@ -442,8 +446,8 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
plt.plot(estimator.log_v_truth[i, :], "k", linewidth=3, linestyle="--")
plt.plot(estimator.log_filt_lin_vel[i, :], color="darkgoldenrod", linewidth=3, linestyle="--")
plt.legend(["FL", "FR", "HL", "HR", "Avg", "Truth", "Filtered"])
- plt.xlim([2000, 8000])
- plt.suptitle("Estimation of the linear velocity of the trunk (in base frame)")
+ # plt.xlim([14000, 15000])
+ plt.suptitle("Estimation of the linear velocity of the trunk (in base frame)")"""
"""plt.figure()
for i in range(3):
@@ -460,7 +464,8 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
for j in range(4):
plt.plot(logger.feet_vel[i, j, :], linewidth=3)
plt.suptitle("Velocity of feet over time")"""
- plt.show(block=True)
+
+ # plt.show(block=True)
return logger
diff --git a/processing.py b/processing.py
index 95d3105c..bd24e163 100644
--- a/processing.py
+++ b/processing.py
@@ -29,7 +29,7 @@ def process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, tsid_con
# Retrieve data from the simulation (position/orientation/velocity of the robot)
# Stored in pyb_sim.qmes12 and pyb_sim.vmes12 (quantities in PyBullet world frame)
pyb_sim.retrieve_pyb_data()
- estimator.log_v_truth[:, estimator.k_log] = tsid_controller.vtsid[0:3, 0]
+ estimator.log_v_truth[:, estimator.k_log] = estimator.b_baseVel.ravel()
pyb_sim.qmes12[0:3, 0] = estimator.cheat_lin_pos
pyb_sim.qmes12[3:7, 0] = estimator.quat_oMb
pyb_sim.vmes12[0:3, 0] = (estimator.oMb.rotation @ np.array([estimator.filt_lin_vel]).transpose()).ravel()
@@ -93,10 +93,10 @@ def process_states(solo, k, k_mpc, velID, pyb_sim, interface, joystick, tsid_con
joystick.update_v_ref(k, velID)
# Legs have a limited length so the reference velocity has to be limited
- v_max = (4 / tsid_controller.T_gait) * 0.155
+ # v_max = (4 / tsid_controller.T_gait) * 0.155
# math.sqrt(0.3**2 - pyb_sim.qmes12[2, 0]**2) # (length leg - 2 cm)** 2 - h_base ** 2
- (joystick.v_ref[0:2])[joystick.v_ref[0:2] > v_max] = v_max
- (joystick.v_ref[0:2])[joystick.v_ref[0:2] < -v_max] = -v_max
+ # (joystick.v_ref[0:2])[joystick.v_ref[0:2] > v_max] = v_max
+ # (joystick.v_ref[0:2])[joystick.v_ref[0:2] < -v_max] = -v_max
return 0
@@ -304,18 +304,19 @@ def process_pdp(pyb_sim, myController):
return (myController.run_PDplus()) # .reshape((12, 1))
-def process_pybullet(pyb_sim, k, envID, jointTorques):
+def process_pybullet(pyb_sim, k, envID, velID, jointTorques):
"""Update the torques applied by the actuators of the quadruped and run one step of simulation
Args:
pyb_sim (object): PyBullet simulation
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
jointTorques (12x1 array): Reference torques for the actuators
"""
# Check the state of the robot to trigger events and update the simulator camera
- pyb_sim.check_pyb_env(k, envID, pyb_sim.qmes12)
+ pyb_sim.check_pyb_env(k, envID, velID, pyb_sim.qmes12)
# Set control torque for all joints
pyb.setJointMotorControlArray(pyb_sim.robotId, pyb_sim.revoluteJointIndices,
diff --git a/utils.py b/utils.py
index cb3706a0..aa25620e 100644
--- a/utils.py
+++ b/utils.py
@@ -432,12 +432,13 @@ class pybullet_simulator:
pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=-50, cameraPitch=-35,
cameraTargetPosition=[0.0, 0.6, 0.0])
- def check_pyb_env(self, k, envID, qmes12):
+ 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
"""
@@ -460,6 +461,10 @@ class pybullet_simulator:
self.apply_external_force(k, 12000, 400, np.array([2.0, 0.0, 0.0]), np.zeros((3,)))
self.apply_external_force(k, 16000, 400, np.array([-2.0, 0.0, 0.0]), np.zeros((3,)))"""
+ 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([-3.0, 0.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])"""
@@ -507,6 +512,8 @@ class pybullet_simulator:
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
--
GitLab