def test_computeRpyJacobianTimeDerivative(self): # Check zero at zero velocity r = random() * 2 * pi - pi p = random() * pi - pi / 2 y = random() * 2 * pi - pi rpy = np.array([r, p, y]) rpydot = np.zeros(3) dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot) self.assertTrue((dj0 == np.zeros((3, 3))).all()) djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL) self.assertTrue((djL == np.zeros((3, 3))).all()) djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD) self.assertTrue((djW == np.zeros((3, 3))).all()) djA = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((djA == np.zeros((3, 3))).all()) # Check correct identities between different versions rpydot = np.random.rand(3) dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot) djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL) djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD) djA = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((dj0 == djL).all()) self.assertTrue((djW == djA).all()) R = rpyToMatrix(rpy) jL = computeRpyJacobian(rpy, pin.LOCAL) jW = computeRpyJacobian(rpy, pin.WORLD) omegaL = jL.dot(rpydot) omegaW = jW.dot(rpydot) self.assertApprox(omegaW, R.dot(omegaL)) self.assertApprox(djW, pin.skew(omegaW).dot(R).dot(jL) + R.dot(djL)) self.assertApprox(djW, R.dot(pin.skew(omegaL)).dot(jL) + R.dot(djL))
def test_skew(self): u = np.random.rand((3)) v = np.random.rand((3)) u_skew = pin.skew(u) u_unskew = pin.unSkew(u_skew) self.assertApprox(u, u_unskew) v_skew = pin.skew(v) u_v_square = pin.skewSquare(u, v) self.assertApprox(u_v_square, u_skew.dot(v_skew))
def test_skew(self): v3 = rand(3) self.assertApprox(v3, unSkew(skew(v3))) self.assertLess(np.linalg.norm(skew(v3).dot(v3)), 1e-10) x, y, z = tuple(rand(3).tolist()) M = np.array([[0., x, y], [-x, 0., z], [-y, -z, 0.]]) self.assertApprox(M, skew(unSkew(M))) rhs = rand(3) self.assertApprox(np.cross(v3, rhs, axis=0), skew(v3).dot(rhs)) self.assertApprox(M.dot(rhs), np.cross(unSkew(M), rhs, axis=0)) x, y, z = tuple(v3.tolist()) self.assertApprox(skew(v3), np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]))
def test_se3(self): R, p, m = self.R, self.p, self.m X = np.vstack( [np.hstack([R, pin.skew(p).dot(R)]), np.hstack([zero([3, 3]), R])]) self.assertApprox(m.action, X) M = np.vstack([ np.hstack([R, np.expand_dims(p, 1)]), np.array([[0., 0., 0., 1.]]) ]) self.assertApprox(m.homogeneous, M) m2 = pin.SE3.Random() self.assertApprox((m * m2).homogeneous, m.homogeneous.dot(m2.homogeneous)) self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous)) p = rand(3) self.assertApprox(m * p, m.rotation.dot(p) + m.translation) self.assertApprox( m.actInv(p), m.rotation.T.dot(p) - m.rotation.T.dot(m.translation)) # 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 skewSquare(self): v3 = rand(3) Mss = skewSquare(v3) Ms = skew(v3) Mss_ref = Ms.dot(Ms) self.assertApprox(Mss,Mss_ref)
def test_computeRpyJacobian(self): # Check identity at zero rpy = np.zeros(3) j0 = computeRpyJacobian(rpy) self.assertTrue((j0 == np.eye(3)).all()) jL = computeRpyJacobian(rpy, pin.LOCAL) self.assertTrue((jL == np.eye(3)).all()) jW = computeRpyJacobian(rpy, pin.WORLD) self.assertTrue((jW == np.eye(3)).all()) jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((jA == np.eye(3)).all()) # Check correct identities between different versions r = random() * 2 * pi - pi p = random() * pi - pi / 2 y = random() * 2 * pi - pi rpy = np.array([r, p, y]) R = rpyToMatrix(rpy) j0 = computeRpyJacobian(rpy) jL = computeRpyJacobian(rpy, pin.LOCAL) jW = computeRpyJacobian(rpy, pin.WORLD) jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED) self.assertTrue((j0 == jL).all()) self.assertTrue((jW == jA).all()) self.assertApprox(jW, R.dot(jL)) # Check against finite differences rpydot = np.random.rand(3) eps = 1e-7 tol = 1e-4 dRdr = (rpyToMatrix(r + eps, p, y) - R) / eps dRdp = (rpyToMatrix(r, p + eps, y) - R) / eps dRdy = (rpyToMatrix(r, p, y + eps) - R) / eps Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2] omegaL = jL.dot(rpydot) self.assertTrue(np.allclose(Rdot, R.dot(pin.skew(omegaL)), atol=tol)) omegaW = jW.dot(rpydot) self.assertTrue(np.allclose(Rdot, pin.skew(omegaW).dot(R), atol=tol))
def forcePose(self, name, force): """ computes unit vectors describing the force and populates the pose matrix """ unit_force = force / np.linalg.norm(force) res = np.cross(self.x_axis, unit_force, axis=0) res_norm = np.linalg.norm(res) if res_norm <= 1.e-8: return np.eye(3) projection = np.dot(self.x_axis, unit_force) res_skew = pin.skew(res) rotation = np.eye(3) + res_skew + np.dot(res_skew, res_skew) * (1 - projection) / res_norm**2 pose = self.data.oMf[self.model.getFrameId(name)] return pin.SE3(rotation, pose.translation)
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 * pin.skew(I.lever).transpose() * pin.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 rotationMatrixFromTwoVectors(a, b): a_copy = a / np.linalg.norm(a) b_copy = b / np.linalg.norm(b) a_cross_b = np.cross(a_copy, b_copy, axis=0) s = np.linalg.norm(a_cross_b) if libcrocoddyl_pywrap.getNumpyType() == np.matrix: warnings.warn( "Numpy matrix supports will be removed in future release", DeprecationWarning, stacklevel=2) if s == 0: return np.matrix(np.eye(3)) c = np.asscalar(a_copy.T * b_copy) ab_skew = pinocchio.skew(a_cross_b) return np.matrix( np.eye(3)) + ab_skew + ab_skew * ab_skew * (1 - c) / s**2 else: if s == 0: return np.eye(3) c = np.dot(a_copy, b_copy) ab_skew = pinocchio.skew(a_cross_b) return np.eye(3) + ab_skew + np.dot(ab_skew, ab_skew) * (1 - c) / s**2
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 = pin.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, pin.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 J_M_times_P(M, P): return np.hstack((M.rotation, -np.dot(M.rotation, pinocchio.skew(P))))
def compute_force_qp(self, q, dq, cnt_array, w_com): """Computes the forces needed to generated a desired centroidal wrench. Args: q: Generalized robot position configuration. q: Generalized robot velocity configuration. cnt_array: Array with {0, 1} of #endeffector size indicating if an endeffector is in contact with the ground or not. Forces are only computed for active endeffectors. w_com: Desired centroidal wrench to achieve given forces. Returns: Computed forces as a plain array of size 3 * num_endeffectors. """ robot = self.robot # com = np.reshape(np.array(q[0:3]), (3,)) com = pin.centerOfMass(robot.pin_robot.model, robot.pin_robot.data, q, dq) r = [ robot.pin_robot.data.oMf[i].translation - com for i in self.eff_ids ] # Use the contact activation from the plan to determine which of the forces # should be active. N = (int)(robot.nb_ee) assert len(cnt_array) == robot.nb_ee # Setup the QP problem. Q = 2. * np.eye(3 * N + 6) Q[-6:-3, -6:-3] = np.diag(self.qp_penalty_lin) Q[-3:, -3:] = np.diag(self.qp_penalty_ang) p = np.zeros(3 * N + 6) A = np.zeros((6, 3 * N + 6)) b = w_com G = np.zeros((5 * N, 3 * N + 6)) h = np.zeros((5 * N)) j = 0 for i in range(robot.nb_ee): if cnt_array[i] == 0: continue A[:3, 3 * j:3 * (j + 1)] = np.eye(3) A[3:, 3 * j:3 * (j + 1)] = pin.skew(r[i]) G[5 * j + 0, 3 * j + 0] = 1 # mu Fz - Fx >= 0 G[5 * j + 0, 3 * j + 2] = -self.mu G[5 * j + 1, 3 * j + 0] = -1 # mu Fz + Fx >= 0 G[5 * j + 1, 3 * j + 2] = -self.mu G[5 * j + 2, 3 * j + 1] = 1 # mu Fz - Fy >= 0 G[5 * j + 2, 3 * j + 2] = -self.mu G[5 * j + 3, 3 * j + 1] = -1 # mu Fz + Fy >= 0 G[5 * j + 3, 3 * j + 2] = -self.mu G[5 * j + 4, 3 * j + 2] = -1 # Fz >= 0 j += 1 A[:, -6:] = np.eye(6) solx = quadprog_solve_qp(Q, p, G, h, A, b) F = np.zeros(3 * len(cnt_array)) j = 0 for i in range(len(cnt_array)): if cnt_array[i] == 0: continue F[3 * i:3 * (i + 1)] = solx[3 * j:3 * (j + 1)] j += 1 return F
#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})