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