def test_spatial_velocity(self): velocity = SpatialVelocity() # - Accessors. self.assertTrue(isinstance(velocity.rotational(), np.ndarray)) self.assertTrue(isinstance(velocity.translational(), np.ndarray)) self.assertEqual(velocity.rotational().shape, (3,)) self.assertEqual(velocity.translational().shape, (3,)) # - Fully-parameterized constructor. w = [0.1, 0.3, 0.5] v = [0., 1., 2.] velocity1 = SpatialVelocity(w=w, v=v) self.assertTrue(np.allclose(velocity1.rotational(), w)) self.assertTrue(np.allclose(velocity1.translational(), v))
def test_spatial_velocity(self): velocity = SpatialVelocity() # - Accessors. self.assertTrue(isinstance(velocity.rotational(), np.ndarray)) self.assertTrue(isinstance(velocity.translational(), np.ndarray)) self.assertEqual(velocity.rotational().shape, (3, )) self.assertEqual(velocity.translational().shape, (3, )) # - Fully-parameterized constructor. w = [0.1, 0.3, 0.5] v = [0., 1., 2.] velocity1 = SpatialVelocity(w=w, v=v) self.assertTrue(np.allclose(velocity1.rotational(), w)) self.assertTrue(np.allclose(velocity1.translational(), v))
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = plant.CalcRelativeTransform(context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = plant.CalcPointsPositions(context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) nq = plant.num_positions() nv = plant.num_velocities() wrt_list = [ (JacobianWrtVariable.kQDot, nq), (JacobianWrtVariable.kV, nv), ] for wrt, nw in wrt_list: Jw_ABp_E = plant.CalcJacobianSpatialVelocity(context=context, with_respect_to=wrt, frame_B=base_frame, p_BP=np.zeros(3), frame_A=world_frame, frame_E=world_frame) self.assert_sane(Jw_ABp_E) self.assertEqual(Jw_ABp_E.shape, (6, nw)) # Compute body pose. X_WBase = plant.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame) plant.SetFreeBodyPose(context=context, body=base, X_WB=X_WB_desired) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) plant.SetFreeBodySpatialVelocity(context=context, body=base, V_WB=v_WB) v_base = plant.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue( np.allclose(v_base.translational(), v_WB.translational())) # Compute accelerations. vdot = np.zeros(nv) A_WB_array = plant.CalcSpatialAccelerationsFromVdot(context=context, known_vdot=vdot) self.assertEqual(len(A_WB_array), plant.num_bodies())
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = plant.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = plant.CalcPointsPositions( context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) nq = plant.num_positions() nv = plant.num_velocities() wrt_list = [ (JacobianWrtVariable.kQDot, nq), (JacobianWrtVariable.kV, nv), ] for wrt, nw in wrt_list: Jw_ABp_E = plant.CalcJacobianSpatialVelocity( context=context, with_respect_to=wrt, frame_B=base_frame, p_BP=np.zeros(3), frame_A=world_frame, frame_E=world_frame) self.assert_sane(Jw_ABp_E) self.assertEqual(Jw_ABp_E.shape, (6, nw)) # Compute body pose. X_WBase = plant.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame) plant.SetFreeBodyPose( context=context, body=base, X_WB=X_WB_desired) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) plant.SetFreeBodySpatialVelocity( context=context, body=base, V_WB=v_WB) v_base = plant.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue(np.allclose(v_base.translational(), v_WB.translational())) # Compute accelerations. vdot = np.zeros(nv) A_WB_array = plant.CalcSpatialAccelerationsFromVdot( context=context, known_vdot=vdot) self.assertEqual(len(A_WB_array), plant.num_bodies())
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = tree.CalcRelativeTransform( context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.CalcPointsPositions( context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) # Compute body pose. X_WBase = tree.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # All body poses. X_WB_list = tree.CalcAllBodyPosesInWorld(context) self.assertEqual(len(X_WB_list), 4) for X_WB in X_WB_list: self.assertIsInstance(X_WB, Isometry3) # Compute body velocities. v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertEqual(len(v_WB_list), 4) for v_WB in v_WB_list: self.assertIsInstance(v_WB, SpatialVelocity) # - Sanity check. v_WBase_flat = np.hstack(( v_WB_list[0].rotational(), v_WB_list[0].translational())) self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6))) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame) tree.SetFreeBodyPoseOrThrow( body=base, X_WB=X_WB_desired, context=context) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) tree.SetFreeBodySpatialVelocityOrThrow( body=base, V_WB=v_WB, context=context) v_base = tree.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue(np.allclose(v_base.translational(), v_WB.translational()))
def test_multibody_tree_kinematics(self): file_name = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") plant = MultibodyPlant() AddModelFromSdfFile(file_name, plant) plant.Finalize() context = plant.CreateDefaultContext() tree = plant.tree() world_frame = plant.world_frame() base = plant.GetBodyByName("base") base_frame = plant.GetFrameByName("base") X_WL = tree.CalcRelativeTransform(context, frame_A=world_frame, frame_B=base_frame) self.assertIsInstance(X_WL, Isometry3) p_AQi = tree.CalcPointsPositions(context=context, frame_B=base_frame, p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T, frame_A=world_frame).T self.assertTupleEqual(p_AQi.shape, (2, 3)) Jv_WL = tree.CalcFrameGeometricJacobianExpressedInWorld( context=context, frame_B=base_frame, p_BoFo_B=[0, 0, 0]) self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities())) # Compute body pose. X_WBase = tree.EvalBodyPoseInWorld(context, base) self.assertIsInstance(X_WBase, Isometry3) # All body poses. X_WB_list = tree.CalcAllBodyPosesInWorld(context) self.assertEqual(len(X_WB_list), 4) for X_WB in X_WB_list: self.assertIsInstance(X_WB, Isometry3) # Compute body velocities. v_WB_list = tree.CalcAllBodySpatialVelocitiesInWorld(context) self.assertEqual(len(v_WB_list), 4) for v_WB in v_WB_list: self.assertIsInstance(v_WB, SpatialVelocity) # - Sanity check. v_WBase_flat = np.hstack( (v_WB_list[0].rotational(), v_WB_list[0].translational())) self.assertTrue(np.allclose(v_WBase_flat, np.zeros(6))) # Set pose for the base. X_WB_desired = Isometry3.Identity() X_WB = tree.CalcRelativeTransform(context, world_frame, base_frame) tree.SetFreeBodyPoseOrThrow(body=base, X_WB=X_WB_desired, context=context) self.assertTrue(np.allclose(X_WB.matrix(), X_WB_desired.matrix())) # Set a spatial velocity for the base. v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6]) tree.SetFreeBodySpatialVelocityOrThrow(body=base, V_WB=v_WB, context=context) v_base = tree.EvalBodySpatialVelocityInWorld(context, base) self.assertTrue(np.allclose(v_base.rotational(), v_WB.rotational())) self.assertTrue( np.allclose(v_base.translational(), v_WB.translational()))