2 import pinocchio
as pin
5 from test_case
import PinocchioTestCase
10 self.
model = pin.buildSampleModelHumanoidRandom()
24 f.type = pin.FrameType.BODY
25 self.assertTrue(f.type == pin.FrameType.BODY)
30 f.name =
'new_hip_frame' 31 self.assertTrue(f.name ==
'new_hip_frame')
38 self.assertTrue(f.parent == newparent)
42 self.assertTrue(np.allclose(f.placement.homogeneous, self.
frame_placement.homogeneous))
43 new_placement = pin.SE3.Random()
44 f.placement = new_placement
45 self.assertTrue(np.allclose(f.placement.homogeneous, new_placement.homogeneous))
49 frame1 = pin.Frame(
"name", 1, 2, M, pin.OP_FRAME)
50 frame2 = pin.Frame(
"name", 1, 2, M, pin.OP_FRAME)
51 frame3 = pin.Frame(
"othername", 3, 4, pin.SE3.Random(), pin.BODY)
53 self.assertTrue(frame1 == frame2)
54 self.assertFalse(frame1 != frame2)
55 self.assertTrue(frame1 != frame3)
56 self.assertFalse(frame1 == frame3)
61 frame = pin.Frame(
"name", 1, 2, pin.SE3.Random(), pin.OP_FRAME)
62 filename =
"frame.pickle" 63 with
open(filename,
'wb')
as f:
66 with
open(filename,
'rb')
as f:
67 frame_copy = pickle.load(f)
69 self.assertEqual(frame, frame_copy)
73 q = pin.randomConfiguration(self.
model)
74 v = np.random.rand(self.
model.nv)
75 a = np.random.rand(self.
model.nv)
76 pin.forwardKinematics(self.
model, data, q, v, a)
83 v = pin.getFrameVelocity(self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL)
85 v = pin.getFrameVelocity(self.
model, data, self.
frame_idx, pin.ReferenceFrame.WORLD)
87 v = pin.getFrameVelocity(self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
92 a = pin.getFrameAcceleration(self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL)
94 a = pin.getFrameAcceleration(self.
model, data, self.
frame_idx, pin.ReferenceFrame.WORLD)
96 a = pin.getFrameAcceleration(self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
99 a = pin.getFrameClassicalAcceleration(self.
model, data, self.
frame_idx)
100 a = pin.getFrameClassicalAcceleration(self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL)
101 a = pin.getFrameClassicalAcceleration(self.
model, data, self.
frame_idx, pin.ReferenceFrame.WORLD)
102 a = pin.getFrameClassicalAcceleration(self.
model, data, self.
frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
106 data = model.createData()
108 q = pin.neutral(model)
109 v = np.random.rand((model.nv))
112 J1 = pin.computeFrameJacobian(model,data,q,frame_id)
113 J2 = pin.computeFrameJacobian(model,data,q,frame_id,pin.LOCAL)
116 data2 = model.createData()
118 pin.computeJointJacobians(model,data2,q)
119 J3 = pin.getFrameJacobian(model,data2,frame_id,pin.LOCAL)
122 dJ1 = pin.frameJacobianTimeVariation(model,data,q,v,frame_id,pin.LOCAL)
124 data3 = model.createData()
125 pin.computeJointJacobiansTimeVariation(model,data3,q,v)
127 dJ2 = pin.getFrameJacobianTimeVariation(model,data3,frame_id,pin.LOCAL)
130 if __name__ ==
'__main__':
def test_parent_get_set(self)
def test_frame_algo(self)
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
def assertApprox(self, a, b, eps=1e-6)
def test_frame_equality(self)
def test_placement_get_set(self)
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
def test_type_get_set(self)
def test_name_get_set(self)