Esempio n. 1
0
 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))
Esempio n. 2
0
 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))
Esempio n. 3
0
    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())
Esempio n. 4
0
    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())
Esempio n. 5
0
    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()))
Esempio n. 6
0
    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()))