def calcDiff(self, data, x, recalc=True): if recalc: self.calc(data, x) dv_dq, da_dq, da_dv, da_da = pinocchio.getJointAccelerationDerivatives( self.pinocchio, data.pinocchio, data.joint, pinocchio.ReferenceFrame.LOCAL) dv_dq, dv_dvq = pinocchio.getJointVelocityDerivatives( self.pinocchio, data.pinocchio, data.joint, pinocchio.ReferenceFrame.LOCAL) vw = data.v.angular vv = data.v.linear data.Aq[:, :] = (data.fXj * da_dq)[:3, :] + skew(vw) * ( data.fXj * dv_dq)[:3, :] - skew(vv) * (data.fXj * dv_dq)[3:, :] data.Av[:, :] = (data.fXj * da_dv)[:3, :] + skew(vw) * data.J - skew(vv) * data.Jw R = data.pinocchio.oMf[self.frame].rotation if self.gains[0] != 0.: data.Aq[:, :] += self.gains[0] * R * pinocchio.getFrameJacobian( self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] if self.gains[1] != 0.: data.Aq[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dq) data.Av[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dvq)
def test_se3(self): R, p, m = self.R, self.p, self.m X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])]) self.assertApprox(m.action, X) M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)]) self.assertApprox(m.homogeneous, M) m2 = se3.SE3.Random() self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous) self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous)) p = rand(3) self.assertApprox(m * p, m.rotation * p + m.translation) self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation) p = np.vstack([p, 1]) self.assertApprox(m * p, m.homogeneous * p) self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p) p = rand(6) self.assertApprox(m * p, m.action * p) self.assertApprox(m.actInv(p), npl.inv(m.action) * p) p = rand(5) with self.assertRaises(ValueError): m * p with self.assertRaises(ValueError): m.actInv(p) with self.assertRaises(ValueError): m.actInv('42')
def test_se3(self): R, p, m = self.R, self.p, self.m X = np.vstack( [np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])]) self.assertApprox(m.action, X) M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)]) self.assertApprox(m.homogeneous, M) m2 = se3.SE3.Random() self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous) self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous)) p = rand(3) self.assertApprox(m * p, m.rotation * p + m.translation) self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation) p = np.vstack([p, 1]) self.assertApprox(m * p, m.homogeneous * p) self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p) p = rand(6) self.assertApprox(m * p, m.action * p) self.assertApprox(m.actInv(p), npl.inv(m.action) * p) p = rand(5) with self.assertRaises(ValueError): m * p with self.assertRaises(ValueError): m.actInv(p) with self.assertRaises(ValueError): m.actInv('42')
def test_se3(self): R, p, m = self.R, self.p, self.m X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])]) self.assertApprox(m.action, X) M = np.vstack([np.hstack([R, p]), np.matrix([0., 0., 0., 1.], np.double)]) self.assertApprox(m.homogeneous, M) m2 = pin.SE3.Random() self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous) self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous)) p = rand(3) self.assertApprox(m * p, m.rotation * p + m.translation) self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation) ## not supported # p = np.vstack([p, 1]) # self.assertApprox(m * p, m.homogeneous * p) # self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p) ## not supported # p = rand(6) # self.assertApprox(m * p, m.action * p) # self.assertApprox(m.actInv(p), npl.inv(m.action) * p) # Currently, the different cases do not throw the same exception type. # To have a more robust test, only Exception is checked. # In the comments, the most specific actual exception class at the time of writing p = rand(5) with self.assertRaises(Exception): # RuntimeError m * p with self.assertRaises(Exception): # RuntimeError m.actInv(p) with self.assertRaises(Exception): # Boost.Python.ArgumentError (subclass of TypeError) m.actInv('42')
def test_action_matrix(self): amb = pin.SE3.Random() aXb = amb.action self.assertTrue(np.allclose(aXb[:3,:3], amb.rotation)) # top left 33 corner = rotation of amb self.assertTrue(np.allclose(aXb[3:,3:], amb.rotation)) # bottom right 33 corner = rotation of amb tblock = skew(amb.translation)*amb.rotation self.assertTrue(np.allclose(aXb[:3,3:], tblock)) # top right 33 corner = translation + rotation self.assertTrue(np.allclose(aXb[3:,:3], zero([3,3]))) # bottom left 33 corner = 0
def test_dynamic_parameters(self): I = pin.Inertia.Random() v = I.toDynamicParameters() self.assertApprox(v[0], I.mass) self.assertApprox(v[1:4], I.mass * I.lever) I_o = I.inertia + I.mass * skew(I.lever).transpose() * skew(I.lever) I_ov = np.matrix([[float(v[4]), float(v[5]), float(v[7])], [float(v[5]), float(v[6]), float(v[8])], [float(v[7]), float(v[8]), float(v[9])]]) self.assertApprox(I_o, I_ov) I2 = pin.Inertia.FromDynamicParameters(v) self.assertApprox(I2, I)
def test_action_matrix(self): amb = se3.SE3.Random() aXb = amb.action self.assertTrue(np.allclose( aXb[:3, :3], amb.rotation)) # top left 33 corner = rotation of amb self.assertTrue(np.allclose( aXb[3:, 3:], amb.rotation)) # bottom right 33 corner = rotation of amb tblock = skew(amb.translation) * amb.rotation self.assertTrue(np.allclose( aXb[:3, 3:], tblock)) # top right 33 corner = translation + rotation self.assertTrue(np.allclose(aXb[3:, :3], zero([3, 3]))) # bottom left 33 corner = 0
def test_se3(self): R, p, m = self.R, self.p, self.m X = np.vstack( [np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])]) self.assertApprox(m.action, X) M = np.vstack( [np.hstack([R, p]), np.matrix([0., 0., 0., 1.], np.double)]) self.assertApprox(m.homogeneous, M) m2 = se3.SE3.Random() self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous) self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous)) p = rand(3) self.assertApprox(m * p, m.rotation * p + m.translation) self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation) ## not supported # p = np.vstack([p, 1]) # self.assertApprox(m * p, m.homogeneous * p) # self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p) ## not supported # p = rand(6) # self.assertApprox(m * p, m.action * p) # self.assertApprox(m.actInv(p), npl.inv(m.action) * p) # Currently, the different cases do not throw the same exception type. # To have a more robust test, only Exception is checked. # In the comments, the most specific actual exception class at the time of writing p = rand(5) with self.assertRaises(Exception): # RuntimeError m * p with self.assertRaises(Exception): # RuntimeError m.actInv(p) with self.assertRaises( Exception ): # Boost.Python.ArgumentError (subclass of TypeError) m.actInv('42')
iv.__defaults__ = (robot, ) parent.__defaults__ = (robot, ) jFromIdx.__defaults__ = (robot, ) # --- SE3 operators Mcross = lambda x, y: Motion(x).cross(Motion(y)).vector Mcross.__doc__ = "Motion cross product" Fcross = lambda x, y: Motion(x).cross(Force(y)).vector Fcross.__doc__ = "Force cross product" MCross = lambda V, v: np.bmat([Mcross(V[:, i], v) for i in range(V.shape[1])]) FCross = lambda V, f: np.bmat([Fcross(V[:, i], f) for i in range(V.shape[1])]) adj = lambda nu: np.bmat([[skew(nu[3:]), skew(nu[:3])], [zero([3, 3]), skew(nu[3:])]]) adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)" adjdual = lambda nu: np.bmat([[skew(nu[3:]), zero([3, 3])], [skew(nu[:3]), skew(nu[3:])]]) adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' " td = np.tensordot quad = lambda H, v: np.matrix(td(td(H, v, [2, 0]), v, [1, 0])).T quad.__doc__ = '''Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]''' def np_prettyprint(sarg='{: 0.5f}', eps=5e-7): mformat = lambda x, sarg=sarg, eps=eps: sarg.format(x) if abs( x) > eps else ' 0. '
def calcw(q, vq, aq): pinocchio.forwardKinematics(model, data, q, vq, aq) return data.v[-1].angular dv_dqn = df_dq(model, partial(calcv, vq=vq, aq=aq), q) dw_dqn = df_dq(model, partial(calcw, vq=vq, aq=aq), q) assertNumDiff( dv_dq[:3, :], dv_dqn, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff( dv_dq[3:, :], dw_dqn, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) daa_dq = da_dq[:3, :] + skew(vw) * dv_dq[:3, :] - skew(vv) * dv_dq[3:, :] assertNumDiff( daa_dq, daa_dqn, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) # ------------------------------------------------------------------------ # Check contact dynamics 3D contact del vq, aq q = pinocchio.randomConfiguration(model) v = rand(model.nv) * 2 - 1 tau = rand(model.nv) * 2 - 1 for i, f in enumerate(fs): fs[i] = f * 0 pinocchio.computeJointJacobians(model, data, q) J6 = pinocchio.getJointJacobian(model, data, model.joints[-1].id,
#ancestorsOf.__init__.__defaults__ = (robot,) iv.__defaults__ = (robot,) parent.__defaults__ = (robot,) jFromIdx.__defaults__ = (robot,) # --- SE3 operators Mcross = lambda x,y: Motion(x).cross(Motion(y)).vector Mcross.__doc__ = "Motion cross product" Fcross = lambda x,y: Motion(x).cross(Force(y)).vector Fcross.__doc__ = "Force cross product" MCross = lambda V,v: np.bmat([ Mcross(V[:,i],v) for i in range(V.shape[1]) ]) FCross = lambda V,f: np.bmat([ Fcross(V[:,i],f) for i in range(V.shape[1]) ]) adj = lambda nu: np.bmat([[ skew(nu[3:]),skew(nu[:3])],[zero([3,3]),skew(nu[3:])]]) adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)" adjdual = lambda nu: np.bmat([[ skew(nu[3:]),zero([3,3])],[skew(nu[:3]),skew(nu[3:])]]) adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' " td = np.tensordot quad = lambda H,v: np.matrix(td(td(H,v,[2,0]),v,[1,0])).T quad.__doc__ = '''Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]''' def np_prettyprint(sarg = '{: 0.5f}',eps=5e-7): mformat = lambda x,sarg = sarg,eps=eps: sarg.format(x) if abs(x)>eps else ' 0. ' np.set_printoptions(formatter={'float': mformat})