From fbcd7a494a850bea191a359e6101aea6cbca302e Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Tue, 9 Aug 2022 14:26:43 +0200
Subject: [PATCH] working on rising foot
---
config/walk_parameters.yaml | 2 +-
.../quadruped_reactive_walking/Controller.py | 33 ++++++++------
.../WB_MPC/ProblemData.py | 18 ++++----
.../WB_MPC/Target.py | 8 ++--
.../tools/PyBulletSimulator.py | 45 +++++++++++++++++--
5 files changed, 75 insertions(+), 31 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index f670bd48..b6416697 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -7,7 +7,7 @@ robot:
PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
DEMONSTRATION: false # Enable/disable demonstration functionalities
SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
- enable_pyb_GUI: false # Enable/disable PyBullet GUI
+ enable_pyb_GUI: true # Enable/disable PyBullet GUI
envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 190ef617..b6496bab 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -46,7 +46,8 @@ class Interpolator:
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.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
@@ -102,7 +103,8 @@ class Interpolator:
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.scatter(y=self.q2[i], x=2 * self.dt,
+ color="violet", marker="+")
plt.subplot(3, 2, (i * 2) + 2)
plt.title("Velocity interpolation")
@@ -110,7 +112,8 @@ class Interpolator:
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.scatter(y=self.v2[i], x=2 * self.dt,
+ color="violet", marker="+")
plt.show()
@@ -122,7 +125,7 @@ class DummyDevice:
self.base_position = np.zeros(3)
self.base_position[2] = 0.1944
self.b_base_velocity = np.zeros(3)
- self.baseState = ((0.0, 0.0, 0.2607495), (0.0, 0.0, 0.0, 1.0))
+ self.baseState = ((0.0, 0.0, 0.2607495), (0.0, 0.0, 0.0, 1.0))
self.baseVel = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
class IMU:
@@ -138,7 +141,6 @@ class DummyDevice:
self.velocities = np.zeros(12)
-
class Controller:
def __init__(self, pd, params, q_init, t):
"""
@@ -153,7 +155,8 @@ class Controller:
self.pd = pd
self.params = params
- self.gait = np.repeat(np.array([0, 0, 0, 0]).reshape((1, 4)), self.pd.T, axis=0)
+ self.gait = np.concatenate([np.repeat(np.array([1, 1, 1, 1]).reshape((1, 4)), self.pd.init_steps, axis=0),
+ np.repeat(np.array([1, 0, 1, 1]).reshape((1, 4)), self.pd.target_steps - 1, axis=0)])
self.q_init = pd.q0
self.k = 0
@@ -261,8 +264,8 @@ class Controller:
# q, v = self.interpolator.interpolate(t)
# else:
# q, v = self.integrate_x(m)
- q = xs[1][: self.pd.nq]
- v = xs[1][self.pd.nq :]
+ q = xs[1][7: self.pd.nq]
+ v = xs[1][6 + self.pd.nq:]
self.result.q_des = q[:]
self.result.v_des = v[:]
@@ -426,9 +429,10 @@ class Controller:
pin.difference(
self.pd.model,
m["x_m"][: self.pd.nq],
- self.mpc_result.xs[0][: self.pd.nq],
+ self.mpc_result.xs[0][: self.pd.nq]
+
),
- m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
+ m["x_m"][self.pd.nq:] - self.mpc_result.xs[0][self.pd.nq:]
]
)
# x_diff = self.mpc_result.xs[0] - m["x_m"]
@@ -459,13 +463,16 @@ class Controller:
legend = ["Hip", "Shoulder", "Knee"]
fig, axs = plt.subplots(3)
- [axs[0].plot(np.array(self.mpc_result.xs)[:, joint]) for joint in range(3)]
+ [axs[0].plot(np.array(self.mpc_result.xs)[:, joint])
+ for joint in range(3)]
axs[0].legend(legend)
- [axs[1].plot(np.array(self.mpc_result.xs)[:, 3 + joint]) for joint in range(3)]
+ [axs[1].plot(np.array(self.mpc_result.xs)[:, 3 + joint])
+ for joint in range(3)]
axs[1].legend(legend)
- [axs[2].plot(np.array(self.mpc_result.us)[:, joint]) for joint in range(3)]
+ [axs[2].plot(np.array(self.mpc_result.us)[:, joint])
+ for joint in range(3)]
axs[2].legend(legend)
return axs
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index f2eeeadc..8e822e48 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -8,8 +8,8 @@ class problemDataAbstract:
self.dt = param.dt_mpc # OCP dt
self.dt_wbc = param.dt_wbc
self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
- self.init_steps = 0
- self.target_steps = 150
+ self.init_steps = 10
+ self.target_steps = 90
self.T = self.init_steps + self.target_steps - 1
self.robot = erd.load("solo12")
@@ -89,25 +89,25 @@ class ProblemData(problemDataAbstract):
# Cost function weights
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e3
- # self.friction_cone_w = 1e3 * 0
+ self.foot_tracking_w = 1e2
+ self.friction_cone_w = 1e3
self.control_bound_w = 1e3
- self.control_reg_w = 1e0
+ self.control_reg_w = 1e1
self.state_reg_w = np.array([0] * 3
+ [1e1] * 3
+ [1e0] * 3
- + [1e-5] * 3
+ + [1e-3] * 3
+ [1e0] * 6
+ [0] * 6
+ [1e1] * 3
- + [1e0] * 3
+ + [1e-1] * 3
+ [1e1] * 6
)
self.terminal_velocity_w = np.array(
- [0] * self.nq + [1e3] * self.nv)
+ [0] * self.nv + [1e3] * self.nv)
self.xref = self.x0
- self.uref = self.u0
+ self.uref =self.u0
class ProblemDataFull(problemDataAbstract):
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 90f34a01..d2cf603b 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -9,10 +9,10 @@ class Target:
pin.forwardKinematics(pd.model, pd.rdata, pd.q0, pd.v0)
pin.updateFramePlacements(pd.model, pd.rdata)
self.foot_pose = pd.rdata.oMf[pd.rfFootId].translation.copy()
- self.A = np.array([0, 0.03, 0.03])
- self.offset = np.array([0.05, -0.02, 0.06])
- self.freq = np.array([0, 0.5, 0.5])
- self.phase = np.array([0, np.pi / 2, 0])
+ self.A = np.array([0, 0.05, 0.05])
+ self.offset = np.array([0.05, 0, 0.06])
+ self.freq = np.array([0, 0.5 * 0, 0.5*0])
+ self.phase = np.array([0, 0, np.pi / 2])
self.t_offset = 0
def shift(self):
diff --git a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
index 07f59c9c..22e140b9 100644
--- a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
+++ b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
@@ -42,7 +42,8 @@ class pybullet_simulator:
# Either use a flat ground or a rough terrain
if use_flat_plane:
- """ self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
+
+ self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
self.planeIdbis = pyb.loadURDF("plane.urdf") # Flat plane
# Tune position and orientation of plane
@@ -55,7 +56,7 @@ class pybullet_simulator:
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
@@ -269,12 +270,12 @@ class pybullet_simulator:
pyb.setGravity(0, 0, -9.81)
# Load Quadruped robot
- robotStartPos = [0, 0, 0.2607495]
+ robotStartPos = [0, 0, 0]
robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0])
pyb.setAdditionalSearchPath(
EXAMPLE_ROBOT_DATA_MODEL_DIR + "/solo_description/robots"
)
- self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation, useFixedBase = 1)
+ 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]
@@ -301,6 +302,42 @@ class pybullet_simulator:
forces=jointTorques,
)
+ # Get position of feet in world frame with base at (0, 0, 0)
+ feetLinksID = [3, 7, 11, 15]
+ linkStates = pyb.getLinkStates(self.robotId, feetLinksID)
+
+ # Get minimum height of feet (they are in the ground since base is at 0, 0, 0)
+ z_min = linkStates[0][4][2]
+ i_min = 0
+ i = 1
+ for link in linkStates[1:]:
+ if link[4][2] < z_min:
+ z_min = link[4][2]
+ i_min = i
+ 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(),
+ )
+
+ # 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:
+ 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(),
+ )
# 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]])
--
GitLab