def test_multibody_add_joint(self): """ Tests joint constructors and `AddJoint`. """ instance_file = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") # Add different joints between multiple model instances. # TODO(eric.cousineau): Remove the multiple instances and use # programmatically constructed bodies once this API is exposed in # Python. num_joints = 2 plant = MultibodyPlant() parser = Parser(plant) instances = [] for i in range(num_joints + 1): instance = parser.AddModelFromFile(instance_file, "instance_{}".format(i)) instances.append(instance) proximal_frame = "base" distal_frame = "lower_link" joints = [ RevoluteJoint( name="revolve_things", frame_on_parent=plant.GetBodyByName(distal_frame, instances[1]).body_frame(), frame_on_child=plant.GetBodyByName(proximal_frame, instances[2]).body_frame(), axis=[0, 0, 1], damping=0.), WeldJoint( name="weld_things", parent_frame_P=plant.GetBodyByName(distal_frame, instances[0]).body_frame(), child_frame_C=plant.GetBodyByName(proximal_frame, instances[1]).body_frame(), X_PC=RigidTransform.Identity()), ] for joint in joints: joint_out = plant.AddJoint(joint) self.assertIs(joint, joint_out) # The model is now complete. plant.Finalize() for joint in joints: self._test_joint_api(joint)
def test_solve(self): plant = MultibodyPlant(0) M_AAo_A = SpatialInertia(1, np.zeros(3), UnitInertia(1, 1, 1)) body = plant.AddRigidBody("body", M_AAo_A) plant.AddJoint( PrismaticJoint("joint", plant.world_frame(), body.body_frame(), [1, 0, 0])) plant.Finalize() path = PiecewisePolynomial.CubicWithContinuousSecondDerivatives( [0., 1.], np.array([[0., 1.]]), [0.], [0.]) gridpoints = Toppra.CalcGridPoints(path, CalcGridPointsOptions()) toppra = Toppra(path=path, plant=plant, gridpoints=gridpoints) toppra.AddJointAccelerationLimit([-1.], [1.]) trajectory = toppra.SolvePathParameterization() self.assertIsInstance(trajectory, PiecewisePolynomial)