def cid(q_, v_, tau_):
pinocchio.computeJointJacobians(model, data, q_)
J6 = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy()
J = J6[:3, :]
b = pinocchio.rnea(model, data, q_, v_, zero(model.nv)).copy()
M = pinocchio.crba(model, data, q_).copy()
pinocchio.forwardKinematics(model, data, q_, v_, zero(model.nv))
gamma = data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear)
K = np.bmat([[M, J.T], [J, zero([3, 3])]])
r = np.concatenate([tau_ - b, -gamma])
return inv(K) * r
def moveArrow(self, name, pos1, pos2):
if (ENABLE_VIEWER):
length = norm(pos1 - pos2)
self.robot.viewer.gui.resizeArrow('world/' + name,
self.arrow_radius[name], length)
# compute rotation matrix
R = np.matlib.identity(3)
a = pos2 - pos1
if (norm(a) != 0.0):
a /= norm(a)
R[:, 0] = a
R[:, 1] = cross(a,
np.matrix([1., 0., 0.]).T)
if (norm(R[:, 1]) == 0.0):
R[:, 1] = cross(a,
np.matrix([0., 1., 0.]).T)
R[:, 1] /= norm(R[:, 1])
R[:, 2] = cross(a, R[:, 1])
R[:, 2] /= norm(R[:, 2])
rpy = matrixToRpy(R)
self.updateObjectConfigRpy(name, pos1, rpy)
def calc(self, data, x):
# We suppose forwardKinematics(q,v,np.zero), computeJointJacobian and updateFramePlacement already
# computed.
assert (self.ref is not None or self.gains[0] == 0.)
data.v = pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio,
self.frame).copy()
vw = data.v.angular
vv = data.v.linear
J6 = pinocchio.getFrameJacobian(self.pinocchio, data.pinocchio,
self.frame,
pinocchio.ReferenceFrame.LOCAL)
data.J[:, :] = J6[:3, :]
data.Jw[:, :] = J6[3:, :]
data.a0[:] = (pinocchio.getFrameAcceleration(
self.pinocchio, data.pinocchio, self.frame).linear +
cross(vw, vv)).flat
if self.gains[0] != 0.:
data.a0[:] += self.gains[0] * (
m2a(data.pinocchio.oMf[self.frame].translation) - self.ref)
if self.gains[1] != 0.:
data.a0[:] += self.gains[1] * m2a(vv)
def collision(self, c2, data=None, oMj1=None, oMj2=None):
if data is not None:
oMj1 = data.oMi[self.jointParent]
oMj2 = data.oMi[c2.jointParent]
M1 = oMj1 * self.placement
M2 = oMj2 * c2.placement
assert(self.isCapsule() and c2.isCapsule())
l1 = self.length
r1 = self.radius
l2 = c2.length
r2 = c2.radius
a1 = M1.act(np.matrix([0, 0, -l1 / 2]).T)
b1 = M2.act(np.matrix([0, 0, -l2 / 2]).T)
a2 = M1.act(np.matrix([0, 0, +l1 / 2]).T)
b2 = M2.act(np.matrix([0, 0, +l2 / 2]).T)
ab = pinv(np.hstack([a1 - a2, b2 - b1])) * (b2 - a2)
if (0 <= ab).all() and (ab <= 1).all():
asat = bsat = False
pa = a2 + ab[0, 0] * (a1 - a2)
pb = b2 + ab[1, 0] * (b1 - b2)
else:
asat = bsat = True
i = np.argmin(np.vstack([ab, 1 - ab]))
pa = a2 if i == 0 else a1
pb = b2 if i == 1 else b1
if i == 0 or i == 2: # fix a to pa, search b
b = (pinv(b1 - b2) * (pa - b2))[0, 0]
if b < 0:
pb = b2
elif b > 1:
pb = b1
else:
pb = b2 + b * (b1 - b2)
bsat = False
else: # fix b
a = (pinv(a1 - a2) * (pb - a2))[0, 0]
if a < 0:
pa = a2
elif a > 1:
pa = a1
else:
pa = a2 + a * (a1 - a2)
asat = False
dist = norm(pa - pb) - (r1 + r2)
if norm(pa - pb) > 1e-3:
# Compute witness points
ab = pa - pb
ab /= norm(ab)
wa = pa - ab * r1
wb = pb + ab * r2
# Compute normal matrix
x = np.matrix([1., 0, 0]).T
r1 = cross(ab, x)
if norm(r1) < 1e-2:
x = np.matrix([0, 1., 0]).T
r1 = cross(ab, x)
r1 /= norm(r1)
r2 = cross(ab, r1)
R = np.hstack([r1, r2, ab])
self.dist = dist
c2.dist = dist
self.w = wa
c2.w = wb
self.R = R
c2.R = R
return dist
def test_cross(self):
a = np.matrix('2. 0. 0.').T
b = np.matrix('0. 3. 0.').T
c = np.matrix('0. 0. 6.').T
self.assertApprox(cross(a, b), c)
def collision(self, c2, data=None, oMj1=None, oMj2=None):
if data is not None:
oMj1 = data.oMi[self.jointParent]
oMj2 = data.oMi[c2.jointParent]
M1 = oMj1 * self.placement
M2 = oMj2 * c2.placement
assert (self.isCapsule() and c2.isCapsule())
l1 = self.length
r1 = self.radius
l2 = c2.length
r2 = c2.radius
a1 = M1.act(np.array([0, 0, -l1 / 2]))
b1 = M2.act(np.array([0, 0, -l2 / 2]))
a2 = M1.act(np.array([0, 0, +l1 / 2]))
b2 = M2.act(np.array([0, 0, +l2 / 2]))
a1a2_b1b2 = np.array([a1 - a2, b1 - b2]).T
ab = pinv(a1a2_b1b2) * (b2 - a2)
if (0 <= ab).all() and (ab <= 1).all():
asat = bsat = False
pa = a2 + ab[0, 0] * (a1 - a2)
pb = b2 + ab[1, 0] * (b1 - b2)
else:
asat = bsat = True
i = np.argmin(np.vstack([ab, 1 - ab]))
pa = a2 if i == 0 else a1
pb = b2 if i == 1 else b1
if i == 0 or i == 2: # fix a to pa, search b
b = (pinv((b1 - b2).reshape(3, 1)) * (pa - b2))[0, 0]
if b < 0:
pb = b2
elif b > 1:
pb = b1
else:
pb = b2 + b * (b1 - b2)
bsat = False
else: # fix b
a = (pinv((a1 - a2).reshape(3, 1)) * (pb - a2))[0, 0]
if a < 0:
pa = a2
elif a > 1:
pa = a1
else:
pa = a2 + a * (a1 - a2)
asat = False
dist = norm(pa - pb) - (r1 + r2)
if norm(pa - pb) > 1e-3:
# Compute witness points
ab = pa - pb
ab /= norm(ab)
wa = pa - ab * r1
wb = pb + ab * r2
# Compute normal matrix
x = np.matrix([1., 0, 0]).T
r1 = cross(ab, x)
if norm(r1) < 1e-2:
x = np.matrix([0, 1., 0]).T
r1 = cross(ab, x)
r1 /= norm(r1)
r2 = cross(ab, r1)
R = np.hstack([r1, r2, ab.reshape(3, 1)])
self.dist = dist
c2.dist = dist
self.w = wa
c2.w = wb
self.R = R
c2.R = R
return dist
def fgamma(q_, v_, a_):
pinocchio.forwardKinematics(model, data, q_, v_, a_)
return data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear)
def calcwxv(q, vq, aq):
pinocchio.forwardKinematics(model, data, q, vq, aq)
return cross(data.v[-1].angular, data.v[-1].linear)
r = data.oMi[-1].translation oa = M * ia ov = R * iv.linear ow = R * iv.angular vp = v + a * dt pinocchio.forwardKinematics(model, data, qp, vp) ivp = data.v[-1].copy() ovp = data.oMi[-1].rotation * data.v[-1].linear rdot = ov rddot = (ovp - ov) / dt ia + Motion(rdot, O3).cross(iv) assert (absmax(R * ia.linear - (rddot - cross(ow, rdot))) < 10 * dt) # --- check lin acc vq = rand(model.nv) * 2 - 1 aq = rand(model.nv) * 2 - 1 # alpha = [ rddot, wdot ] + [rdot,0] x nu # alpha_l = rddot - w x rdot pinocchio.forwardKinematics(model, data, q, vq, aq) iv = data.v[-1].copy() ia = data.a[-1].copy() M = data.oMi[-1].copy() R = M.rotation r = data.oMi[-1].translation v = iv.linear
def test_cross(self):
a = np.matrix('2. 0. 0.').T
b = np.matrix('0. 3. 0.').T
c = np.matrix('0. 0. 6.').T
self.assertApprox(cross(a, b), c)
