Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

multibody: Implement UniversalJoint #13055

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 61 additions & 16 deletions bindings/pydrake/multibody/test/plant_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
SpatialInertia_,
UniformGravityFieldElement_,
UnitInertia_,
UniversalJoint_,
WeldJoint_,
world_index,
world_model_instance,
Expand Down Expand Up @@ -1039,17 +1040,15 @@ def test_multibody_add_joint(self, T):
`HasJointActuatorNamed`.
"""

def make_weld(plant, P, C):
# TODO(eric.cousineau): Update WeldJoint arg names to be consistent
# with other joints.
return WeldJoint_[T](
name="weld",
parent_frame_P=P,
child_frame_C=C,
X_PC=RigidTransform_[float](),
def make_ball_rpy_joint(plant, P, C):
return BallRpyJoint_[T](
name="ball_rpy",
frame_on_parent=P,
frame_on_child=C,
damping=2.,
)

def make_prismatic(plant, P, C):
def make_prismatic_joint(plant, P, C):
return PrismaticJoint_[T](
name="prismatic",
frame_on_parent=P,
Expand All @@ -1058,7 +1057,7 @@ def make_prismatic(plant, P, C):
damping=2.,
)

def make_revolute(plant, P, C):
def make_revolute_joint(plant, P, C):
return RevoluteJoint_[T](
name="revolute",
frame_on_parent=P,
Expand All @@ -1067,19 +1066,30 @@ def make_revolute(plant, P, C):
damping=2.,
)

def make_ball_rpy_joint(plant, P, C):
return BallRpyJoint_[T](
name="ball_rpy",
def make_universal_joint(plant, P, C):
return UniversalJoint_[T](
name="universal",
frame_on_parent=P,
frame_on_child=C,
damping=2.,
)

def make_weld_joint(plant, P, C):
# TODO(eric.cousineau): Update WeldJoint arg names to be consistent
# with other joints.
return WeldJoint_[T](
name="weld",
parent_frame_P=P,
child_frame_C=C,
X_PC=RigidTransform_[float](),
)

make_joint_list = [
make_weld,
make_prismatic,
make_revolute,
make_ball_rpy_joint,
make_prismatic_joint,
make_revolute_joint,
make_universal_joint,
make_weld_joint,
]

for make_joint in make_joint_list:
Expand All @@ -1102,6 +1112,41 @@ def make_ball_rpy_joint(plant, P, C):
plant.Finalize()
self._test_joint_api(T, joint)

context = plant.CreateDefaultContext()
if joint.name() == "ball_rpy":
set_point = np.array([1., 2., 3.])
joint.set_angles(context=context, angles=set_point)
self.assertEqual(len(joint.get_angles(context=context)), 3)
joint.set_angular_velocity(context=context, w_FM=set_point)
self.assertEqual(
len(joint.get_angular_velocity(context=context)), 3)
elif joint.name() == "prismatic":
set_point = 1.
joint.set_translation(context=context, translation=set_point)
self.assertIsInstance(
joint.get_translation(context=context), T)
joint.set_translation_rate(context=context,
translation_dot=set_point)
self.assertIsInstance(
joint.get_translation_rate(context=context), T)
elif joint.name() == "revolute":
set_point = 1.
joint.set_angle(context=context, angle=set_point)
self.assertIsInstance(joint.get_angle(context=context), T)
elif joint.name() == "universal":
set_point = np.array([1., 2.])
joint.set_angles(context=context, angles=set_point)
self.assertEqual(len(joint.get_angles(context=context)), 2)
joint.set_angular_rates(context=context, theta_dot=set_point)
self.assertEqual(
len(joint.get_angular_rates(context=context)), 2)
elif joint.name() == "weld":
# No joint specifc methods to test
pass
else:
raise TypeError(
"Joint type " + joint.name() + " not recognized.")

@numpy_compare.check_all_types
def test_multibody_add_frame(self, T):
MultibodyPlant = MultibodyPlant_[T]
Expand Down
25 changes: 25 additions & 0 deletions bindings/pydrake/multibody/tree_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "drake/multibody/tree/revolute_joint.h"
#include "drake/multibody/tree/revolute_spring.h"
#include "drake/multibody/tree/rigid_body.h"
#include "drake/multibody/tree/universal_joint.h"
#include "drake/multibody/tree/weld_joint.h"

namespace drake {
Expand Down Expand Up @@ -320,6 +321,30 @@ void DoScalarDependentDefinitions(py::module m, T) {
cls_doc.set_random_angle_distribution.doc);
}

// UniversalJoint
{
using Class = UniversalJoint<T>;
constexpr auto& cls_doc = doc.UniversalJoint;
auto cls = DefineTemplateClassWithDefault<Class, Joint<T>>(
m, "UniversalJoint", param, cls_doc.doc);
cls // BR
.def(
py::init<const string&, const Frame<T>&, const Frame<T>&, double>(),
py::arg("name"), py::arg("frame_on_parent"),
py::arg("frame_on_child"), py::arg("damping") = 0, cls_doc.ctor.doc)
.def("get_angles", &Class::get_angles, py::arg("context"),
cls_doc.get_angles.doc)
.def("set_angles", &Class::set_angles, py::arg("context"),
py::arg("angles"), cls_doc.set_angles.doc)
.def("get_angular_rates", &Class::get_angular_rates, py::arg("context"),
cls_doc.get_angular_rates.doc)
.def("set_angular_rates", &Class::set_angular_rates, py::arg("context"),
py::arg("theta_dot"), cls_doc.set_angular_rates.doc)
.def("set_random_angles_distribution",
&Class::set_random_angles_distribution, py::arg("angles"),
cls_doc.set_random_angles_distribution.doc);
}

// WeldJoint
{
using Class = WeldJoint<T>;
Expand Down
10 changes: 10 additions & 0 deletions multibody/tree/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ drake_cc_library(
"rigid_body.cc",
"space_xyz_mobilizer.cc",
"uniform_gravity_field_element.cc",
"universal_joint.cc",
"universal_mobilizer.cc",
"weld_joint.cc",
"weld_mobilizer.cc",
Expand Down Expand Up @@ -146,6 +147,7 @@ drake_cc_library(
"rigid_body.h",
"space_xyz_mobilizer.h",
"uniform_gravity_field_element.h",
"universal_joint.h",
"universal_mobilizer.h",
"weld_joint.h",
"weld_mobilizer.h",
Expand Down Expand Up @@ -502,6 +504,14 @@ drake_cc_googletest(
],
)

drake_cc_googletest(
name = "universal_joint_test",
deps = [
":tree",
"//common/test_utilities:expect_throws_message",
],
)

drake_cc_googletest(
name = "weld_joint_test",
deps = [
Expand Down
3 changes: 0 additions & 3 deletions multibody/tree/ball_rpy_joint.h
Original file line number Diff line number Diff line change
Expand Up @@ -245,9 +245,6 @@ class BallRpyJoint final : public Joint<T> {
template <typename>
friend class BallRpyJoint;

// Friend class to facilitate testing.
friend class JointTester;

// Returns the mobilizer implementing this joint.
// The internal implementation of this joint could change in a future version.
// However its public API should remain intact.
Expand Down
3 changes: 0 additions & 3 deletions multibody/tree/prismatic_joint.h
Original file line number Diff line number Diff line change
Expand Up @@ -311,9 +311,6 @@ class PrismaticJoint final : public Joint<T> {
// private members of PrismaticJoint<T>.
template <typename> friend class PrismaticJoint;

// Friend class to facilitate testing.
friend class JointTester;

// Returns the mobilizer implementing this joint.
// The internal implementation of this joint could change in a future version.
// However its public API should remain intact.
Expand Down
2 changes: 0 additions & 2 deletions multibody/tree/space_xyz_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,6 @@ class SpaceXYZMobilizer final : public MobilizerImpl<T, 3, 3> {
const Frame<T>& outboard_frame_M) :
MobilizerBase(inboard_frame_F, outboard_frame_M) {}

bool is_floating() const override { return true; }

bool has_quaternion_dofs() const override { return false; }

/// Retrieves from `context` the three space x-y-z angles θ₁, θ₂, θ₃ which
Expand Down
16 changes: 4 additions & 12 deletions multibody/tree/test/free_rotating_body_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -48,20 +48,12 @@ GTEST_TEST(RollPitchYawTest, TimeDerivatives) {
FreeRotatingBodyPlant<double> free_body_plant(benchmark_.get_I(),
benchmark_.get_J());

// We expect the body in this model to be a floating body, however not modeled
// using a quaternion mobilizer (it uses a SpaceXYZMobilizer).
EXPECT_TRUE(free_body_plant.body().is_floating());
// The body in this model is not a floating body but is free to rotate. The
// rotation is not modeled using a quaternion mobilizer (it uses a
// SpaceXYZMobilizer).
EXPECT_FALSE(free_body_plant.body().is_floating());
EXPECT_FALSE(free_body_plant.body().has_quaternion_dofs());

// For this simple example with a single floating body we can verify indexes
// into the state. In addition, the test above verifies we are not using a
// quaternions, but a SpaceXYZMobilizer (3-dofs).
// In the state vector x for the model, positions q go first followed by
// velocities v. Similarly, angular components go first, followed by
// translational components.
EXPECT_EQ(free_body_plant.body().floating_positions_start(), 0);
EXPECT_EQ(free_body_plant.body().floating_velocities_start(), 3);

// Simulator will create a Context by calling this system's
// CreateDefaultContext(). This in turn will initialize its state by making a
// call to this system's SetDefaultState().
Expand Down
Loading