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]
def main():
rospy.init_node('controller_2dof', anonymous=True)
rospy.Rate(10)
# rospy.sleep(0.2)
# Setup data of QP.
# Weights.
w1 = 1e-3
w2 = 1e-3
w3 = 1
w4 = 1 # joint goal
# Joint limits.
q0_max = 0.05
q1_max = 0.15
# Links
l1 = 0.1
l2 = 0.2
l3 = 0.3
# Initial joint values.
q0_init = q0 = 0.02
q1_init = q1 = -0.15
# Joint target.
q_des = 0.6
q0_des = -0.025
q1_des = 0.05
q0_goal = True
q1_goal = False
# Slack limits.
e_max = 1000
e0_max = 1000
# Velocity limits.(+-)
v0_max = 0.05
v1_max = 0.1
# Acceleration limits.(+-)
a0_max = 0.05 * 0.5
a1_max = 0.1 * 0.5
# Others
precision = 5e-3
joint_precision = 5e-3
p = 10
pj = 5
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
vel_init = 0
nWSR = np.array([100])
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(4, 7)
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0,
w4
]).reshape((4, 4))
A = np.array([
1.0, 1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]).reshape((7, 4))
g = np.array([0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e_max, -e0_max])
ub = np.array([v0_max, v1_max, e_max, e0_max])
lbA = np.array(
[error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max, 0, 0])
ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max, 0, 0])
# Setting up QProblem object.
if q0_goal is False and q1_goal is False:
print("\nNo joint goal specified\n")
return -1
elif (q0_goal is True and q1_goal is False) or (q0_goal is False
and q1_goal is True):
if q0_goal is True:
lbA[5] = (q0_des - q0)
ubA[5] = (q0_des - q0)
# A[0, 0] = 0.0
A[5, 0] = 1.0
A[5, 3] = 1.0
else:
lbA[5] = (q1_des - q1)
ubA[5] = (q1_des - q1)
A[5, 1] = 1.0
A[5, 3] = 1.0
else:
A[0, 0] = 0.0
A[0, 1] = 0.0
A[0, 2] = 0.0
A[5, 0] = 1.0
A[5, 2] = 1.0
A[6, 1] = 1.0
A[6, 3] = 1.0
p = 0
error = 0
lbA[0] = 0
ubA[0] = 0
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.LOW
example.setOptions(options)
print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" %
(q_eef, q_des, error, q0, q1))
print A
i = 0
limit = abs(error)
ok = False
Opt = np.zeros(4)
# Plotting
t = np.array(i)
pos_eef = np.array(q_eef)
pos_0 = np.array(q0)
pos_1 = np.array(q1)
vel_0 = np.array(vel_init)
vel_1 = np.array(vel_init)
vel_eef = np.array(vel_init)
p_error = np.array(error)
eef_goal = np.array(q_des)
q0_set = np.array(q0_des)
q1_set = np.array(q1_des)
return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Init of QP-Problem returned without success! ERROR MESSAGE: "
return -1
while not rospy.is_shutdown():
while (limit > precision or not ok) and i < 2000:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error
lbA[1] = -q0_max - q0
lbA[2] = -q1_max - q1
lbA[3] = a0_const * Opt[0] - a0_max
lbA[4] = a1_const * Opt[1] - a1_max
ubA[0] = error
ubA[1] = q0_max - q0
ubA[2] = q1_max - q1
ubA[3] = a0_const * Opt[0] + a0_max
ubA[4] = a1_const * Opt[1] + a1_max
return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
rospy.logerr(
"Hotstart of QP-Problem returned without success!")
return -1
# Get and print solution of QP.
example.getPrimalSolution(Opt)
q0 += Opt[0] / 100
q1 += Opt[1] / 100
q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
limit = abs(error)
# print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % (
# Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1)
# Depending on joint goals
if q0_goal is False and q1_goal is False:
ok = True
error = 0
limit = 0
elif q0_goal is True and q1_goal is False:
lbA[5] = pj * (q0_des - q0)
ubA[5] = pj * (q0_des - q0)
if abs(lbA[5]) < joint_precision:
ok = True
# print "\n q0_error = %g, i = %g \n" % (lbA[5], i)
else:
ok = False
elif q0_goal is False and q1_goal is True:
lbA[5] = pj * (q1_des - q1)
ubA[5] = pj * (q1_des - q1)
if abs(lbA[5]) < joint_precision:
ok = True
# print "\n q0_error = %g, i = %g \n" % (lbA[5], i)
else:
lbA[5] = pj * (q0_des - q0)
ubA[5] = pj * (q0_des - q0)
lbA[6] = pj * (q1_des - q1)
ubA[6] = pj * (q1_des - q1)
if abs(lbA[5]) < joint_precision and abs(
lbA[6]) < joint_precision:
ok = True
# print "\n q0_error = %g, q1_error = %g \n" % (lbA[5], lbA[6])
# Plotting arrays
pos_eef = np.hstack((pos_eef, q_eef))
pos_0 = np.hstack((pos_0, q0))
pos_1 = np.hstack((pos_1, q1))
vel_0 = np.hstack((vel_0, Opt[0]))
vel_1 = np.hstack((vel_1, Opt[1]))
vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1]))
p_error = np.hstack((p_error, error))
eef_goal = np.hstack((eef_goal, q_des))
q0_set = np.hstack((q0_set, q0_des))
q1_set = np.hstack((q1_set, q1_des))
t = np.hstack((t, i))
# Plot
t_eef = go.Scatter(y=pos_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_eef',
line=dict(dash='dash'))
t_p0 = go.Scatter(y=pos_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q0',
line=dict(dash='dash'))
t_p1 = go.Scatter(y=pos_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q1',
line=dict(dash='dash'))
t_v0 = go.Scatter(y=vel_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q0')
t_v1 = go.Scatter(y=vel_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q1')
t_veef = go.Scatter(y=vel_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_eef')
t_er = go.Scatter(y=p_error,
x=t,
marker=dict(size=4, ),
mode='lines',
name='error')
t_g1 = go.Scatter(y=q0_set,
x=t,
marker=dict(size=4, ),
mode='lines',
name='joint0_goal',
line=dict(dash='dot'))
t_g2 = go.Scatter(y=q1_set,
x=t,
marker=dict(size=4, ),
mode='lines',
name='joint1_goal',
line=dict(dash='dot'))
t_goal = go.Scatter(y=eef_goal,
x=t,
marker=dict(size=4, ),
mode='lines',
name='eef_goal',
line=dict(dash='dot'))
if q0_goal == True:
data_q0 = [t_p0, t_v0, t_g1]
data_q1 = [t_p1, t_v1]
else:
data_q0 = [t_p0, t_v0]
data_q1 = [t_p1, t_v1, t_g2]
layout = dict(title="Initial position q0 =%g.\n" % (q0_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
tickcolor='#060',
),
yaxis=dict(title='Position / Velocity',
gridwidth=3,
range=[-0.15, .1]),
font=dict(size=18))
fig0 = dict(data=data_q0, layout=layout)
plotly.offline.plot(fig0,
filename='joint_goals_q0.html',
image='png',
image_filename='joint_q0')
layout = dict(title="Initial position q1 = %g.\n" % (q1_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
tickcolor='#060',
),
yaxis=dict(
title='Position / Velocity',
gridwidth=3,
),
font=dict(size=18))
fig1 = dict(data=data_q1, layout=layout)
plotly.offline.plot(fig1,
filename='joint_goals_q1.html',
image='png',
image_filename='joint_q1')
data_eef = [t_eef, t_veef, t_goal]
layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" %
(q_eef_init, q_des),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
yaxis=dict(title='Position / Velocity', gridwidth=3),
font=dict(size=16))
fig = dict(data=data_eef, layout=layout_eef)
plotly.offline.plot(fig,
filename='joint_goals_eef.html',
image='png',
image_filename='joint_eef')
print "\n i = ", i
return 0
print ok
class AbstractSolver(object):
"""
Abstract solver class to use as base for all solvers. The basic problem has
the following structure:
minimize 0.5*||D*x - d||^2
subject to Alb <= A*x <= Aub
lb <= x <= ub
NB: Before it was:
subject to G*x + g >= 0
where
"""
NO_WARM_START = False
name = ""
# solver name
n = 0
# number of variables
m_in = -1
# number of inequalities
D = []
# quadratic cost matrix
d = []
# quadratic cost vector
G = []
# inequality matrix
g = []
# inequality vector
bounds = []
# bounds on the problem variables
H = []
# Hessian H = G^T*G
dD = []
# Product dD = d^T*D
x0 = []
# initial guess
solver = ''
# type of solver to use
accuracy = 0
# accuracy used by the solver for termination
maxIter = 0
# max number of iterations
verb = 0
# verbosity level of the solver (0=min, 2=max)
iter = 0
# current iteration number
computationTime = 0.0
# total computation time
qpTime = 0.0
# time taken to solve the QP(s) only
iterationNumber = 0
# total number of iterations
approxProb = 0
# approximated probability of G*x+g>0
initialized = False
# true if solver has been initialized
nActiveInequalities = 0
# number of active inequality constraints
nViolatedInequalities = 0
# number of violated inequalities
outerIter = 0
# number of outer (Newton) iterations
qpOasesSolver = []
options = []
# qp oases solver's options
softInequalityIndexes = []
epsilon = np.sqrt(np.finfo(float).eps)
INEQ_VIOLATION_THR = 1e-4
def __init__(self,
n,
m_in,
solver='slsqp',
accuracy=1e-6,
maxIter=100,
verb=0):
self.name = "AbstractSolver"
self.n = n
self.iter = 0
self.solver = solver
self.accuracy = accuracy
self.maxIter = maxIter
self.verb = verb
self.qpOasesAnalyser = SolutionAnalysis()
self.changeInequalityNumber(m_in)
return
def setSoftInequalityIndexes(self, indexes):
self.softInequalityIndexes = indexes
def changeInequalityNumber(self, m_in):
# print "[%s] Changing number of inequality constraints from %d to %d" % (self.name, self.m_in, m_in);
if (m_in == self.m_in):
return
self.m_in = m_in
self.iter = 0
self.qpOasesSolver = SQProblem(self.n, m_in)
#, HessianType.POSDEF SEMIDEF
self.options = Options()
self.options.setToReliable()
if (self.verb <= 0):
self.options.printLevel = PrintLevel.NONE
elif (self.verb == 1):
self.options.printLevel = PrintLevel.LOW
elif (self.verb == 2):
self.options.printLevel = PrintLevel.MEDIUM
elif (self.verb > 2):
self.options.printLevel = PrintLevel.DEBUG_ITER
print "set high print level"
# self.options.enableRegularisation = False;
# self.options.enableFlippingBounds = BooleanType.FALSE
# self.options.initialStatusBounds = SubjectToStatus.INACTIVE
# self.options.setToMPC();
# self.qpOasesSolver.printOptions();
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
def setProblemData(self, D, d, A, lbA, ubA, lb, ub, x0=None):
self.D = D
self.d = d.squeeze()
if (A.shape[0] == self.m_in and A.shape[1] == self.n):
self.A = A
self.lbA = lbA.squeeze()
self.ubA = ubA.squeeze()
else:
print "[%s] ERROR. Wrong size of the constraint matrix, %d rather than %d" % (
self.name, A.shape[0], self.m_in)
if (lb.shape[0] == self.n and ub.shape[0] == self.n):
self.lb = lb.squeeze()
self.ub = ub.squeeze()
else:
print "[%s] ERROR. Wrong size of the bound vectors, %d and %d rather than %d" % (
self.name, lb.shape[0], ub.shape[0], self.n)
# self.bounds = self.n*[(-1e10,1e10)];
if (x0 is None):
self.x0 = np.zeros(self.n)
else:
self.x0 = x0.squeeze()
self.H = np.dot(self.D.T, self.D)
self.dD = np.dot(self.D.T, self.d)
def solve(self,
D,
d,
A,
lbA,
ubA,
lb,
ub,
x0=None,
maxIter=None,
maxTime=100.0):
if (self.NO_WARM_START):
self.qpOasesSolver = SQProblem(self.n, self.m_in)
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
if (maxIter is None):
maxIter = self.maxIter
self.iter = 0
self.qpTime = 0.0
self.removeSoftInequalities = False
self.setProblemData(D, d, A, lbA, ubA, lb, ub, x0)
start = time.time()
x = np.zeros(self.x0.shape)
if (self.solver == 'slsqp'):
(x, fx, self.iterationNumber, imode,
smode) = fmin_slsqp(self.f_cost,
self.x0,
fprime=self.f_cost_grad,
f_ieqcons=self.f_inequalities,
fprime_ieqcons=self.f_inequalities_jac,
bounds=self.bounds,
iprint=self.verb,
iter=maxIter,
acc=self.accuracy,
full_output=1)
self.fx = fx
x = np.array(x)
''' Exit modes are defined as follows :
-1 : Gradient evaluation required (g & a)
0 : Optimization terminated successfully.
1 : Function evaluation required (f & c)
2 : More equality constraints than independent variables
3 : More than 3*n iterations in LSQ subproblem
4 : Inequality constraints incompatible
5 : Singular matrix E in LSQ subproblem
6 : Singular matrix C in LSQ subproblem
7 : Rank-deficient equality constraint subproblem HFTI
8 : Positive directional derivative for linesearch
9 : Iteration limit exceeded
'''
if (self.verb > 0 and imode != 0 and imode !=
9): #do not print error msg if iteration limit exceeded
print "[%s] *** ERROR *** %s" % (self.name, smode)
elif (self.solver == 'qpoases'):
# ubA = np.array(self.m_in*[1e9]);
# lb = np.array([ b[0] for b in self.bounds]);
# ub = np.array([ b[1] for b in self.bounds]);
# A = self.get_linear_inequality_matrix();
self.iter = 0
#total iters of qpoases
# lbA = -self.get_linear_inequality_vector();
Hess = self.f_cost_hess(x)
grad = self.f_cost_grad(x)
self.fx = self.f_cost(x)
maxActiveSetIter = np.array([maxIter - self.iter])
maxComputationTime = np.array(maxTime)
if (self.initialized == False):
imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb,
self.ub, self.lbA, self.ubA,
maxActiveSetIter,
maxComputationTime)
if (imode == 0):
self.initialized = True
else:
imode = self.qpOasesSolver.hotstart(Hess, grad, self.A,
self.lb, self.ub, self.lbA,
self.ubA, maxActiveSetIter,
maxComputationTime)
if (imode ==
PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED):
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
imode = self.qpOasesSolver.init(Hess, grad, self.A,
self.lb, self.ub, self.lbA,
self.ubA, maxActiveSetIter,
maxComputationTime)
if (imode == 0):
self.initialized = True
self.qpTime += maxComputationTime
self.iter = 1 + maxActiveSetIter[0]
self.iterationNumber = self.iter
''' if the solution found is unfeasible check whether the initial guess is feasible '''
self.qpOasesSolver.getPrimalSolution(x)
ineq_marg = self.f_inequalities(x)
qpUnfeasible = False
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
qpUnfeasible = True
if (x0 is not None):
ineq_marg = self.f_inequalities(x0)
if (not (ineq_marg < -self.INEQ_VIOLATION_THR).any()):
if (self.verb > 0):
print "[%s] Solution found is unfeasible but initial guess is feasible" % (
self.name)
qpUnfeasible = False
x = np.copy(x0)
''' if both the solution found and the initial guess are unfeasible remove the soft constraints '''
if (qpUnfeasible and len(self.softInequalityIndexes) > 0):
# remove soft inequality constraints and try to solve again
self.removeSoftInequalities = True
maxActiveSetIter[0] = maxIter
lbAsoft = np.copy(self.lbA)
ubAsoft = np.copy(self.ubA)
lbAsoft[self.softInequalityIndexes] = -1e100
ubAsoft[self.softInequalityIndexes] = 1e100
imode = self.qpOasesSolver.init(Hess, grad, self.A, self.lb,
self.ub, lbAsoft, ubAsoft,
maxActiveSetIter)
self.qpOasesSolver.getPrimalSolution(x)
ineq_marg = self.f_inequalities(x)
ineq_marg[self.softInequalityIndexes] = 1.0
qpUnfeasible = False
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
''' if the solution found is unfeasible check whether the initial guess is feasible '''
if (x0 is not None):
x = np.copy(x0)
ineq_marg = self.f_inequalities(x)
ineq_marg[self.softInequalityIndexes] = 1.0
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
print "[%s] WARNING Problem unfeasible even without soft constraints" % (
self.name), np.min(ineq_marg), imode
qpUnfeasible = True
elif (self.verb > 0):
print "[%s] Initial guess is feasible for the relaxed problem" % (
self.name)
else:
print "[%s] No way to get a feasible solution (no initial guess)" % (
self.name), np.min(ineq_marg)
elif (self.verb > 0):
print "[%s] Solution found and initial guess are unfeasible, but relaxed problem is feasible" % (
self.name)
if (qpUnfeasible):
self.print_qp_oases_error_message(imode, self.name)
if (self.verb > 1):
activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3)
print "[%s] Iter %d, active inequalities %d" % (
self.name, self.iter, activeIneq)
# termination conditions
if (self.iter >= maxIter):
imode = 9
if (self.verb > 1):
print "[%s] Max number of iterations reached %d" % (
self.name, self.iter)
if (self.qpTime >= maxTime):
print "[%s] Max time reached %f after %d iters" % (
self.name, self.qpTime, self.iter)
imode = 9
elif (self.solver == 'sqpoases'):
ubA = np.array(self.m_in * [1e99])
x_newton = np.zeros(x.shape)
x = self.x0
A = self.get_linear_inequality_matrix()
self.iter = 0
#total iters of qpoases
self.outerIter = 0
# number of outer (Newton) iterations
while True:
# compute Newton step
lb = np.array([b[0] for b in self.bounds])
ub = np.array([b[1] for b in self.bounds])
lb -= x
ub -= x
lbA = -np.dot(A, x) - self.get_linear_inequality_vector()
# if(self.outerIter>0):
# if((lbA>0.0).any()):
# print "[%s] Iter %d lbA[%d]=%f"%(self.name,self.outerIter,np.argmax(lbA),np.max(lbA));
Hess = self.f_cost_hess(x)
grad = self.f_cost_grad(x)
self.fx = self.f_cost(x)
maxActiveSetIter = np.array([maxIter - self.iter])
maxComputationTime = np.array(maxTime)
if (self.initialized == False):
imode = self.qpOasesSolver.init(Hess, grad, A, lb, ub, lbA,
ubA, maxActiveSetIter,
maxComputationTime)
if (imode == 0):
self.initialized = True
else:
imode = self.qpOasesSolver.hotstart(
Hess, grad, A, lb, ub, lbA, ubA, maxActiveSetIter,
maxComputationTime)
self.qpTime += maxComputationTime
maxTime -= maxComputationTime
# count iterations
self.iter += 1 + maxActiveSetIter[0]
self.outerIter += 1
self.qpOasesSolver.getPrimalSolution(x_newton)
''' check feasibility of the constraints '''
x_new = x + x_newton
ineq_marg = self.f_inequalities(x_new)
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
if (len(self.softInequalityIndexes) > 0
and self.outerIter == 1):
''' if constraints are unfeasible at first iteration remove soft inequalities '''
if (self.verb > 1):
print '[%s] Remove soft constraints' % (self.name)
self.removeSoftInequalities = True
self.g[self.softInequalityIndexes] = 1e100
self.iter = 0
continue
elif (len(self.softInequalityIndexes) > 0
and self.removeSoftInequalities == False):
''' if constraints are unfeasible at later iteration remove soft inequalities '''
if (self.verb >= 0):
print '[%s] Remove soft constraints at iter %d' % (
self.name, self.outerIter)
self.removeSoftInequalities = True
self.g[self.softInequalityIndexes] = 1e100
continue
else:
if ((lbA > 0.0).any()):
print "[%s] WARNING Problem unfeasible (even without soft constraints) %f" % (
self.name, np.max(lbA)), imode
if (self.verb > 0):
''' qp failed for some reason (e.g. max iter) '''
print "[%s] WARNING imode %d ineq unfeasible iter %d: %f, max(lbA)=%f" % (
self.name, imode, self.outerIter,
np.min(ineq_marg), np.max(lbA))
break
''' if constraints are unfeasible at later iterations exit '''
if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY):
print "[%s] Outer iter %d QPoases says problem is unfeasible but constraints are satisfied: %f" % (
self.name, self.outerIter, np.min(ineq_marg))
ind = np.where(ineq_marg < 0.0)[0]
self.g[ind] -= ineq_marg[ind]
continue
self.print_qp_oases_error_message(imode, self.name)
# check for convergence
newton_dec_squared = np.dot(x_newton, np.dot(Hess, x_newton))
if (0.5 * newton_dec_squared < self.accuracy):
ineq_marg = self.f_inequalities(x)
if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()):
if (self.verb > 0):
print "[%s] Optimization converged in %d steps" % (
self.name, self.iter)
break
elif (self.verb > 0):
v = ineq_marg < -self.INEQ_VIOLATION_THR
print(
self.name, self.outerIter,
"WARNING Solver converged but inequalities violated:",
np.where(v), ineq_marg[v])
elif (self.verb > 1):
print "[%s] Optimization did not converge yet, squared Newton decrement: %f" % (
self.name, newton_dec_squared)
# line search
(alpha, fc, gc, phi, old_fval,
derphi) = line_search(self.f_cost,
self.f_cost_grad,
x,
x_newton,
grad,
self.fx,
c1=0.0001,
c2=0.9)
x_new = x + alpha * x_newton
new_fx = self.f_cost(x_new)
if (self.verb > 1):
print "[%s] line search alpha = %f, fc %d, gc %d, old cost %f, new cost %f" % (
self.name, alpha, fc, gc, self.fx, new_fx)
# Check that inequalities are still satisfied
ineq_marg = self.f_inequalities(x_new)
if ((ineq_marg < -self.INEQ_VIOLATION_THR).any()):
if (self.verb > 1):
print "[%s] WARNING some inequalities are violated with alpha=%f, gonna perform new line search." % (
self.name, alpha)
k = 2.0
for i in range(100):
alpha = min(k * alpha, 1.0)
x_new = x + alpha * x_newton
ineq_marg = self.f_inequalities(x_new)
if ((ineq_marg >= -self.INEQ_VIOLATION_THR).all()):
if (self.verb > 1):
print "[%s] With alpha=%f the solution satisfies the inequalities." % (
self.name, alpha)
break
if (alpha == 1.0):
print "[%s] ERROR With alpha=1 some inequalities are violated, error: %f" % (
self.name, np.min(ineq_marg))
break
x = x_new
if (self.verb > 1):
ineq_marg = self.f_inequalities(x)
activeIneq = np.count_nonzero(np.abs(ineq_marg) < 1e-3)
nViolIneq = np.count_nonzero(
ineq_marg < -self.INEQ_VIOLATION_THR)
print "[%s] Outer iter %d, iter %d, active inequalities %d, violated inequalities %d" % (
self.name, self.outerIter, self.iter, activeIneq,
nViolIneq)
# termination conditions
if (self.iter >= maxIter):
if (self.verb > 1):
print "[%s] Max number of iterations reached %d" % (
self.name, self.iter)
imode = 9
break
if (maxTime < 0.0):
print "[%s] Max time reached %.4f s after %d out iters, %d iters, newtonDec %.6f removeSoftIneq" % (
self.name, self.qpTime, self.outerIter, self.iter,
newton_dec_squared), self.removeSoftInequalities
imode = 9
break
self.iterationNumber = self.iter
else:
print '[%s] Solver type not recognized: %s' % (self.name,
self.solver)
return np.zeros(self.n)
self.computationTime = time.time() - start
ineq = self.f_inequalities(x)
if (self.removeSoftInequalities):
ineq[self.softInequalityIndexes] = 1.0
self.nViolatedInequalities = np.count_nonzero(
ineq < -self.INEQ_VIOLATION_THR)
self.nActiveInequalities = np.count_nonzero(ineq < 1e-3)
self.imode = imode
self.print_solution_info(x)
self.finalize_solution(x)
return (x, imode)
def finalize_solution(self, x):
pass
def f_cost(self, x):
e = np.dot(self.D, x) - self.d
return 0.5 * np.dot(e.T, e)
def f_cost_grad(self, x):
return approx_fprime(x, self.f_cost, self.epsilon)
def f_cost_hess(self, x):
return approx_jacobian(x, self.f_cost_grad, self.epsilon)
def get_linear_inequality_matrix(self):
return self.A
def get_linear_inequality_vectors(self):
return (self.lbA, self.ubA)
def f_inequalities(self, x):
ineq_marg = np.zeros(2 * self.m_in)
Ax = np.dot(self.get_linear_inequality_matrix(), x)
ineq_marg[:self.m_in] = Ax - self.lbA
ineq_marg[self.m_in:] = self.ubA - Ax
return ineq_marg
def f_inequalities_jac(self, x):
return self.get_linear_inequality_matrix()
def print_solution_info(self, x):
if (self.verb > 1):
print(self.name, "Solution is ", x)
def reset(self):
self.initialized = False
def check_grad(self, x=None):
if (x is None):
x = np.random.rand(self.n)
grad = self.f_cost_grad(x)
grad_fd = approx_fprime(x, self.f_cost, self.epsilon)
err = np.sqrt(sum((grad - grad_fd)**2))
print "[%s] Gradient error: %f" % (self.name, err)
return (grad, grad_fd)
def check_hess(self, x=None):
if (x is None):
x = np.random.rand(self.n)
hess = self.f_cost_hess(x)
hess_fd = approx_jacobian(x, self.f_cost_grad, self.epsilon)
err = np.sqrt(np.sum((hess - hess_fd)**2))
print "[%s] Hessian error: %f" % (self.name, err)
return (hess, hess_fd)
def print_qp_oases_error_message(self, imode, solver_name):
if (imode != 0 and imode != PyReturnValue.MAX_NWSR_REACHED):
if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY):
print "[%s] ERROR Qp oases HOTSTART_STOPPED_INFEASIBILITY" % solver_name
# 61
elif (imode == PyReturnValue.MAX_NWSR_REACHED):
print "[%s] ERROR Qp oases RET_MAX_NWSR_REACHED" % solver_name
# 64
elif (imode == PyReturnValue.STEPDIRECTION_FAILED_TQ):
print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_TQ" % solver_name
# 68
elif (imode == PyReturnValue.STEPDIRECTION_FAILED_CHOLESKY):
print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_CHOLESKY" % solver_name
# 69
elif (imode == PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED
):
print "[%s] ERROR Qp oases HOTSTART_FAILED_AS_QP_NOT_INITIALISED" % solver_name
# 54
elif (imode == PyReturnValue.INIT_FAILED_HOTSTART):
print "[%s] ERROR Qp oases INIT_FAILED_HOTSTART" % solver_name
# 36
elif (imode == PyReturnValue.INIT_FAILED_INFEASIBILITY):
print "[%s] ERROR Qp oases INIT_FAILED_INFEASIBILITY" % solver_name
# 37
elif (imode == PyReturnValue.UNKNOWN_BUG):
print "[%s] ERROR Qp oases UNKNOWN_BUG" % solver_name
# 9
else:
print "[%s] ERROR Qp oases %d " % (solver_name, imode)
class qpOASESSolverWrapper(SolverWrapper):
""" A solver wrapper using `qpOASES`.
Parameters
----------
constraint_list: list of :class:`.Constraint`
The constraints the robot is subjected to.
path: :class:`.Interpolator`
The geometric path.
path_discretization: array
The discretized path positions.
"""
def __init__(self, constraint_list, path, path_discretization):
assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!"
super(qpOASESSolverWrapper, self).__init__(
constraint_list, path, path_discretization
)
# Currently only support Canonical Linear Constraint
self.nC = 2 # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u
for i, constraint in enumerate(constraint_list):
if constraint.get_constraint_type() != ConstraintType.CanonicalLinear:
raise NotImplementedError
a, b, c, F, v, _, _ = self.params[i]
if a is not None:
if constraint.identical:
self.nC += F.shape[0]
else:
self.nC += F.shape[1]
self._A = np.zeros((self.nC, self.nV))
self._lA = -np.ones(self.nC)
self._hA = -np.ones(self.nC)
option = Options()
option.printLevel = PrintLevel.NONE
self.solver = SQProblem(self.nV, self.nC)
self.solver.setOptions(option)
def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max):
# NOTE: qpOASES solve QPs of the following form:
# min 0.5 y^T H y + g^T y
# s.t lA <= Ay <= hA
# l <= y <= h
assert i <= self.N and 0 <= i
l = -np.ones(2) * INF
h = np.ones(2) * INF
if x_min is not None:
l[1] = max(l[1], x_min)
if x_max is not None:
h[1] = min(h[1], x_max)
if i < self.N:
delta = self.get_deltas()[i]
if x_next_min is not None:
self._A[0] = [-2 * delta, -1]
self._hA[0] = -x_next_min
self._lA[0] = -INF
else:
self._A[0] = [0, 0]
self._hA[0] = INF
self._lA[0] = -INF
if x_next_max is not None:
self._A[1] = [2 * delta, 1]
self._hA[1] = x_next_max
self._lA[1] = -INF
else:
self._A[1] = [0, 0]
self._hA[1] = INF
self._lA[1] = -INF
cur_index = 2
for j in range(len(self.constraints)):
a, b, c, F, v, ubound, xbound = self.params[j]
# Case 1
if a is not None:
if self.constraints[j].identical:
nC_ = F.shape[0]
self._A[cur_index : cur_index + nC_, 0] = F.dot(a[i])
self._A[cur_index : cur_index + nC_, 1] = F.dot(b[i])
self._hA[cur_index : cur_index + nC_] = v - F.dot(c[i])
self._lA[cur_index : cur_index + nC_] = -INF
else:
nC_ = F[i].shape[0]
self._A[cur_index : cur_index + nC_, 0] = F[i].dot(a[i])
self._A[cur_index : cur_index + nC_, 1] = F[i].dot(b[i])
self._hA[cur_index : cur_index + nC_] = v[i] - F[i].dot(c[i])
self._lA[cur_index : cur_index + nC_] = -INF
cur_index = cur_index + nC_
if ubound is not None:
l[0] = max(l[0], ubound[i, 0])
h[0] = min(h[0], ubound[i, 1])
if xbound is not None:
l[1] = max(l[1], xbound[i, 0])
h[1] = min(h[1], xbound[i, 1])
if H is None:
H = np.zeros((self.get_no_vars(), self.get_no_vars()))
res = self.solver.init(
H, g, self._A, l, h, self._lA, self._hA, np.array([1000])
)
if res == ReturnValue.SUCCESSFUL_RETURN:
var = np.zeros(self.nV)
self.solver.getPrimalSolution(var)
return var
else:
res = np.empty(self.get_no_vars())
res[:] = np.nan
return res
lb=-100.0*np.ones(A_coupl.shape[1])
ub= 100.0*np.ones(A_coupl.shape[1])
return_qp = 0
if (FIRST_QP==True):
options = Options()
options.printLevel = PrintLevel.NONE
qpb = SQProblem(A_coupl.shape[1],A_.shape[0])
qpb.setOptions(options)
qpb.init(H,g,A_,lb,ub,lb_A,ub_A,np.array([QPmaxIt]))
sol=np.zeros(A_coupl.shape[1])
FIRST_QP=False
print "INITIALISATION OF THE QP"
else:
return_qp = qpb.hotstart(H,g,A_,lb,ub,lb_A,ub_A,np.array([QPmaxIt]))
qpb.getPrimalSolution(sol)
log_return_qp.append(return_qp)
solution = np.matrix(sol).T
else:
#Using least square equality constrained : *********************
solution = LSEC(A_coupl,b_coupl,A_,np.matrix(lb_A).T)
qddot = solution[-p.robot.nv:] #qddot
class hotqpOASESSolverWrapper(SolverWrapper):
"""`qpOASES` solver wrapper with hot-start.
This wrapper takes advantage of the warm-start capability of the
qpOASES quadratic programming solver. It uses two different
qp solvers. One to solve for maximized controllable sets and one to
solve for minimized controllable sets. The wrapper selects which solver
to use by looking at the optimization direction.
This solver wrapper also scale data before invoking `qpOASES`.
If the logger "toppra" is set to debug level, qpoases solvers are
initialized with PrintLevel.HIGH. Otherwise, these are initialized
with PrintLevel.NONE
Currently only support Canonical Linear Constraints.
Parameters
----------
constraint_list: :class:`.Constraint` []
The constraints the robot is subjected to.
path: :class:`.Interpolator`
The geometric path.
path_discretization: array
The discretized path positions.
disable_check: bool, optional
Disable check for solution validity. Improve speed by about
20% but entails the possibility that failure is not reported
correctly.
scaling_solverwrapper: bool, optional
If is True, try to scale the data of each optimization before running.
"""
def __init__(
self,
constraint_list,
path,
path_discretization,
disable_check=False,
# scaling_solverwrapper=True,
):
if not qpoases_FOUND:
SolverNotFound("toppra is unable to find any installation of qpoases!")
super(hotqpOASESSolverWrapper, self).__init__(
constraint_list, path, path_discretization
)
self._disable_check = disable_check
# First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u
self.nC = 2 # number of Constraints.
for i, constraint in enumerate(constraint_list):
if constraint.get_constraint_type() != ConstraintType.CanonicalLinear:
raise NotImplementedError
a, b, c, F, v, _, _ = self.params[i]
if a is not None:
if constraint.identical:
self.nC += F.shape[0]
else:
self.nC += F.shape[1]
# qpOASES coefficient arrays
# l <= var <= h
# lA <= A var <= hA
self._A = np.zeros((self.nC, self.nV))
self._lA = -np.ones(self.nC) * QPOASES_INFTY
self._hA = np.ones(self.nC) * QPOASES_INFTY
self._l = -np.ones(2) * QPOASES_INFTY
self._h = np.ones(2) * QPOASES_INFTY
def setup_solver(self):
"""Initiate two internal solvers for warm-start.
"""
option = Options()
if logger.getEffectiveLevel() == logging.DEBUG:
# option.printLevel = PrintLevel.HIGH
option.printLevel = PrintLevel.NONE
else:
option.printLevel = PrintLevel.NONE
self.solver_minimizing = SQProblem(self.nV, self.nC)
self.solver_minimizing.setOptions(option)
self.solver_maximizing = SQProblem(self.nV, self.nC)
self.solver_maximizing.setOptions(option)
self.solver_minimizing_recent_index = -2
self.solver_maximizing_recent_index = -2
def close_solver(self):
self.solver_minimizing = None
self.solver_maximizing = None
def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min, x_next_max):
assert i <= self.N and 0 <= i
# solve the scaled optimization problem
# min 0.5 y^T scale H scale y + g^T scale y
# s.t lA <= A scale y <= hA
# l <= scale y <= h
self._l[:] = -QPOASES_INFTY
self._h[:] = QPOASES_INFTY
if x_min is not None:
self._l[1] = max(self._l[1], x_min)
if x_max is not None:
self._h[1] = min(self._h[1], x_max)
if i < self.N:
delta = self.get_deltas()[i]
if x_next_min is not None:
self._A[0] = [-2 * delta, -1]
self._hA[0] = -x_next_min
else:
self._A[0] = [0, 0]
self._hA[0] = QPOASES_INFTY
self._lA[0] = -QPOASES_INFTY
if x_next_max is not None:
self._A[1] = [2 * delta, 1]
self._hA[1] = x_next_max
else:
self._A[1] = [0, 0]
self._hA[1] = QPOASES_INFTY
self._lA[1] = -QPOASES_INFTY
cur_index = 2
for j in range(len(self.constraints)):
a, b, c, F, v, ubound, xbound = self.params[j]
if a is not None:
if self.constraints[j].identical:
nC_ = F.shape[0]
self._A[cur_index : cur_index + nC_, 0] = F.dot(a[i])
self._A[cur_index : cur_index + nC_, 1] = F.dot(b[i])
self._hA[cur_index : cur_index + nC_] = v - F.dot(c[i])
self._lA[cur_index : cur_index + nC_] = -QPOASES_INFTY
else:
nC_ = F[i].shape[0]
self._A[cur_index : cur_index + nC_, 0] = F[i].dot(a[i])
self._A[cur_index : cur_index + nC_, 1] = F[i].dot(b[i])
self._hA[cur_index : cur_index + nC_] = v[i] - F[i].dot(c[i])
self._lA[cur_index : cur_index + nC_] = -QPOASES_INFTY
cur_index = cur_index + nC_
if ubound is not None:
self._l[0] = max(self._l[0], ubound[i, 0])
self._h[0] = min(self._h[0], ubound[i, 1])
if xbound is not None:
self._l[1] = max(self._l[1], xbound[i, 0])
self._h[1] = min(self._h[1], xbound[i, 1])
# if x_min == x_max, do not solve the 2D linear program, instead, do a line search
if abs(x_min - x_max) < TINY and H is None and self.get_no_vars() == 2:
logger.debug("x_min ({:f}) equals x_max ({:f})".format(x_min, x_max))
u_min = -QPOASES_INFTY
u_max = QPOASES_INFTY
for i in range(self._A.shape[0]):
if self._A[i, 0] > 0:
u_max = min(
u_max, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0]
)
elif self._A[i, 0] < 0:
u_min = max(
u_min, (self._hA[i] - self._A[i, 1] * x_min) / self._A[i, 0]
)
if (u_min - u_max) / abs(u_max) > SMALL: # problem infeasible
logger.debug(
"u_min > u_max by {:f}. Might not be critical. "
"Returning failure.".format(u_min - u_max)
)
return np.array([np.nan, np.nan])
if g[0] < 0:
return np.array([u_max, x_min + 2 * u_max * delta])
else:
return np.array([u_min, x_min + 2 * u_min * delta])
if H is None:
H = (
np.ones((self.get_no_vars(), self.get_no_vars())) * 1e-18
) # regularization, very important
ratio_col1 = 1 / (
np.sum(np.abs(self._A[2:, 0])) + 1e-5
) # the maximum possible value for both ratios is 100000
ratio_col2 = 1 / (np.sum(np.abs(self._A[2:, 1])) + 1e-5)
variable_scales = np.array([ratio_col1, ratio_col2])
# variable_scales = np.array([5000.0, 2000.0])
variable_scales_mat = np.diag(variable_scales)
if logger.isEnabledFor(logging.DEBUG):
logger.debug(
"min ratio col 1 {:f}, col 2 {:f}".format(ratio_col1, ratio_col2)
)
# ratio scaling
self._A = self._A.dot(variable_scales_mat)
self._l = self._l / variable_scales
self._h = self._h / variable_scales
self._g = g * variable_scales
self._H = variable_scales_mat.dot(H).dot(variable_scales_mat)
# rows scaling
row_magnitude = np.sum(np.abs(self._A), axis=1)
row_scaling_mat = np.diag((row_magnitude + 1) ** (-1))
self._A = np.dot(row_scaling_mat, self._A)
self._lA = np.dot(row_scaling_mat, self._lA)
self._hA = np.dot(row_scaling_mat, self._hA)
return_value, var = self._solve_optimization(i)
if return_value == ReturnValue.SUCCESSFUL_RETURN:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("optimal value: {:}".format(var))
if self._disable_check:
return var * variable_scales
# Check for constraint feasibility
success = (
np.all(self._l <= var + TINY)
and np.all(var <= self._h + TINY)
and np.all(np.dot(self._A, var) <= self._hA + TINY)
and np.all(np.dot(self._A, var) >= self._lA - TINY)
)
if not success:
# import ipdb; ipdb.set_trace()
logger.fatal(
"Hotstart fails but qpOASES does not report correctly. \n "
"var: {:}, lower_bound: {:}, higher_bound{:}".format(
var, self._l, self._h
)
)
# TODO: Investigate why this happen and fix the
# relevant code (in qpOASES wrapper)
else:
return var * variable_scales
else:
logger.debug("Optimization fails. qpOASES error code: %d.", return_value)
if (
np.all(0 <= self._hA)
and np.all(0 >= self._lA)
and np.all(0 <= self._h)
and np.all(0 >= self._l)
):
logger.fatal(
"(0, 0) satisfies all constraints => error due to numerical errors.",
self._A,
self._lA,
self._hA,
self._l,
self._h,
)
else:
logger.debug("(0, 0) does not satisfy all constraints.")
return_value = np.empty(self.get_no_vars())
return_value[:] = np.nan
return return_value
def _solve_optimization(self, i):
var = np.zeros(self.nV)
if self._g[1] > 0: # Choose solver_minimizing
if abs(self.solver_minimizing_recent_index - i) > 1:
logger.debug("solver_minimizing [init]")
return_value = self.solver_minimizing.init(
self._H,
self._g,
self._A,
self._l,
self._h,
self._lA,
self._hA,
np.array([1000]),
)
else:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("solver_minimizing [hotstart]")
return_value = self.solver_minimizing.hotstart(
self._H,
self._g,
self._A,
self._l,
self._h,
self._lA,
self._hA,
np.array([1000]),
)
self.solver_minimizing_recent_index = i
self.solver_minimizing.getPrimalSolution(var)
else: # Choose solver_maximizing
if abs(self.solver_maximizing_recent_index - i) > 1:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("solver_maximizing [init]")
return_value = self.solver_maximizing.init(
self._H,
self._g,
self._A,
self._l,
self._h,
self._lA,
self._hA,
np.array([1000]),
)
else:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("solver_maximizing [hotstart]")
return_value = self.solver_maximizing.hotstart(
self._H,
self._g,
self._A,
self._l,
self._h,
self._lA,
self._hA,
np.array([1000]),
)
self.solver_maximizing_recent_index = i
self.solver_maximizing.getPrimalSolution(var)
return return_value, var
def main():
rospy.init_node('controller_2dof', anonymous=True)
rospy.Rate(10)
rospy.sleep(0.2)
# Setup data of QP.
# Weights.
w1 = 1e-3
w2 = 1e-5
w3 = 1 # slack (attractor)
# Joint limits.
q0_max = 0.05
q1_max = 0.15
# Links
l1 = 0.1
l2 = 0.2
l3 = 0.3
# Initial joint values.
q0_init = q0 = -0.04
q1_init = q1 = -0.08
# Joint target.
q_des = 0.65
# Slack limits.
e_max = 1000
# Velocity limits.(+-)
v0_max = 0.05
v1_max = 0.1
# Acceleration limits.(+-)
a0_max = 0.05 * 0.5
a1_max = 0.1 * 0.5
# Others
precision = 1e-3
p = 10
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
vel_init = 0
nWSR = np.array([100])
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(3, 5)
H = np.array([w1, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, w3]).reshape((3, 3))
A = np.array([
1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0,
0.0
]).reshape((5, 3))
g = np.array([0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e_max])
ub = np.array([v0_max, v1_max, e_max])
lbA = np.array([error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max])
ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max])
# Setting up QProblem object
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.LOW
example.setOptions(options)
print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" %
(q_eef, q_des, error, q0, q1))
print A
i = 0
n = 0
limit = abs(error)
ok = False
Opt = np.zeros(3)
# Plotting
t = np.array(i)
pos_eef = np.array(q_eef)
pos_0 = np.array(q0)
pos_1 = np.array(q1)
vel_0 = np.array(vel_init)
vel_1 = np.array(vel_init)
vel_eef = np.array(vel_init)
p_error = np.array(error)
goal = np.array(q_des)
lim1 = np.array(q0_max)
lim2 = np.array(q1_max)
return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Init of QP-Problem returned without success! ERROR MESSAGE: "
return -1
while not rospy.is_shutdown():
while limit > precision and i < 400:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error
lbA[1] = -q0_max - q0
lbA[2] = -q1_max - q1
lbA[3] = a0_const * Opt[0] - a0_max
lbA[4] = a1_const * Opt[1] - a1_max
ubA[0] = error
ubA[1] = q0_max - q0
ubA[2] = q1_max - q1
ubA[3] = a0_const * Opt[0] + a0_max
ubA[4] = a1_const * Opt[1] + a1_max
return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: "
return -1
# Get and print solution of QP.
example.getPrimalSolution(Opt)
q0 += Opt[0] / 100
q1 += Opt[1] / 100
q_eef = l1 + l2 + l3 + q0 + q1
error = p * (q_des - q_eef)
limit = abs(error)
# print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % (
# Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1)
# Plotting arrays
pos_eef = np.hstack((pos_eef, q_eef))
pos_0 = np.hstack((pos_0, q0))
pos_1 = np.hstack((pos_1, q1))
vel_0 = np.hstack((vel_0, Opt[0]))
vel_1 = np.hstack((vel_1, Opt[1]))
vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1]))
goal = np.hstack((goal, q_des))
p_error = np.hstack((p_error, error))
t = np.hstack((t, i))
lim1 = np.hstack((lim1, q0_max))
lim2 = np.hstack((lim2, q1_max))
# Plot
t_eef = go.Scatter(y=pos_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_eef',
line=dict(dash='dash'))
t_p0 = go.Scatter(y=pos_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q0',
line=dict(dash='dash'))
t_p1 = go.Scatter(y=pos_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_q1',
line=dict(dash='dash'))
t_v0 = go.Scatter(y=vel_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q0')
t_v1 = go.Scatter(y=vel_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_q1')
t_q0 = go.Scatter(y=lim1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='limit_0')
t_q1 = go.Scatter(y=lim2,
x=t,
marker=dict(size=4, ),
mode='lines',
name='limit_1')
t_veef = go.Scatter(y=vel_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_eef')
t_er = go.Scatter(y=p_error,
x=t,
marker=dict(size=4, ),
mode='lines+markers',
name='error')
t_goal = go.Scatter(y=goal,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal',
line=dict(dash='dot'))
print "\n i = %g \n" % (i)
data_eef = [t_eef, t_veef, t_goal]
layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" %
(q_eef_init, q_des),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=2,
),
yaxis=dict(title='Position / Velocity'),
font=dict(size=16))
fig = dict(data=data_eef, layout=layout_eef)
plotly.offline.plot(fig,
filename='html/basic_example_eef.html',
image='png',
image_filename='basic_eef')
data = [t_p0, t_v0]
layout = dict(title="Initial position q0 =%g, q1 = %g.\n" %
(q0_init, q1_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.08, .12],
),
font=dict(size=18))
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/basic_example_q0.html',
image='png',
image_filename='basic_0')
data = [t_p1, t_v1]
layout = dict(title="Initial position q0 =%g, q1 = %g.\n" %
(q0_init, q1_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.08, .12],
),
font=dict(size=18))
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/basic_example_q1.html',
image='png',
image_filename='basic_1')
return 0
def qpoases_calculation(self, error_posit):
# Variable initialization
self._feedback.sim_trajectory = [self.current_state]
error_orient = np.array([0.0, 0.0, 0.0])
eef_pose_array = PoseArray()
reached_orientation = False
self.trajectory_length = 0.0
self.old_dist = 0.0
# Setting up QProblem object
vel_calculation = SQProblem(self.problem_size[1], self.problem_size[0])
options = Options()
options.setToDefault()
options.printLevel = PrintLevel.LOW
vel_calculation.setOptions(options)
Opt = np.zeros(self.problem_size[1])
i = 0
# Config iai_naive_kinematics_sim, send commands
clock = ProjectionClock()
velocity_msg = JointState()
velocity_msg.name = self.all_joint_names
velocity_msg.header.stamp = rospy.get_rostime()
velocity_array = [0.0 for x in range(len(self.all_joint_names))]
effort_array = [0.0 for x in range(len(self.all_joint_names))]
velocity_msg.velocity = velocity_array
velocity_msg.effort = effort_array
velocity_msg.position = effort_array
clock.now = rospy.get_rostime()
clock.period.nsecs = 1000000
self.pub_clock.publish(clock)
# Plot for debugging
'''t = np.array([i])
base_weight = np.array(self.jweights[0, 0])
arm_weight = np.array(self.jweights[4, 4])
triang_weight = np.array(self.jweights[3, 3])
low, pos, high, vel_p, error = [], [], [], [], []
error = np.array([0])
for x in range(8+3):
low.append(np.array([self.joint_limits_lower[x]]))
pos.append(np.array(self.joint_values[x]))
vel_p.append(np.array(Opt[x]))
high.append(np.array([self.joint_limits_upper[x]]))#'''
tic = rospy.get_rostime()
while not self.reach_position or not reached_orientation or not self.reach_pregrasp:
i += 1
# Check if client cancelled goal
if not self.active_goal_flag:
self.success = False
break
if i > 500:
self.abort_goal('time_out')
break
# Solve QP, redefine limit vectors of A.
eef_pose_msg = Pose()
nWSR = np.array([100])
[ac_lim_lower, ac_lim_upper] = self.acceleration_limits(Opt)
self.joint_dist_lower_lim = self.joint_limits_lower - self.joint_values
self.joint_dist_upper_lim = self.joint_limits_upper - self.joint_values
self.lbA = np.hstack((error_posit, error_orient,
self.joint_dist_lower_lim, ac_lim_lower))
self.ubA = np.hstack((error_posit, error_orient,
self.joint_dist_upper_lim, ac_lim_upper))
# Recalculate H matrix
self.calculate_weigths(error_posit)
vel_calculation = SQProblem(self.problem_size[1],
self.problem_size[0])
vel_calculation.setOptions(options)
return_value = vel_calculation.init(self.H, self.g, self.A,
self.lb, self.ub, self.lbA,
self.ubA, nWSR)
vel_calculation.getPrimalSolution(Opt)
if return_value != returnValue.SUCCESSFUL_RETURN:
rospy.logerr("QP-Problem returned without success! ")
print 'joint limits: ', self.joint_limits_lower
print 'joint values: ', self.joint_values
print 'position error:', error_posit / self.prop_gain
print 'eef ori: [%.3f %.3f %.3f] ' % (
self.eef_pose.p[0], self.eef_pose.p[1], self.eef_pose.p[2])
print 'goal ori : [%.3f %.3f %.3f] ' % (
self.goal_orient_euler[0],
self.goal_orient_euler[0],
self.goal_orient_euler[0],
)
self.abort_goal('infeasible')
break
for x, vel in enumerate(Opt[4:self.nJoints]):
velocity_msg.velocity[self.start_arm[x]] = vel
velocity_msg.velocity[self.triang_list_pos] = Opt[3]
velocity_msg.velocity[0] = Opt[0]
velocity_msg.velocity[1] = Opt[1]
# Recalculate Error in EEF position
error_posit, limit_p = self.calc_posit_error(error_posit)
# Recalculate Error in EEF orientation
eef_orient = qo.rotation_to_quaternion(self.eef_pose.M)
error_orient, reached_orientation = self.calc_orient_error(
eef_orient, self.goal_quat, 0.35, limit_p)
# Get trajectory length
self.trajectory_length = self.get_trajectory_length(
self.eef_pose.p, self.trajectory_length)
# Print velocity for iai_naive_kinematics_sim
velocity_msg.header.stamp = rospy.get_rostime()
self.pub_velocity.publish(velocity_msg)
clock.now = rospy.get_rostime()
self.pub_clock.publish(clock)
# Action feedback
self.action_status.status = 1
self._feedback.sim_status = self.action_status.text = 'Calculating trajectory'
self._feedback.sim_trajectory.append(self.current_state)
self._feedback.trajectory_length = self.trajectory_length
self.action_server.publish_feedback(self.action_status,
self._feedback)
self.action_server.publish_status()
self.success = True
# Store EEF pose for plotting
eef_pose_msg.position.x = self.eef_pose.p[0]
eef_pose_msg.position.y = self.eef_pose.p[1]
eef_pose_msg.position.z = self.eef_pose.p[2]
eef_pose_msg.orientation.x = eef_orient[0]
eef_pose_msg.orientation.y = eef_orient[1]
eef_pose_msg.orientation.z = eef_orient[2]
eef_pose_msg.orientation.w = eef_orient[3]
eef_pose_array.poses.append(eef_pose_msg)
# Plot for debuging
'''for x in range(8+3):
low[x] = np.hstack((low[x], self.joint_limits_lower[x]))
pos[x] = np.hstack((pos[x], self.joint_values[x]))
vel_p[x] = np.hstack((vel_p[x], Opt[x]))
high[x] = np.hstack((high[x], self.joint_limits_upper[x]))
e = error_posit / self.prop_gain
e_p = sqrt(pow(e[0], 2) + pow(e[1], 2) + pow(e[2], 2))
error = np.hstack((error, e_p))
base_weight = np.hstack((base_weight, self.jweights[0, 0]))
arm_weight = np.hstack((arm_weight, self.jweights[4, 4]))
triang_weight = np.hstack((triang_weight, self.jweights[3, 3]))
t = np.hstack((t, i))#'''
# print '\n iter: ', i
# goal = euler_from_quaternion(self.goal_quat)
# print 'joint_vel: ', Opt[:-6]
# print 'error pos: ', error_posit / self.prop_gain
# print 'eef ori: [%.3f %.3f %.3f] '%(self.eef_pose.p[0],self.eef_pose.p[1], self.eef_pose.p[2])
# print 'ori error: [%.3f %.3f %.3f] '%(error_orient[0], error_orient[1], error_orient[2])
# print 'goal ori : [%.3f %.3f %.3f] '%(goal[0], goal[1], goal[2])
# print 'slack : ', Opt[-6:]
self.final_error = sqrt(
pow(error_posit[0], 2) + pow(error_posit[1], 2) +
pow(error_posit[2], 2))
eef_pose_array.header.stamp = rospy.get_rostime()
eef_pose_array.header.frame_id = self.gripper
self.pub_plot.publish(eef_pose_array)
# Plot
'''plt.style.use('ggplot')
plt.plot(t, arm_weight, 'g-.', lw=3, label='arm')
plt.plot(t, base_weight, 'k--', lw=3, label='base')
plt.plot(t, triang_weight, 'm', lw=2, label='torso')
plt.xlabel('Iterations')
plt.ylabel('Weights')
plt.title('Arm, base and torso weights')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('weights.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
plt.plot(t, error, 'k', lw=2)
plt.xlabel('Iterations')
plt.ylabel('Position Error [m]')
plt.title('Position Error')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('error.tex', figureheight='4cm', figurewidth='16cm')
plt.cla()#'''
'''for x in range(8+3):
plt.plot(t, low[x], lw=1)
plt.plot(t, pos[x], lw=3)
plt.plot(t, vel_p[x], lw=3)
plt.plot(t, high[x], lw=1)
plt.xlabel('Iterations')
plt.ylabel('Position / Velocity')
plt.grid(True)
# plt.show()
tikz_save('joint'+str(x)+'.tex', figureheight='5cm', figurewidth='4.5cm')
plt.cla()'''
'''plt.plot(t, pos[0], 'r--', lw=3, label='x')
plt.plot(t, pos[1], 'b', lw=2, label='y')
plt.xlabel('Iterations')
plt.ylabel('Base Position [m]')
plt.title('Trajectory of the base [m]')
plt.grid(True)
plt.legend()
# plt.show()
tikz_save('base_pos.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
step = 2
plt.plot(t[0:-1:step], pos[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0')
plt.plot(t[0:-1:step], pos[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1')
plt.plot(t[0:-1:step], pos[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2')
plt.plot(t[0:-1:step], pos[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3')
plt.plot(t, pos[7], 'k-.', lw=3, label='j4')
plt.plot(t, pos[8], '--', lw=3, label='j5')
plt.plot(t, pos[9], lw=3, label='j6')
plt.xlabel('Iterations')
plt.ylabel('Position [rad]')
plt.title('Trajectory of the joints')
plt.grid(True)
plt.legend(loc='upper left')
tikz_save('joint_pos.tex', figureheight='8cm', figurewidth='16cm')
plt.cla()
plt.plot(t, vel_p[0], 'r--', lw=3, label='x')
plt.plot(t, vel_p[1], 'b',lw=2, label='y')
plt.xlabel('Iterations')
plt.ylabel('Base Velocity [m/sec]')
plt.title('Velocity of the base')
plt.grid(True)
plt.legend()
tikz_save('base_vel.tex', figureheight='5cm', figurewidth='16cm')
plt.cla()
plt.plot(t[0:-1:step], vel_p[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0')
plt.plot(t[0:-1:step], vel_p[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1')
plt.plot(t[0:-1:step], vel_p[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2')
plt.plot(t[0:-1:step], vel_p[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3')
plt.plot(t, vel_p[7], 'k-.', lw=3, label='j4')
plt.plot(t, vel_p[8], '--', lw=3, label='j5')
plt.plot(t, vel_p[9], lw=3, label='j6')
plt.xlabel('Iterations')
plt.ylabel('Arms Joints Velocity [rad/sec]')
plt.title("Velocity of the arm's joints")
plt.legend(loc='upper left')
plt.grid(True)
tikz_save('joint_vel.tex', figureheight='8cm', figurewidth='16cm')
plt.cla()#'''
toc = rospy.get_rostime()
print(toc.secs - tic.secs), 'sec Elapsed'
return 0
def main():
rospy.init_node('controller_2dof', anonymous=True)
rospy.Rate(10)
# Setup data of QP.
# Joint Weights.
w1 = 1e-3
w2 = 1e-3
# Joint limits.
q0_max = 0.05
q1_max = 0.15
# Links
l1 = 0.1
l2 = 0.2
l3 = 0.25
# Initial joint values.
q0_init = q0 = -0.03
q1_init = q1 = 0.0
# Joint target.
q_des1 = 0.4
q_des2 = 0.7
q_des3 = 0.8
# Slack limits.
e1_max = 1000
e2_max = 1000
e3_max = 1000
# Velocity limits.(+-)
v0_max = 0.05
v1_max = 0.1
# Acceleration limits.(+-)
a0_max = 0.05 * 0.5
a1_max = 0.1 * 0.5
# Others
precision = 1e-3
p = 10
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error1 = p * (q_des1 - q_eef)
error2 = p * (q_des2 - q_eef)
error3 = p * (q_des3 - q_eef)
vel_init = 0
nWSR = np.array([100])
# Dynamic goal weights
sum_error = abs(error1) + abs(error2) + abs(error3)
min_error = min(abs(error1), abs(error2), abs(error3))
max_error = max(abs(error1), abs(error2), abs(error3))
w3 = (1 - (abs(error1) - min_error) /
(max_error - min_error))**3 # goal 1 weight
w4 = (1 - (abs(error2) - min_error) /
(max_error - min_error))**3 # goal 2 weight
w5 = (1 - (abs(error3) - min_error) /
(max_error - min_error))**3 # goal 3 weight
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(5, 7)
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0,
0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5
]).reshape((5, 5))
A = np.array([
1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0,
1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0
]).reshape((7, 5))
g = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max, -e3_max])
ub = np.array([v0_max, v1_max, e1_max, e2_max, e3_max])
lbA = np.array([
error1, error2, error3, (-q0_max - q0), (-q1_max - q0), -a0_max,
-a1_max
])
ubA = np.array(
[error1, error2, error3, (q0_max - q0), (q1_max - q0), a0_max, a1_max])
# Setting up QProblem object
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.LOW
example.setOptions(options)
Opt = np.zeros(5)
print(
"Init pos = %g, goal_1 = %g, goal_2 = %g, error_1 = %g, error_2 = %g, q0 =%g, q1 = %g\n"
% (q_eef, q_des1, q_des2, error1, error2, q0, q1))
print 'A = {}\n'.format(A)
i = 0
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
limit3 = abs(error3)
lim = min([limit1, limit2, limit3])
diff1 = abs(error1)
diff2 = abs(error2)
diff3 = abs(error3)
diff = min([diff1, diff2, diff3])
# Plotting
t = np.array(i)
pos_eef = np.array(q_eef)
pos_0 = np.array(q0)
pos_1 = np.array(q1)
vel_0 = np.array(vel_init)
vel_1 = np.array(vel_init)
vel_eef = np.array(vel_init)
p_error = np.array(error1)
go_1 = np.array(q_des1)
go_2 = np.array(q_des2)
go_3 = np.array(q_des3)
return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Init of QP-Problem returned without success! ERROR MESSAGE: "
return -1
while not rospy.is_shutdown():
while (diff > 0.0009) and lim > precision and i < 400:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error1
lbA[1] = error2
lbA[2] = error3
lbA[3] = -q0_max - q0
lbA[4] = -q1_max - q1
lbA[5] = a0_const * Opt[0] - a0_max
lbA[6] = a1_const * Opt[1] - a1_max
ubA[0] = error1
ubA[1] = error2
ubA[2] = error3
ubA[3] = q0_max - q0
ubA[4] = q1_max - q1
ubA[5] = a0_const * Opt[0] + a0_max
ubA[6] = a1_const * Opt[1] + a1_max
return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: "
return -1
old_error1 = error1
old_error2 = error2
old_error3 = error3
example.getPrimalSolution(Opt)
# Update limits
q0 += Opt[0] / 100
q1 += Opt[1] / 100
q_eef = l1 + l2 + l3 + q0 + q1
error1 = p * (q_des1 - q_eef)
error2 = p * (q_des2 - q_eef)
error3 = p * (q_des3 - q_eef)
# Update weights
min_error = min(abs(error1), abs(error2), abs(error3))
max_error = max(abs(error1), abs(error2), abs(error3))
w3 = (1 - (abs(error1) - min_error) /
(max_error - min_error))**3 # goal 1
w4 = (1 - (abs(error2) - min_error) /
(max_error - min_error))**3 # goal 2
w5 = (1 - (abs(error3) - min_error) /
(max_error - min_error))**3 # goal 3
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3,
0.0, 0.0, 0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5
]).reshape((5, 5))
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
limit3 = abs(error3)
lim = min([limit1, limit2, limit3])
diff1 = abs(error1 - old_error1)
diff2 = abs(error2 - old_error2)
diff3 = abs(error3 - old_error3)
diff = min([diff1, diff2, diff3])
# print 'weights= [ %g, %g, %g ]'%(w3, w4, w5)
# print 'vel = [ %g, %g ], error = [ %g, %g, %g ] \n'% (Opt[0], Opt[1], error1, error2, error3)
# Plotting arrays
pos_eef = np.hstack((pos_eef, q_eef))
pos_0 = np.hstack((pos_0, q0))
pos_1 = np.hstack((pos_1, q1))
vel_0 = np.hstack((vel_0, Opt[0]))
vel_1 = np.hstack((vel_1, Opt[1]))
vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1]))
p_error = np.hstack((p_error, error1))
go_1 = np.hstack((go_1, q_des1))
go_2 = np.hstack((go_2, q_des2))
go_3 = np.hstack((go_3, q_des3))
t = np.hstack((t, i))
# Plot
t_eef = go.Scatter(y=pos_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_eef',
line=dict(dash='dash'))
t_p0 = go.Scatter(y=pos_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_0',
line=dict(dash='dash'))
t_p1 = go.Scatter(y=pos_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_1',
line=dict(dash='dash'))
t_v0 = go.Scatter(y=vel_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_0')
t_v1 = go.Scatter(y=vel_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_1')
t_er = go.Scatter(y=p_error,
x=t,
marker=dict(size=4, ),
mode='lines',
name='error')
t_veef = go.Scatter(y=vel_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_eef')
t_g1 = go.Scatter(y=go_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_1',
line=dict(dash='dot'))
t_g2 = go.Scatter(y=go_2,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_2',
line=dict(dash='dot', width=4))
t_g3 = go.Scatter(y=go_3,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_3',
line=dict(dash='dot', width=6))
data = [t_eef, t_veef, t_g1, t_g2, t_g3]
layout = dict(
title=
"Initial position eef = %g. Goals: goal_1 = %g, goal_2 = %g, goal_3 = %g\n"
% (q_eef_init, q_des1, q_des2, q_des3),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=2,
),
yaxis=dict(title='Position / Velocity'),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_3_weights_EEF.html',
image='png',
image_filename='dyn_3_eef_2')
data = [t_p0, t_v0]
layout = dict(
title="Initial position q0 =%g.\n" % (q0_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.11, .045],
),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig, filename='html/dynamic_3_weights_q0.html')
data = [t_p1, t_v1]
layout = dict(
title="Initial position q1 = %g.\n" % (q1_init),
xaxis=dict(title='Iterations',
autotick=False,
dtick=25,
gridwidth=2),
yaxis=dict(
title='Position / Velocity',
range=[-0.11, .045],
),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig, filename='html/dynamic_3_weights__q1.html')
print "\n i = %g \n" % i
return 0
class SolverLPQpOases(solver_LP_abstract.SolverLPAbstract):
"""
Linear Program solver:
minimize c' x
subject to Alb <= A x <= Aub
lb <= x <= ub
"""
def __init__(self,
name,
maxIter=1000,
maxTime=100.0,
useWarmStart=True,
verb=0):
solver_LP_abstract.SolverLPAbstract.__init__(self, name, maxIter,
maxTime, useWarmStart,
verb)
self._hessian_regularization = DEFAULT_HESSIAN_REGULARIZATION
self._options = Options()
self._options.setToReliable()
if (self._verb <= 1):
self._options.printLevel = PrintLevel.NONE
elif (self._verb == 2):
self._options.printLevel = PrintLevel.LOW
elif (self._verb == 3):
self._options.printLevel = PrintLevel.MEDIUM
elif (self._verb > 3):
self._options.printLevel = PrintLevel.DEBUG_ITER
self._options.enableRegularisation = False
self._options.enableEqualities = True
self._n = -1
self._m_con = -1
self._qpOasesSolver = None
def set_option(self, key, value):
if (key == 'hessian_regularization'):
self.set_hessian_regularization(value)
return True
return False
def set_hessian_regularization(self, reg):
assert reg > 0, "Hessian regularization must be positive"
self._hessian_regularization = reg
if (self._n > 0):
self._Hess = self._hessian_regularization * np.identity(self._n)
''' Solve the linear program
minimize c' x
subject to Alb <= A_in x <= Aub
A_eq x = b
lb <= x <= ub
Return a tuple containing:
status flag
primal solution
dual solution
'''
def solve(self,
c,
lb,
ub,
A_in=None,
Alb=None,
Aub=None,
A_eq=None,
b=None):
start = time.time()
n = c.shape[0]
m_con = 0
if ((A_in is not None) and (A_eq is not None)):
m_con = A_in.shape[0] + A_eq.shape[0]
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
m_in = A_in.shape[0]
self._A_con[:m_in, :] = A_in
self._lb_con[:m_in] = Alb
self._ub_con[:m_in] = Aub
self._A_con[m_in:, :] = A_eq
self._lb_con[m_in:] = b
self._ub_con[m_in:] = b
elif (A_in is not None):
m_con = A_in.shape[0]
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
self._A_con[:, :] = A_in
self._lb_con[:] = Alb
self._ub_con[:] = Aub
elif (A_eq is not None):
m_con = A_eq.shape[0]
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
self._A_con[:, :] = A_eq
self._lb_con[:] = b
self._ub_con[:] = b
else:
m_con = 0
if (m_con != self._m_con or n != self._n):
self._A_con = np.empty((m_con, n))
self._lb_con = np.empty((m_con))
self._ub_con = np.empty((m_con))
if (n != self._n or m_con != self._m_con):
self._qpOasesSolver = SQProblem(n, m_con)
#, HessianType.SEMIDEF);
self._qpOasesSolver.setOptions(self._options)
self._Hess = self._hessian_regularization * np.identity(n)
self._x = np.zeros(n)
self._y = np.zeros(n + m_con)
self._n = n
self._m_con = m_con
self._initialized = False
maxActiveSetIter = np.array([self._maxIter])
maxComputationTime = np.array(self._maxTime)
if (not self._useWarmStart or not self._initialized):
self._imode = self._qpOasesSolver.init(self._Hess, c, self._A_con,
lb, ub, self._lb_con,
self._ub_con,
maxActiveSetIter,
maxComputationTime)
else:
self._imode = self._qpOasesSolver.hotstart(
self._Hess, c, self._A_con, lb, ub, self._lb_con, self._ub_con,
maxActiveSetIter, maxComputationTime)
self._lpTime = maxComputationTime
self._iter = 1 + maxActiveSetIter[0]
if (self._imode == 0):
self._initialized = True
self._lpStatus = solver_LP_abstract.LP_status.OPTIMAL
self._qpOasesSolver.getPrimalSolution(self._x)
self._qpOasesSolver.getDualSolution(self._y)
self.checkConstraints(self._x, lb, ub, A_in, Alb, Aub, A_eq, b)
else:
if (self._imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY
or self._imode == PyReturnValue.INIT_FAILED_INFEASIBILITY):
self._lpStatus = solver_LP_abstract.LP_status.INFEASIBLE
elif (self._imode == PyReturnValue.MAX_NWSR_REACHED):
self._lpStatus = solver_LP_abstract.LP_status.MAX_ITER_REACHED
else:
self._lpStatus = solver_LP_abstract.LP_status.ERROR
self.reset()
self._x = np.zeros(n)
self._y = np.zeros(n + m_con)
if (self._verb > 0):
print "[%s] ERROR Qp oases %s" % (
self._name, qpOasesSolverMsg(self._imode))
if (self._lpTime >= self._maxTime):
if (self._verb > 0):
print "[%s] Max time reached %f after %d iters" % (
self._name, self._lpTime, self._iter)
self._imode = 9
self._computationTime = time.time() - start
return (self._lpStatus, self._x, self._y)
class hotqpOASESSolverWrapper(SolverWrapper):
"""A solver wrapper using `qpOASES`.
This wrapper takes advantage of the warm-start capability of
qpOASES quadratic programming solver by using two different
solvers. One to solve for maximized controllable sets and one to
solve for minimized controllable sets.
If the logger "toppra" is set to debug level, qpoases solvers are
initialized with PrintLevel.HIGH. Otherwise, these are initialized
with PrintLevel.NONE
Parameters
----------
constraint_list: list of :class:`.Constraint`
The constraints the robot is subjected to.
path: :class:`.Interpolator`
The geometric path.
path_discretization: array
The discretized path positions.
disable_check: bool, optional
Disable check for solution validity. Improve speed by about
20% but entails the possibility that failure is not reported
correctly.
"""
def __init__(self,
constraint_list,
path,
path_discretization,
disable_check=False):
assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!"
super(hotqpOASESSolverWrapper, self).__init__(constraint_list, path,
path_discretization)
self._disable_check = disable_check
# Currently only support Canonical Linear Constraint
self.nC = 2 # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u
for i, constraint in enumerate(constraint_list):
if constraint.get_constraint_type(
) != ConstraintType.CanonicalLinear:
raise NotImplementedError
a, b, c, F, v, _, _ = self.params[i]
if a is not None:
if constraint.identical:
self.nC += F.shape[0]
else:
self.nC += F.shape[1]
self._A = np.zeros((self.nC, self.nV))
self._lA = -np.ones(self.nC) * INF
self._hA = np.ones(self.nC) * INF
self._l = -np.ones(2) * INF
self._h = np.ones(2) * INF
def setup_solver(self):
option = Options()
if logger.getEffectiveLevel() == logging.DEBUG:
# option.printLevel = PrintLevel.HIGH
option.printLevel = PrintLevel.NONE
else:
option.printLevel = PrintLevel.NONE
self.solver_up = SQProblem(self.nV, self.nC)
self.solver_up.setOptions(option)
self.solver_down = SQProblem(self.nV, self.nC)
self.solver_down.setOptions(option)
self.solver_up_recent_index = -2
self.solver_down_recent_index = -2
def close_solver(self):
self.solver_up = None
self.solver_down = None
def solve_stagewise_optim(self, i, H, g, x_min, x_max, x_next_min,
x_next_max):
# NOTE: qpOASES solve QPs of the following form:
# min 0.5 y^T H y + g^T y
# s.t lA <= Ay <= hA
# l <= y <= h
assert i <= self.N and 0 <= i
self._l[:] = -INF
self._h[:] = INF
if x_min is not None:
self._l[1] = max(self._l[1], x_min)
if x_max is not None:
self._h[1] = min(self._h[1], x_max)
if i < self.N:
delta = self.get_deltas()[i]
if x_next_min is not None:
self._A[0] = [-2 * delta, -1]
self._hA[0] = -x_next_min
else:
self._A[0] = [0, 0]
self._hA[0] = INF
if x_next_max is not None:
self._A[1] = [2 * delta, 1]
self._hA[1] = x_next_max
else:
self._A[1] = [0, 0]
self._hA[1] = INF
cur_index = 2
for j in range(len(self.constraints)):
a, b, c, F, v, ubound, xbound = self.params[j]
if a is not None:
if self.constraints[j].identical:
nC_ = F.shape[0]
self._A[cur_index:cur_index + nC_, 0] = F.dot(a[i])
self._A[cur_index:cur_index + nC_, 1] = F.dot(b[i])
self._hA[cur_index:cur_index + nC_] = v - F.dot(c[i])
self._lA[cur_index:cur_index + nC_] = -INF
else:
nC_ = F[i].shape[0]
self._A[cur_index:cur_index + nC_, 0] = F[i].dot(a[i])
self._A[cur_index:cur_index + nC_, 1] = F[i].dot(b[i])
self._hA[cur_index:cur_index + nC_] = v[i] - F[i].dot(c[i])
self._lA[cur_index:cur_index + nC_] = -INF
cur_index = cur_index + nC_
if ubound is not None:
self._l[0] = max(self._l[0], ubound[i, 0])
self._h[0] = min(self._h[0], ubound[i, 1])
if xbound is not None:
self._l[1] = max(self._l[1], xbound[i, 0])
self._h[1] = min(self._h[1], xbound[i, 1])
if H is None:
H = np.zeros((self.get_no_vars(), self.get_no_vars()))
# Select what solver to use
if g[1] > 0: # Choose solver_up
if abs(self.solver_up_recent_index - i) > 1:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("Choose solver [up] - init")
res = self.solver_up.init(H, g, self._A, self._l, self._h,
self._lA, self._hA, np.array([1000]))
else:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("Choose solver [up] - hotstart")
res = self.solver_up.hotstart(H, g, self._A, self._l, self._h,
self._lA, self._hA,
np.array([1000]))
self.solver_up_recent_index = i
else: # Choose solver_down
if abs(self.solver_down_recent_index - i) > 1:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("Choose solver [down] - init")
res = self.solver_down.init(H, g, self._A, self._l,
self._h, self._lA, self._hA,
np.array([1000]))
else:
if logger.isEnabledFor(logging.DEBUG):
logger.debug("Choose solver [down] - hotstart")
res = self.solver_down.hotstart(H, g, self._A, self._l,
self._h, self._lA, self._hA,
np.array([1000]))
self.solver_down_recent_index = i
if res == ReturnValue.SUCCESSFUL_RETURN:
var = np.zeros(self.nV)
if g[1] > 0:
self.solver_up.getPrimalSolution(var)
else:
self.solver_down.getPrimalSolution(var)
if self._disable_check:
return var
# Check for constraint feasibility
success = (np.all(self._l <= var + eps)
and np.all(var <= self._h + eps)
and np.all(np.dot(self._A, var) <= self._hA + eps)
and np.all(np.dot(self._A, var) >= self._lA - eps))
if not success:
# import ipdb; ipdb.set_trace()
logger.fatal(
"Hotstart fails but qpOASES does not report correctly. \n "
"var: {:}, lower_bound: {:}, higher_bound{:}".format(
var, self._l, self._h))
# TODO: Investigate why this happen and fix the
# relevant code (in qpOASES wrapper)
else:
return var
res = np.empty(self.get_no_vars())
res[:] = np.nan
return res
def main():
rospy.init_node('controller_2dof', anonymous=True)
rospy.Rate(10)
# Setup data of QP.
# Joint Weights.
w1 = 1e-3
w2 = 1e-3
# Joint limits.
q0_max = 0.05
q1_max = 0.15
# Links
l1 = 0.1
l2 = 0.2
l3 = 0.3
# Initial joint values.
q0_init = q0 = 0.01
q1_init = q1 = -0.02
# Joint target.
q_des1 = 0.45
q_des2 = 0.78
# Slack limits.
e1_max = 1000
e2_max = 1000
# Velocity limits.(+-)
v0_max = 0.05
v1_max = 0.1
# Acceleration limits.(+-)
a0_max = 0.05 * 0.5
a1_max = 0.1 * 0.5
# Others
precision = 1e-2
p = 10
q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1
error1 = p * (q_des1 - q_eef)
error2 = p * (q_des2 - q_eef)
vel_init = 0
nWSR = np.array([100])
# Dynamic goal weights
w3 = (error2 / (error1 + error2))**2 # goal 1 weight
w4 = (error1 / (error1 + error2))**2 # goal 2 weight
# Acceleration
a0_const = (v0_max - a0_max) / v0_max
a1_const = (v1_max - a1_max) / v1_max
example = SQProblem(4, 6)
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0,
w4
]).reshape((4, 4))
A = np.array([
1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0
]).reshape((6, 4))
g = np.array([0.0, 0.0, 0.0, 0.0])
lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max])
ub = np.array([v0_max, v1_max, e1_max, e2_max])
lbA = np.array(
[error1, error2, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max])
ubA = np.array(
[error1, error2, (q0_max - q0), (q1_max - q0), a0_max, a1_max])
# Setting up QProblem object
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.LOW
example.setOptions(options)
Opt = np.zeros(4)
print(
"Init pos = %g, goal_1 = %g, goal_2 = %g, error_1 = %g, error_2 = %g, q0 =%g, q1 = %g\n"
% (q_eef, q_des1, q_des2, error1, error2, q0, q1))
print A
i = 0
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
lim = min([limit1, limit2])
diff1 = abs(error1)
diff2 = abs(error2)
diff = min([diff1, diff2])
# Plotting
t = np.array(i)
pos_eef = np.array(q_eef)
pos_0 = np.array(q0)
pos_1 = np.array(q1)
vel_0 = np.array(vel_init)
vel_1 = np.array(vel_init)
vel_eef = np.array(vel_init)
p_error = np.array(error1)
r_max = np.array(q_des1)
r_min = np.array(q_des2)
return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Init of QP-Problem returned without success! ERROR MESSAGE: "
return -1
while not rospy.is_shutdown():
#while (diff1 > 0.00005 or diff2 > 0.0005) and limit1 > precision and limit2 > precision and i < 400:
while diff > 0.00005 and lim > precision and i < 400:
# Solve QP.
i += 1
nWSR = np.array([100])
lbA[0] = error1
lbA[1] = error2
lbA[2] = -q0_max - q0
lbA[3] = -q1_max - q1
lbA[4] = a0_const * Opt[0] - a0_max
lbA[5] = a1_const * Opt[1] - a1_max
ubA[0] = error1
ubA[1] = error2
ubA[2] = q0_max - q0
ubA[3] = q1_max - q1
ubA[4] = a0_const * Opt[0] + a0_max
ubA[5] = a1_const * Opt[1] + a1_max
return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR)
if return_value != returnValue.SUCCESSFUL_RETURN:
print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: "
return -1
old_error1 = error1
old_error2 = error2
# Update limits
example.getPrimalSolution(Opt)
q0 += Opt[0] / 100
q1 += Opt[1] / 100
q_eef = l1 + l2 + l3 + q0 + q1
error1 = p * (q_des1 - q_eef)
error2 = p * (q_des2 - q_eef)
# Update weights
w3 = (error2 / (error1 + error2))**2
w4 = (error1 / (error1 + error2))**2
H = np.array([
w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0,
0.0, 0.0, w4
]).reshape((4, 4))
# Stopping conditions
limit1 = abs(error1)
limit2 = abs(error2)
lim = min([limit1, limit2])
diff1 = abs(error1 - old_error1)
diff2 = abs(error2 - old_error2)
diff = min([diff1, diff2])
#print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, w3= %g, w4= %g, q0= %g q1= %g \n" % (
#Opt[0], Opt[1], Opt[2], Opt[3], q_eef, w3, w4, q0, q1)
print w3, w4
# Plotting arrays
pos_eef = np.hstack((pos_eef, q_eef))
pos_0 = np.hstack((pos_0, q0))
pos_1 = np.hstack((pos_1, q1))
vel_0 = np.hstack((vel_0, Opt[0]))
vel_1 = np.hstack((vel_1, Opt[1]))
vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1]))
p_error = np.hstack((p_error, error1))
r_max = np.hstack((r_max, q_des1))
r_min = np.hstack((r_min, q_des2))
t = np.hstack((t, i))
# Plot
t_eef = go.Scatter(y=pos_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_eef',
line=dict(dash='dash'))
t_p0 = go.Scatter(y=pos_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_0',
line=dict(dash='dash'))
t_p1 = go.Scatter(y=pos_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='pos_1',
line=dict(dash='dash'))
t_v0 = go.Scatter(y=vel_0,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_0')
t_v1 = go.Scatter(y=vel_1,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_1')
t_veef = go.Scatter(y=vel_eef,
x=t,
marker=dict(size=4, ),
mode='lines',
name='vel_eef')
t_er = go.Scatter(y=p_error,
x=t,
marker=dict(size=4, ),
mode='lines',
name='error')
t_g1 = go.Scatter(y=r_max,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_1',
line=dict(dash='dot'))
t_g2 = go.Scatter(y=r_min,
x=t,
marker=dict(size=4, ),
mode='lines',
name='goal_2',
line=dict(dash='dot', width=4))
data = [t_eef, t_veef, t_g1, t_g2]
layout = dict(
title="Initial position EEF = %g. Goals: goal_1 = %g, goal_2 = %g"
% (q_eef_init, q_des1, q_des2),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
font=dict(size=18),
yaxis=dict(title='Position / Velocity'),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_weights_eef.html',
image='png',
image_filename='dynamic_2')
data = [t_p0, t_v0]
layout = dict(title="Initial position q0 =%g." % (q0_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
yaxis=dict(title='Position / Velocity',
gridwidth=3,
range=[-0.15, 0.05]),
font=dict(size=18))
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_weights_q0.html',
image='png',
image_filename='dynamic_3')
data = [t_p1, t_v1]
layout = dict(
title="Initial position q1 = %g." % (q1_init),
xaxis=dict(
title='Iterations',
autotick=False,
dtick=25,
gridwidth=3,
),
yaxis=dict(title='Position / Velocity',
gridwidth=3,
range=[-0.15, 0.05]),
font=dict(size=18),
)
fig = dict(data=data, layout=layout)
plotly.offline.plot(fig,
filename='html/dynamic_weights_q1.html',
image='png',
image_filename='dynamic_4')
print "\n i = %g \n" % i
return 0
class StaggeredProjections (object):
"""
Algorithm described in "Staggered projections for frictional contact in multibody systems".
"""
USE_DIAGONAL_GENERATORS = True;
name = ""; # name of this instance
n = 0; # number of variables
k = 0; # number of contact points
D = []; # friction cost matrix
ET = []; # friction inequality matrix
mu_alpha = []; # friction inequality upper-bound vector
N = []; # contact cost matrix
bounds_contact = []; # bounds on the problem variables
bounds_friction = []; # bounds on the problem variables
H_contact = []; # Hessian H = G^T*G
g_contact = []; # Product dD = d^T*D
H_friction = []; # Hessian H = G^T*G
g_friction = []; # Product dD = d^T*D
alpha = [] # contact impulses
beta = []; # friciton impulses
f0 = []; # initial guess for the friction impulses projected in joint space
accuracy=0; # accuracy used by the solver for termination
maxIter=0; # max number of iterations
verb=0; # verbosity level of the solver (0=min, 2=max)
iter = 0; # current iteration number
iter_contact = 0;
iter_friction = 0;
qpTime_contact = 0.0;
qpTime_friction = 0.0;
t = 0; # number of times the method simulate has been called
meanComputationTime = 0.0;
maxComputationTime = 0.0;
maxIterations = 0;
meanIterations = 0.0;
initialized_contact = False; # true if solver has been initialized
initialized_friction = False; # true if solver has been initialized
solverContact = [];
solverFriction = [];
options = []; # qp oases solver's options
epsilon = np.sqrt(np.finfo(float).eps);
INEQ_VIOLATION_THR = 1e-4;
def __init__ (self, n, mu, accuracy=1e-4, maxIter=30, verb=0):
self.name = "StagProj";
self.n = n;
self.mu = mu;
self.iter = 0;
self.accuracy = accuracy;
self.maxIter = maxIter;
self.verb = verb;
self.options = Options();
if(self.verb<=0):
self.options.printLevel = PrintLevel.NONE;
elif(self.verb==1):
self.options.printLevel = PrintLevel.LOW;
elif(self.verb==2):
self.options.printLevel = PrintLevel.MEDIUM;
elif(self.verb>2):
self.options.printLevel = PrintLevel.DEBUG_ITER;
print "set high print level"
self.options.enableRegularisation = True;
self.changeContactsNumber(0);
self.S_T = np.zeros((self.n+6,self.n));
self.S_T[6:6+self.n,:] = np.identity(self.n);
self.t = 0;
self.meanComputationTime = 0.0;
self.maxComputationTime = 0.0;
self.meanIterations = 0.0;
self.maxIterations = 0.0;
return;
def changeContactsNumber(self, k):
if(self.verb>1):
print "[%s] Changing number of contacts from %d to %d" % (self.name, self.k, k);
self.k = k;
self.iter = 0;
self.initialized_contact = False;
self.initialized_friction = False;
if(k>0):
self.solverContact = SQProblem(self.k, 0);
self.solverFriction = SQProblem(4*self.k, self.k);
self.solverContact.setOptions(self.options);
self.solverFriction.setOptions(self.options);
self.contact_forces = np.zeros(self.k*3);
self.f0 = np.zeros(self.n+6); # reset initial guess
self.bounds_contact = self.k*[(0.0,1e9)];
self.bounds_friction = (4*self.k)*[(0.0,1e9)];
self.beta = np.zeros(4*self.k);
self.N = np.zeros((self.n+6, self.k));
self.dJv = np.zeros(self.k);
self.lb_contact = np.array([ b[0] for b in self.bounds_contact]);
self.ub_contact = np.array([ b[1] for b in self.bounds_contact]);
self.alpha = np.zeros(self.k);
''' compute constraint matrix '''
self.ET = np.zeros((self.k, 4*self.k));
for i in range(0,self.k):
self.ET[i,4*i:4*i+4] = 1;
self.D = np.zeros((self.n+6, 4*self.k));
''' compute tangent directions matrix '''
self.T = np.zeros((3, 4)); # tangent directions
if(self.USE_DIAGONAL_GENERATORS):
tmp = 1.0/sqrt(2.0);
self.T[:,0] = np.array([+tmp, +tmp, 0]);
self.T[:,1] = np.array([+tmp, -tmp, 0]);
self.T[:,2] = np.array([-tmp, +tmp, 0]);
self.T[:,3] = np.array([-tmp, -tmp, 0]);
else:
self.T[:,0] = np.array([+1, 0, 0]);
self.T[:,1] = np.array([-1, 0, 0]);
self.T[:,2] = np.array([ 0, +1, 0]);
self.T[:,3] = np.array([ 0, -1, 0]);
self.lb_friction = np.array([ b[0] for b in self.bounds_friction]);
self.ub_friction = np.array([ b[1] for b in self.bounds_friction]);
self.lbA_friction = -1e100*np.ones(self.k);
def simulate(self, v, M, h, tau, dt, contactList, dJv_list, maxIter=None, maxTime=100.0):
start = time.time();
if(maxIter==None):
maxIter = self.maxIter;
self.maxTime = maxTime;
self.iter = 0;
self.min_res = 1e10; # minimum residual
self.rel_err = 1e10; # relative error
self.contactList = contactList;
self.dJv_list = dJv_list;
self.dt = dt;
''' If necessary resize the solvers '''
if(self.k!=len(contactList)):
self.changeContactsNumber(len(contactList));
''' Compute next momentum assuming no contacts '''
self.M_inv = np.linalg.inv(M);
self.M_vp = np.dot(M,v) - dt*h;
self.M_vp[6:] += dt*tau;
self.v_p = np.dot(self.M_inv, self.M_vp);
if(self.k==0):
return (self.v_p, np.zeros(0));
self.f_old = np.copy(self.f0);
while (self.rel_err>self.accuracy and self.iter<maxIter):
self.N_alpha = self.compute_contact_projection(-self.M_vp-self.f_old);
if(self.mu<1e-3):
break;
self.f_new = self.compute_friction_projection(-self.M_vp-self.N_alpha);
self.rel_err = self.compute_relative_error();
self.residual = self.compute_residual();
if(self.residual < self.min_res):
self.min_res = self.residual;
self.f_star = np.copy(self.f_new); # cache best solution
self.iter += 1;
self.f_old = np.copy(self.f_new);
if(self.verb>1):
print "Iter %d err %f resid %f alpha %.3f beta %.3f" % (self.iter, self.rel_err, self.residual,
np.linalg.norm(self.alpha)/dt,
np.linalg.norm(self.beta)/dt);
if(self.verb>0 and self.rel_err>self.accuracy):
print "[Stag-Proj] Algorithm did not converge",
print "Iter %d err %f resid %f alpha %.3f beta %.3f" % (self.iter, self.rel_err, self.residual,
np.linalg.norm(self.alpha)/dt,
np.linalg.norm(self.beta)/dt);
# time.sleep(2);
if(self.mu>1e-3):
self.f0 = np.copy(self.f_star);
self.N_alpha = self.compute_contact_projection(-self.M_vp-self.f0);
v_next = self.v_p + np.dot(self.M_inv, self.N_alpha+self.f0);
ddx = np.zeros(self.k);
fz = np.zeros(self.k);
v_xy = np.zeros(2*self.k);
f_xy = np.zeros(2*self.k);
for i in range(self.k):
''' compute normal contact accelerations and contact forces to check KKT conditions '''
ddx[i] = np.dot(self.contactList[i][2,:], v_next) + dt*self.dJv_list[i][2];
fz[i] = self.alpha[i]/dt;
if(self.verb>0):
''' compute tangent velocities and forces '''
v_xy[2*i:2*i+2] = np.dot(self.contactList[i][0:2,:], v_next);
f_xy[2*i:2*i+2] = np.dot(self.T[0:2,:], self.beta[4*i:4*i+4])/dt;
slide_acc = 1e-2;
if(np.linalg.norm(v_xy[2*i:2*i+2])>slide_acc and fz[i]>10.0):
print "Contact point %d is sliding, fxy/fz=(%.2f,%.2f), fz=%.1f, v=(%.2f,%.2f,%.2f)"%(i,f_xy[2*i]/fz[i],f_xy[2*i+1]/fz[i],fz[i],
v_xy[2*i],v_xy[2*i+1],ddx[i]);
# time.sleep(1);
''' compute contact forces '''
self.contact_forces[3*i:3*i+3] = np.dot(self.T, self.beta[4*i:4*i+4])/dt;
self.contact_forces[3*i+2] = self.alpha[i]/dt;
self.v_xy = v_xy;
self.f_xy = f_xy;
if(np.dot(ddx,fz)>self.accuracy):
print 'ERROR STAG-PROJ: ddx*fz = %f' % np.dot(ddx,fz);
print (' ddx: ', ddx);
print (' fz: ', fz);
self.computationTime = time.time()-start;
self.t += 1;
self.meanComputationTime = ((self.t-1)*self.meanComputationTime + self.computationTime)/self.t;
self.meanIterations = ((self.t-1)*self.meanIterations + self.iter)/self.t;
if(self.maxComputationTime < self.computationTime):
self.maxComputationTime = self.computationTime;
if(self.maxIterations < self.iter):
self.maxIterations = self.iter;
return (v_next, self.contact_forces);
def compute_contact_projection(self, z):
if(self.iter==0):
for i in range(0,self.k):
self.N[:,i] = self.contactList[i][2,:].T;
self.dJv[i] = self.dJv_list[i][2];
self.H_contact = np.dot(self.N.transpose(), np.dot(self.M_inv, self.N)) + 1e-10*np.identity(self.k);
self.g_contact = -np.dot(self.N.transpose(), np.dot(self.M_inv, z)) + self.dt*self.dJv;
A = np.zeros((0,self.k));
lbA = np.zeros(0);
ubA = np.zeros(0);
maxActiveSetIter = np.array([2*self.k]);
maxComputationTime = np.array(self.maxTime);
if(self.initialized_contact==False):
imode = self.solverContact.init(self.H_contact, self.g_contact, A, self.lb_contact, self.ub_contact,
lbA, ubA, maxActiveSetIter, maxComputationTime);
if(imode!=PyReturnValue.INIT_FAILED_INFEASIBILITY):
self.initialized_contact = True;
else:
imode = self.solverContact.hotstart(self.H_contact, self.g_contact, A, self.lb_contact, self.ub_contact,
lbA, ubA, maxActiveSetIter, maxComputationTime);
self.qpTime_contact += maxComputationTime;
self.iter_contact += maxActiveSetIter[0];
if(imode==0 or imode==PyReturnValue.MAX_NWSR_REACHED):
self.solverContact.getPrimalSolution(self.alpha);
self.print_qp_oases_error_message(imode, "Contact solver");
return np.dot(self.N, self.alpha);
def compute_friction_projection(self, z):
if(self.iter==0):
self.T_dJ_dq = np.zeros(4*self.k);
for i in range(0,self.k):
self.D[:,4*i:4*i+4] = np.dot(self.contactList[i].T, self.T);
self.T_dJ_dq[4*i:4*i+4] += self.dt * np.dot(self.T[0:2,:].T, self.dJv_list[i][0:2]);
self.H_friction = np.dot(self.D.transpose(), np.dot(self.M_inv, self.D));
''' compute constraint vector (upper bound) '''
self.mu_alpha = self.mu * self.alpha;
self.g_friction = -np.dot(self.D.transpose(), np.dot(self.M_inv, z)) + self.T_dJ_dq;
maxActiveSetIter = np.array([8*self.k]);
maxComputationTime = np.array(self.maxTime);
if(self.initialized_friction==False):
imode = self.solverFriction.init(self.H_friction, self.g_friction, self.ET, self.lb_friction, self.ub_friction,
self.lbA_friction, self.mu_alpha, maxActiveSetIter, maxComputationTime);
if(imode==0):
self.initialized_friction = True;
else:
imode = self.solverFriction.hotstart(self.H_friction, self.g_friction, self.ET, self.lb_friction, self.ub_friction,
self.lbA_friction, self.mu_alpha, maxActiveSetIter, maxComputationTime);
self.qpTime_friction += maxComputationTime;
self.iter_friction += maxActiveSetIter[0];
if(imode==0 or imode==PyReturnValue.MAX_NWSR_REACHED):
self.solverFriction.getPrimalSolution(self.beta);
self.print_qp_oases_error_message(imode, "Friction solver");
return np.dot(self.D, self.beta);
def compute_relative_error(self):
f_diff = self.f_new - self.f_old;
num = np.dot(f_diff.T, np.dot(self.M_inv, f_diff));
den = np.dot(self.f_old.T, np.dot(self.M_inv, self.f_old));
if(den!=0.0):
return num/den;
return 1e10;
def compute_residual(self):
res = 0.0;
tmp = self.v_p + np.dot(self.M_inv, self.N_alpha + self.f_new);
for i in range(self.k):
res += abs(self.alpha[i]*np.dot(self.N[:,i].T, tmp));
return res;
def reset(self):
self.initialized_contact = False;
self.initialized_friction = False;
def print_qp_oases_error_message(self, imode, solver_name):
if(imode!=0 and imode!=63 and self.verb>0):
if(imode==PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY):
print "[%s] ERROR Qp oases HOTSTART_STOPPED_INFEASIBILITY" % solver_name; # 60
elif(imode==PyReturnValue.MAX_NWSR_REACHED):
print "[%s] ERROR Qp oases RET_MAX_NWSR_REACHED" % solver_name; # 63
elif(imode==PyReturnValue.STEPDIRECTION_FAILED_CHOLESKY):
print "[%s] ERROR Qp oases STEPDIRECTION_FAILED_CHOLESKY" % solver_name; # 68
elif(imode==PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED):
print "[%s] ERROR Qp oases HOTSTART_FAILED_AS_QP_NOT_INITIALISED" % solver_name; # 53
elif(imode==PyReturnValue.INIT_FAILED_INFEASIBILITY):
print "[%s] ERROR Qp oases INIT_FAILED_INFEASIBILITY" % solver_name; # 37
# RET_INIT_FAILED_HOTSTART = 36
elif(imode==PyReturnValue.UNKNOWN_BUG):
print "[%s] ERROR Qp oases UNKNOWN_BUG" % solver_name; # 9
else:
print "[%s] ERROR Qp oases %d " % (solver_name, imode);
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