def get_dq(self, q, e1, J1, e2, J2, e3, J3, e4, J4):
de1 = self.lambda1 * e1
de2 = self.lambda2 * e2
de3 = self.lambda3 * e3
de4 = self.lambda4 * e4
W = self.w1 * np.dot(J1.T, J1) + self.w2 * np.dot(
J2.T, J2) + self.w3 * np.dot(J3.T, J3) + self.w4 * np.dot(
J4.T, J4)
p = -2 * (self.w1 * np.dot(J1.T, de1) + self.w2 * np.dot(J2.T, de2) +
self.w3 * np.dot(J3.T, de3) + self.w4 * np.dot(J4.T, de4))
lower_limits = np.maximum((self.qmin - q[7:]) / self.dt, self.dqmin)
upper_limits = np.minimum((self.qmax - q[7:]) / self.dt, self.dqmax)
lower_limits = np.hstack((self.lfb, lower_limits))
upper_limits = np.hstack((self.ufb, upper_limits))
# Solver
solver = QProblemB(19)
options = Options()
options.setToMPC()
options.printLevel = PrintLevel.LOW
solver.setOptions(options)
nWSR = np.array([10])
solver.init(W, p, lower_limits, upper_limits, nWSR)
dq = np.zeros(19)
solver.getPrimalSolution(dq)
return dq
def test_m44_mpc_sparse(self):
test_name = 'mm44_mpc_sparse.txt'
print("Test: ", test_name)
# QP Options
options = Options()
options.setToMPC()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 19, 'One ore more tests failed.'
def test_m44_mpc_sparse(self):
test_name ='mm44_mpc_sparse.txt'
print("Test: ", test_name)
# QP Options
options = Options()
options.setToMPC()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 19, 'One ore more tests failed.'
def solve(self, i, vref):
"""build the problem and solve it
"""
#we build the N-sized reference vectors
if (i + self.N < self.ntime + 1):
#take the values stored in the rows (i to N)
vref_pred = vref[i:i + self.N, :]
else:
# If the matrix doesn't have N rows ahead, then repeat the last row to fill the void
vref_pred = np.vstack(
(vref[i:self.ntime, :],
np.dot(np.ones((self.N - self.ntime + i, 1)),
np.expand_dims(vref[self.ntime - 1, :], axis=0))))
#get condensed matrices (constant from on step to another)
[Sx, Sy, Svx, Svy, St, Svt, Scx, Scy, Ux, Uu, D_diag] = self.condense()
#build matrix V
V, V_dash = self.get_walk_matrices(i)
##################stuff for the solver###############################
#build constraint vectors (changing from one step to another)
#footsteps and CoP upper bounds
#ubounds CoP x position
ubounds1 = np.vstack(([self.CoP_ubounds[0] for _ in range(self.N)]))
ubounds1 = ubounds1 - np.dot(np.dot(Scx, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][0])
#ubounds CoP y position
ubounds2 = np.vstack([self.CoP_ubounds[1] for _ in range(self.N)])
ubounds2 = ubounds2 - np.dot(np.dot(Scy, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][1])
#ubounds feet x position
ubounds3 = np.vstack((self.feet_ubounds[0], self.feet_ubounds[2]))
ubounds3 = ubounds3 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 0, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#ubounds feet y position
ubounds4 = np.vstack((self.feet_ubounds[1], self.feet_ubounds[3]))
ubounds4 = ubounds4 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 1, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#ubounds feet angles
ubounds5 = self.feet_angle_ubound * np.ones((self.future_steps, 1))
ubounds5 = ubounds5 + np.vstack(
(self.f_current[np.newaxis, np.newaxis, 0,
2], np.zeros((self.future_steps - 1, 1))))
ubA = np.vstack((ubounds1, ubounds2, ubounds3, ubounds4, ubounds5))
#footsteps and CoP lower bounds
#lbounds CoP x position
lbounds1 = np.vstack([self.CoP_lbounds[0] for _ in range(self.N)])
lbounds1 = lbounds1 - np.dot(np.dot(Scx, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][0])
#lbounds CoP y position
lbounds2 = np.vstack([self.CoP_lbounds[1] for _ in range(self.N)])
lbounds2 = lbounds2 - np.dot(np.dot(Scy, D_diag), np.dot(
Ux, self.x.T)) + np.dot(V, self.f_current[0][1])
#lbounds feet x position
lbounds3 = np.vstack((self.feet_lbounds[0], self.feet_lbounds[2]))
lbounds3 = lbounds3 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 0, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#lbounds feet y position
lbounds4 = np.vstack((self.feet_lbounds[1], self.feet_lbounds[3]))
lbounds4 = lbounds4 + np.vstack(
(np.dot(self.current_rotation[np.newaxis, 1, :],
self.f_current[0, 0:2, np.newaxis]),
np.zeros((self.future_steps - 1, 1))))
#lbounds feet angles
lbounds5 = self.feet_angle_lbound * np.ones((self.future_steps, 1))
lbounds5 = lbounds5 + np.vstack(
(self.f_current[np.newaxis, np.newaxis, 0,
2], np.zeros((self.future_steps - 1, 1))))
lbA = np.vstack((lbounds1, lbounds2, lbounds3, lbounds4, lbounds5))
#big A matrix (constraints matrix)
A = np.zeros((self.n_constraints, self.n_dec))
#A elements for constraints for CoP position
A[0:self.N, :] = np.hstack(
(np.dot(np.dot(Scx, D_diag),
Uu), -V_dash, np.zeros((self.N, 2 * self.future_steps))))
A[self.N:2 * self.N, :] = np.hstack(
(np.dot(np.dot(Scy, D_diag),
Uu), np.zeros((self.N, self.future_steps)), -V_dash,
np.zeros((self.N, self.future_steps))))
A[2 * self.N, 3 * self.N] = self.current_rotation[0, 0]
A[2 * self.N, 3 * self.N + 2] = self.current_rotation[0, 1]
A[2 * self.N + 1, 3 * self.N] = -self.current_rotation[0, 0]
A[2 * self.N + 1, 3 * self.N + 1] = self.current_rotation[0, 0]
A[2 * self.N + 1, 3 * self.N + 2] = -self.current_rotation[0, 1]
A[2 * self.N + 1, 3 * self.N + 3] = self.current_rotation[0, 1]
A[2 * self.N + 2, 3 * self.N] = self.current_rotation[1, 0]
A[2 * self.N + 2, 3 * self.N + 2] = self.current_rotation[1, 1]
A[2 * self.N + 3, 3 * self.N] = -self.current_rotation[1, 0]
A[2 * self.N + 3, 3 * self.N + 1] = self.current_rotation[1, 0]
A[2 * self.N + 3, 3 * self.N + 2] = -self.current_rotation[1, 1]
A[2 * self.N + 3, 3 * self.N + 3] = self.current_rotation[1, 1]
#max angle between feet constraints
A[2 * self.N + 4, 3 * self.N + 4] = 1
A[2 * self.N + 5, 3 * self.N + 4] = -1
A[2 * self.N + 5, 3 * self.N + 5] = 1
#QP solver takes one dim arrays
lbA = lbA.reshape((lbA.shape[0], ))
ubA = ubA.reshape((ubA.shape[0], ))
#############################################################################HESSIAN AND GRADIENT####################
H = np.zeros((self.n_dec, self.n_dec))
H[0:3*self.N, 0:3*self.N] = self.alpha*np.ones((3*self.N, 3*self.N)) + self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(Svx, Uu)) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(Svy, Uu)) + \
+ self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(Svt, Uu)) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(Scx, D_diag), Uu)) + \
+ self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(Scy, D_diag), Uu)) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(St, Uu))
H[0:3 * self.N, 3 * self.N:] = np.hstack(
(-self.gamma * np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, V_dash),
-self.gamma * np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, V_dash),
-self.gamma * np.dot(np.dot(St, Uu).T, V_dash)))
H[3 * self.N:, 0:3 * self.N] = np.vstack(
(-self.gamma * np.dot(V_dash.T, np.dot(np.dot(Scx, D_diag), Uu)),
-self.gamma * np.dot(V_dash.T, np.dot(np.dot(Scy, D_diag), Uu)),
-self.gamma * np.dot(V_dash.T, np.dot(St, Uu))))
H[3 * self.N:, 3 * self.N:] = self.gamma * block_diag(
np.dot(V_dash.T, V_dash), np.dot(V_dash.T, V_dash),
np.dot(V_dash.T, V_dash))
g = np.zeros((self.n_dec, 1))
g[0:3*self.N, :] = self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(np.dot(Svx, Ux), self.x.T) - vref_pred[:, 0][np.newaxis].T) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(np.dot(Svy, Ux), self.x.T) - vref_pred[:, 1][np.newaxis].T) + \
self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(np.dot(Svt, Ux), self.x.T) - vref_pred[:, 2][np.newaxis].T) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][0])) + \
+ self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][1])) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(np.dot(St, Ux), self.x.T)) - \
- self.gamma*np.dot(np.dot(St, Uu).T, np.dot(V, self.f_current[0][2]))
g[3 * self.N:50, :] = -self.gamma * np.dot(
V_dash.T,
np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) -
np.dot(V, self.f_current[0][0]))
g[50:52, :] = -self.gamma * np.dot(
V_dash.T,
np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) -
np.dot(V, self.f_current[0][1]))
g[52:, :] = -self.gamma * np.dot(
V_dash.T,
np.dot(np.dot(St, Ux), self.x.T) - np.dot(V, self.f_current[0][2]))
g = g.reshape((g.shape[0], ))
###########################################################################################################################
#solver options - MPC
myOptions = Options()
myOptions.setToMPC()
self.QP.setOptions(myOptions)
#setting lb and ub to huge numbers - otherwise solver gives errors
lb = -1000000000. * np.ones((self.n_dec, ))
ub = 1000000000. * np.ones((self.n_dec, ))
#solve QP - QP.init()
self.QP.init(H, g, A, lb, ub, lbA, ubA, nWSR=np.array([1000000]))
#get the solution (getPrimalSolution())
x_opt = np.zeros(self.n_dec)
self.QP.getPrimalSolution(x_opt)
#update the state of the system, CoP and the footstep (take the first elements of the solution)
self.controls = np.array([[x_opt[0], x_opt[1], x_opt[2]]])
self.x = (np.dot(self.model.A(self.T), self.x.T) +
np.dot(self.model.B(self.T), self.controls.T)).T
self.CoP = np.dot(self.model.D(self.T), self.x.T)
#first future step
self.f_future = np.array([[
x_opt[3 * self.N], x_opt[3 * self.N + self.future_steps],
x_opt[3 * self.N + 2 * self.future_steps]
]])
#change the current (every 8 samples except the first step) foot position
if np.mod(i, self.samples_per_step) == self.samples_per_step - 2:
self.f_current = self.f_future
#rotate the footstep bounds
self.current_rotation = np.array(
[[np.cos(self.f_current[0, 2]),
np.sin(self.f_current[0, 2])],
[-np.sin(self.f_current[0, 2]),
np.cos(self.f_current[0, 2])]])
#swap footstep constraints
tmp1, tmp2 = self.feet_ubounds[1], self.feet_lbounds[1]
self.feet_ubounds[1], self.feet_lbounds[1] = self.feet_ubounds[
3], self.feet_lbounds[3]
self.feet_ubounds[3], self.feet_lbounds[3] = tmp1, tmp2
#return all the variables of interest
return [self.x, self.f_current, self.CoP, self.controls]
class ClassicGenerator(BaseGenerator):
"""
Reimplementation of current state-of-the-art pattern
generator for HRP-2 of CNRS-LAAS, Toulouse.
Solve QPs for position and orientation of CoM and feet
independently of each other in each timestep.
First solve for orientations, then solve for the postions.
"""
def __init__(
self, N=16, T=0.1, T_step=0.8,
fsm_state='D', fsm_sl=1
):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(ClassicGenerator, self).__init__(
N, T, T_step, fsm_state, fsm_sl
)
# TODO for speed up one can define members of BaseGenerator as
# direct views of QP data structures according to walking report
# Maybe do that later!
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # number of working set recalculations
self.options = Options()
self.options.setToMPC()
self.options.printLevel = PrintLevel.LOW
# FOR ORIENTATIONS
# define dimensions
self.ori_nv = 2*self.N
self.ori_nc = (
self.nc_fvel_eq
+ self.nc_fpos_ineq
+ self.nc_fvel_ineq
)
# setup problem
self.ori_dofs = numpy.zeros(self.ori_nv)
self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
self.ori_qp.setOptions(self.options) # load NMPC options
self.ori_H = numpy.zeros((self.ori_nv,self.ori_nv))
self.ori_A = numpy.zeros((self.ori_nc,self.ori_nv))
self.ori_g = numpy.zeros((self.ori_nv,))
self.ori_lb = -numpy.ones((self.ori_nv,))*1e+08
self.ori_ub = numpy.ones((self.ori_nv,))*1e+08
self.ori_lbA = -numpy.ones((self.ori_nc,))*1e+08
self.ori_ubA = numpy.ones((self.ori_nc,))*1e+08
self._ori_qp_is_initialized = False
# save computation time and working set recalculations
self.ori_qp_nwsr = 0.0
self.ori_qp_cputime = 0.0
# FOR POSITIONS
# define dimensions
self.pos_nv = 2*(self.N + self.nf)
self.pos_nc = (
self.nc_cop
+ self.nc_foot_position
+ self.nc_fchange_eq
)
# setup problem
self.pos_dofs = numpy.zeros(self.pos_nv)
self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
self.pos_qp.setOptions(self.options)
self.pos_H = numpy.zeros((self.pos_nv,self.pos_nv))
self.pos_A = numpy.zeros((self.pos_nc,self.pos_nv))
self.pos_g = numpy.zeros((self.pos_nv,))
self.pos_lb = -numpy.ones((self.pos_nv,))*1e+08
self.pos_ub = numpy.ones((self.pos_nv,))*1e+08
self.pos_lbA = -numpy.ones((self.pos_nc,))*1e+08
self.pos_ubA = numpy.ones((self.pos_nc,))*1e+08
self._pos_qp_is_initialized = False
# save computation time and working set recalculations
self.pos_qp_nwsr = 0.0
self.pos_qp_cputime = 0.0
# dummy matrices
self._ori_Q = numpy.zeros((2*self.N, 2*self.N))
self._ori_p = numpy.zeros((2*self.N,))
self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf))
self._pos_p = numpy.zeros((self.N + self.nf,))
# add additional keys that should be saved
self._data_keys.append('ori_qp_nwsr')
self._data_keys.append('ori_qp_cputime')
self._data_keys.append('pos_qp_nwsr')
self._data_keys.append('pos_qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def solve(self):
""" Process and solve problem, s.t. pattern generator data is consistent """
self._preprocess_solution()
self._solve_qp()
self._postprocess_solution()
def _preprocess_solution(self):
""" Update matrices and get them into the QP data structures """
# rename for convenience
N = self.N
nf = self.nf
# ORIENTATIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._ori_qp_is_initialized:
# ori_dofs = ( dddF_k_qR )
# ( dddF_k_qL )
self.ori_dofs[:N] = self.dddF_k_qR
self.ori_dofs[N:] = self.dddF_k_qL
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k_q )
self._update_ori_Q() # updates values in _Q
self.ori_H [:,:] = self._ori_Q
# g = ( p_k_q )
self._update_ori_p() # updates values in _p
self.ori_g [:] = self._ori_p
# ORIENTATION LINEAR CONSTRAINTS
# velocity constraints on support foot to freeze movement
a = 0
b = self.nc_fvel_eq
self.ori_A [a:b] = self.A_fvel_eq
self.ori_lbA[a:b] = self.B_fvel_eq
self.ori_ubA[a:b] = self.B_fvel_eq
# box constraints for maximum orientation change
a = self.nc_fvel_eq
b = self.nc_fvel_eq + self.nc_fpos_ineq
self.ori_A [a:b] = self.A_fpos_ineq
self.ori_lbA[a:b] = self.lbB_fpos_ineq
self.ori_ubA[a:b] = self.ubB_fpos_ineq
# box constraints for maximum angular velocity
a = self.nc_fvel_eq + self.nc_fpos_ineq
b = self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq
self.ori_A [a:b] = self.A_fvel_ineq
self.ori_lbA[a:b] = self.lbB_fvel_ineq
self.ori_ubA[a:b] = self.ubB_fvel_ineq
# ORIENTATION BOX CONSTRAINTS
#self.ori_lb [...] = 0.0
#self.ori_ub [...] = 0.0
# POSITIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._pos_qp_is_initialized:
# pos_dofs = ( dddC_kp1_x )
# ( F_k_x )
# ( dddC_kp1_y )
# ( F_k_y )
self.pos_dofs[0 :0+N ] = self.dddC_k_x
self.pos_dofs[0+N:0+N+nf] = self.F_k_x
a = N+nf
self.pos_dofs[a :a+N ] = self.dddC_k_y
self.pos_dofs[a+N:a+N+nf] = self.F_k_y
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k 0 )
# ( 0 Q_k )
self._update_pos_Q() # updates values in _Q
self.pos_H [ :N+nf, :N+nf] = self._pos_Q
self.pos_H [-N-nf:, -N-nf:] = self._pos_Q
# g = ( p_k_x )
# ( p_k_y )
self._update_pos_p('x') # updates values in _p
self.pos_g [ :N+nf] = self._pos_p
self._update_pos_p('y') # updates values in _p
self.pos_g [-N-nf:] = self._pos_p
# lbA <= A x <= ubA
# ( ) <= ( Acop ) <= ( Bcop )
# (eqBfoot) <= ( eqAfoot ) <= ( eqBfoot )
# ( ) <= ( Afoot ) <= ( Bfoot )
# CoP constraints
a = 0
b = self.nc_cop
self.pos_A [a:b] = self.Acop
self.pos_lbA[a:b] = self.lbBcop
self.pos_ubA[a:b] = self.ubBcop
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.pos_A [a:b] = self.Afoot
self.pos_lbA[a:b] = self.lbBfoot
self.pos_ubA[a:b] = self.ubBfoot
#foot equality constraints
a = self.nc_cop + self.nc_foot_position
b = self.nc_cop + self.nc_foot_position + self.nc_fchange_eq
self.pos_A [a:b] = self.eqAfoot
self.pos_lbA[a:b] = self.eqBfoot
self.pos_ubA[a:b] = self.eqBfoot
# NOTE: they stay plus minus infinity, i.e. 1e+08
#self.pos_lb [...] = 0.0
#self.pos_ub [...] = 0.0
def _update_ori_Q(self):
'''
Update Hessian block Q according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
Q = ( QR 0 )
( 0 QL )
QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
Pvu = self.Pvu
# assemble Hessian matrix
# QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
a = 0; b = N
c = 0; d = N
QR = self._ori_Q[a:b,c:d]
QR[...] = alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR).dot(Pvu)
# QL = ( a * Pvu^T * E_FL^T * E_FL * Pvu )
# Q = ( * , * )
# ( * ,[*])
a = N; b = 2*N
c = N; d = 2*N
QL = self._ori_Q[a:b,c:d]
QL[...] = alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL).dot(Pvu)
def _update_ori_p(self):
"""
Update pass gradient block p according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
p = ( pR )
( pL )
pR = ( a * Pvu^T * E_FR^T * (E_FR * Pvs * f_k_qR - dC_kp1_q_ref[N/2] )
"""
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
f_k_qR = self.f_k_qR
f_k_qL = self.f_k_qL
Pvs = self.Pvs
Pvu = self.Pvu
dC_kp1_q_ref = self.dC_kp1_q_ref
# pR = ( a * Pvu^T * E_FL^T * (E_FL * Pvs * f_k_qL + dC_kp1_q_ref) )
# p = ([*]) =
# ( * )
a = 0; b = N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR.dot(Pvs).dot(f_k_qR) - dC_kp1_q_ref)
# p = ( * ) =
# ([*])
a = N; b = 2*N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL.dot(Pvs).dot(f_k_qL) - dC_kp1_q_ref)
def _update_pos_Q(self):
'''
Update Hessian block Q according to walking report
Q = ( a*Pvu*Pvu + b*Ppu*E*T*E*Ppu + c*Pzu*Pzu + d*I, -c*Pzu*V_kp1 )
( -c*Pzu*V_kp1, c*V_kp1*V_kp1 )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# cast matrices for convenience
Ppu = numpy.asmatrix(self.Ppu)
Pvu = numpy.asmatrix(self.Pvu)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# Q = ([*], * ) = a*Pvu*Pvu + b*Ppu*E*E*Ppu + c*Pzu*Pzu + d*I
# ( * , * )
a = 0; b = N
c = 0; d = N
self._pos_Q[a:b,c:d] = alpha * Pvu.transpose() * Pvu \
+ gamma * Pzu.transpose() * Pzu \
+ delta * numpy.eye(N)
# Q = ( * ,[*])
# ( * , * )
a = 0; b = N
c = N; d = N+nf
self._pos_Q[a:b,c:d] = -gamma * Pzu.transpose() * V_kp1
# Q = ( * , * ) = ( * , [*] )^T
# ( [*], * ) ( * , * )
dummy = self._pos_Q[a:b,c:d]
a = N; b = N+nf
c = 0; d = N
self._pos_Q[a:b,c:d] = dummy.transpose()
# Q = ( * , * )
# ( * ,[*])
a = N; b = N+nf
c = N; d = N+nf
self._pos_Q[a:b,c:d] = gamma * V_kp1.transpose() * V_kp1
def _update_pos_p(self, case=None):
"""
Update pass gradient block p according to walking report
p = ( a*Pvu*(Pvs*ck - Refk+1) + b*Ppu*E*(E*Pps*cx - Refk+1) + c*Pzu*(Pzs*ck - vk+1*fk )
( -c*Vk+1*(Pzs*ck - vk+1*fk )
"""
if case == 'x':
f_k = self.f_k_x
c_k = utility.cast_array_as_matrix(self.c_k_x)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_x_ref)
elif case == 'y':
f_k = self.f_k_y
c_k = utility.cast_array_as_matrix(self.c_k_y)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_y_ref)
else:
err_str = 'Please use either case "x" or "y" for this routine'
raise AttributeError(err_str)
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
v_kp1 = utility.cast_array_as_matrix(self.v_kp1)
Pvs = numpy.asmatrix(self.Pvs)
Pvu = numpy.asmatrix(self.Pvu)
Pps = numpy.asmatrix(self.Pps)
Ppu = numpy.asmatrix(self.Ppu)
Pzs = numpy.asmatrix(self.Pzs)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# p = ([*]) =
# ( * )
a = 0; b = N
self._pos_p[a:b] = (
alpha * Pvu.transpose() *(Pvs*c_k - dC_kp1_ref)
+ gamma * Pzu.transpose() *(Pzs*c_k - v_kp1*f_k)
#+ b*Ppu.transpose() * E.transpose() * E * Ppu \
).ravel()
# p = ( * ) =
# ([*])
a = N; b = N+nf
self._pos_p[a:b] = (
-gamma * V_kp1.transpose() * (Pzs*c_k - v_kp1*f_k)
).ravel()
def _solve_qp(self):
"""
Solve QP first run with init functionality and other runs with warmstart
"""
#sys.stdout.write('Solve for orientations:\n')
if not self._ori_qp_is_initialized:
ret, nwsr, cputime = self.ori_qp.init(
self.ori_H, self.ori_g, self.ori_A,
self.ori_lb, self.ori_ub,
self.ori_lbA, self.ori_ubA,
self.nwsr, self.cpu_time
)
self._ori_qp_is_initialized = True
else:
ret, nwsr, cputime = self.ori_qp.hotstart(
self.ori_H, self.ori_g, self.ori_A,
self.ori_lb, self.ori_ub,
self.ori_lbA, self.ori_ubA,
self.nwsr, self.cpu_time
)
# orientation primal solution
self.ori_qp.getPrimalSolution(self.ori_dofs)
# save qp solver data
self.ori_qp_nwsr = nwsr # working set recalculations
self.ori_qp_cputime = cputime*1000. # in milliseconds
#sys.stdout.write('Solve for positions:\n')
if not self._pos_qp_is_initialized:
ret, nwsr, cputime = self.pos_qp.init(
self.pos_H, self.pos_g, self.pos_A,
self.pos_lb, self.pos_ub,
self.pos_lbA, self.pos_ubA,
self.nwsr, self.cpu_time
)
self._pos_qp_is_initialized = True
else:
ret, nwsr, cputime = self.pos_qp.hotstart(
self.pos_H, self.pos_g, self.pos_A,
self.pos_lb, self.pos_ub,
self.pos_lbA, self.pos_ubA,
self.nwsr, self.cpu_time
)
# position primal solution
self.pos_qp.getPrimalSolution(self.pos_dofs)
# save qp solver data
self.pos_qp_nwsr = nwsr # working set recalculations
self.pos_qp_cputime = cputime*1000. # in milliseconds
def _postprocess_solution(self):
""" Get solution and put it back into generator data structures """
# rename for convenience
N = self.N
nf = self.nf
# extract dofs
# ori_dofs = ( dddF_k_qR )
# ( dddF_k_qL )
self.dddF_k_qR[:] = self.ori_dofs[:N]
self.dddF_k_qL[:] = self.ori_dofs[N:]
# extract dofs
# pos_dofs = ( dddC_kp1_x )
# ( F_k_x )
# ( dddC_kp1_y )
# ( F_k_y )
self.dddC_k_x[:] = self.pos_dofs[0 :0+N ]
self.F_k_x[:] = self.pos_dofs[0+N:0+N+nf]
a = N + nf
self.dddC_k_y[:] = self.pos_dofs[a :a+N ]
self.F_k_y[:] = self.pos_dofs[a+N:a+N+nf]
class ClassicGenerator(BaseGenerator):
"""
Reimplementation of current state-of-the-art pattern
generator for HRP-2 of CNRS-LAAS, Toulouse.
Solve QPs for position and orientation of CoM and feet
independently of each other in each timestep.
First solve for orientations, then solve for the postions.
"""
def __init__(self, N=16, T=0.1, T_step=0.8, fsm_state='D', fsm_sl=1):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(ClassicGenerator, self).__init__(N, T, T_step, fsm_state, fsm_sl)
# TODO for speed up one can define members of BaseGenerator as
# direct views of QP data structures according to walking report
# Maybe do that later!
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # number of working set recalculations
self.options = Options()
self.options.setToMPC()
self.options.printLevel = PrintLevel.LOW
# FOR ORIENTATIONS
# define dimensions
self.ori_nv = 2 * self.N
self.ori_nc = (self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq)
# setup problem
self.ori_dofs = numpy.zeros(self.ori_nv)
self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
self.ori_qp.setOptions(self.options) # load NMPC options
self.ori_H = numpy.zeros((self.ori_nv, self.ori_nv))
self.ori_A = numpy.zeros((self.ori_nc, self.ori_nv))
self.ori_g = numpy.zeros((self.ori_nv, ))
self.ori_lb = -numpy.ones((self.ori_nv, )) * 1e+08
self.ori_ub = numpy.ones((self.ori_nv, )) * 1e+08
self.ori_lbA = -numpy.ones((self.ori_nc, )) * 1e+08
self.ori_ubA = numpy.ones((self.ori_nc, )) * 1e+08
self._ori_qp_is_initialized = False
# save computation time and working set recalculations
self.ori_qp_nwsr = 0.0
self.ori_qp_cputime = 0.0
# FOR POSITIONS
# define dimensions
self.pos_nv = 2 * (self.N + self.nf)
self.pos_nc = (self.nc_cop + self.nc_foot_position +
self.nc_fchange_eq)
# setup problem
self.pos_dofs = numpy.zeros(self.pos_nv)
self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
self.pos_qp.setOptions(self.options)
self.pos_H = numpy.zeros((self.pos_nv, self.pos_nv))
self.pos_A = numpy.zeros((self.pos_nc, self.pos_nv))
self.pos_g = numpy.zeros((self.pos_nv, ))
self.pos_lb = -numpy.ones((self.pos_nv, )) * 1e+08
self.pos_ub = numpy.ones((self.pos_nv, )) * 1e+08
self.pos_lbA = -numpy.ones((self.pos_nc, )) * 1e+08
self.pos_ubA = numpy.ones((self.pos_nc, )) * 1e+08
self._pos_qp_is_initialized = False
# save computation time and working set recalculations
self.pos_qp_nwsr = 0.0
self.pos_qp_cputime = 0.0
# dummy matrices
self._ori_Q = numpy.zeros((2 * self.N, 2 * self.N))
self._ori_p = numpy.zeros((2 * self.N, ))
self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf))
self._pos_p = numpy.zeros((self.N + self.nf, ))
# add additional keys that should be saved
self._data_keys.append('ori_qp_nwsr')
self._data_keys.append('ori_qp_cputime')
self._data_keys.append('pos_qp_nwsr')
self._data_keys.append('pos_qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def solve(self):
""" Process and solve problem, s.t. pattern generator data is consistent """
self._preprocess_solution()
self._solve_qp()
self._postprocess_solution()
def _preprocess_solution(self):
""" Update matrices and get them into the QP data structures """
# rename for convenience
N = self.N
nf = self.nf
# ORIENTATIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._ori_qp_is_initialized:
# ori_dofs = ( dddF_k_qR )
# ( dddF_k_qL )
self.ori_dofs[:N] = self.dddF_k_qR
self.ori_dofs[N:] = self.dddF_k_qL
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k_q )
self._update_ori_Q() # updates values in _Q
self.ori_H[:, :] = self._ori_Q
# g = ( p_k_q )
self._update_ori_p() # updates values in _p
self.ori_g[:] = self._ori_p
# ORIENTATION LINEAR CONSTRAINTS
# velocity constraints on support foot to freeze movement
a = 0
b = self.nc_fvel_eq
self.ori_A[a:b] = self.A_fvel_eq
self.ori_lbA[a:b] = self.B_fvel_eq
self.ori_ubA[a:b] = self.B_fvel_eq
# box constraints for maximum orientation change
a = self.nc_fvel_eq
b = self.nc_fvel_eq + self.nc_fpos_ineq
self.ori_A[a:b] = self.A_fpos_ineq
self.ori_lbA[a:b] = self.lbB_fpos_ineq
self.ori_ubA[a:b] = self.ubB_fpos_ineq
# box constraints for maximum angular velocity
a = self.nc_fvel_eq + self.nc_fpos_ineq
b = self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq
self.ori_A[a:b] = self.A_fvel_ineq
self.ori_lbA[a:b] = self.lbB_fvel_ineq
self.ori_ubA[a:b] = self.ubB_fvel_ineq
# ORIENTATION BOX CONSTRAINTS
#self.ori_lb [...] = 0.0
#self.ori_ub [...] = 0.0
# POSITIONS
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._pos_qp_is_initialized:
# pos_dofs = ( dddC_kp1_x )
# ( F_k_x )
# ( dddC_kp1_y )
# ( F_k_y )
self.pos_dofs[0:0 + N] = self.dddC_k_x
self.pos_dofs[0 + N:0 + N + nf] = self.F_k_x
a = N + nf
self.pos_dofs[a:a + N] = self.dddC_k_y
self.pos_dofs[a + N:a + N + nf] = self.F_k_y
# TODO guess initial active set
# this requires changes to the python interface
# define QP matrices
# H = ( Q_k 0 )
# ( 0 Q_k )
self._update_pos_Q() # updates values in _Q
self.pos_H[:N + nf, :N + nf] = self._pos_Q
self.pos_H[-N - nf:, -N - nf:] = self._pos_Q
# g = ( p_k_x )
# ( p_k_y )
self._update_pos_p('x') # updates values in _p
self.pos_g[:N + nf] = self._pos_p
self._update_pos_p('y') # updates values in _p
self.pos_g[-N - nf:] = self._pos_p
# lbA <= A x <= ubA
# ( ) <= ( Acop ) <= ( Bcop )
# (eqBfoot) <= ( eqAfoot ) <= ( eqBfoot )
# ( ) <= ( Afoot ) <= ( Bfoot )
# CoP constraints
a = 0
b = self.nc_cop
self.pos_A[a:b] = self.Acop
self.pos_lbA[a:b] = self.lbBcop
self.pos_ubA[a:b] = self.ubBcop
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.pos_A[a:b] = self.Afoot
self.pos_lbA[a:b] = self.lbBfoot
self.pos_ubA[a:b] = self.ubBfoot
#foot equality constraints
a = self.nc_cop + self.nc_foot_position
b = self.nc_cop + self.nc_foot_position + self.nc_fchange_eq
self.pos_A[a:b] = self.eqAfoot
self.pos_lbA[a:b] = self.eqBfoot
self.pos_ubA[a:b] = self.eqBfoot
# NOTE: they stay plus minus infinity, i.e. 1e+08
#self.pos_lb [...] = 0.0
#self.pos_ub [...] = 0.0
def _update_ori_Q(self):
'''
Update Hessian block Q according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
Q = ( QR 0 )
( 0 QL )
QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
Pvu = self.Pvu
# assemble Hessian matrix
# QR = ( a * Pvu^T * E_FR^T * E_FR * Pvu )
a = 0
b = N
c = 0
d = N
QR = self._ori_Q[a:b, c:d]
QR[...] = alpha * Pvu.transpose().dot(
E_FR.transpose()).dot(E_FR).dot(Pvu)
# QL = ( a * Pvu^T * E_FL^T * E_FL * Pvu )
# Q = ( * , * )
# ( * ,[*])
a = N
b = 2 * N
c = N
d = 2 * N
QL = self._ori_Q[a:b, c:d]
QL[...] = alpha * Pvu.transpose().dot(
E_FL.transpose()).dot(E_FL).dot(Pvu)
def _update_ori_p(self):
"""
Update pass gradient block p according to walking report
min a/2 || dC_kp1_q_ref - dF_k_q ||_2^2
p = ( pR )
( pL )
pR = ( a * Pvu^T * E_FR^T * (E_FR * Pvs * f_k_qR - dC_kp1_q_ref[N/2] )
"""
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
f_k_qR = self.f_k_qR
f_k_qL = self.f_k_qL
Pvs = self.Pvs
Pvu = self.Pvu
dC_kp1_q_ref = self.dC_kp1_q_ref
# pR = ( a * Pvu^T * E_FL^T * (E_FL * Pvs * f_k_qL + dC_kp1_q_ref) )
# p = ([*]) =
# ( * )
a = 0
b = N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR.dot(Pvs).dot(f_k_qR) - dC_kp1_q_ref)
# p = ( * ) =
# ([*])
a = N
b = 2 * N
self._ori_p[a:b] = \
alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL.dot(Pvs).dot(f_k_qL) - dC_kp1_q_ref)
def _update_pos_Q(self):
'''
Update Hessian block Q according to walking report
Q = ( a*Pvu*Pvu + b*Ppu*E*T*E*Ppu + c*Pzu*Pzu + d*I, -c*Pzu*V_kp1 )
( -c*Pzu*V_kp1, c*V_kp1*V_kp1 )
'''
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# cast matrices for convenience
Ppu = numpy.asmatrix(self.Ppu)
Pvu = numpy.asmatrix(self.Pvu)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# Q = ([*], * ) = a*Pvu*Pvu + b*Ppu*E*E*Ppu + c*Pzu*Pzu + d*I
# ( * , * )
a = 0
b = N
c = 0
d = N
self._pos_Q[a:b,c:d] = alpha * Pvu.transpose() * Pvu \
+ gamma * Pzu.transpose() * Pzu \
+ delta * numpy.eye(N)
# Q = ( * ,[*])
# ( * , * )
a = 0
b = N
c = N
d = N + nf
self._pos_Q[a:b, c:d] = -gamma * Pzu.transpose() * V_kp1
# Q = ( * , * ) = ( * , [*] )^T
# ( [*], * ) ( * , * )
dummy = self._pos_Q[a:b, c:d]
a = N
b = N + nf
c = 0
d = N
self._pos_Q[a:b, c:d] = dummy.transpose()
# Q = ( * , * )
# ( * ,[*])
a = N
b = N + nf
c = N
d = N + nf
self._pos_Q[a:b, c:d] = gamma * V_kp1.transpose() * V_kp1
def _update_pos_p(self, case=None):
"""
Update pass gradient block p according to walking report
p = ( a*Pvu*(Pvs*ck - Refk+1) + b*Ppu*E*(E*Pps*cx - Refk+1) + c*Pzu*(Pzs*ck - vk+1*fk )
( -c*Vk+1*(Pzs*ck - vk+1*fk )
"""
if case == 'x':
f_k = self.f_k_x
c_k = utility.cast_array_as_matrix(self.c_k_x)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_x_ref)
elif case == 'y':
f_k = self.f_k_y
c_k = utility.cast_array_as_matrix(self.c_k_y)
dC_kp1_ref = utility.cast_array_as_matrix(self.dC_kp1_y_ref)
else:
err_str = 'Please use either case "x" or "y" for this routine'
raise AttributeError(err_str)
# rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
# matrices
v_kp1 = utility.cast_array_as_matrix(self.v_kp1)
Pvs = numpy.asmatrix(self.Pvs)
Pvu = numpy.asmatrix(self.Pvu)
Pps = numpy.asmatrix(self.Pps)
Ppu = numpy.asmatrix(self.Ppu)
Pzs = numpy.asmatrix(self.Pzs)
Pzu = numpy.asmatrix(self.Pzu)
V_kp1 = numpy.asmatrix(self.V_kp1)
# p = ([*]) =
# ( * )
a = 0
b = N
self._pos_p[a:b] = (
alpha * Pvu.transpose() * (Pvs * c_k - dC_kp1_ref) +
gamma * Pzu.transpose() * (Pzs * c_k - v_kp1 * f_k)
#+ b*Ppu.transpose() * E.transpose() * E * Ppu \
).ravel()
# p = ( * ) =
# ([*])
a = N
b = N + nf
self._pos_p[a:b] = (-gamma * V_kp1.transpose() *
(Pzs * c_k - v_kp1 * f_k)).ravel()
class NMPCGenerator(BaseGenerator):
"""
Implementation of the combined problems using NMPC techniques.
Solve QP for position and orientation of CoM and feet simultaneously in
each timestep. Calculates derivatives and updates states in each step.
"""
def __init__(self, N=16, T=0.1, T_step=0.8, fsm_state='D', fsm_sl=1):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(NMPCGenerator, self).__init__(N, T, T_step, fsm_state, fsm_sl)
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x.T * H(w0) * x + x.T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # # of working set recalculations
self.options = Options()
self.options.setToMPC()
#self.options.printLevel = PrintLevel.LOW
# define variable dimensions
# variables of: position + orientation
self.nv = 2 * (self.N + self.nf) + 2 * N
# define constraint dimensions
self.nc_pos = (self.nc_cop + self.nc_foot_position +
self.nc_fchange_eq)
self.nc_ori = (self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq)
self.nc = (
# position
self.nc_pos
# orientation
+ self.nc_ori)
# setup problem
self.dofs = numpy.zeros(self.nv)
self.qp = SQProblem(self.nv, self.nc)
# load NMPC options
self.qp.setOptions(self.options)
self.qp_H = numpy.eye(self.nv, self.nv)
self.qp_A = numpy.zeros((self.nc, self.nv))
self.qp_g = numpy.zeros((self.nv, ))
self.qp_lb = -numpy.ones((self.nv, )) * 1e+08
self.qp_ub = numpy.ones((self.nv, )) * 1e+08
self.qp_lbA = -numpy.ones((self.nc, )) * 1e+08
self.qp_ubA = numpy.ones((self.nc, )) * 1e+08
self._qp_is_initialized = False
# save computation time and working set recalculations
self.qp_nwsr = 0.0
self.qp_cputime = 0.0
# setup analyzer for solution analysis
analyser = SolutionAnalysis()
# helper matrices for common expressions
self.Hx = numpy.zeros((1, 2 * (N + nf)), dtype=float)
self.Q_k_x = numpy.zeros((N + nf, N + nf), dtype=float)
self.p_k_x = numpy.zeros((N + nf, ), dtype=float)
self.p_k_y = numpy.zeros((N + nf, ), dtype=float)
self.Hq = numpy.zeros((1, 2 * N), dtype=float)
self.Q_k_qR = numpy.zeros((N, N), dtype=float)
self.Q_k_qL = numpy.zeros((N, N), dtype=float)
self.p_k_qR = numpy.zeros((N), dtype=float)
self.p_k_qL = numpy.zeros((N), dtype=float)
self.A_pos_x = numpy.zeros((self.nc_pos, 2 * (N + nf)), dtype=float)
self.A_pos_q = numpy.zeros((self.nc_pos, 2 * N), dtype=float)
self.ubA_pos = numpy.zeros((self.nc_pos, ), dtype=float)
self.lbA_pos = numpy.zeros((self.nc_pos, ), dtype=float)
self.A_ori = numpy.zeros((self.nc_ori, 2 * N), dtype=float)
self.ubA_ori = numpy.zeros((self.nc_ori, ), dtype=float)
self.lbA_ori = numpy.zeros((self.nc_ori, ), dtype=float)
self.derv_Acop_map = numpy.zeros((self.nc_cop, self.N), dtype=float)
self.derv_Afoot_map = numpy.zeros((self.nc_foot_position, self.N),
dtype=float)
self._update_foot_selection_matrix()
# add additional keys that should be saved
self._data_keys.append('qp_nwsr')
self._data_keys.append('qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def solve(self):
""" Process and solve problem, s.t. pattern generator data is consistent """
self._preprocess_solution()
self._solve_qp()
self._postprocess_solution()
def _preprocess_solution(self):
""" Update matrices and get them into the QP data structures """
# rename for convenience
N = self.N
nf = self.nf
# dofs
dofs = self.dofs
dddC_k_x = self.dddC_k_x
F_k_x = self.F_k_x
dddC_k_y = self.dddC_k_y
F_k_y = self.F_k_y
dddF_k_qR = self.dddF_k_qR
dddF_k_qL = self.dddF_k_qL
# inject dofs for convenience
# dofs = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# ( dddF_k_q ) N
# ( dddF_k_q ) N
dofs[0:0 + N] = dddC_k_x
dofs[0 + N:0 + N + nf] = F_k_x
a = N + nf
dofs[a:a + N] = dddC_k_y
dofs[a + N:a + N + nf] = F_k_y
a = 2 * (N + nf)
dofs[a:a + N] = dddF_k_qR
dofs[-N:] = dddF_k_qL
# define position and orientation dofs
# U_k = ( U_k_xy, U_k_q).T
# U_k_xy = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# U_k_q = ( dddF_k_q ) N
# ( dddF_k_q ) N
# position dofs
U_k = self.dofs
U_k_xy = U_k[:2 * (N + nf)]
U_k_x = U_k_xy[:(N + nf)]
U_k_y = U_k_xy[(N + nf):]
# orientation dofs
U_k_q = U_k[-2 * N:]
U_k_qR = U_k_q[:N]
U_k_qL = U_k_q[N:]
# position dimensions
nU_k = U_k.shape[0]
nU_k_xy = U_k_xy.shape[0]
nU_k_x = U_k_x.shape[0]
nU_k_y = U_k_y.shape[0]
# orientation dimensions
nU_k_q = U_k_q.shape[0]
nU_k_qR = U_k_qR.shape[0]
nU_k_qL = U_k_qL.shape[0]
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._qp_is_initialized:
# TODO guess initial active set
# this requires changes to the python interface
pass
# calculate some common sub expressions
self._calculate_common_expressions()
# calculate Jacobian parts that are non-trivial, i.e. wrt. to orientation
self._calculate_derivatives()
# POSITION QP
# rename matrices
Q_k_x = self.Q_k_x
Q_k_y = self.Q_k_x # NOTE it's exactly the same!
p_k_x = self.p_k_x
p_k_y = self.p_k_y
# ORIENTATION QP
# rename matrices
Q_k_qR = self.Q_k_qR
Q_k_qL = self.Q_k_qL
p_k_qR = self.p_k_qR
p_k_qL = self.p_k_qL
# define QP matrices
# Gauss-Newton Hessian approximation
# define sub blocks
# H = (Hxy | Hqx)
# (Hxq | Hqq)
Hxx = self.qp_H[:nU_k_xy, :nU_k_xy]
Hxq = self.qp_H[:nU_k_xy, nU_k_xy:]
Hqx = self.qp_H[nU_k_xy:, :nU_k_xy]
Hqq = self.qp_H[nU_k_xy:, nU_k_xy:]
# fill matrices
Hxx[:nU_k_x, :nU_k_x] = Q_k_x
Hxx[-nU_k_y:, -nU_k_y:] = Q_k_y
Hqq[:nU_k_qR, :nU_k_qR] = Q_k_qR
Hqq[-nU_k_qL:, -nU_k_qL:] = Q_k_qL
#self.qp_H[...] = numpy.eye(self.nv)
# Gradient of Objective
# define sub blocks
# g = (gx)
# (gq)
gx = self.qp_g[:nU_k_xy]
gq = self.qp_g[-nU_k_q:]
# gx = ( U_k_x.T Q_k_x + p_k_x )
gx[:nU_k_x] = U_k_x.dot(Q_k_x) + p_k_x
gx[-nU_k_y:] = U_k_y.dot(Q_k_y) + p_k_y
# gq = ( U_k_q.T Q_k_q + p_k_q )
gq[:nU_k_qR] = U_k_qR.dot(Q_k_qR) + p_k_qR
gq[-nU_k_qL:] = U_k_qL.dot(Q_k_qL) + p_k_qL
# CONSTRAINTS
# A = ( A_xy, A_xyq )
# ( 0, A_q )
A_xy = self.qp_A[:self.nc_pos, :nU_k_xy]
A_xyq = self.qp_A[:self.nc_pos, -nU_k_q:]
lbA_xy = self.qp_lbA[:self.nc_pos]
ubA_xy = self.qp_ubA[:self.nc_pos]
A_q = self.qp_A[-self.nc_ori:, -nU_k_q:]
lbA_q = self.qp_lbA[-self.nc_ori:]
ubA_q = self.qp_ubA[-self.nc_ori:]
# linearized constraints are given by
# lbA - A * U_k <= nablaA * Delta_U_k <= ubA - A * U_k
A_xy[...] = self.A_pos_x
A_xyq[...] = self.A_pos_q
lbA_xy[...] = self.lbA_pos - self.A_pos_x.dot(U_k_xy)
ubA_xy[...] = self.ubA_pos - self.A_pos_x.dot(U_k_xy)
A_q[...] = self.A_ori
lbA_q[...] = self.lbA_ori - self.A_ori.dot(U_k_q)
ubA_q[...] = self.ubA_ori - self.A_ori.dot(U_k_q)
def _calculate_common_expressions(self):
"""
encapsulation of complicated matrix assembly of former orientation and
position QP sub matrices
"""
#rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
Pvs = self.Pvs
Pvu = self.Pvu
Pzs = self.Pzs
Pzu = self.Pzu
c_k_x = self.c_k_x
c_k_y = self.c_k_y
f_k_x = self.f_k_x
f_k_y = self.f_k_y
f_k_qR = self.f_k_qR
f_k_qL = self.f_k_qL
v_kp1 = self.v_kp1
V_kp1 = self.V_kp1
dC_kp1_x_ref = self.dC_kp1_x_ref
dC_kp1_y_ref = self.dC_kp1_y_ref
dC_kp1_q_ref = self.dC_kp1_q_ref
# POSITION QP MATRICES
# Q_k_x = ( Q_k_xXX Q_k_xXF ) = Q_k_y
# ( Q_k_xFX Q_k_xFF )
Q_k_x = self.Q_k_x
a = 0
b = N
c = 0
d = N
Q_k_xXX = Q_k_x[a:b, c:d]
a = 0
b = N
c = N
d = N + nf
Q_k_xXF = Q_k_x[a:b, c:d]
a = N
b = N + nf
c = 0
d = N
Q_k_xFX = Q_k_x[a:b, c:d]
a = N
b = N + nf
c = N
d = N + nf
Q_k_xFF = Q_k_x[a:b, c:d]
# Q_k_xXX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# Q_k_xXF = ( -0.5 * c * Pzu^T * V_kp1 )
# Q_k_xFX = ( -0.5 * c * Pzu^T * V_kp1 )^T
# Q_k_xFF = ( 0.5 * c * V_kp1^T * V_kp1 )
Q_k_xXX[...] = (alpha * Pvu.transpose().dot(Pvu) +
gamma * Pzu.transpose().dot(Pzu) +
delta * numpy.eye(N))
Q_k_xXF[...] = -gamma * Pzu.transpose().dot(V_kp1)
Q_k_xFX[...] = Q_k_xXF.transpose()
Q_k_xFF[...] = gamma * V_kp1.transpose().dot(V_kp1)
# p_k_x = ( p_k_xX )
# ( p_k_xF )
p_k_x = self.p_k_x
p_k_xX = p_k_x[:N]
p_k_xF = p_k_x[-nf:]
# p_k_xX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# p_k_xF = ( -0.5 * c * Pzu^T * V_kp1 )
p_k_xX[...] = alpha * Pvu.transpose().dot( Pvs.dot(c_k_x) - dC_kp1_x_ref) \
+ gamma * Pzu.transpose().dot( Pzs.dot(c_k_x) - v_kp1.dot(f_k_x))
p_k_xF[...] = -gamma * V_kp1.transpose().dot(
Pzs.dot(c_k_x) - v_kp1.dot(f_k_x))
# p_k_y = ( p_k_yX )
# ( p_k_yF )
p_k_y = self.p_k_y
p_k_yX = p_k_y[:N]
p_k_yF = p_k_y[-nf:]
# p_k_yX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# p_k_yF = ( -0.5 * c * Pzu^T * V_kp1 )
p_k_yX[...] = alpha * Pvu.transpose().dot( Pvs.dot(c_k_y) - dC_kp1_y_ref) \
+ gamma * Pzu.transpose().dot( Pzs.dot(c_k_y) - v_kp1.dot(f_k_y))
p_k_yF[...] = -gamma * V_kp1.transpose().dot(
Pzs.dot(c_k_y) - v_kp1.dot(f_k_y))
# ORIENTATION QP MATRICES
# Q_k_qR = ( 0.5 * a * Pvu^T * E_FR^T * E_FR * Pvu )
Q_k_qR = self.Q_k_qR
Q_k_qR[...] = alpha * Pvu.transpose().dot(
E_FR.transpose()).dot(E_FR).dot(Pvu)
# p_k_qR = ( a * Pvu^T * E_FR^T * (E_FR * Pvs * f_k_qR + dC_kp1_q_ref) )
p_k_qR = self.p_k_qR
p_k_qR[...] = alpha * Pvu.transpose().dot(
E_FR.transpose()).dot(E_FR.dot(Pvs).dot(f_k_qR) - dC_kp1_q_ref)
# Q_k_qL = ( 0.5 * a * Pvu^T * E_FL^T * E_FL * Pvu )
Q_k_qL = self.Q_k_qL
Q_k_qL[...] = alpha * Pvu.transpose().dot(
E_FL.transpose()).dot(E_FL).dot(Pvu)
# p_k_qL = ( a * Pvu^T * E_FL^T * (E_FL * Pvs * f_k_qL + dC_kp1_q_ref) )
p_k_qL = self.p_k_qL
p_k_qL[...] = alpha * Pvu.transpose().dot(
E_FL.transpose()).dot(E_FL.dot(Pvs).dot(f_k_qL) - dC_kp1_q_ref)
# LINEAR CONSTRAINTS
# CoP constraints
a = 0
b = self.nc_cop
self.A_pos_x[a:b] = self.Acop
self.lbA_pos[a:b] = self.lbBcop
self.ubA_pos[a:b] = self.ubBcop
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.A_pos_x[a:b] = self.Afoot
self.lbA_pos[a:b] = self.lbBfoot
self.ubA_pos[a:b] = self.ubBfoot
#foot equality constraints
a = self.nc_cop + self.nc_foot_position
b = self.nc_cop + self.nc_foot_position + self.nc_fchange_eq
self.A_pos_x[a:b] = self.eqAfoot
self.lbA_pos[a:b] = self.eqBfoot
self.ubA_pos[a:b] = self.eqBfoot
# velocity constraints on support foot to freeze movement
a = 0
b = self.nc_fvel_eq
self.A_ori[a:b] = self.A_fvel_eq
self.lbA_ori[a:b] = self.B_fvel_eq
self.ubA_ori[a:b] = self.B_fvel_eq
# box constraints for maximum orientation change
a = self.nc_fvel_eq
b = self.nc_fvel_eq + self.nc_fpos_ineq
self.A_ori[a:b] = self.A_fpos_ineq
self.lbA_ori[a:b] = self.lbB_fpos_ineq
self.ubA_ori[a:b] = self.ubB_fpos_ineq
# box constraints for maximum angular velocity
a = self.nc_fvel_eq + self.nc_fpos_ineq
b = self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq
self.A_ori[a:b] = self.A_fvel_ineq
self.lbA_ori[a:b] = self.lbB_fvel_ineq
self.ubA_ori[a:b] = self.ubB_fvel_ineq
def _calculate_derivatives(self):
""" calculate the Jacobian of the constraints function """
# COP CONSTRAINTS
# build the constraint enforcing the center of pressure to stay inside
# the support polygon given through the convex hull of the foot.
# define dummy values
# D_kp1 = (D_kp1x, Dkp1_y)
D_kp1 = numpy.zeros((self.nFootEdge * self.N, 2 * self.N), dtype=float)
D_kp1x = D_kp1[:, :self.N] # view on big matrix
D_kp1y = D_kp1[:, -self.N:] # view on big matrix
b_kp1 = numpy.zeros((self.nFootEdge * self.N, ), dtype=float)
# change entries according to support order changes in D_kp1
theta_vec = [self.f_k_q, self.F_k_q[0], self.F_k_q[1]]
for i in range(self.N):
theta = theta_vec[self.supportDeque[i].stepNumber]
# NOTE THIS CHANGES DUE TO APPLYING THE DERIVATIVE!
rotMat = numpy.array([
# old
# [ cos(theta), sin(theta)],
# [-sin(theta), cos(theta)]
# new: derivative wrt to theta
[-numpy.sin(theta), numpy.cos(theta)],
[-numpy.cos(theta), -numpy.sin(theta)]
])
if self.supportDeque[i].foot == "left":
A0 = self.A0lf.dot(rotMat)
B0 = self.ubB0lf
D0 = self.A0dlf.dot(rotMat)
d0 = self.ubB0dlf
else:
A0 = self.A0rf.dot(rotMat)
B0 = self.ubB0rf
D0 = self.A0drf.dot(rotMat)
d0 = self.ubB0drf
# get support foot and check if it is double support
for j in range(self.nf):
if self.V_kp1[i, j] == 1:
if self.fsm_states[j] == 'D':
A0 = D0
B0 = d0
else:
pass
for k in range(self.nFootEdge):
# get d_i+1^x(f^theta)
D_kp1x[i * self.nFootEdge + k, i] = A0[k][0]
# get d_i+1^y(f^theta)
D_kp1y[i * self.nFootEdge + k, i] = A0[k][1]
# get right hand side of equation
b_kp1[i * self.nFootEdge + k] = B0[k]
#rename for convenience
N = self.N
nf = self.nf
PzuV = self.PzuV
PzuVx = self.PzuVx
PzuVy = self.PzuVy
PzsC = self.PzsC
PzsCx = self.PzsCx
PzsCy = self.PzsCy
v_kp1fc = self.v_kp1fc
v_kp1fc_x = self.v_kp1fc_x
v_kp1fc_y = self.v_kp1fc_y
# build constraint transformation matrices
# PzuV = ( PzuVx )
# ( PzuVy )
# PzuVx = ( Pzu | -V_kp1 | 0 | 0 )
PzuVx[:, :self.
N] = self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
PzuVx[:, self.N:self.N + self.nf] = -self.V_kp1
# PzuVy = ( 0 | 0 | Pzu | -V_kp1 )
PzuVy[:, -self.N - self.nf:-self.
nf] = self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
PzuVy[:, -self.nf:] = -self.V_kp1
# PzuV = ( PzsCx ) = ( Pzs * c_k_x)
# ( PzsCy ) ( Pzs * c_k_y)
PzsCx[...] = self.Pzs.dot(self.c_k_x) #+ self.v_kp1.dot(self.f_k_x)
PzsCy[...] = self.Pzs.dot(self.c_k_y) #+ self.v_kp1.dot(self.f_k_y)
# v_kp1fc = ( v_kp1fc_x ) = ( v_kp1 * f_k_x)
# ( v_kp1fc_y ) ( v_kp1 * f_k_y)
v_kp1fc_x[...] = self.v_kp1.dot(self.f_k_x)
v_kp1fc_y[...] = self.v_kp1.dot(self.f_k_y)
# build CoP linear constraints
# NOTE D_kp1 is member and D_kp1 = ( D_kp1x | D_kp1y )
# D_kp1x,y contains entries from support polygon
dummy = D_kp1.dot(PzuV)
dummy = dummy.dot(self.dofs[:2 * (N + nf)])
# CoP constraints
a = 0
b = self.nc_cop
self.A_pos_q[a:b, :N] = \
dummy.dot(self.derv_Acop_map).dot(self.E_FR_bar).dot(self.Ppu)
self.A_pos_q[a:b,-N:] = \
dummy.dot(self.derv_Acop_map).dot(self.E_FL_bar).dot(self.Ppu)
# FOOT POSITION CONSTRAINTS
# defined on the horizon
# inequality constraint on both feet A u + B <= 0
# A0 R(theta) [Fx_k+1 - Fx_k] <= ubB0
# [Fy_k+1 - Fy_k]
matSelec = numpy.array([[1, 0], [-1, 1]])
footSelec = numpy.array([[self.f_k_x, 0], [self.f_k_y, 0]])
theta_vec = [self.f_k_q, self.F_k_q[0]]
# rotation matrice from F_k+1 to F_k
# NOTE THIS CHANGES DUE TO APPLYING THE DERIVATIVE!
rotMat1 = numpy.array([
# old
# [cos(theta_vec[0]), sin(theta_vec[0])],
# [-sin(theta_vec[0]), cos(theta_vec[0])]
# new: derivative wrt to theta
[-numpy.sin(theta_vec[0]),
numpy.cos(theta_vec[0])],
[-numpy.cos(theta_vec[0]), -numpy.sin(theta_vec[0])]
])
rotMat2 = numpy.array([
# old
# [cos(theta_vec[1]), sin(theta_vec[1])],
# [-sin(theta_vec[1]), cos(theta_vec[1])]
# new
[-numpy.sin(theta_vec[1]),
numpy.cos(theta_vec[1])],
[-numpy.cos(theta_vec[1]), -numpy.sin(theta_vec[1])]
])
nf = self.nf
nEdges = self.A0l.shape[0]
N = self.N
ncfoot = nf * nEdges
if self.currentSupport.foot == "left":
A_f1 = self.A0r.dot(rotMat1)
A_f2 = self.A0l.dot(rotMat2)
else:
A_f1 = self.A0l.dot(rotMat1)
A_f2 = self.A0r.dot(rotMat2)
tmp1 = numpy.array([A_f1[:, 0], numpy.zeros((nEdges, ), dtype=float)])
tmp2 = numpy.array([numpy.zeros((nEdges, ), dtype=float), A_f2[:, 0]])
tmp3 = numpy.array([A_f1[:, 1], numpy.zeros((nEdges, ), dtype=float)])
tmp4 = numpy.array([numpy.zeros(nEdges, ), A_f2[:, 1]])
X_mat = numpy.concatenate((tmp1.T, tmp2.T), 0)
A0x = X_mat.dot(matSelec)
Y_mat = numpy.concatenate((tmp3.T, tmp4.T), 0)
A0y = Y_mat.dot(matSelec)
dummy = numpy.concatenate((numpy.zeros(
(ncfoot, N),
dtype=float), A0x, numpy.zeros((ncfoot, N), dtype=float), A0y), 1)
dummy = dummy.dot(self.dofs[:2 * (N + nf)])
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.A_pos_q[a:b, :N] = dummy.dot(self.derv_Afoot_map).dot(
self.E_FR_bar).dot(self.Ppu)
self.A_pos_q[a:b, -N:] = dummy.dot(self.derv_Afoot_map).dot(
self.E_FL_bar).dot(self.Ppu)
def _solve_qp(self):
"""
Solve QP first run with init functionality and other runs with warmstart
"""
self.cpu_time = 2.9 # ms
self.nwsr = 1000 # unlimited bounded
if not self._qp_is_initialized:
ret, nwsr, cputime = self.qp.init(self.qp_H, self.qp_g, self.qp_A,
self.qp_lb, self.qp_ub,
self.qp_lbA, self.qp_ubA,
self.nwsr, self.cpu_time)
self._qp_is_initialized = True
else:
ret, nwsr, cputime = self.qp.hotstart(self.qp_H, self.qp_g,
self.qp_A, self.qp_lb,
self.qp_ub, self.qp_lbA,
self.qp_ubA, self.nwsr,
self.cpu_time)
# orientation primal solution
self.qp.getPrimalSolution(self.dofs)
# save qp solver data
self.qp_nwsr = nwsr # working set recalculations
self.qp_cputime = cputime * 1000. # in milliseconds (set to 2.9ms)
def _postprocess_solution(self):
""" Get solution and put it back into generator data structures """
# rename for convenience
N = self.N
nf = self.nf
# extract dofs
# dofs = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# ( dddF_k_q ) N
# ( dddF_k_q ) N
# NOTE this time we add an increment to the existing values
# data(k+1) = data(k) + alpha * dofs
# TODO add line search when problematic
alpha = 1.0
# x values
self.dddC_k_x[:] += alpha * self.dofs[0:0 + N]
self.F_k_x[:] += alpha * self.dofs[0 + N:0 + N + nf]
# y values
a = N + nf
self.dddC_k_y[:] += alpha * self.dofs[a:a + N]
self.F_k_y[:] += alpha * self.dofs[a + N:a + N + nf]
# feet orientation
a = 2 * (N + nf)
self.dddF_k_qR[:] += alpha * self.dofs[a:a + N]
self.dddF_k_qL[:] += alpha * self.dofs[-N:]
def update(self):
"""
overload update function to define time dependent support foot selection
matrix.
"""
ret = super(NMPCGenerator, self).update()
# update selection matrix when something has changed
self._update_foot_selection_matrix()
return ret
def _update_foot_selection_matrix(self):
""" get right foot selection matrix """
i = 0
for j in range(self.N):
self.derv_Acop_map[i:i + self.nFootEdge, j] = 1.0
i += self.nFootEdge
self.derv_Afoot_map[...] = 0.0
i = self.nFootPosHullEdges
for j in range(self.nf - 1):
for k in range(self.N):
if self.V_kp1[k, j] == 1:
self.derv_Afoot_map[i:i + self.nFootPosHullEdges, k] = 1.0
i += self.nFootPosHullEdges
break
else:
self.derv_Afoot_map[i:i + self.nFootPosHullEdges, j] = 0.0
class NMPCGenerator(BaseGenerator):
"""
Implementation of the combined problems using NMPC techniques.
Solve QP for position and orientation of CoM and feet simultaneously in
each timestep. Calculates derivatives and updates states in each step.
"""
def __init__(
self, N=16, T=0.1, T_step=0.8,
fsm_state='D', fsm_sl=1
):
"""
Initialize pattern generator matrices through base class
and allocate two QPs one for optimzation of orientation and
one for position of CoM and feet.
"""
super(NMPCGenerator, self).__init__(
N, T, T_step, fsm_state, fsm_sl
)
# The pattern generator has to solve the following kind of
# problem in each iteration
# min_x 1/2 * x.T * H(w0) * x + x.T g(w0)
# s.t. lbA(w0) <= A(w0) * x <= ubA(w0)
# lb(w0) <= x <= ub(wo)
# Because of varying H and A, we have to use the
# SQPProblem class, which supports this kind of QPs
# rename for convenience
N = self.N
nf = self.nf
# define some qpOASES specific things
self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
self.nwsr = 100 # # of working set recalculations
self.options = Options()
self.options.setToMPC()
#self.options.printLevel = PrintLevel.LOW
# define variable dimensions
# variables of: position + orientation
self.nv = 2*(self.N+self.nf) + 2*N
# define constraint dimensions
self.nc_pos = (
self.nc_cop
+ self.nc_foot_position
+ self.nc_fchange_eq
)
self.nc_ori = (
self.nc_fvel_eq
+ self.nc_fpos_ineq
+ self.nc_fvel_ineq
)
self.nc = (
# position
self.nc_pos
# orientation
+ self.nc_ori
)
# setup problem
self.dofs = numpy.zeros(self.nv)
self.qp = SQProblem(self.nv, self.nc)
# load NMPC options
self.qp.setOptions(self.options)
self.qp_H = numpy.eye(self.nv,self.nv)
self.qp_A = numpy.zeros((self.nc,self.nv))
self.qp_g = numpy.zeros((self.nv,))
self.qp_lb = -numpy.ones((self.nv,))*1e+08
self.qp_ub = numpy.ones((self.nv,))*1e+08
self.qp_lbA = -numpy.ones((self.nc,))*1e+08
self.qp_ubA = numpy.ones((self.nc,))*1e+08
self._qp_is_initialized = False
# save computation time and working set recalculations
self.qp_nwsr = 0.0
self.qp_cputime = 0.0
# setup analyzer for solution analysis
analyser = SolutionAnalysis()
# helper matrices for common expressions
self.Hx = numpy.zeros((1, 2*(N+nf)), dtype=float)
self.Q_k_x = numpy.zeros((N+nf, N+nf), dtype=float)
self.p_k_x = numpy.zeros((N+nf,), dtype=float)
self.p_k_y = numpy.zeros((N+nf,), dtype=float)
self.Hq = numpy.zeros((1, 2*N), dtype=float)
self.Q_k_qR = numpy.zeros((N, N), dtype=float)
self.Q_k_qL = numpy.zeros((N, N), dtype=float)
self.p_k_qR = numpy.zeros((N), dtype=float)
self.p_k_qL = numpy.zeros((N), dtype=float)
self.A_pos_x = numpy.zeros((self.nc_pos, 2*(N+nf)), dtype=float)
self.A_pos_q = numpy.zeros((self.nc_pos, 2*N), dtype=float)
self.ubA_pos = numpy.zeros((self.nc_pos,), dtype=float)
self.lbA_pos = numpy.zeros((self.nc_pos,), dtype=float)
self.A_ori = numpy.zeros((self.nc_ori, 2*N), dtype=float)
self.ubA_ori = numpy.zeros((self.nc_ori,), dtype=float)
self.lbA_ori = numpy.zeros((self.nc_ori,), dtype=float)
self.derv_Acop_map = numpy.zeros((self.nc_cop, self.N), dtype=float)
self.derv_Afoot_map = numpy.zeros((self.nc_foot_position, self.N), dtype=float)
self._update_foot_selection_matrix()
# add additional keys that should be saved
self._data_keys.append('qp_nwsr')
self._data_keys.append('qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)
def solve(self):
""" Process and solve problem, s.t. pattern generator data is consistent """
self._preprocess_solution()
self._solve_qp()
self._postprocess_solution()
def _preprocess_solution(self):
""" Update matrices and get them into the QP data structures """
# rename for convenience
N = self.N
nf = self.nf
# dofs
dofs = self.dofs
dddC_k_x = self.dddC_k_x
F_k_x = self.F_k_x
dddC_k_y = self.dddC_k_y
F_k_y = self.F_k_y
dddF_k_qR = self.dddF_k_qR
dddF_k_qL = self.dddF_k_qL
# inject dofs for convenience
# dofs = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# ( dddF_k_q ) N
# ( dddF_k_q ) N
dofs[0 :0+N ] = dddC_k_x
dofs[0+N:0+N+nf] = F_k_x
a = N+nf
dofs[a :a+N ] = dddC_k_y
dofs[a+N:a+N+nf] = F_k_y
a = 2*(N+nf)
dofs[ a:a+N] = dddF_k_qR
dofs[ -N:] = dddF_k_qL
# define position and orientation dofs
# U_k = ( U_k_xy, U_k_q).T
# U_k_xy = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# U_k_q = ( dddF_k_q ) N
# ( dddF_k_q ) N
# position dofs
U_k = self.dofs
U_k_xy = U_k[ :2*(N+nf)]
U_k_x = U_k_xy[:(N+nf)]
U_k_y = U_k_xy[(N+nf):]
# orientation dofs
U_k_q = U_k [-2*N: ]
U_k_qR = U_k_q[ :N]
U_k_qL = U_k_q[ N: ]
# position dimensions
nU_k = U_k.shape[0]
nU_k_xy = U_k_xy.shape[0]
nU_k_x = U_k_x.shape[0]
nU_k_y = U_k_y.shape[0]
# orientation dimensions
nU_k_q = U_k_q.shape[0]
nU_k_qR = U_k_qR.shape[0]
nU_k_qL = U_k_qL.shape[0]
# initialize with actual values, else take last known solution
# NOTE for warmstart last solution is taken from qpOASES internal memory
if not self._qp_is_initialized:
# TODO guess initial active set
# this requires changes to the python interface
pass
# calculate some common sub expressions
self._calculate_common_expressions()
# calculate Jacobian parts that are non-trivial, i.e. wrt. to orientation
self._calculate_derivatives()
# POSITION QP
# rename matrices
Q_k_x = self.Q_k_x
Q_k_y = self.Q_k_x # NOTE it's exactly the same!
p_k_x = self.p_k_x
p_k_y = self.p_k_y
# ORIENTATION QP
# rename matrices
Q_k_qR = self.Q_k_qR
Q_k_qL = self.Q_k_qL
p_k_qR = self.p_k_qR
p_k_qL = self.p_k_qL
# define QP matrices
# Gauss-Newton Hessian approximation
# define sub blocks
# H = (Hxy | Hqx)
# (Hxq | Hqq)
Hxx = self.qp_H[:nU_k_xy,:nU_k_xy]
Hxq = self.qp_H[:nU_k_xy,nU_k_xy:]
Hqx = self.qp_H[nU_k_xy:,:nU_k_xy]
Hqq = self.qp_H[nU_k_xy:,nU_k_xy:]
# fill matrices
Hxx[ :nU_k_x, :nU_k_x] = Q_k_x
Hxx[-nU_k_y:,-nU_k_y:] = Q_k_y
Hqq[ :nU_k_qR, :nU_k_qR] = Q_k_qR
Hqq[-nU_k_qL:,-nU_k_qL:] = Q_k_qL
#self.qp_H[...] = numpy.eye(self.nv)
# Gradient of Objective
# define sub blocks
# g = (gx)
# (gq)
gx = self.qp_g[:nU_k_xy]
gq = self.qp_g[-nU_k_q:]
# gx = ( U_k_x.T Q_k_x + p_k_x )
gx[ :nU_k_x] = U_k_x.dot(Q_k_x) + p_k_x
gx[-nU_k_y:] = U_k_y.dot(Q_k_y) + p_k_y
# gq = ( U_k_q.T Q_k_q + p_k_q )
gq[ :nU_k_qR] = U_k_qR.dot(Q_k_qR) + p_k_qR
gq[-nU_k_qL:] = U_k_qL.dot(Q_k_qL) + p_k_qL
# CONSTRAINTS
# A = ( A_xy, A_xyq )
# ( 0, A_q )
A_xy = self.qp_A [:self.nc_pos,:nU_k_xy]
A_xyq = self.qp_A [:self.nc_pos,-nU_k_q:]
lbA_xy = self.qp_lbA[:self.nc_pos]
ubA_xy = self.qp_ubA[:self.nc_pos]
A_q = self.qp_A [-self.nc_ori:,-nU_k_q:]
lbA_q = self.qp_lbA[-self.nc_ori:]
ubA_q = self.qp_ubA[-self.nc_ori:]
# linearized constraints are given by
# lbA - A * U_k <= nablaA * Delta_U_k <= ubA - A * U_k
A_xy[...] = self.A_pos_x
A_xyq[...] = self.A_pos_q
lbA_xy[...] = self.lbA_pos - self.A_pos_x.dot(U_k_xy)
ubA_xy[...] = self.ubA_pos - self.A_pos_x.dot(U_k_xy)
A_q[...] = self.A_ori
lbA_q[...] = self.lbA_ori - self.A_ori.dot(U_k_q)
ubA_q[...] = self.ubA_ori - self.A_ori.dot(U_k_q)
def _calculate_common_expressions(self):
"""
encapsulation of complicated matrix assembly of former orientation and
position QP sub matrices
"""
#rename for convenience
N = self.N
nf = self.nf
# weights
alpha = self.a
beta = self.b
gamma = self.c
delta = self.d
# matrices
E_FR = self.E_FR
E_FL = self.E_FL
Pvs = self.Pvs
Pvu = self.Pvu
Pzs = self.Pzs
Pzu = self.Pzu
c_k_x = self.c_k_x
c_k_y = self.c_k_y
f_k_x = self.f_k_x
f_k_y = self.f_k_y
f_k_qR = self.f_k_qR
f_k_qL = self.f_k_qL
v_kp1 = self.v_kp1
V_kp1 = self.V_kp1
dC_kp1_x_ref = self.dC_kp1_x_ref
dC_kp1_y_ref = self.dC_kp1_y_ref
dC_kp1_q_ref = self.dC_kp1_q_ref
# POSITION QP MATRICES
# Q_k_x = ( Q_k_xXX Q_k_xXF ) = Q_k_y
# ( Q_k_xFX Q_k_xFF )
Q_k_x = self.Q_k_x
a = 0; b = N
c = 0; d = N
Q_k_xXX = Q_k_x[a:b,c:d]
a = 0; b = N
c = N; d = N+nf
Q_k_xXF = Q_k_x[a:b,c:d]
a = N; b = N+nf
c = 0; d = N
Q_k_xFX = Q_k_x[a:b,c:d]
a = N; b = N+nf
c = N; d = N+nf
Q_k_xFF = Q_k_x[a:b,c:d]
# Q_k_xXX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# Q_k_xXF = ( -0.5 * c * Pzu^T * V_kp1 )
# Q_k_xFX = ( -0.5 * c * Pzu^T * V_kp1 )^T
# Q_k_xFF = ( 0.5 * c * V_kp1^T * V_kp1 )
Q_k_xXX[...] = (
alpha * Pvu.transpose().dot(Pvu)
+ gamma * Pzu.transpose().dot(Pzu)
+ delta * numpy.eye(N)
)
Q_k_xXF[...] = - gamma * Pzu.transpose().dot(V_kp1)
Q_k_xFX[...] = Q_k_xXF.transpose()
Q_k_xFF[...] = gamma * V_kp1.transpose().dot(V_kp1)
# p_k_x = ( p_k_xX )
# ( p_k_xF )
p_k_x = self.p_k_x
p_k_xX = p_k_x[ :N]
p_k_xF = p_k_x[-nf: ]
# p_k_xX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# p_k_xF = ( -0.5 * c * Pzu^T * V_kp1 )
p_k_xX[...] = alpha * Pvu.transpose().dot( Pvs.dot(c_k_x) - dC_kp1_x_ref) \
+ gamma * Pzu.transpose().dot( Pzs.dot(c_k_x) - v_kp1.dot(f_k_x))
p_k_xF[...] =-gamma * V_kp1.transpose().dot(Pzs.dot(c_k_x) - v_kp1.dot(f_k_x))
# p_k_y = ( p_k_yX )
# ( p_k_yF )
p_k_y = self.p_k_y
p_k_yX = p_k_y[ :N]
p_k_yF = p_k_y[-nf: ]
# p_k_yX = ( 0.5 * a * Pvu^T * Pvu + c * Pzu^T * Pzu + d * I )
# p_k_yF = ( -0.5 * c * Pzu^T * V_kp1 )
p_k_yX[...] = alpha * Pvu.transpose().dot( Pvs.dot(c_k_y) - dC_kp1_y_ref) \
+ gamma * Pzu.transpose().dot( Pzs.dot(c_k_y) - v_kp1.dot(f_k_y))
p_k_yF[...] =-gamma * V_kp1.transpose().dot(Pzs.dot(c_k_y) - v_kp1.dot(f_k_y))
# ORIENTATION QP MATRICES
# Q_k_qR = ( 0.5 * a * Pvu^T * E_FR^T * E_FR * Pvu )
Q_k_qR = self.Q_k_qR
Q_k_qR[...] = alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR).dot(Pvu)
# p_k_qR = ( a * Pvu^T * E_FR^T * (E_FR * Pvs * f_k_qR + dC_kp1_q_ref) )
p_k_qR = self.p_k_qR
p_k_qR[...] = alpha * Pvu.transpose().dot(E_FR.transpose()).dot(E_FR.dot(Pvs).dot(f_k_qR) - dC_kp1_q_ref)
# Q_k_qL = ( 0.5 * a * Pvu^T * E_FL^T * E_FL * Pvu )
Q_k_qL = self.Q_k_qL
Q_k_qL[...] = alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL).dot(Pvu)
# p_k_qL = ( a * Pvu^T * E_FL^T * (E_FL * Pvs * f_k_qL + dC_kp1_q_ref) )
p_k_qL = self.p_k_qL
p_k_qL[...] = alpha * Pvu.transpose().dot(E_FL.transpose()).dot(E_FL.dot(Pvs).dot(f_k_qL) - dC_kp1_q_ref)
# LINEAR CONSTRAINTS
# CoP constraints
a = 0
b = self.nc_cop
self.A_pos_x[a:b] = self.Acop
self.lbA_pos[a:b] = self.lbBcop
self.ubA_pos[a:b] = self.ubBcop
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.A_pos_x[a:b] = self.Afoot
self.lbA_pos[a:b] = self.lbBfoot
self.ubA_pos[a:b] = self.ubBfoot
#foot equality constraints
a = self.nc_cop + self.nc_foot_position
b = self.nc_cop + self.nc_foot_position + self.nc_fchange_eq
self.A_pos_x[a:b] = self.eqAfoot
self.lbA_pos[a:b] = self.eqBfoot
self.ubA_pos[a:b] = self.eqBfoot
# velocity constraints on support foot to freeze movement
a = 0
b = self.nc_fvel_eq
self.A_ori [a:b] = self.A_fvel_eq
self.lbA_ori[a:b] = self.B_fvel_eq
self.ubA_ori[a:b] = self.B_fvel_eq
# box constraints for maximum orientation change
a = self.nc_fvel_eq
b = self.nc_fvel_eq + self.nc_fpos_ineq
self.A_ori [a:b] = self.A_fpos_ineq
self.lbA_ori[a:b] = self.lbB_fpos_ineq
self.ubA_ori[a:b] = self.ubB_fpos_ineq
# box constraints for maximum angular velocity
a = self.nc_fvel_eq + self.nc_fpos_ineq
b = self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq
self.A_ori [a:b] = self.A_fvel_ineq
self.lbA_ori[a:b] = self.lbB_fvel_ineq
self.ubA_ori[a:b] = self.ubB_fvel_ineq
def _calculate_derivatives(self):
""" calculate the Jacobian of the constraints function """
# COP CONSTRAINTS
# build the constraint enforcing the center of pressure to stay inside
# the support polygon given through the convex hull of the foot.
# define dummy values
# D_kp1 = (D_kp1x, Dkp1_y)
D_kp1 = numpy.zeros( (self.nFootEdge*self.N, 2*self.N), dtype=float )
D_kp1x = D_kp1[:, :self.N] # view on big matrix
D_kp1y = D_kp1[:,-self.N:] # view on big matrix
b_kp1 = numpy.zeros( (self.nFootEdge*self.N,), dtype=float )
# change entries according to support order changes in D_kp1
theta_vec = [self.f_k_q,self.F_k_q[0],self.F_k_q[1]]
for i in range(self.N):
theta = theta_vec[self.supportDeque[i].stepNumber]
# NOTE THIS CHANGES DUE TO APPLYING THE DERIVATIVE!
rotMat = numpy.array([
# old
# [ cos(theta), sin(theta)],
# [-sin(theta), cos(theta)]
# new: derivative wrt to theta
[-numpy.sin(theta), numpy.cos(theta)],
[-numpy.cos(theta),-numpy.sin(theta)]
])
if self.supportDeque[i].foot == "left" :
A0 = self.A0lf.dot(rotMat)
B0 = self.ubB0lf
D0 = self.A0dlf.dot(rotMat)
d0 = self.ubB0dlf
else :
A0 = self.A0rf.dot(rotMat)
B0 = self.ubB0rf
D0 = self.A0drf.dot(rotMat)
d0 = self.ubB0drf
# get support foot and check if it is double support
for j in range(self.nf):
if self.V_kp1[i,j] == 1:
if self.fsm_states[j] == 'D':
A0 = D0
B0 = d0
else:
pass
for k in range(self.nFootEdge):
# get d_i+1^x(f^theta)
D_kp1x[i*self.nFootEdge+k, i] = A0[k][0]
# get d_i+1^y(f^theta)
D_kp1y[i*self.nFootEdge+k, i] = A0[k][1]
# get right hand side of equation
b_kp1 [i*self.nFootEdge+k] = B0[k]
#rename for convenience
N = self.N
nf = self.nf
PzuV = self.PzuV
PzuVx = self.PzuVx
PzuVy = self.PzuVy
PzsC = self.PzsC
PzsCx = self.PzsCx
PzsCy = self.PzsCy
v_kp1fc = self.v_kp1fc
v_kp1fc_x = self.v_kp1fc_x
v_kp1fc_y = self.v_kp1fc_y
# build constraint transformation matrices
# PzuV = ( PzuVx )
# ( PzuVy )
# PzuVx = ( Pzu | -V_kp1 | 0 | 0 )
PzuVx[:, :self.N ] = self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
PzuVx[:,self.N:self.N+self.nf] = -self.V_kp1
# PzuVy = ( 0 | 0 | Pzu | -V_kp1 )
PzuVy[:,-self.N-self.nf:-self.nf] = self.Pzu # TODO this is constant in matrix and should go into the build up matrice part
PzuVy[:, -self.nf: ] = -self.V_kp1
# PzuV = ( PzsCx ) = ( Pzs * c_k_x)
# ( PzsCy ) ( Pzs * c_k_y)
PzsCx[...] = self.Pzs.dot(self.c_k_x) #+ self.v_kp1.dot(self.f_k_x)
PzsCy[...] = self.Pzs.dot(self.c_k_y) #+ self.v_kp1.dot(self.f_k_y)
# v_kp1fc = ( v_kp1fc_x ) = ( v_kp1 * f_k_x)
# ( v_kp1fc_y ) ( v_kp1 * f_k_y)
v_kp1fc_x[...] = self.v_kp1.dot(self.f_k_x)
v_kp1fc_y[...] = self.v_kp1.dot(self.f_k_y)
# build CoP linear constraints
# NOTE D_kp1 is member and D_kp1 = ( D_kp1x | D_kp1y )
# D_kp1x,y contains entries from support polygon
dummy = D_kp1.dot(PzuV)
dummy = dummy.dot(self.dofs[:2*(N+nf)])
# CoP constraints
a = 0
b = self.nc_cop
self.A_pos_q[a:b, :N] = \
dummy.dot(self.derv_Acop_map).dot(self.E_FR_bar).dot(self.Ppu)
self.A_pos_q[a:b,-N:] = \
dummy.dot(self.derv_Acop_map).dot(self.E_FL_bar).dot(self.Ppu)
# FOOT POSITION CONSTRAINTS
# defined on the horizon
# inequality constraint on both feet A u + B <= 0
# A0 R(theta) [Fx_k+1 - Fx_k] <= ubB0
# [Fy_k+1 - Fy_k]
matSelec = numpy.array([ [1, 0],[-1, 1] ])
footSelec = numpy.array([ [self.f_k_x, 0],[self.f_k_y, 0] ])
theta_vec = [self.f_k_q, self.F_k_q[0]]
# rotation matrice from F_k+1 to F_k
# NOTE THIS CHANGES DUE TO APPLYING THE DERIVATIVE!
rotMat1 = numpy.array([
# old
# [cos(theta_vec[0]), sin(theta_vec[0])],
# [-sin(theta_vec[0]), cos(theta_vec[0])]
# new: derivative wrt to theta
[-numpy.sin(theta_vec[0]), numpy.cos(theta_vec[0])],
[-numpy.cos(theta_vec[0]),-numpy.sin(theta_vec[0])]
])
rotMat2 = numpy.array([
# old
# [cos(theta_vec[1]), sin(theta_vec[1])],
# [-sin(theta_vec[1]), cos(theta_vec[1])]
# new
[-numpy.sin(theta_vec[1]), numpy.cos(theta_vec[1])],
[-numpy.cos(theta_vec[1]),-numpy.sin(theta_vec[1])]
])
nf = self.nf
nEdges = self.A0l.shape[0]
N = self.N
ncfoot = nf * nEdges
if self.currentSupport.foot == "left":
A_f1 = self.A0r.dot(rotMat1)
A_f2 = self.A0l.dot(rotMat2)
else :
A_f1 = self.A0l.dot(rotMat1)
A_f2 = self.A0r.dot(rotMat2)
tmp1 = numpy.array( [A_f1[:,0],numpy.zeros((nEdges,),dtype=float)] )
tmp2 = numpy.array( [numpy.zeros((nEdges,),dtype=float),A_f2[:,0]] )
tmp3 = numpy.array( [A_f1[:,1],numpy.zeros((nEdges,),dtype=float)] )
tmp4 = numpy.array( [numpy.zeros(nEdges,),A_f2[:,1]] )
X_mat = numpy.concatenate( (tmp1.T,tmp2.T) , 0)
A0x = X_mat.dot(matSelec)
Y_mat = numpy.concatenate( (tmp3.T,tmp4.T) , 0)
A0y = Y_mat.dot(matSelec)
dummy = numpy.concatenate ((
numpy.zeros((ncfoot,N),dtype=float), A0x,
numpy.zeros((ncfoot,N),dtype=float), A0y
), 1
)
dummy = dummy.dot(self.dofs[:2*(N+nf)])
#foot inequality constraints
a = self.nc_cop
b = self.nc_cop + self.nc_foot_position
self.A_pos_q[a:b, :N] = dummy.dot(self.derv_Afoot_map).dot(self.E_FR_bar).dot(self.Ppu)
self.A_pos_q[a:b,-N:] = dummy.dot(self.derv_Afoot_map).dot(self.E_FL_bar).dot(self.Ppu)
def _solve_qp(self):
"""
Solve QP first run with init functionality and other runs with warmstart
"""
self.cpu_time = 2.9 # ms
self.nwsr = 1000 # unlimited bounded
if not self._qp_is_initialized:
ret, nwsr, cputime = self.qp.init(
self.qp_H, self.qp_g, self.qp_A,
self.qp_lb, self.qp_ub,
self.qp_lbA, self.qp_ubA,
self.nwsr, self.cpu_time
)
self._qp_is_initialized = True
else:
ret, nwsr, cputime = self.qp.hotstart(
self.qp_H, self.qp_g, self.qp_A,
self.qp_lb, self.qp_ub,
self.qp_lbA, self.qp_ubA,
self.nwsr, self.cpu_time
)
# orientation primal solution
self.qp.getPrimalSolution(self.dofs)
# save qp solver data
self.qp_nwsr = nwsr # working set recalculations
self.qp_cputime = cputime*1000. # in milliseconds (set to 2.9ms)
def _postprocess_solution(self):
""" Get solution and put it back into generator data structures """
# rename for convenience
N = self.N
nf = self.nf
# extract dofs
# dofs = ( dddC_k_x ) N
# ( F_k_x ) nf
# ( dddC_k_y ) N
# ( F_k_y ) nf
# ( dddF_k_q ) N
# ( dddF_k_q ) N
# NOTE this time we add an increment to the existing values
# data(k+1) = data(k) + alpha * dofs
# TODO add line search when problematic
alpha = 1.0
# x values
self.dddC_k_x[:] += alpha * self.dofs[0 :0+N ]
self.F_k_x[:] += alpha * self.dofs[0+N:0+N+nf]
# y values
a = N + nf
self.dddC_k_y[:] += alpha * self.dofs[a :a+N ]
self.F_k_y[:] += alpha * self.dofs[a+N:a+N+nf]
# feet orientation
a =2*(N + nf)
self.dddF_k_qR[:] += alpha * self.dofs[ a:a+N]
self.dddF_k_qL[:] += alpha * self.dofs[ -N:]
def update(self):
"""
overload update function to define time dependent support foot selection
matrix.
"""
ret = super(NMPCGenerator, self).update()
# update selection matrix when something has changed
self._update_foot_selection_matrix()
return ret
def _update_foot_selection_matrix(self):
""" get right foot selection matrix """
i = 0
for j in range(self.N):
self.derv_Acop_map[i:i+self.nFootEdge,j] = 1.0
i += self.nFootEdge
self.derv_Afoot_map[...] = 0.0
i = self.nFootPosHullEdges
for j in range(self.nf-1):
for k in range(self.N):
if self.V_kp1[k,j] == 1:
self.derv_Afoot_map[i:i+self.nFootPosHullEdges,k] = 1.0
i += self.nFootPosHullEdges
break
else:
self.derv_Afoot_map[i:i+self.nFootPosHullEdges,j] = 0.0