diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index 45d92909e8a4dc93c5d5cd0a16639f7b11b28be1..266eea8fc74ffbb78a620d84658e2aa967de0f99 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -172,8 +172,8 @@ class Params {
std::vector<double> I_mat; // Inertia matrix
std::vector<double> CoM_offset; // Center of Mass offset
double h_ref; // Reference height for the base
- std::vector<double> shoulders; // Position of shoulders in base frame
- std::vector<double> footsteps_init; // Initial 3D position of footsteps in base frame
+ std::vector<double> shoulders; // Position of shoulders in horizontal frame
+ std::vector<double> footsteps_init; // Initial 3D position of footsteps in horizontal frame
std::vector<double> footsteps_under_shoulders; // Positions of footsteps to be "under the shoulder"
};
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 1128124a00264785453fb46dc182d1228127deba..08dc19de8ba5fdffc7aedee60c47c4793faa8063 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -45,6 +45,7 @@ set(${PY_NAME}_TOOLS_PYTHON
Utils.py
qualisysClient.py
kinematics_utils.py
+ Interpolator.py
)
foreach(python ${${PY_NAME}_PYTHON})
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 4bc4ca35ea9f3c9ee5de32b5a768a4f6bf10bc0a..17a11a4b491092fdd334e316598b32a01a2b2402 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -10,8 +10,7 @@ from .WB_MPC.Target import Target
from .tools.Utils import init_robot, quaternionToRPY
from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
from .tools.kinematics_utils import get_translation_array
-
-COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
+from .tools.Interpolator import Interpolator
class Result:
@@ -29,96 +28,6 @@ class Result:
self.v_des = np.zeros(12)
self.tau_ff = np.zeros(12)
-
-class Interpolator:
- def __init__(self, params, x0):
- self.dt = params.dt_mpc
- self.type = params.interpolation_type
-
- if self.type == 3:
- self.ts = np.repeat(np.linspace(0, 2 * self.dt, 3), 2)
-
- self.update(x0, x0)
-
- def update(self, x0, x1, x2=None):
- self.q0 = x0[7:19]
- self.q1 = x1[7:19]
- self.v0 = x0[19 + 6 :]
- self.v1 = x1[19 + 6 :]
- if self.type == 0: # Linear
- self.alpha = 0.0
- self.beta = self.v1
- self.gamma = self.q0
- elif self.type == 1: # Quadratic fixed velocity
- self.alpha = 2 * (self.q1 - self.q0 - self.v0 * self.dt) / self.dt**2
- self.beta = self.v0
- self.gamma = self.q0
- elif self.type == 2: # Quadratic time variable
- for i in range(3):
- q0 = self.q0[i]
- v0 = self.v0[i]
- q1 = self.q1[i]
- v1 = self.v1[i]
- if (q1 == q0) or (v1 == -v0):
- self.alpha[i] = 0.0
- self.beta[i] = 0.0
- self.gamma[i] = q1
- self.delta = 1.0
- else:
- self.alpha[i] = (v1**2 - v0**2) / (2 * (q1 - q0))
- self.beta[i] = v0
- self.gamma[i] = q0
- self.delta = 2 * (q1 - q0) / (v1 + v0) / self.dt
- elif self.type == 3: # Spline interpolation
- from scipy.interpolate import KroghInterpolator
-
- if x2 is not None:
- self.q2 = x2[7:19]
- self.v2 = x2[19 + 6 :]
- self.y = [self.q0, self.v0, self.q1, self.v1, self.q2, self.v2]
- self.krog = KroghInterpolator(self.ts, np.array(self.y))
- else:
- self.y = [self.q0, self.v0, self.q1, self.v1]
- self.krog = KroghInterpolator(self.ts[:4], np.array(self.y))
-
- def interpolate(self, t):
- if self.type == 3:
- q = self.krog(t)
- v = self.krog.derivative(t)
- return q, v
-
- if self.type == 2:
- t *= self.delta
- q = 1 / 2 * self.alpha * t**2 + self.beta * t + self.gamma
- v = self.v1 if self.type == 1 else self.alpha * t + self.beta
-
- return q, v
-
- def plot(self, n, dt):
- import matplotlib.pyplot as plt
-
- ts = np.linspace(0.0, 2 * self.dt, 2 * n + 1)
- plt.style.use("seaborn")
- for i in range(3):
- plt.subplot(3, 2, (i * 2) + 1)
- plt.title("Position interpolation")
- plt.plot(ts, [self.interpolate(t)[0][i] for t in ts])
- plt.scatter(y=self.q0[i], x=0.0, color="violet", marker="+")
- plt.scatter(y=self.q1[i], x=self.dt, color="violet", marker="+")
- if self.type == 3 and self.q2 is not None:
- plt.scatter(y=self.q2[i], x=2 * self.dt, color="violet", marker="+")
-
- plt.subplot(3, 2, (i * 2) + 2)
- plt.title("Velocity interpolation")
- plt.plot(ts, [self.interpolate(t)[1][i] for t in ts])
- plt.scatter(y=self.v0[i], x=0.0, color="violet", marker="+")
- plt.scatter(y=self.v1[i], x=self.dt, color="violet", marker="+")
- if self.type == 3 and self.v2 is not None:
- plt.scatter(y=self.v2[i], x=2 * self.dt, color="violet", marker="+")
-
- plt.show()
-
-
class DummyDevice:
def __init__(self, h):
self.imu = self.IMU()
@@ -171,7 +80,7 @@ class Controller:
self.result.q_des = self.pd.q0[7:].copy()
self.result.v_des = self.pd.v0[6:].copy()
- pin.forwardKinematics(self.pd.model, self.pd.rdata, self.pd.q0, np.zeros(18))
+ pin.forwardKinematics(self.pd.model, self.pd.rdata, self.pd.q0)
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
foot_pose = self.pd.rdata.oMf[self.pd.feet_ids[1]].translation
@@ -458,10 +367,10 @@ class Controller:
# bp_m = np.array([e for tup in device.baseState for e in tup])
# bv_m = np.array([e for tup in device.baseVel for e in tup])
-
self.q[:3] = self.q_estimate[:3]
self.q[3:6] = quaternionToRPY(self.q_estimate[3:7]).ravel()
self.q[6:] = self.q_estimate[7:]
+
self.v = self.estimator.get_v_reference()
self.x = np.concatenate([self.q_estimate, self.v_estimate])
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index acd3f26b0c384a4d6d7b129b06e6bf107778ecfc..e28de2df01f74640d20b2bdc831c2a0c8614097a 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -11,19 +11,15 @@ class Target:
self.dt_wbc = params.dt_wbc
self.k_per_step = 160
- self.position = np.array(params.footsteps_under_shoulders).reshape(
- (3, 4), order="F"
- )
-
if params.movement == "circle":
- self.A = np.array([0.05, 0., 0.04])
+ self.A = np.array([0.05, 0.0, 0.04])
self.offset = np.array([0.05, 0, 0.05])
- self.freq = np.array([0.5, 0., 0.5])
- self.phase = np.array([-np.pi/2-0.5, 0., -np.pi/2])
+ self.freq = np.array([0.5, 0.0, 0.5])
+ self.phase = np.array([-np.pi / 2 - 0.5, 0.0, -np.pi / 2])
elif params.movement == "step":
self.p0 = foot_pose
self.p1 = foot_pose.copy() + np.array([0.025, 0.0, 0.03])
- self.v1 = np.array([0.5, 0., 0.])
+ self.v1 = np.array([0.5, 0.0, 0.0])
self.p2 = foot_pose.copy() + np.array([0.05, 0.0, 0.0])
self.T = self.k_per_step * self.dt_wbc
@@ -31,9 +27,11 @@ class Target:
self.update_time = -1
else:
- self.target_footstep = self.position
+ self.target_footstep = np.array(
+ self.params.footsteps_init.tolist()
+ ).reshape((3, 4), order="F")
self.ramp_length = 100
- self.target_ramp = np.linspace(0., 0.1, self.ramp_length)
+ self.target_ramp = np.linspace(0.0, 0.1, self.ramp_length)
def compute(self, k):
footstep = np.zeros((3, 4))
@@ -44,7 +42,9 @@ class Target:
footstep[2, 1] += 0.015
else:
footstep = self.target_footstep.copy()
- footstep[2, 1] = self.target_ramp[k] if k < self.ramp_length else self.target_ramp[-1]
+ footstep[2, 1] = (
+ self.target_ramp[k] if k < self.ramp_length else self.target_ramp[-1]
+ )
return footstep
@@ -69,7 +69,6 @@ class Target:
target = self.p0
velocity = -self.v1
-
k_step = k % self.k_per_step
if n_step != self.update_time:
self.update_interpolator(initial, target, velocity)
@@ -86,4 +85,3 @@ class Target:
p = self.krog(t)
# v = self.krog.derivative(t)
return p
-
diff --git a/python/quadruped_reactive_walking/tools/Interpolator.py b/python/quadruped_reactive_walking/tools/Interpolator.py
new file mode 100644
index 0000000000000000000000000000000000000000..6b12257381061609ae0e0176c9bea354213b1c78
--- /dev/null
+++ b/python/quadruped_reactive_walking/tools/Interpolator.py
@@ -0,0 +1,91 @@
+COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
+
+import numpy as np
+
+class Interpolator:
+ def __init__(self, params, x0):
+ self.dt = params.dt_mpc
+ self.type = params.interpolation_type
+
+ if self.type == 3:
+ self.ts = np.repeat(np.linspace(0, 2 * self.dt, 3), 2)
+
+ self.update(x0, x0)
+
+ def update(self, x0, x1, x2=None):
+ self.q0 = x0[7:19]
+ self.q1 = x1[7:19]
+ self.v0 = x0[19 + 6 :]
+ self.v1 = x1[19 + 6 :]
+ if self.type == 0: # Linear
+ self.alpha = 0.0
+ self.beta = self.v1
+ self.gamma = self.q0
+ elif self.type == 1: # Quadratic fixed velocity
+ self.alpha = 2 * (self.q1 - self.q0 - self.v0 * self.dt) / self.dt**2
+ self.beta = self.v0
+ self.gamma = self.q0
+ elif self.type == 2: # Quadratic time variable
+ for i in range(3):
+ q0 = self.q0[i]
+ v0 = self.v0[i]
+ q1 = self.q1[i]
+ v1 = self.v1[i]
+ if (q1 == q0) or (v1 == -v0):
+ self.alpha[i] = 0.0
+ self.beta[i] = 0.0
+ self.gamma[i] = q1
+ self.delta = 1.0
+ else:
+ self.alpha[i] = (v1**2 - v0**2) / (2 * (q1 - q0))
+ self.beta[i] = v0
+ self.gamma[i] = q0
+ self.delta = 2 * (q1 - q0) / (v1 + v0) / self.dt
+ elif self.type == 3: # Spline interpolation
+ from scipy.interpolate import KroghInterpolator
+
+ if x2 is not None:
+ self.q2 = x2[7:19]
+ self.v2 = x2[19 + 6 :]
+ self.y = [self.q0, self.v0, self.q1, self.v1, self.q2, self.v2]
+ self.krog = KroghInterpolator(self.ts, np.array(self.y))
+ else:
+ self.y = [self.q0, self.v0, self.q1, self.v1]
+ self.krog = KroghInterpolator(self.ts[:4], np.array(self.y))
+
+ def interpolate(self, t):
+ if self.type == 3:
+ q = self.krog(t)
+ v = self.krog.derivative(t)
+ return q, v
+
+ if self.type == 2:
+ t *= self.delta
+ q = 1 / 2 * self.alpha * t**2 + self.beta * t + self.gamma
+ v = self.v1 if self.type == 1 else self.alpha * t + self.beta
+
+ return q, v
+
+ def plot(self, n, dt):
+ import matplotlib.pyplot as plt
+
+ ts = np.linspace(0.0, 2 * self.dt, 2 * n + 1)
+ plt.style.use("seaborn")
+ for i in range(3):
+ plt.subplot(3, 2, (i * 2) + 1)
+ plt.title("Position interpolation")
+ plt.plot(ts, [self.interpolate(t)[0][i] for t in ts])
+ plt.scatter(y=self.q0[i], x=0.0, color="violet", marker="+")
+ plt.scatter(y=self.q1[i], x=self.dt, color="violet", marker="+")
+ if self.type == 3 and self.q2 is not None:
+ plt.scatter(y=self.q2[i], x=2 * self.dt, color="violet", marker="+")
+
+ plt.subplot(3, 2, (i * 2) + 2)
+ plt.title("Velocity interpolation")
+ plt.plot(ts, [self.interpolate(t)[1][i] for t in ts])
+ plt.scatter(y=self.v0[i], x=0.0, color="violet", marker="+")
+ plt.scatter(y=self.v1[i], x=self.dt, color="violet", marker="+")
+ if self.type == 3 and self.v2 is not None:
+ plt.scatter(y=self.v2[i], x=2 * self.dt, color="violet", marker="+")
+
+ plt.show()
\ No newline at end of file
diff --git a/python/quadruped_reactive_walking/tools/Utils.py b/python/quadruped_reactive_walking/tools/Utils.py
index 48a37fcdda05aa16a2944e1f37b366307a6b7640..6bdb61099c63eb8a5367868c895410c3df40e6b9 100644
--- a/python/quadruped_reactive_walking/tools/Utils.py
+++ b/python/quadruped_reactive_walking/tools/Utils.py
@@ -4,7 +4,8 @@ import pinocchio as pin
def init_robot(q_init, params):
- """Load the solo model and initialize the Gepetto viewer if it is enabled
+ """
+ Load the solo model and initialize some parameters
Args:
q_init (array): the default position of the robot actuators
@@ -14,27 +15,21 @@ def init_robot(q_init, params):
solo = load("solo12")
q = solo.q0.reshape((-1, 1))
- # Initialisation of the position of footsteps to be under the shoulder
- q[7:, 0] = 0.0
- pin.framesForwardKinematics(solo.model, solo.data, q)
-
# Initial angular positions of actuators
q[7:, 0] = q_init
+ pin.framesForwardKinematics(solo.model, solo.data, q)
# Initialisation of model quantities
pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
pin.updateFramePlacements(solo.model, solo.data)
pin.crba(solo.model, solo.data, solo.q0)
+ LEGS = ["FL", "FR", "HL", "HR"]
+
# Initialisation of the position of footsteps
initial_footsteps = np.zeros((3, 4))
h_init = 0.0
- 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(leg + "_FOOT") for leg in LEGS]
for i in range(4):
initial_footsteps[:, i] = solo.data.oMf[indexes[i]].translation
h = (solo.data.oMf[1].translation - solo.data.oMf[indexes[i]].translation)[2]
@@ -43,34 +38,30 @@ def init_robot(q_init, params):
initial_footsteps[2, :] = 0.0
# Initialisation of the position of shoulders
- shoulders_init = np.zeros((3, 4))
- indexes = [4, 12, 20, 28] # Shoulder indexes
+ initial_shoulders = np.zeros((3, 4))
+ indexes = [solo.model.getFrameId(leg + "_SHOULDER") for leg in LEGS]
for i in range(4):
- shoulders_init[:, i] = solo.data.oMf[indexes[i]].translation
+ initial_shoulders[:, i] = solo.data.oMf[indexes[i]].translation
# Saving data
- params.h_ref = h_init
- # Mass of the whole urdf model (also = to Ycrb[1].mass)
+ params.h_ref = h_init # + 0.155 # Adding foot radius
params.mass = solo.data.mass[0]
- # Composite rigid body inertia in q_init position
params.I_mat = solo.data.Ycrb[1].inertia.ravel().tolist()
params.CoM_offset = (solo.data.com[0][:3] - q[0:3, 0]).tolist()
params.CoM_offset[1] = 0.0
params.mpc_wbc_ratio = int(params.dt_mpc / params.dt_wbc)
params.T = params.gait.shape[0] - 1
-
- # Use initial feet pos as reference
for i in range(4):
for j in range(3):
- params.shoulders[3 * i + j] = shoulders_init[j, i]
+ params.shoulders[3 * i + j] = initial_shoulders[j, i]
params.footsteps_init[3 * i + j] = initial_footsteps[j, i]
- params.footsteps_under_shoulders[3 * i + j] = initial_footsteps[j, i]
def quaternionToRPY(quat):
- """Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1)"""
-
+ """
+ Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1)
+ """
qx = quat[0]
qy = quat[1]
qz = quat[2]
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 3c46683466d1f00e52af0e166e70129fb36aba18..5755b97a021bd45f30e980e971942822b2e19e90 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -206,7 +206,7 @@ Vector3 Estimator::computeBaseVelocityFromFoot(int footId) {
pinocchio::updateFramePlacement(velocityModel_, velocityData_, feetFrames_[footId]);
pinocchio::SE3 contactFrame = velocityData_.oMf[feetFrames_[footId]];
Vector3 frameVelocity =
- pinocchio::getFrameVelocity(velocityModel_, velocityData_, feetFrames_[footId], pinocchio::LOCAL).linear()
+ pinocchio::getFrameVelocity(velocityModel_, velocityData_, feetFrames_[footId], pinocchio::LOCAL).linear();
frameVelocity(0) += footRadius_ * (vActuators_(1 + 3 * footId) + vActuators_(2 + 3 * footId));
return contactFrame.translation().cross(IMUAngularVelocity_) - contactFrame.rotation() * frameVelocity;
}