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)
