def check_contact_forces(self, q, qd, qdd):
import cvxopt.solvers
cvxopt.solvers.options['show_progress'] = False # disable output
with self.hrp.rave:
self.hrp.rave.SetDOFValues(q)
self.hrp.rave.SetDOFVelocities(qd)
tau = self.rave.ComputeInverseDynamics(qdd)
J, contacts = self.compute_contact_jacobian(
q, qd, self.support_state, returncontacts=True)
ncontacts = len(contacts)
T, fc = self.get_friction_cones(ncontacts)
PJT, Ptau = J.T[50:, :], tau[50:]
# QP: minimize norm(dot(PJT, f) - Ptau)
# s.t. dot(T, f) <= fc
#
qp_P = cvxopt.matrix(dot(PJT.T, PJT))
qp_q = cvxopt.matrix(-dot(Ptau, PJT))
qp_G = cvxopt.matrix(T)
qp_h = cvxopt.matrix(fc)
qp_x = cvxopt.solvers.qp(qp_P, qp_q, qp_G, qp_h)['x']
f = array(qp_x).reshape((3 * ncontacts,))
error = norm(dot(PJT, f) - Ptau)
thres = 300 * 1e-2 # norm(Ptau) is around 300 N.m
if error > thres / 2: # more than half the allowed error
print "danger zone: (%f > %f) " % (error, thres / 2)
assert error < thres, \
"contact forces cannot realize the motion " \
"(%f > %f) " % (error, thres)
return contacts, f
def connect(q0, q1, support):
"""Bezier curve with as-linear-as-possible velocity."""
assert not hrp.self_collides(q0)
assert not hrp.self_collides(q1)
qd_ref = (q1 - q0) / norm(q1 - q0)
qd0 = project_foot_velocity(q0, qd_ref)
qd1 = project_foot_velocity(q1, qd_ref)
traj = active_bezier_interpolate(
q0, qd0, q1, qd1, ACTUATED_DOFS, support)
return traj
def _enforce(self, q_warm):
self.converged = False
self.robot.rave.SetDOFValues(q_warm)
self.robot.rave.SetActiveDOFs(self.active_indexes)
q_max = array([DOF_SCALE * dof.ulim for dof in self.active_dofs])
q_min = array([DOF_SCALE * dof.llim for dof in self.active_dofs])
I = eye(self.nb_active_dof)
q = full_to_active(q_warm, self.active_dofs)
self.robot.rave.SetActiveDOFValues(q)
for itnum in xrange(self.max_iter):
conv_vect = array([norm(task.f()) for task in self.tasks])
if numpy.all(conv_vect < self.conv_thres):
self.converged = True
break
if DEBUG_IK:
conv = ["%10.8f" % x for x in conv_vect]
print " %4d: %s" % (itnum, ' '.join(conv))
ker_proj = eye(self.nb_active_dof)
dq = zeros(self.nb_active_dof)
qd_max_reg = self.gain * (q_max - q)
qd_min_reg = self.gain * (q - q_min)
for i, task in enumerate(self.tasks):
J = task.J()
Jn = dot(J, ker_proj)
b = -self.gain * task.f() - dot(J, dq)
In = eye(Jn.shape[0])
sr_inv = dot(Jn.T, linalg.inv(dot(Jn, Jn.T) + 1e-8 * In))
dq += dot(sr_inv, b)
ker_proj = dot(ker_proj, I - dot(linalg.pinv(Jn), Jn))
qd_max_reg = self.gain * (q_max - q)
qd_min_reg = self.gain * (q - q_min)
q += solve_ineq(I, dq, I, qd_max_reg, qd_min_reg)
self.robot.rave.SetActiveDOFValues(q)
return self.robot.rave.GetDOFValues()
