Hi there,
I’m doing some nmpc things and trying to recurrent a controller based on centroidal-dynamics model of quadruped robot, which is similar to what is already done in OCS2. The state variables are the centroidal momentum, the 6 dimension posture of the center of mass and the angle of 12 joints on 4 legs. The control variables are ground reaction forces and velocity of 12 joints.
I’m just testing a standing case, which just set 4 feet on the ground to support. There’s 2 kinds of constraints the control variables should subject to. The first is the friction cone, which is linearized to square pyramid in advanced. The second one is that the support feet(4 feet in the standing case) shouldn’t move, which has a high non-linear correlation with the velocity of 12 joints and the posture of the center of mass because of forward kinematics of the robot. Actually this constraint is also a function of the derivative of state. Maybe is is infeasible in acados?
To formulate the model and constraints, I link pinocchio3(a Rigid Body Dynamics algorithms implementation) to casadi to do forward kinematics things. First of all I just simplified the second kind of constraints to let the velocity of 12 joints be 0, and everything went well on my simulation project based on MuJoCo, successfully keeping the robot standing without moving. But once I turned to the real constraints – compute feet velocity via pinocchio and let them be 0, the solver crashed and reported “SQP_RTI: QP solver returned error status 3 QP iteration 15”. It couldn’t be caused by pinocchio because the centroidal dynamics model is formulated via it and it runs well in my first attempt.
Here is my python code:
import numpy as np
import casadi as ca
from acados_template import AcadosModel
import pinocchio as pin
from pinocchio import casadi as cpin
from pinocchio.robot_wrapper import RobotWrapper
def compute_centriodal_momentum_matrix(m, d, q):
v = ca.SX.zeros(18)
cpin.forwardKinematics(m, d, q)
cpin.updateFramePlacements(m, d)
cpin.ccrba(m, d, q, v)
return d.Ag
def compute_r_com_to_foot(m, d, q, id):
cpin.forwardKinematics(m, d, q)
cpin.updateFramePlacements(m, d)
r = ca.SX.sym("r", 12)
r[0:3] = d.oMf[id[0]].translation - q[0:3]
r[3:6] = d.oMf[id[1]].translation - q[0:3]
r[6:9] = d.oMf[id[2]].translation - q[0:3]
r[9:12] = d.oMf[id[3]].translation - q[0:3]
return r
def compute_end_vel(m, d, q, v, id):
cpin.forwardKinematics(m, d, q, v)
cpin.updateFramePlacements(m, d)
end_vel = ca.SX.sym("end_vel", 12)
for i in range(4):
end_vel[3 * i:3 * i + 3] = cpin.getFrameVelocity(
m, d, id[i], cpin.ReferenceFrame.LOCAL_WORLD_ALIGNED).linear
return end_vel
# 单刚体动力学Centriodal模型
class Centriodal_Model(object):
def __init__(self, ):
model = AcadosModel()
model.name = "centriodal"
h_com = ca.SX.sym("h_com", 6)
qb = ca.SX.sym("qb", 6)
qj = ca.SX.sym("qj", 12)
f = ca.SX.sym("f", 12)
qj_dot = ca.SX.sym("qj_dot", 12)
state_vars = ca.SX.sym("state_vars", 24)
state_vars = ca.vertcat(h_com, qb, qj)
control_vars = ca.SX.sym("control_vars", 24)
control_vars = ca.vertcat(f, qj_dot)
mass = 12.454
mg = ca.SX.zeros(3)
mg[2] = -9.81 * mass
# load model, using pinocchio.casadi
jointComposite = pin.JointModelComposite(2)
jointComposite.addJoint(pin.JointModelTranslation())
jointComposite.addJoint(pin.JointModelSphericalZYX())
robot = RobotWrapper.BuildFromURDF("../urdf/a1.urdf", "",
jointComposite)
cmodel = cpin.Model(robot.model)
cdata = cmodel.createData()
# need full config to compute A
full_config = ca.SX.sym("full_config", 18)
full_config[0:6] = qb
full_config[6:18] = qj
# centriodal_momentum_matrix
A_com = ca.SX.sym("centriodal_momentum_matrix", 6, 18)
A_com = compute_centriodal_momentum_matrix(cmodel, cdata, full_config)
A_b = ca.SX.sym("A_b", 6, 6)
A_b = A_com[0:6, 0:6]
A_j = ca.SX.sym("A_j", 6, 12)
A_j = A_com[0:6, 6:18]
qb_dot = ca.SX.sym("qb_dot", 6)
qb_dot = ca.inv(A_b) @ (h_com - A_j @ qj_dot)
full_config_dot = ca.SX.sym("full_config_dot", 18)
full_config_dot = ca.vertcat(qb_dot, qj_dot)
frame_id = [
cmodel.getFrameId("FL_foot"),
cmodel.getFrameId("FR_foot"),
cmodel.getFrameId("RL_foot"),
cmodel.getFrameId("RR_foot")
]
# r_com_to_foot
p_foot = ca.SX.sym("p_foot", 12)
r_com_to_foot = ca.SX.sym("r_com_to_foot", 12)
# r_com_to_foot = compute_r_com_to_foot(cmodel, cdata, full_config,
# frame_id)
for i in range(4):
r_com_to_foot[3 * i:3 * i +
3] = p_foot[3 * i:3 * i + 3] - state_vars[6:9]
v_foot_world = ca.SX.sym("v_foot_world", 12)
v_foot_world = compute_end_vel(cmodel, cdata, full_config,
full_config_dot, frame_id)
# gait table
gait_data = ca.SX.sym("gait_data", 4)
# tau = r.cross * f
tau = (ca.cross(r_com_to_foot[0:3], f[0:3], -1) +
ca.cross(r_com_to_foot[3:6], f[3:6], -1) +
ca.cross(r_com_to_foot[6:9], f[6:9], -1) +
ca.cross(r_com_to_foot[9:12], f[9:12], -1))
f_sum = f[0:3] + f[3:6] + f[6:9] + f[9:12]
# 状态空间
state_space = ca.vertcat(f_sum + mg, tau, qb_dot, qj_dot)
state_vars_dot = ca.SX.sym("state_vars_dot", 24)
f_expl = state_space
model.f_expl_expr = f_expl
model.f_impl_expr = state_vars_dot - f_expl
model.x = state_vars
model.xdot = state_vars_dot
model.u = control_vars
# 参数由足端位置和步态信息构成
model.p = ca.vertcat(p_foot, gait_data)
mu = 0.4
nonlinear_constraint = ca.SX.sym("nonlinear_constraint", 32)
for i in range(4):
# 足底cone约束
nonlinear_constraint[i * 8 + 0] = (f[i * 3 + 0] -
mu * f[i * 3 + 2])
nonlinear_constraint[i * 8 + 1] = (f[i * 3 + 1] -
mu * f[i * 3 + 2])
nonlinear_constraint[i * 8 + 2] = (f[i * 3 + 0] +
mu * f[i * 3 + 2])
nonlinear_constraint[i * 8 + 3] = (f[i * 3 + 1] +
mu * f[i * 3 + 2])
nonlinear_constraint[i * 8 + 4] = f[i * 3 + 2]
#support foot with Zero vel
nonlinear_constraint[i * 8 + 5:i * 8 +
8] = v_foot_world[i * 3:i * 3 + 3]
# nonlinear_constraint[i * 8 + 5] = qj_dot[i * 3 + 0]
# nonlinear_constraint[i * 8 + 6] = qj_dot[i * 3 + 1]
# nonlinear_constraint[i * 8 + 7] = qj_dot[i * 3 + 2]
model.con_h_expr = nonlinear_constraint
constraint = ca.types.SimpleNamespace()
constraint.lower_bound = np.array([
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
])
constraint.upper_bound = np.array([
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
])
self.model = model
self.constraint = constraint
I generated c code and called it in c++. The error is that:
SQP_RTI: QP solver returned error status 3 QP iteration 15
Could anyone tell me what’s wrong? Is acados unable to solve high non-linear constraints with HPIPM(aka a solver for quadratic programs (QP) and quadratically-constrained quadratic programs (QCQP))? Or could I solve this by doing differentiation of the second constraints to linearize it via casadi’s AD in advance?
Thanks a lot in advance for your help!