def setup_solver(self):
option = Options()
if logger.getEffectiveLevel() == logging.DEBUG:
option.printLevel = PrintLevel.HIGH
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 __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 get_dq(self, q, e1, J1, e2, J2, e3, J3, e4, J4):
de1 = self.lambda1 * e1
de2 = self.lambda2 * e2
de3 = self.lambda3 * e3
de4 = self.lambda4 * e4
W = self.w1 * np.dot(J1.T, J1) + self.w2 * np.dot(
J2.T, J2) + self.w3 * np.dot(J3.T, J3) + self.w4 * np.dot(
J4.T, J4)
p = -2 * (self.w1 * np.dot(J1.T, de1) + self.w2 * np.dot(J2.T, de2) +
self.w3 * np.dot(J3.T, de3) + self.w4 * np.dot(J4.T, de4))
lower_limits = np.maximum((self.qmin - q[7:]) / self.dt, self.dqmin)
upper_limits = np.minimum((self.qmax - q[7:]) / self.dt, self.dqmax)
lower_limits = np.hstack((self.lfb, lower_limits))
upper_limits = np.hstack((self.ufb, upper_limits))
# Solver
solver = QProblemB(19)
options = Options()
options.setToMPC()
options.printLevel = PrintLevel.LOW
solver.setOptions(options)
nWSR = np.array([10])
solver.init(W, p, lower_limits, upper_limits, nWSR)
dq = np.zeros(19)
solver.getPrimalSolution(dq)
return dq
def 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 test_example7(self):
H = np.array([ 0.8514828085899353, -0.15739890933036804, -0.081726007163524628, -0.530426025390625, 0.16773293912410736,
-0.15739890933036804, 1.1552412509918213, 0.57780224084854126, -0.0072606131434440613, 0.010559185408055782,
-0.081726007163524628, 0.57780224084854126, 0.28925251960754395, 5.324830453901086e-006, -3.0256599075073609e-006,
-0.530426025390625, -0.0072606131434440613, 5.324830453901086e-006, 0.35609596967697144, -0.15124998986721039,
0.16773293912410736, 0.010559185408055782, -3.0256599075073609e-006, -0.15124998986721039,
0.15129712224006653], dtype=float).reshape((5, 5))
g = np.array([0.30908384919166565, 0.99325823783874512, 0.49822014570236206, -0.26309865713119507, 0.024296050891280174], dtype=float).reshape((5,))
A = np.array([1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1], dtype=float).reshape((5, 5))
lb = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
ub = np.array([ 0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
lbA = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
ubA = np.array([0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
# Setting up QProblem object.
qp = QProblem(5, 5)
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 100
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
result = np.zeros((5,))
qp.getPrimalSolution(result)
def fit(self, X):
q, n = len(self.alphas), X.shape[0]
self.X = X
if self.kernel == 'gauss':
H = self.kernel_fun(X, gamma=self.gamma)
else:
H = self.kernel_fun(X, X)
H = 1 / 2 * (H + np.transpose(H))
H = repmat(H, q, q)
g = np.zeros(q * n)
A = np.zeros((q, q * n))
lbA = np.ones(q)
lb = np.zeros(q * n)
ub = np.ones(q * n)
for i in range(q):
start = i * n + 1
end = start + n - 1
ub[start:end] = 1 / (n * (1 - self.alphas[i]))
A[i, start:end] = 1
qp = QProblem(q * n, q)
if not self.verbose:
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
suc = qp.init(H, g, A, lb, ub, lbA, lbA, self.max_iter)
if suc == ReturnValue.MAX_NWSR_REACHED:
msg = "qPOASES reached the maximum number of iterations ({}). ".format(
self.max_iter)
msg += "\nThe resulting regions may not be reliable"
print(msg)
etas = np.zeros(q * n)
qp.getPrimalSolution(etas)
etas = etas.reshape(q, n)
etastars = etas.sum(axis=0)
nus = 1 - self.alphas
SVidx = np.arange(len(etastars))[etastars > self.tol]
nSV = len(SVidx)
ub = 1 / n * nus
rhos = np.zeros(q)
for j, eta in enumerate(etas):
choose = np.logical_and(eta > self.tol, eta < ub[j])
hyperSVidx = np.arange(len(eta))[choose]
if len(hyperSVidx) == 0:
hyperSVidx = np.arange(len(eta))[eta > self.tol]
rhos[j] = max(
np.dot(H[hyperSVidx][:, SVidx], etastars[SVidx]) / q)
else:
rhos[j] = np.median(
np.dot(H[hyperSVidx][:, SVidx], etastars[SVidx]) / q)
self.rhos = rhos
self.etastars = etastars
tmp = np.dot(H[:, SVidx], etastars[SVidx]) / q
self.rhobounds = tmp.max(), tmp.min()
return self
def solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None):
'''
Solve the least square problem:
minimize || A*x-b ||^2
subject to lb_in <= A_in*x <= ub_in
lb <= x <= ub
'''
n = A.shape[1]
m_in = 0
if A_in is not None:
m_in = A_in.shape[0]
if lb_in is None:
lb_in = np.array(m_in * [-1e99])
if ub_in is None:
ub_in = np.array(m_in * [1e99])
if lb is None:
lb = np.array(n * [-1e99])
if ub is None:
ub = np.array(n * [1e99])
Hess = np.dot(A.transpose(), A)
grad = -np.dot(A.transpose(), b)
maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n])
maxComputationTime = np.array([600.0])
options = Options()
options.printLevel = PrintLevel.LOW
# NONE, LOW, MEDIUM
options.enableRegularisation = True
print('Gonna solve QP...')
if m_in == 0:
qpOasesSolver = QProblemB(n)
# , HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter,
maxComputationTime)
else:
qpOasesSolver = SQProblem(n, m_in)
# , HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in,
maxActiveSetIter, maxComputationTime)
print('QP solved in %f seconds and %d iterations' %
(maxComputationTime[0], maxActiveSetIter[0]))
if imode != 0 and imode != 63:
print("ERROR Qp oases %d " % (imode))
x_norm = np.zeros(n)
# solution of the normalized problem
qpOasesSolver.getPrimalSolution(x_norm)
return x_norm
def solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None):
n = A.shape[1]
m_in = 0
if (A_in != None):
m_in = A_in.shape[0]
if (lb_in == None):
lb_in = np.array(m_in * [-1e99])
if (ub_in == None):
ub_in = np.array(m_in * [1e99])
if (lb == None):
lb = np.array(n * [-1e99])
if (ub == None):
ub = np.array(n * [1e99])
Hess = np.dot(A.transpose(), A)
grad = -np.dot(A.transpose(), b)
maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n])
maxComputationTime = np.array([600.0])
options = Options()
options.printLevel = PrintLevel.LOW
#NONE, LOW, MEDIUM
options.enableRegularisation = True
print 'Gonna solve QP...'
if (m_in == 0):
qpOasesSolver = QProblemB(n)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter,
maxComputationTime)
else:
qpOasesSolver = SQProblem(n, m_in)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in,
maxActiveSetIter, maxComputationTime)
print 'QP solved in %f seconds and %d iterations' % (maxComputationTime[0],
maxActiveSetIter[0])
if (imode != 0 and imode != 63):
print "ERROR Qp oases %d " % (imode)
x_norm = np.zeros(n)
# solution of the normalized problem
qpOasesSolver.getPrimalSolution(x_norm)
return x_norm
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 test_id_hessian(self):
"""Very simple example for testing qpOASES (using QProblem class)."""
path = os.path.join(testing_path, "dev_idhessian_data")
#Setup data for QP.
H = np.loadtxt(os.path.join(path, "H.txt"))
g = np.loadtxt(os.path.join(path, "g.txt"))
A = np.loadtxt(os.path.join(path, "A.txt"))
lb = np.loadtxt(os.path.join(path, "lb.txt"))
ub = np.loadtxt(os.path.join(path, "ub.txt"))
lbA = np.loadtxt(os.path.join(path, "lbA.txt"))
ubA = np.loadtxt(os.path.join(path, "ubA.txt"))
#Setting up QProblem object.
qp = QProblem(72,144)
options = Options()
options.numRefinementSteps = 1
options.printLevel = PrintLevel.NONE
#options.setToMPC()
#options.setToReliable()
#options.enableFlippingBounds = BooleanType.FALSE
options.enableRamping = BooleanType.FALSE
#options.enableRamping = BooleanType.TRUE
#options.enableFarBounds = BooleanType.FALSE
#options.enableRamping = BooleanType.FALSE
#options.printLevel = PL_LOW
#options.enableFullLITests = BooleanType.FALSE
#options.boundRelaxation = 1.0e-1
qp.setOptions( options )
#Solve QP.
nWSR = 1200
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
def solveLeastSquare(A, b, lb=None, ub=None, A_in=None, lb_in=None, ub_in=None):
n = A.shape[1];
m_in = 0;
if(A_in!=None):
m_in = A_in.shape[0];
if(lb_in==None):
lb_in = np.array(m_in*[-1e99]);
if(ub_in==None):
ub_in = np.array(m_in*[1e99]);
if(lb==None):
lb = np.array(n*[-1e99]);
if(ub==None):
ub = np.array(n*[1e99]);
Hess = np.dot(A.transpose(),A);
grad = -np.dot(A.transpose(),b);
maxActiveSetIter = np.array([100+2*m_in+2*n]);
maxComputationTime = np.array([600.0]);
options = Options();
options.printLevel = PrintLevel.LOW; #NONE, LOW, MEDIUM
options.enableRegularisation = True;
print 'Gonna solve QP...';
if(m_in==0):
qpOasesSolver = QProblemB(n); #, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options);
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter, maxComputationTime);
else:
qpOasesSolver = SQProblem(n, m_in); #, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options);
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in, maxActiveSetIter, maxComputationTime);
# print 'QP solved in %f seconds and %d iterations' % (maxComputationTime[0],maxActiveSetIter[0]);
if(imode!=0 and imode!=63):
print "ERROR Qp oases %d " % (imode);
x_norm = np.zeros(n); # solution of the normalized problem
qpOasesSolver.getPrimalSolution(x_norm);
return x_norm;
def test_id_hessian(self):
"""Very simple example for testing qpOASES (using QProblem class)."""
path = os.path.join(testing_path, "dev_idhessian_data")
#Setup data for QP.
H = np.loadtxt(os.path.join(path, "H.txt"))
g = np.loadtxt(os.path.join(path, "g.txt"))
A = np.loadtxt(os.path.join(path, "A.txt"))
lb = np.loadtxt(os.path.join(path, "lb.txt"))
ub = np.loadtxt(os.path.join(path, "ub.txt"))
lbA = np.loadtxt(os.path.join(path, "lbA.txt"))
ubA = np.loadtxt(os.path.join(path, "ubA.txt"))
#Setting up QProblem object.
qp = QProblem(72, 144)
options = Options()
options.numRefinementSteps = 1
options.printLevel = PrintLevel.NONE
#options.setToMPC()
#options.setToReliable()
#options.enableFlippingBounds = BooleanType.FALSE
options.enableRamping = BooleanType.FALSE
#options.enableRamping = BooleanType.TRUE
#options.enableFarBounds = BooleanType.FALSE
#options.enableRamping = BooleanType.FALSE
#options.printLevel = PL_LOW
#options.enableFullLITests = BooleanType.FALSE
#options.boundRelaxation = 1.0e-1
qp.setOptions(options)
#Solve QP.
nWSR = 1200
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
def test_example1b(self):
"""Example for qpOASES main function using the QProblemB class."""
# Setup data of first QP.
H = np.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
g = np.array([1.5, 1.0])
lb = np.array([0.5, -2.0])
ub = np.array([5.0, 2.0])
# Setup data of second QP.
g_new = np.array([1.0, 1.5])
lb_new = np.array([0.0, -1.0])
ub_new = np.array([5.0, -0.5])
# Setting up QProblemB object.
qp = QProblemB(2)
options = Options()
# options.enableFlippingBounds = BooleanType.FALSE
options.initialStatusBounds = SubjectToStatus.INACTIVE
options.numRefinementSteps = 1
options.enableCholeskyRefactorisation = 1
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, lb, ub, nWSR)
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# Solve second QP.
nWSR = 10
qp.hotstart(g_new, lb_new, ub_new, nWSR)
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# Get and print solution of second QP.
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
cmd = os.path.join(bin_path, "example1b")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
# get c++ solution from std
pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
match = pattern.findall(stdout)
xOpt_expected = match[-1][0]
xOpt_expected = xOpt_expected.split(",")
xOpt_expected = np.asarray(xOpt_expected, dtype=float)
pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
match = pattern.findall(stdout)
print match
objVal_expected = match[-1]
objVal_expected = np.asarray(objVal_expected, dtype=float)
print("xOpt_actual =", xOpt_actual)
print("xOpt_expected =", xOpt_expected)
print("objVal_actual = ", objVal_actual)
print("objVal_expected = ", objVal_expected)
assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
def solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None,
maxIterations=None,
maxComputationTime=60.0,
regularization=1e-8):
n = A.shape[1]
m_in = 0
A = np.asarray(A)
b = np.asarray(b).squeeze()
if (A_in is not None):
m_in = A_in.shape[0]
if (lb_in is None):
lb_in = np.array(m_in * [-1e99])
else:
lb_in = np.asarray(lb_in).squeeze()
if (ub_in is None):
ub_in = np.array(m_in * [1e99])
else:
ub_in = np.asarray(ub_in).squeeze()
if (lb is None):
lb = np.array(n * [-1e99])
else:
lb = np.asarray(lb).squeeze()
if (ub is None):
ub = np.array(n * [1e99])
else:
ub = np.asarray(ub).squeeze()
# 0.5||Ax-b||^2 = 0.5(x'A'Ax - 2b'Ax + b'b) = 0.5x'A'Ax - b'Ax +0.5b'b
Hess = np.dot(A.T, A) + regularization * np.identity(n)
grad = -np.dot(A.T, b)
if (maxIterations is None):
maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n])
else:
maxActiveSetIter = np.array([maxIterations])
maxComputationTime = np.array([maxComputationTime])
options = Options()
options.printLevel = PrintLevel.NONE
#NONE, LOW, MEDIUM
options.enableRegularisation = False
if (m_in == 0):
qpOasesSolver = QProblemB(n)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
# beware that the Hessian matrix may be modified by this function
imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter,
maxComputationTime)
else:
qpOasesSolver = SQProblem(n, m_in)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in,
maxActiveSetIter, maxComputationTime)
x = np.empty(n)
qpOasesSolver.getPrimalSolution(x)
#print "QP cost:", 0.5*(np.linalg.norm(np.dot(A, x)-b)**2);
return (imode, np.asmatrix(x).T)
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 test_qp_setup_with_toy_example_hack_from_qpoases_manual(self):
gen = ClassicGenerator()
options = Options()
options.printLevel = PrintLevel.LOW
# define matrices from qpoases manual
H = numpy.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
A = numpy.array([1.0, 1.0]).reshape((1, 2))
g = numpy.array([1.5, 1.0])
lb = numpy.array([0.5, -2.0])
ub = numpy.array([5.0, 2.0])
lbA = numpy.array([-1.0])
ubA = numpy.array([2.0])
x = numpy.array([0.5, -1.5])
f = -6.25e-02
# hack pattern generator
gen.ori_nv = 2
gen.ori_nc = 1
gen.ori_dofs = numpy.zeros((2, ))
gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc)
gen.ori_qp.setOptions(options)
gen.ori_H = H
gen.ori_A = A
gen.ori_g = g
gen.ori_lb = lb
gen.ori_ub = ub
gen.ori_lbA = lbA
gen.ori_ubA = ubA
gen.pos_nv = 2
gen.pos_nc = 1
gen.pos_dofs = numpy.zeros((2, ))
gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc)
gen.pos_qp.setOptions(options)
gen.pos_H = H
gen.pos_A = A
gen.pos_g = g
gen.pos_lb = lb
gen.pos_ub = ub
gen.pos_lbA = lbA
gen.pos_ubA = ubA
# test first qp solution
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
# define matrices for warmstart
H_new = numpy.array([1.0, 0.5, 0.5, 0.5]).reshape((2, 2))
A_new = numpy.array([1.0, 5.0]).reshape((1, 2))
g_new = numpy.array([1.0, 1.5])
lb_new = numpy.array([0.0, -1.0])
ub_new = numpy.array([5.0, -0.5])
lbA_new = numpy.array([-2.0])
ubA_new = numpy.array([1.0])
x = numpy.array([0.5, -0.5])
f = -1.875e-01
# hack pattern generator
gen.ori_H = H_new
gen.ori_A = A_new
gen.ori_g = g_new
gen.ori_lb = lb_new
gen.ori_ub = ub_new
gen.ori_lbA = lbA_new
gen.pos_H = H_new
gen.pos_A = A_new
gen.pos_g = g_new
gen.pos_lb = lb_new
gen.pos_ub = ub_new
gen.pos_lbA = lbA_new
# test qp warmstart
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
def test_example1b(self):
"""Example for qpOASES main function using the QProblemB class."""
# Setup data of first QP.
H = np.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
g = np.array([1.5, 1.0])
lb = np.array([0.5, -2.0])
ub = np.array([5.0, 2.0])
# Setup data of second QP.
g_new = np.array([1.0, 1.5])
lb_new = np.array([0.0, -1.0])
ub_new = np.array([5.0, -0.5])
# Setting up QProblemB object.
qp = QProblemB(2)
options = Options()
# options.enableFlippingBounds = BooleanType.FALSE
options.initialStatusBounds = SubjectToStatus.INACTIVE
options.numRefinementSteps = 1
options.enableCholeskyRefactorisation = 1
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, lb, ub, nWSR)
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# Solve second QP.
nWSR = 10
qp.hotstart(g_new, lb_new, ub_new, nWSR)
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
print 'xOpt_actual:', xOpt_actual
print 'objVal_actual:', objVal_actual
# Get and print solution of second QP.
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
cmd = os.path.join(bin_path, "example1b")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
# get c++ solution from std
pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
match = pattern.findall(stdout)
xOpt_expected = match[-1][0]
xOpt_expected = xOpt_expected.split(",")
xOpt_expected = np.asarray(xOpt_expected, dtype=float)
pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
match = pattern.findall(stdout)
print match
objVal_expected = match[-1]
objVal_expected = np.asarray(objVal_expected, dtype=float)
print("xOpt_actual =", xOpt_actual)
print("xOpt_expected =", xOpt_expected)
print("objVal_actual = ", objVal_actual)
print("objVal_expected = ", objVal_expected)
assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
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
def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self):
gen = ClassicGenerator()
options = Options()
options.printLevel = PrintLevel.LOW
# define matrices from qpoases manual
H = numpy.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
A = numpy.array([ 1.0, 1.0 ]).reshape((1,2))
g = numpy.array([ 1.5, 1.0 ])
lb = numpy.array([ 0.5, -2.0 ])
ub = numpy.array([ 5.0, 2.0 ])
lbA = numpy.array([ -1.0 ])
ubA = numpy.array([ 2.0 ])
x = numpy.array([0.5, -1.5])
f = -6.25e-02
# hack pattern generator
gen.ori_nv = 2
gen.ori_nc = 1
gen.ori_dofs = numpy.zeros((2,))
gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc)
gen.ori_qp.setOptions(options)
gen.ori_H = H
gen.ori_A = A
gen.ori_g = g
gen.ori_lb = lb
gen.ori_ub = ub
gen.ori_lbA = lbA
gen.ori_ubA = ubA
gen.pos_nv = 2
gen.pos_nc = 1
gen.pos_dofs = numpy.zeros((2,))
gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc)
gen.pos_qp.setOptions(options)
gen.pos_H = H
gen.pos_A = A
gen.pos_g = g
gen.pos_lb = lb
gen.pos_ub = ub
gen.pos_lbA = lbA
gen.pos_ubA = ubA
# test first qp solution
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
# define matrices for warmstart
H_new = numpy.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
A_new = numpy.array([ 1.0, 5.0 ]).reshape((1,2))
g_new = numpy.array([ 1.0, 1.5 ])
lb_new = numpy.array([ 0.0, -1.0 ])
ub_new = numpy.array([ 5.0, -0.5 ])
lbA_new = numpy.array([ -2.0 ])
ubA_new = numpy.array([ 1.0 ])
x = numpy.array([0.5, -0.5])
f = -1.875e-01
# hack pattern generator
gen.ori_H = H_new
gen.ori_A = A_new
gen.ori_g = g_new
gen.ori_lb = lb_new
gen.ori_ub = ub_new
gen.ori_lbA = lbA_new
gen.pos_H = H_new
gen.pos_A = A_new
gen.pos_g = g_new
gen.pos_lb = lb_new
gen.pos_ub = ub_new
gen.pos_lbA = lbA_new
# test qp warmstart
gen._solve_qp()
# get solution
# NOTE post_process put entries into array that dont match anymore
gen.pos_qp.getPrimalSolution(gen.pos_dofs)
gen.ori_qp.getPrimalSolution(gen.ori_dofs)
assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
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
def test_example2(self):
# Example for qpOASES main function using the SQProblem class.
# Setup data of first QP.
H = np.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
A = np.array([ 1.0, 1.0 ]).reshape((2,1))
g = np.array([ 1.5, 1.0 ])
lb = np.array([ 0.5, -2.0 ])
ub = np.array([ 5.0, 2.0 ])
lbA = np.array([ -1.0 ])
ubA = np.array([ 2.0 ])
# Setup data of second QP.
H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
A_new = np.array([ 1.0, 5.0 ]).reshape((2,1))
g_new = np.array([ 1.0, 1.5 ])
lb_new = np.array([ 0.0, -1.0 ])
ub_new = np.array([ 5.0, -0.5 ])
lbA_new = np.array([ -2.0 ])
ubA_new = np.array([ 1.0 ])
# Setting up SQProblem object and solution analyser.
qp = SQProblem(2, 1)
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
analyser = SolutionAnalysis()
# get c++ solution from std
cmd = os.path.join(bin_path, "example2")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
print(stdout)
# Solve first QP ...
nWSR = 10
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# ... and analyse it.
maxKKTviolation = np.zeros(1)
analyser.getMaxKKTviolation(qp, maxKKTviolation)
print("maxKKTviolation: %e\n"%maxKKTviolation)
actual = np.asarray(maxKKTviolation)
pattern = re.compile(r'maxKKTviolation: (?P<maxKKTviolation>[0-9+-e.]*)')
match = pattern.findall(stdout)
expected = np.asarray(match[0], dtype=float)
assert_almost_equal(actual, expected, decimal=7)
# Solve second QP ...
nWSR = 10
qp.hotstart(H_new, g_new, A_new,
lb_new, ub_new,
lbA_new, ubA_new, nWSR)
# ... and analyse it.
analyser.getMaxKKTviolation(qp, maxKKTviolation)
print("maxKKTviolation: %e\n"%maxKKTviolation)
actual = np.asarray(maxKKTviolation)
expected = np.asarray(match[1], dtype=float)
assert_almost_equal(actual, expected, decimal=7)
# ------------ VARIANCE-COVARIANCE EVALUATION --------------------
Var = np.zeros(5*5)
Primal_Dual_Var = np.zeros(5*5)
Var.reshape((5,5))[0,0] = 1.
Var.reshape((5,5))[1,1] = 1.
# ( 1 0 0 0 0 )
# ( 0 1 0 0 0 )
# Var = ( 0 0 0 0 0 )
# ( 0 0 0 0 0 )
# ( 0 0 0 0 0 )
analyser.getVarianceCovariance(qp, Var, Primal_Dual_Var)
print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5)))
actual = Primal_Dual_Var.reshape((5,5))
pattern = re.compile(r'Primal_Dual_VAR = (?P<VAR>.*)',
re.DOTALL)
print(stdout)
match = pattern.search(stdout)
expected = match.group('VAR').strip().split("\n")
expected = [x.strip().split() for x in expected]
print(expected)
expected = np.asarray(expected, dtype=float)
assert_almost_equal(actual, expected, decimal=7)
def solve(self, i, vref):
"""build the problem and solve it
"""
#we build the N-sized reference vectors
if(i + self.N < self.ntime + 1):
#take the values stored in the rows (i to N)
vref_pred = vref[i:i+self.N, :]
else:
# If the matrix doesn't have N rows ahead, then repeat the last row to fill the void
vref_pred = np.vstack((vref[i:self.ntime, :], np.dot( np.ones((self.N - self.ntime + i, 1)),
np.expand_dims(vref[self.ntime - 1, :], axis=0))))
#get condensed matrices (constant from on step to another)
[Sx, Sy, Svx, Svy, St, Svt, Scx, Scy, Ux, Uu, D_diag] = self.condense()
#build matrix V
V, V_dash = self.get_walk_matrices(i)
##################stuff for the solver###############################
#build constraint vectors (changing from one step to another)
#footsteps and CoP upper bounds
#ubounds CoP x position
ubounds1 = np.vstack(([self.CoP_ubounds[0] for _ in range(self.N)]))
ubounds1 = ubounds1 - np.dot(np.dot(Scx, D_diag), np.dot(Ux, self.x.T)) + np.dot(V, self.f_current[0][0])
#ubounds CoP y position
ubounds2 = np.vstack([self.CoP_ubounds[1] for _ in range(self.N)])
ubounds2 = ubounds2 - np.dot(np.dot(Scy, D_diag), np.dot(Ux, self.x.T)) + np.dot(V, self.f_current[0][1])
#ubounds feet x position
ubounds3 = np.vstack((self.feet_ubounds[0], self.feet_ubounds[2]))
ubounds3 = ubounds3 + np.vstack((np.dot(self.current_rotation[np.newaxis, 0, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps-1, 1))))
#ubounds feet y position
ubounds4 = np.vstack((self.feet_ubounds[1], self.feet_ubounds[3]))
ubounds4 = ubounds4 + np.vstack((np.dot(self.current_rotation[np.newaxis, 1, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps-1, 1))))
#ubounds feet angles
ubounds5 = self.feet_angle_ubound*np.ones((self.future_steps, 1))
ubounds5 = ubounds5 + np.vstack((self.f_current[np.newaxis, np.newaxis, 0, 2], np.zeros((self.future_steps-1, 1))))
ubA = np.vstack((ubounds1, ubounds2, ubounds3, ubounds4, ubounds5))
#footsteps and CoP lower bounds
#lbounds CoP x position
lbounds1 = np.vstack([self.CoP_lbounds[0] for _ in range(self.N)])
lbounds1 = lbounds1 - np.dot(np.dot(Scx, D_diag), np.dot(Ux, self.x.T)) + np.dot(V, self.f_current[0][0])
#lbounds CoP y position
lbounds2 = np.vstack([self.CoP_lbounds[1] for _ in range(self.N)])
lbounds2 = lbounds2 - np.dot(np.dot(Scy, D_diag), np.dot(Ux, self.x.T)) + np.dot(V, self.f_current[0][1])
#lbounds feet x position
lbounds3 = np.vstack((self.feet_lbounds[0], self.feet_lbounds[2]))
lbounds3 = lbounds3 + np.vstack((np.dot(self.current_rotation[np.newaxis, 0, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps-1, 1))))
#lbounds feet y position
lbounds4 = np.vstack((self.feet_lbounds[1], self.feet_lbounds[3]))
lbounds4 = lbounds4 + np.vstack((np.dot(self.current_rotation[np.newaxis, 1, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps-1, 1))))
#lbounds feet angles
lbounds5 = self.feet_angle_lbound*np.ones((self.future_steps, 1))
lbounds5 = lbounds5 + np.vstack((self.f_current[np.newaxis, np.newaxis, 0, 2], np.zeros((self.future_steps-1, 1))))
lbA = np.vstack((lbounds1, lbounds2, lbounds3, lbounds4, lbounds5))
#big A matrix (constraints matrix)
A = np.zeros((self.n_constraints, self.n_dec))
#A elements for constraints for CoP position
A[0:self.N, :] = np.hstack((np.dot(np.dot(Scx, D_diag), Uu), -V_dash, np.zeros((self.N, 2*self.future_steps))))
A[self.N:2*self.N, :] = np.hstack((np.dot(np.dot(Scy, D_diag), Uu), np.zeros((self.N, self.future_steps)), -V_dash, np.zeros((self.N, self.future_steps))))
A[2*self.N, 3*self.N] = self.current_rotation[0, 0]
A[2*self.N, 3*self.N+2] = self.current_rotation[0, 1]
A[2*self.N+1, 3*self.N] = -self.current_rotation[0, 0]
A[2*self.N+1, 3*self.N+1] = self.current_rotation[0, 0]
A[2*self.N+1, 3*self.N+2] = -self.current_rotation[0, 1]
A[2*self.N+1, 3*self.N+3] = self.current_rotation[0, 1]
A[2*self.N+2, 3*self.N] = self.current_rotation[1, 0]
A[2*self.N+2, 3*self.N+2] = self.current_rotation[1, 1]
A[2*self.N+3, 3*self.N] = -self.current_rotation[1, 0]
A[2*self.N+3, 3*self.N+1] = self.current_rotation[1, 0]
A[2*self.N+3, 3*self.N+2] = -self.current_rotation[1, 1]
A[2*self.N+3, 3*self.N+3] = self.current_rotation[1, 1]
#max angle between feet constraints
A[2*self.N+4, 3*self.N+4] = 1
A[2*self.N+5, 3*self.N+4] = -1
A[2*self.N+5, 3*self.N+5] = 1
#QP solver takes one dim arrays
lbA = lbA.reshape((lbA.shape[0],))
ubA = ubA.reshape((ubA.shape[0],))
#############################################################################HESSIAN AND GRADIENT####################
H = np.zeros((self.n_dec, self.n_dec))
H[0:3*self.N, 0:3*self.N] = self.alpha*np.ones((3*self.N, 3*self.N)) + self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(Svx, Uu)) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(Svy, Uu)) + \
+ self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(Svt, Uu)) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(Scx, D_diag), Uu)) + \
+ self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(Scy, D_diag), Uu)) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(St, Uu))
H[0:3*self.N, 3*self.N:] = np.hstack((-self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, V_dash), -self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, V_dash), -self.gamma*np.dot(np.dot(St, Uu).T, V_dash)))
H[3*self.N:, 0:3*self.N] = np.vstack((-self.gamma*np.dot(V_dash.T, np.dot(np.dot(Scx, D_diag), Uu)), -self.gamma*np.dot(V_dash.T, np.dot(np.dot(Scy, D_diag), Uu)), -self.gamma*np.dot(V_dash.T, np.dot(St, Uu))))
H[3*self.N:, 3*self.N:] = self.gamma*block_diag(np.dot(V_dash.T, V_dash), np.dot(V_dash.T, V_dash), np.dot(V_dash.T, V_dash))
g = np.zeros((self.n_dec, 1))
g[0:3*self.N, :] = self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(np.dot(Svx, Ux), self.x.T) - vref_pred[:, 0][np.newaxis].T) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(np.dot(Svy, Ux), self.x.T) - vref_pred[:, 1][np.newaxis].T) + \
self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(np.dot(Svt, Ux), self.x.T) - vref_pred[:, 2][np.newaxis].T) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][0])) + \
+ self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][1])) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(np.dot(St, Ux), self.x.T)) - \
- self.gamma*np.dot(np.dot(St, Uu).T, np.dot(V, self.f_current[0][2]))
g[3*self.N:50, :] = -self.gamma*np.dot(V_dash.T, np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][0]))
g[50:52, :] = -self.gamma*np.dot(V_dash.T, np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][1]))
g[52:, :] = -self.gamma*np.dot(V_dash.T, np.dot(np.dot(St, Ux), self.x.T) - np.dot(V, self.f_current[0][2]))
g = g.reshape((g.shape[0],))
###########################################################################################################################
#solver options - MPC
myOptions = Options()
myOptions.printLevel = PrintLevel.NONE
myOptions.setToMPC()
self.QP.setOptions(myOptions)
#setting lb and ub to huge numbers - otherwise solver gives errors
lb = -1000000000.*np.ones((self.n_dec,))
ub = 1000000000.*np.ones((self.n_dec,))
#solve QP - QP.init()
self.QP.init(H, g, A, lb, ub, lbA, ubA, nWSR=np.array([1000000]))
#get the solution (getPrimalSolution())
x_opt = np.zeros(self.n_dec)
self.QP.getPrimalSolution(x_opt)
#update the state of the system, CoP and the footstep (take the first elements of the solution)
self.controls = np.array([[x_opt[0], x_opt[1], x_opt[2]]])
self.x = (np.dot(self.model.A(self.T), self.x.T) + np.dot(self.model.B(self.T), self.controls.T)).T
self.CoP = np.dot(self.model.D(self.T), self.x.T)
#first future step
self.f_future = np.array([[x_opt[3*self.N], x_opt[3*self.N+self.future_steps], x_opt[3*self.N+2*self.future_steps]]])
#change the current (every 8 samples except the first step) foot position
if np.mod(i, self.samples_per_step) == self.samples_per_step-2:
self.f_current = self.f_future
#rotate the footstep bounds
self.current_rotation = np.array([[ np.cos(self.f_current[0, 2]), np.sin(self.f_current[0, 2])],
[ -np.sin(self.f_current[0, 2]), np.cos(self.f_current[0, 2])]])
#swap footstep constraints
tmp1, tmp2 = self.feet_ubounds[1], self.feet_lbounds[1]
self.feet_ubounds[1], self.feet_lbounds[1] = self.feet_ubounds[3], self.feet_lbounds[3]
self.feet_ubounds[3], self.feet_lbounds[3] = tmp1, tmp2
#return all the variables of interest
return [self.x, self.f_current, self.CoP, self.controls]
def test_example1(self):
return 0
# Example for qpOASES main function using the QProblem class.
#Setup data of first QP.
H = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
A = np.array([1.0, 1.0 ]).reshape((2,1))
g = np.array([1.5, 1.0 ])
lb = np.array([0.5, -2.0])
ub = np.array([5.0, 2.0 ])
lbA = np.array([-1.0 ])
ubA = np.array([2.0])
# Setup data of second QP.
g_new = np.array([1.0, 1.5])
lb_new = np.array([0.0, -1.0])
ub_new = np.array([5.0, -0.5])
lbA_new = np.array([-2.0])
ubA_new = np.array([1.0])
# Setting up QProblemB object.
qp = QProblem(2, 1)
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# Solve second QP.
nWSR = 10
qp.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
# Get and print solution of second QP.
xOpt_actual = np.zeros(2)
qp.getPrimalSolution(xOpt_actual)
xOpt_actual = np.asarray(xOpt_actual, dtype=float)
objVal_actual = qp.getObjVal()
objVal_actual = np.asarray(objVal_actual, dtype=float)
cmd = os.path.join(bin_path, "example1")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
print(stdout)
# get c++ solution from std
pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
match = pattern.search(stdout)
xOpt_expected = match.group('xOpt')
xOpt_expected = xOpt_expected.split(",")
xOpt_expected = np.asarray(xOpt_expected, dtype=float)
pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
match = pattern.search(stdout)
objVal_expected = match.group('objVal')
objVal_expected = np.asarray(objVal_expected, dtype=float)
print("xOpt_actual =", xOpt_actual)
print("xOpt_expected =", xOpt_expected)
print("objVal_actual = ", objVal_actual)
print("objVal_expected = ", objVal_expected)
assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
# details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with qpsolvers. If not, see <http://www.gnu.org/licenses/>.
from numpy import array, hstack, ones, vstack, zeros
from qpoases import PyOptions as Options
from qpoases import PyPrintLevel as PrintLevel
from qpoases import PyQProblem as QProblem
from qpoases import PyQProblemB as QProblemB
from qpoases import PyReturnValue as ReturnValue
__infty = 1e10
options = Options()
options.printLevel = PrintLevel.NONE
def qpoases_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None,
max_wsr=1000):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using qpOASES <https://projects.coin-or.org/qpOASES>.
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
# A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
# details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with qpsolvers. If not, see <http://www.gnu.org/licenses/>.
from numpy import array, hstack, ones, vstack, zeros
from qpoases import PyOptions as Options
from qpoases import PyPrintLevel as PrintLevel
from qpoases import PyQProblem as QProblem
from qpoases import PyQProblemB as QProblemB
from qpoases import PyReturnValue as ReturnValue
__infty__ = 1e10
__options__ = Options()
__options__.printLevel = PrintLevel.NONE
def qpoases_solve_qp(P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None,
verbose=False,
max_wsr=1000):
"""
Solve a Quadratic Program defined as:
.. math::
# A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
# details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with qpsolvers. If not, see <http://www.gnu.org/licenses/>.
from numpy import array, hstack, ones, vstack, zeros
from qpoases import PyOptions as Options
from qpoases import PyPrintLevel as PrintLevel
from qpoases import PyQProblem as QProblem
from qpoases import PyQProblemB as QProblemB
from qpoases import PyReturnValue as ReturnValue
__infty = 1e10
options = Options()
options.printLevel = PrintLevel.NONE
def qpoases_solve_qp(P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None,
max_wsr=1000):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
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
