def test_m44_reliable_sparse(self):
test_name = 'mm44_reliable_sparse.txt'
print(test_name)
# QP Options
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 0, 'One ore more tests failed.'
def test_m44_reliable_sparse(self):
test_name = 'mm44_reliable_sparse.txt'
print(test_name)
# QP Options
options = Options()
options.setToReliable()
options.printLevel = PrintLevel.NONE
isSparse = True
useHotstarts = False
# run QP benchmarks
results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
self.nWSR, self.cpu_time, self.TOL)
# print and write results
string = results2str(results)
print(string)
write_results(test_name, string)
assert get_nfail(results) <= 0, 'One ore more tests failed.'
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 ComAccLP(object):
"""
LP solver dedicated to finding the maximum center of mass (CoM) deceleration in a
specified direction, subject to the friction cone constraints.
This is possible thanks to the simplifying assumption that the CoM acceleration
is going to be parallel to its velocity. This allows us to represent the 3d
com trajectory by means of a 1d trajectory alpha(t):
c(t) = c0 + alpha(t) v
v = c0 / ||c0||
dc = dAlpha(t) v
ddc(t) = ddAlpha(t) v
The operation amounts to solving the following parametric Linear Program:
minimize ddAlpha + w sum_i(f_i)
subject to A f = a ddAlpha + b alpha + d
f >= 0
where:
f are the contact forces generator coefficients
ddAlpha is the magnitude of the CoM acceleration
alpha is the magnitude of the CoM displacement (with respect to its initial position c0)
w regularization parameter
Given a CoM position (by means of a value of alpha), this class can compute the
minimum com acceleration in direction v (i.e. the maximum acceleration in direction -v).
Since this is a piecewise-linear function of alpha, it can also compute its derivative with respect
to alpha, and the boundaries of the alpha-region in which the derivative remains constant.
"""
NO_WARM_START = False
name = ""
# solver name
n = 0
# number of variables
m_in = 0
# number of constraints (i.e. 6)
Hess = []
# Hessian
grad = []
# gradient
A = None
# constraint matrix multiplying the contact force generators
b = None
# constraint vector multiplying the CoM position parameter alpha
d = None
# constraint vector
mass = 0.0
# robot mass
g = None
# 3d gravity vector
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
initialized = False
# true if solver has been initialized
qpOasesSolver = []
options = []
# qp oases solver's options
epsilon = np.sqrt(np.finfo(float).eps)
INEQ_VIOLATION_THR = 1e-4
def __init__(self,
name,
c0,
v,
contact_points,
contact_normals,
mu,
g,
mass,
maxIter=10000,
verb=0,
regularization=1e-5):
''' Constructor
@param c0 Initial CoM position
@param v Opposite of the direction in which you want to maximize the CoM acceleration (typically that would be
the CoM velocity direction)
@param g Gravity vector
@param regularization Weight of the force minimization, the higher this value, the sparser the solution
'''
self.name = name
self.maxIter = maxIter
self.verb = verb
self.m_in = 6
self.initialized = False
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.qpOasesSolver.printOptions()
self.b = np.zeros(6)
self.d = np.empty(6)
self.c0 = np.empty(3)
self.v = np.empty(3)
self.constrUB = np.zeros(self.m_in) + 1e100
self.constrLB = np.zeros(self.m_in) - 1e100
self.set_problem_data(c0, v, contact_points, contact_normals, mu, g,
mass, regularization)
def set_com_state(self, c0, v):
assert np.asarray(
c0).squeeze().shape[0] == 3, "Com position vector has not size 3"
assert np.asarray(v).squeeze(
).shape[0] == 3, "Com acceleration direction vector has not size 3"
self.c0 = np.asarray(c0).squeeze()
self.v = np.asarray(v).squeeze().copy()
if (norm(v) == 0.0):
raise ValueError(
"[%s] Norm of com acceleration direction v is zero!" %
self.name)
self.v /= norm(self.v)
self.constrMat[:3, -1] = self.mass * self.v
self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v)
self.b[3:] = self.mass * np.cross(v, self.g)
self.d[:3] = self.mass * self.g
self.d[3:] = self.mass * np.cross(c0, self.g)
def set_contacts(self,
contact_points,
contact_normals,
mu,
regularization=1e-5):
# compute matrix A, which maps the force generator coefficients into the centroidal wrench
(self.A,
self.G4) = compute_centroidal_cone_generators(contact_points,
contact_normals, mu)
# since the size of the problem may have changed we need to recreate the solver and all the problem matrices/vectors
self.n = contact_points.shape[0] * 4 + 1
self.qpOasesSolver = SQProblem(self.n, self.m_in)
#, HessianType.SEMIDEF);
self.qpOasesSolver.setOptions(self.options)
self.Hess = INITIAL_HESSIAN_REGULARIZATION * np.identity(self.n)
self.grad = np.ones(self.n) * regularization
self.grad[-1] = 1.0
self.constrMat = np.zeros((self.m_in, self.n))
self.constrMat[:, :-1] = self.A
self.constrMat[:3, -1] = self.mass * self.v
self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v)
self.lb = np.zeros(self.n)
self.lb[-1] = -1e100
self.ub = np.array(self.n * [
1e100,
])
self.x = np.zeros(self.n)
self.y = np.zeros(self.n + self.m_in)
self.initialized = False
def set_problem_data(self,
c0,
v,
contact_points,
contact_normals,
mu,
g,
mass,
regularization=1e-5):
assert g.shape[0] == 3, "Gravity vector has not size 3"
assert mass > 0.0, "Mass is not positive"
self.mass = mass
self.g = np.asarray(g).squeeze()
self.set_contacts(contact_points, contact_normals, mu, regularization)
self.set_com_state(c0, v)
def compute_max_deceleration_derivative(self):
''' Compute the derivative of the max CoM deceleration (i.e. the solution of the last LP)
with respect to the parameter alpha (i.e. the CoM position parameter). Moreover,
it also computes the bounds within which this derivative is valid (alpha_min, alpha_max).
'''
act_set = np.where(self.y[:self.n - 1] != 0.0)[0]
# indexes of active bound constraints
n_as = act_set.shape[0]
if (n_as > self.n - 6):
raise ValueError(
"[%s] ERROR Too many active constraints: %d (rather than %d)" %
(self.name, n_as, self.n - 6))
if (self.verb > 0 and n_as < self.n - 6):
print "[%s] INFO Less active constraints than expected: %d (rather than %d)" % (
self.name, n_as, self.n - 6)
self.K = np.zeros((n_as + 6, self.n))
self.k1 = np.zeros(n_as + 6)
self.k2 = np.zeros(n_as + 6)
self.K[:n_as, :] = np.identity(self.n)[act_set, :]
self.K[-6:, :] = self.constrMat
self.k1[-6:] = self.b
self.k2[-6:] = self.d
U, s, VT = np.linalg.svd(self.K)
rank = (s > EPS).sum()
K_inv_k1 = np.dot(VT[:rank, :].T,
(1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k1))
K_inv_k2 = np.dot(VT[:rank, :].T,
(1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k2))
if (rank < self.n):
Z = VT[rank:, :].T
P = np.dot(
np.dot(Z, np.linalg.inv(np.dot(Z.T, np.dot(self.Hess, Z)))),
Z.T)
K_inv_k1 -= np.dot(P, np.dot(self.Hess, K_inv_k1))
K_inv_k2 -= np.dot(P,
np.dot(self.Hess, K_inv_k2) + self.grad)
# Check that the solution you get by solving the KKT is the same found by the solver
x_kkt = K_inv_k1 * self.alpha + K_inv_k2
if (norm(self.x - x_kkt) > 10 * EPS):
warnings.warn("[%s] ERROR x different from x_kkt. x=" %
(self.name) + str(self.x) + "\nx_kkt=" + str(x_kkt) +
" " + str(norm(self.x - x_kkt)))
# store the derivative of the solution w.r.t. the parameter alpha
dx = K_inv_k1[-1]
# act_set_mat * alpha >= act_set_vec
act_set_mat = K_inv_k1[:-1]
act_set_vec = -K_inv_k2[:-1]
for i in range(act_set_mat.shape[0]):
if (abs(act_set_mat[i]) > EPS):
act_set_vec[i] /= abs(act_set_mat[i])
act_set_mat[i] /= abs(act_set_mat[i])
if (act_set_mat * self.alpha < act_set_vec - EPS).any():
raise ValueError(
"ERROR: after normalization current alpha violates constraints "
+ str(act_set_mat * self.alpha - act_set_vec))
ind_pos = np.where(act_set_mat > EPS)[0]
if (ind_pos.shape[0] > 0):
alpha_min = np.max(act_set_vec[ind_pos])
else:
alpha_min = -1e10
# warnings.warn("[%s] alpha_min seems unbounded %.7f"%(self.name, np.max(act_set_mat)));
ind_neg = np.where(act_set_mat < -EPS)[0]
if (ind_neg.shape[0] > 0):
alpha_max = np.min(-act_set_vec[ind_neg])
else:
alpha_max = 1e10
# warnings.warn("[%s] alpha_max seems unbounded %.7f"%(self.name, np.min(act_set_mat)));
if (alpha_min > alpha_max):
raise ValueError("ERROR alpha_min %.3f > alpha_max %.3f" %
(alpha_min, alpha_max))
return (dx, alpha_min, alpha_max)
def compute_max_deceleration(self, alpha, maxIter=None, maxTime=100.0):
start = time.time()
self.alpha = alpha
if (self.NO_WARM_START):
self.qpOasesSolver = SQProblem(self.n, self.m_in)
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
if (maxIter == None):
maxIter = self.maxIter
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
self.constrUB[:6] = np.dot(self.b, alpha) + self.d
self.constrLB[:6] = self.constrUB[:6]
while (True):
if (not self.initialized):
self.imode = self.qpOasesSolver.init(self.Hess, self.grad,
self.constrMat, self.lb,
self.ub, self.constrLB,
self.constrUB,
maxActiveSetIter,
maxComputationTime)
else:
self.imode = self.qpOasesSolver.hotstart(
self.grad, self.lb, self.ub, self.constrLB, self.constrUB,
maxActiveSetIter, maxComputationTime)
if (self.imode == 0):
self.initialized = True
if (self.imode == 0
or self.imode == PyReturnValue.INIT_FAILED_INFEASIBILITY or
self.imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY
or self.Hess[0, 0] >= MAX_HESSIAN_REGULARIZATION):
break
self.initialized = False
self.Hess *= 10.0
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
if (self.verb > -1):
print "[%s] WARNING %s. Increasing Hessian regularization to %f" % (
self.name, qpOasesSolverMsg(self.imode), self.Hess[0, 0])
self.qpTime = maxComputationTime
self.iter = 1 + maxActiveSetIter[0]
if (self.imode == 0):
self.qpOasesSolver.getPrimalSolution(self.x)
self.qpOasesSolver.getDualSolution(self.y)
if ((self.x < self.lb - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR lower bound violated" %
(self.name) + str(self.x) + str(self.lb))
if ((self.x > self.ub + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR upper bound violated" %
(self.name) + str(self.x) + str(self.ub))
if ((np.dot(self.constrMat, self.x) >
self.constrUB + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint upper bound violated " %
(self.name) +
str(np.min(np.dot(self.constrMat, self.x) -
self.constrUB)))
if ((np.dot(self.constrMat, self.x) <
self.constrLB - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint lower bound violated " %
(self.name) +
str(np.max(np.dot(self.constrMat, self.x) -
self.constrLB)))
(dx, alpha_min,
alpha_max) = self.compute_max_deceleration_derivative()
else:
self.initialized = False
dx = 0.0
alpha_min = 0.0
alpha_max = 0.0
if (self.verb > 0):
print "[%s] ERROR Qp oases %s" % (self.name,
qpOasesSolverMsg(self.imode))
if (self.qpTime >= maxTime):
if (self.verb > 0):
print "[%s] Max time reached %f after %d iters" % (
self.name, self.qpTime, self.iter)
self.imode = 9
self.computationTime = time.time() - start
return (self.imode, self.x[-1], dx, alpha_min, alpha_max)
def getContactForces(self):
''' Get the contact forces obtained by solving the last LP '''
cg = 4
nContacts = self.G4.shape[1] / cg
f = np.empty((3, nContacts))
for i in range(nContacts):
f[:, i] = np.dot(self.G4[:, cg * i:cg * i + cg],
self.x[cg * i:cg * i + cg])
return f
def reset(self):
self.initialized = False
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)
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
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)
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
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 SplineTrajectoryNd(object):
def __init__(self, dim, minimal_order, smin, smax):
assert dim > 0 and isinstance(dim, int)
assert minimal_order >= 2 and isinstance(dim, int)
assert smin < smax
self.dim = dim
self.minimal_order = minimal_order
self.support = (float(smin), float(smax))
self.splines = []
self.splines_support = list(self.support)
self.way_points = [np.matrix(np.empty([self.dim, 1]))] * 2
self.way_points_constraints = [([None] * (self.minimal_order - 1)),
([None] * (self.minimal_order - 1))]
def setInitialWayPoint(self, point):
assert point.shape[0] == self.dim
self.way_points[0] = np.asmatrix(point).copy()
def setFinalWayPoint(self, point):
assert point.shape[0] == self.dim
self.way_points[-1] = np.asmatrix(point).copy()
def numWayPoints(self):
return len(self.way_points)
def addWayPoint(self, point, s):
assert s >= self.support[0] and s <= self.support[1]
assert point.shape == (self.dim, 1)
if s == self.support[0]:
self.setInitialWayPoint(point)
id = 0
elif s == self.support[1]:
self.setFinalWayPoint(point)
id = self.numWayPoints() - 1
elif s in self.splines_support:
id = self.splines_support.index(s)
self.way_points[id] = point.copy()
else:
indexes = [
k for k, val in enumerate(self.splines_support) if val >= s
]
id = indexes[0]
self.way_points.insert(id, point.copy())
self.splines_support.insert(id, s)
self.way_points_constraints.insert(id, [None] *
(self.minimal_order - 1))
return id
def addWayPointConstraint(self, way_point, way_point_constraint,
diff_order):
assert isinstance(way_point_constraint, WayPointConstraint)
vector = way_point_constraint.vector
assert vector.shape == (self.dim, 1)
assert diff_order >= 1
if isinstance(way_point, int):
id = way_point
elif isinstance(way_point, np.matrix):
assert way_point.shape == (self.dim, 1)
try:
id = [np.array_equal(way_point, x)
for x in self.way_points].index(True)
except:
raise ValueError('Invalid way_point argument')
else:
raise ValueError('Invalid way_point argument')
constraints = self.way_points_constraints[id]
constraint_id = diff_order - 1
if constraint_id < len(constraints):
constraints[constraint_id] = copy(way_point_constraint)
else:
raise ValueError('This case is not handled yet')
def __initSolver(self, arg_dim, num_in):
self.solver = SQProblem(arg_dim, num_in)
# , HessianType.POSDEF SEMIDEF
self.options = Options()
self.options.setToReliable()
self.solver.setOptions(self.options)
def compute(self):
self.splines = []
num_variables = 0
num_equalities = 0
num_inequalities = 0
self.spline_var_dim = []
self.spline_var_pos = []
self.spline_const_dim = []
self.spline_const_pos = []
spline_var_pos = 0
for k in range(self.numWayPoints() - 1):
smin = self.splines_support[k]
smax = self.splines_support[k + 1]
constraints = self.way_points_constraints[k]
spline_position_order = 1 # The spline must pass by two points
spline_differential_order = 0
# We must ensure the continuity with the next spline
if k < self.numWayPoints() - 2:
spline_differential_order += self.minimal_order - 1
# In addition, we must add the derivative constraints which concerns the current point
spline_additional_differential_order = 0
for i, cons in enumerate(constraints):
if cons is not None:
spline_additional_differential_order += 1
if cons.type == WayPointConstraint.ConstraintType.FREE:
num_variables += 1
# A special care for the last way points
if (k == self.numWayPoints() - 2):
next_constraints = self.way_points_constraints[-1]
for i, cons in enumerate(next_constraints):
if cons is not None:
spline_additional_differential_order += 1
if cons.type == WayPointConstraint.ConstraintType.FREE:
num_variables += 1
spline_differential_order += spline_additional_differential_order
spline_order = spline_position_order + spline_differential_order
spline_order = max(self.minimal_order, spline_order)
spline_var_dim = (spline_order + 1) * self.dim
num_variables += spline_var_dim
spline_const_dim = (spline_order + 1) * self.dim
num_equalities += spline_const_dim
# Create spline
spline = SplineNd(self.dim, spline_order, smin, smax)
spline.setWayPoint(self.way_points[k])
self.splines.append(spline)
# Update spline positions and dimensions
self.spline_var_dim.append(spline_var_dim)
self.spline_var_pos.append(spline_var_pos)
spline_var_pos += spline_var_dim
# Set up the problem
A_eq = np.matrix((np.zeros((num_equalities, num_variables))))
self.A_eq = A_eq
b_eq = np.matrix((np.zeros((num_equalities, 1))))
self.b_eq = b_eq
A_in = np.matrix((np.zeros((num_inequalities, num_variables))))
self.A_in = A_in
b_in = np.matrix((np.zeros((num_inequalities, 1))))
self.b_in = b_in
num_splines = len(self.splines)
eq_const_id = 0
for k, spline in enumerate(self.splines):
spline_var_pos = self.spline_var_pos[k]
spline_var_dim = self.spline_var_dim[k]
# The spline must pass by two points
points = (self.way_points[k], self.way_points[k + 1])
support = spline.support
for p_id, point in enumerate(points):
var_pos = spline_var_pos
for i in range(self.dim):
slice = range(var_pos, var_pos + spline.order + 1)
res = spline.computeDifferentialConstraint(
support[p_id], 0)
A_eq[eq_const_id, slice] = res
b_eq[eq_const_id] = point[i]
var_pos += spline.order + 1
eq_const_id += 1
# The spline has some continuity constraints
if k < num_splines - 1:
next_spline = self.splines[k + 1]
next_spline_var_pos = self.spline_var_pos[k + 1]
next_spline_var_dim = self.spline_var_dim[k + 1]
s1 = support[1]
#continuity_eq_const_id = eq_const_id
for diff_order in range(1, self.minimal_order):
var_pos = spline_var_pos
next_var_pos = next_spline_var_pos
for i in range(self.dim):
slice = range(var_pos, var_pos + spline.order + 1)
res = spline.computeDifferentialConstraint(
s1, diff_order)
A_eq[eq_const_id, slice] = res
next_slice = range(
next_var_pos, next_var_pos + next_spline.order + 1)
res = next_spline.computeDifferentialConstraint(
s1, diff_order)
A_eq[eq_const_id, next_slice] = -res
b_eq[eq_const_id] = 0.
var_pos += spline.order + 1
next_var_pos += next_spline.order + 1
eq_const_id += 1
# The spline may have some initial constraints
constraints = self.way_points_constraints[k]
for cons_id, cons in enumerate(constraints):
if cons is not None:
var_pos = spline_var_pos
vector = cons.vector
for i in range(self.dim):
slice = range(var_pos, var_pos + spline.order + 1)
res = spline.computeDifferentialConstraint(
support[0], cons_id + 1)
A_eq[eq_const_id, slice] = res
b_eq[eq_const_id] = vector[i]
var_pos += spline.order + 1
eq_const_id += 1
# The last spline may have terminal constraints
if k == num_splines - 1:
constraints = self.way_points_constraints[k + 1]
for cons_id, cons in enumerate(constraints):
if cons is not None:
var_pos = spline_var_pos
vector = cons.vector
for i in range(self.dim):
slice = range(var_pos, var_pos + spline.order + 1)
res = spline.computeDifferentialConstraint(
support[1], cons_id + 1)
A_eq[eq_const_id, slice] = res
b_eq[eq_const_id] = vector[i]
var_pos += spline.order + 1
eq_const_id += 1
if k < num_splines - 1:
next_spline = self.splines[k + 1]
next_spline_dim = self.spline_var_dim[k + 1]
snext = self.splines_support[k + 1]
# Add equality constraint
for i in range(self.minimal_order + 1):
res = spline.computeDifferentialConstraint(snext, i)
sol_naive = np.linalg.pinv(A_eq) * b_eq
# Solve with qpOASES
for k, spline in enumerate(self.splines):
spline_var_pos = self.spline_var_pos[k]
spline_var_dim = self.spline_var_dim[k]
spline.setCoeffs(sol_naive[spline_var_pos:spline_var_pos +
spline_var_dim])
spline.__compute__()
self.num_variables = num_variables
self.num_equalities = num_equalities
self.num_inequalities = num_inequalities
def findSpline(self, s):
if s <= self.support[0]:
id = 0
elif s >= self.support[1]:
id = len(self.splines) - 1
else:
indexes = [
k for k, val in enumerate(self.splines_support) if s >= val
]
id = indexes[-1]
return id
def eval(self, s):
id = self.findSpline(s)
assert id >= 0
return self.splines[id].eval(s)
def evals(self, s, until_order):
id = self.findSpline(s)
assert id >= 0
return self.splines[id].evals(s, until_order)
def diffEval(self, s, order):
id = self.findSpline(s)
assert id >= 0
assert order >= 0
return self.splines[id].diffEval(s, order)
