Skip to content
Snippets Groups Projects
Commit 0da59dc1 authored by thomascbrs's avatar thomascbrs
Browse files

Add offset and fix error indexing xref

parent 25b90d14
No related branches found
No related tags found
1 merge request!6Merge devel croco 20/08/2021 bis
Pipeline #15710 failed
Stage: test
Hide whitespace changes
Inline Side-by-side
scripts/crocoddyl_class/MPC_crocoddyl.py
+ 17
− 9
View file @ 0da59dc1
......@@ -3,6 +3,7 @@
import crocoddyl
import numpy as np
import quadruped_walkgen as quadruped_walkgen
np.set_printoptions(formatter={'float': lambda x: "{0:0.7f}".format(x)}, linewidth = 450)
class MPC_crocoddyl:
......@@ -30,8 +31,8 @@ class MPC_crocoddyl:
else:
self.mu = mu
# self.stateWeight = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 0.25, 0.2, 0.2, 5., 0.0, 0.0, 0.3])
# self.stateWeights = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3])
# Weights on the state vector
self.w_x = 0.3
self.w_y = 0.3
......@@ -48,19 +49,19 @@ class MPC_crocoddyl:
self.stateWeights = 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])
self.forceWeights = 1*np.array(4*[0.007, 0.007, 0.007]) # Weight Vector : Force Norm
self.forceWeights = 1*np.array(4*[0.0071, 0.0071, 0.0071]) # Weight Vector : Force Norm
self.frictionWeights = 1.0 # Weight Vector : Friction cone cost
self.min_fz = 0.2 # Minimum normal force (N)
self.max_fz = 25 # Maximum normal force (N)
self.shoulderWeights = 5. # Weight on the shoulder term :
self.shoulderWeights = 0. # Weight on the shoulder term :
self.shoulder_hlim = 0.235 # shoulder maximum height
# Integration scheme
self.implicit_integration = True
self.implicit_integration = False
self.relative_forces = True
self.max_iteration = 10 # Max iteration ddp solver
self.warm_start = True # Warm Start for the solver
......@@ -68,8 +69,12 @@ class MPC_crocoddyl:
# Position of the feet
self.fsteps = np.full((params.N_gait, 12), np.nan)
self.gait = np.zeros((params.N_gait, 4))
self.xref = np.full((12, int(params.T_gait / params.dt_mpc + 1 )), np.nan)
self.index = 0
# Offset CoM
self.offset_com = -0.03
# Action models
self.ListAction = []
if linearModel:
......@@ -117,7 +122,7 @@ class MPC_crocoddyl:
for j in range(self.index):
# Update model
self.ListAction[j].updateModel(np.reshape(self.fsteps[j, :], (3, 4), order='F'),
xref[:, j+1], self.gait[j, :])
xref[:, j], self.gait[j, :])
# Update model of the terminal model
self.terminalModel.updateModel(np.reshape(
......@@ -133,9 +138,11 @@ class MPC_crocoddyl:
xref : desired state vector
fsteps : feet predicted positions
"""
self.xref[:,:] = xref
self.xref[2,:] += self.offset_com
# Update the dynamic depending on the predicted feet position
self.updateProblem(fsteps, xref)
self.updateProblem(fsteps, self.xref)
self.x_init.clear()
self.u_init.clear()
......@@ -147,7 +154,7 @@ class MPC_crocoddyl:
self.u_init.append(np.repeat(self.gait[self.index-1, :], 3)*np.array(4*[0.5, 0.5, 5.]))
self.x_init = self.ddp.xs[2:]
self.x_init.insert(0, xref[:, 0])
self.x_init.insert(0, self.xref[:, 0])
self.x_init.append(self.ddp.xs[-1])
self.ddp.solve(self.x_init, self.u_init, self.max_iteration)
......@@ -206,6 +213,7 @@ class MPC_crocoddyl:
# integration scheme
model.implicit_integration = self.implicit_integration
model.relative_forces = self.relative_forces
def updateActionModels(self):
"""Update the quadruped model with the new weights or model parameters.
......
scripts/crocoddyl_class/MPC_crocoddyl_planner.py
+ 18
− 11
View file @ 0da59dc1
......@@ -31,7 +31,7 @@ class MPC_crocoddyl_planner():
else:
self.mu = mu
# self.stateWeight = np.sqrt([0., 0., 20.0, 1., 1., 0., 0.5, 0.5, 0., 0.0, 0.0, 0.5])
# self.stateWeights = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3])
# Weights Vector : States
self.w_x = 0.3
......@@ -64,6 +64,9 @@ class MPC_crocoddyl_planner():
self.min_fz = 1.
self.relative_forces = True
# Offset CoM
self.offset_com = -0.03
# Gait matrix
self.gait = np.zeros((params.N_gait, 4))
self.gait_old = np.zeros(4)
......@@ -82,6 +85,7 @@ class MPC_crocoddyl_planner():
# Initial foot location (local frame, X,Y plan)
self.shoulders = [0.1946, 0.14695, 0.1946, -0.14695, -0.1946, 0.14695, -0.1946, -0.14695]
self.xref = np.full((12, int(params.T_gait / params.dt_mpc + 1 )), np.nan)
# Index to stop the feet optimisation
self.index_lock_time = int(params.lock_time / params.dt_mpc) # Row index in the gait matrix when the optimisation of the feet should be stopped
......@@ -128,7 +132,10 @@ class MPC_crocoddyl_planner():
footsteps : current position of the feet (given by planner)
l_stop : current and target position of the feet (given by footstepTragectory generator)
"""
self.updateProblem(k, xref, footsteps, l_stop)
self.xref[:,:] = xref
self.xref[2,:] += self.offset_com
self.updateProblem(k, self.xref, footsteps, l_stop)
self.ddp.solve(self.x_init, self.u_init, self.max_iteration)
# Reset to 0 the stopWeights for next optimisation
......@@ -164,24 +171,24 @@ class MPC_crocoddyl_planner():
# Augmented model, first node, j = 0
self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[0, :], (3, 4), order='F'),
l_stop, xref[:, 1], self.gait[0, :])
l_stop, xref[:, 0], self.gait[0, :])
self.action_models.append(self.models_augmented[index_augmented])
index_augmented += 1
# Warm-start
self.x_init.append(np.concatenate([xref[:, 1], p0]))
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, :])]))
while np.any(self.gait[j, :]):
if np.any(self.gait[j, :] - self.gait[j-1, :]):
# Step model
self.models_step[index_step].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
xref[:, j+1], self.gait[j, :] - self.gait[j-1, :])
xref[:, j], self.gait[j, :] - self.gait[j-1, :])
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+1], self.gait[j, :])
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:
......@@ -206,14 +213,14 @@ class MPC_crocoddyl_planner():
index_step += 1
index_augmented += 1
# Warm-start
self.x_init.append(np.concatenate([xref[:, j+1], p0]))
self.x_init.append(np.concatenate([xref[:, j], p0]))
self.u_init.append(np.zeros(8))
self.x_init.append(np.concatenate([xref[:, j+1], p0]))
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, :])]))
else:
self.models_augmented[index_augmented].updateModel(np.reshape(footsteps[j, :], (3, 4), order='F'),
l_stop, xref[:, j+1], self.gait[j, :])
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]
......@@ -230,7 +237,7 @@ class MPC_crocoddyl_planner():
index_augmented += 1
# Warm-start
self.x_init.append(np.concatenate([xref[:, j+1], p0]))
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, :])]))
# Update row matrix
......@@ -239,7 +246,7 @@ class MPC_crocoddyl_planner():
# Update terminal model
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[:, j-1], p0]))
self.x_init.append(np.concatenate([xref[:, -1], p0]))
self.problem = crocoddyl.ShootingProblem(np.zeros(20), self.action_models, self.terminal_model)
self.problem.x0 = np.concatenate([xref[:, 0], p0])
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment