From 40f44d8e16ca54dcac3b681af3798d78d3d96e1e Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Tue, 5 May 2020 19:39:20 -0400 Subject: [PATCH 1/4] Remove unused JointTester friend class and fix floating mobilizer bug --- multibody/tree/ball_rpy_joint.h | 3 --- multibody/tree/prismatic_joint.h | 3 --- multibody/tree/space_xyz_mobilizer.h | 2 -- multibody/tree/test/free_rotating_body_test.cc | 16 ++++------------ multibody/tree/weld_joint.h | 3 --- 5 files changed, 4 insertions(+), 23 deletions(-) diff --git a/multibody/tree/ball_rpy_joint.h b/multibody/tree/ball_rpy_joint.h index a854e650bfa2..351603ce5c48 100644 --- a/multibody/tree/ball_rpy_joint.h +++ b/multibody/tree/ball_rpy_joint.h @@ -245,9 +245,6 @@ class BallRpyJoint final : public Joint { template 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. diff --git a/multibody/tree/prismatic_joint.h b/multibody/tree/prismatic_joint.h index af13df532d27..71506e6a92a8 100644 --- a/multibody/tree/prismatic_joint.h +++ b/multibody/tree/prismatic_joint.h @@ -311,9 +311,6 @@ class PrismaticJoint final : public Joint { // private members of PrismaticJoint. template 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. diff --git a/multibody/tree/space_xyz_mobilizer.h b/multibody/tree/space_xyz_mobilizer.h index 6e913f80ac93..23feb5ffaf28 100644 --- a/multibody/tree/space_xyz_mobilizer.h +++ b/multibody/tree/space_xyz_mobilizer.h @@ -73,8 +73,6 @@ class SpaceXYZMobilizer final : public MobilizerImpl { const Frame& 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 diff --git a/multibody/tree/test/free_rotating_body_test.cc b/multibody/tree/test/free_rotating_body_test.cc index 4d8a1c2ace01..378ff8020206 100644 --- a/multibody/tree/test/free_rotating_body_test.cc +++ b/multibody/tree/test/free_rotating_body_test.cc @@ -48,20 +48,12 @@ GTEST_TEST(RollPitchYawTest, TimeDerivatives) { FreeRotatingBodyPlant 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(). diff --git a/multibody/tree/weld_joint.h b/multibody/tree/weld_joint.h index d577df027a26..e3ddeea7a478 100644 --- a/multibody/tree/weld_joint.h +++ b/multibody/tree/weld_joint.h @@ -105,9 +105,6 @@ class WeldJoint final : public Joint { // private members of WeldJoint. template friend class WeldJoint; - // 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. From ef0732ab3d0567288625403a6d4e5d95cfc71265 Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Tue, 28 Apr 2020 21:22:51 -0400 Subject: [PATCH 2/4] Add Joint specific binding tests and alphabetize joints --- bindings/pydrake/multibody/test/plant_test.py | 66 +++++++++++++------ 1 file changed, 47 insertions(+), 19 deletions(-) diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index 8947fc56a0f3..113204ff2fc2 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/bindings/pydrake/multibody/test/plant_test.py @@ -1039,17 +1039,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, @@ -1058,7 +1056,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, @@ -1067,19 +1065,21 @@ def make_revolute(plant, P, C): damping=2., ) - 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_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_weld_joint, ] for make_joint in make_joint_list: @@ -1102,6 +1102,34 @@ 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() == "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] From 38eda89ef619c49fcdd294f82377957f18d7bcda Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Tue, 28 Apr 2020 21:20:56 -0400 Subject: [PATCH 3/4] Implement UniversalJoint --- multibody/tree/BUILD.bazel | 10 + multibody/tree/test/universal_joint_test.cc | 260 ++++++++++++++++++ multibody/tree/universal_joint.cc | 69 +++++ multibody/tree/universal_joint.h | 284 ++++++++++++++++++++ 4 files changed, 623 insertions(+) create mode 100644 multibody/tree/test/universal_joint_test.cc create mode 100644 multibody/tree/universal_joint.cc create mode 100644 multibody/tree/universal_joint.h diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index 88993666b467..02afcd294bd4 100644 --- a/multibody/tree/BUILD.bazel +++ b/multibody/tree/BUILD.bazel @@ -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", @@ -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", @@ -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 = [ diff --git a/multibody/tree/test/universal_joint_test.cc b/multibody/tree/test/universal_joint_test.cc new file mode 100644 index 000000000000..9ba13f83519e --- /dev/null +++ b/multibody/tree/test/universal_joint_test.cc @@ -0,0 +1,260 @@ +// clang-format: off +#include "drake/multibody/tree/multibody_tree-inl.h" +// clang-format: on + +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/multibody/tree/rigid_body.h" +#include "drake/multibody/tree/universal_joint.h" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace multibody { +namespace { + +using Eigen::Vector2d; +using systems::Context; + +constexpr double kPositionLowerLimit = -1.0; +constexpr double kPositionUpperLimit = 1.5; +constexpr double kVelocityLowerLimit = -1.1; +constexpr double kVelocityUpperLimit = 1.6; +constexpr double kAccelerationLowerLimit = -1.2; +constexpr double kAccelerationUpperLimit = 1.7; +constexpr double kDamping = 3; +constexpr double kPositionNonZeroDefault = 0.25; + +class UniversalJointTest : public ::testing::Test { + public: + // Creates a MultibodyTree model with a body hanging from a universal joint. + void SetUp() override { + // Spatial inertia for adding bodies. The actual value is not important for + // these tests and therefore we do not initialize it. + const SpatialInertia M_B; // Default construction is ok for this. + + // Create an empty model. + auto model = std::make_unique>(); + + // Add some bodies so we can add joints between them: + body_ = &model->AddBody(M_B); + + // Add a universal joint between the world and body1: + joint_ = &model->AddJoint("Joint", model->world_body(), + std::nullopt, *body_, + std::nullopt, kDamping); + mutable_joint_ = dynamic_cast*>( + &model->get_mutable_joint(joint_->index())); + DRAKE_DEMAND(mutable_joint_); + mutable_joint_->set_position_limits( + Vector2d::Constant(kPositionLowerLimit), + Vector2d::Constant(kPositionUpperLimit)); + mutable_joint_->set_velocity_limits( + Vector2d::Constant(kVelocityLowerLimit), + Vector2d::Constant(kVelocityUpperLimit)); + mutable_joint_->set_acceleration_limits( + Vector2d::Constant(kAccelerationLowerLimit), + Vector2d::Constant(kAccelerationUpperLimit)); + + // We are done adding modeling elements. Transfer tree to system and get + // a Context. + system_ = std::make_unique>( + std::move(model)); + context_ = system_->CreateDefaultContext(); + } + + const internal::MultibodyTree& tree() const { + return internal::GetInternalTree(*system_); + } + + protected: + std::unique_ptr> system_; + std::unique_ptr> context_; + + const RigidBody* body_{nullptr}; + const UniversalJoint* joint_{nullptr}; + UniversalJoint* mutable_joint_{nullptr}; +}; + +TEST_F(UniversalJointTest, Type) { + const Joint& base = *joint_; + EXPECT_EQ(base.type_name(), UniversalJoint::kTypeName); +} + +// Verify the expected number of dofs. +TEST_F(UniversalJointTest, NumDOFs) { + EXPECT_EQ(tree().num_positions(), 2); + EXPECT_EQ(tree().num_velocities(), 2); + EXPECT_EQ(joint_->num_positions(), 2); + EXPECT_EQ(joint_->num_velocities(), 2); + EXPECT_EQ(joint_->position_start(), 0); + EXPECT_EQ(joint_->velocity_start(), 0); +} + +TEST_F(UniversalJointTest, GetJointLimits) { + EXPECT_EQ(joint_->position_lower_limits().size(), 2); + EXPECT_EQ(joint_->position_upper_limits().size(), 2); + EXPECT_EQ(joint_->velocity_lower_limits().size(), 2); + EXPECT_EQ(joint_->velocity_upper_limits().size(), 2); + EXPECT_EQ(joint_->acceleration_lower_limits().size(), 2); + EXPECT_EQ(joint_->acceleration_upper_limits().size(), 2); + + EXPECT_EQ(joint_->position_lower_limits(), + Vector2d::Constant(kPositionLowerLimit)); + EXPECT_EQ(joint_->position_upper_limits(), + Vector2d::Constant(kPositionUpperLimit)); + EXPECT_EQ(joint_->velocity_lower_limits(), + Vector2d::Constant(kVelocityLowerLimit)); + EXPECT_EQ(joint_->velocity_upper_limits(), + Vector2d::Constant(kVelocityUpperLimit)); + EXPECT_EQ(joint_->acceleration_lower_limits(), + Vector2d::Constant(kAccelerationLowerLimit)); + EXPECT_EQ(joint_->acceleration_upper_limits(), + Vector2d::Constant(kAccelerationUpperLimit)); + EXPECT_EQ(joint_->damping(), kDamping); +} + +// Context-dependent value access. +TEST_F(UniversalJointTest, ContextDependentAccess) { + const Vector2d some_value(M_PI_2, 0.3); + // Angle access: + joint_->set_angles(context_.get(), some_value); + EXPECT_EQ(joint_->get_angles(*context_), some_value); + + // Angular rate access: + joint_->set_angular_rates(context_.get(), some_value); + EXPECT_EQ(joint_->get_angular_rates(*context_), some_value); +} + +// Tests API to apply torques to individual dof of joint. +TEST_F(UniversalJointTest, AddInOneForce) { + const double some_value = M_PI_2; + const double kEpsilon = std::numeric_limits::epsilon(); + MultibodyForces forces1(tree()); + MultibodyForces forces2(tree()); + + for (int ii = 0; ii < joint_->num_positions(); ii++) { + // Add value twice: + joint_->AddInOneForce(*context_, ii, some_value, &forces1); + joint_->AddInOneForce(*context_, ii, some_value, &forces1); + // Add value only once: + joint_->AddInOneForce(*context_, ii, some_value, &forces2); + } + // Add forces2 into itself (same as adding torque twice): + forces2.AddInForces(forces2); + + // forces1 and forces2 should be equal: + EXPECT_EQ(forces1.generalized_forces(), forces2.generalized_forces()); + auto F2 = forces2.body_forces().cbegin(); + for (auto& F1 : forces1.body_forces()) + EXPECT_TRUE(F1.IsApprox(*F2++, kEpsilon)); +} + +TEST_F(UniversalJointTest, Clone) { + auto model_clone = tree().CloneToScalar(); + const auto& joint_clone = model_clone->get_variant(*joint_); + + EXPECT_EQ(joint_clone.name(), joint_->name()); + EXPECT_EQ(joint_clone.frame_on_parent().index(), + joint_->frame_on_parent().index()); + EXPECT_EQ(joint_clone.frame_on_child().index(), + joint_->frame_on_child().index()); + EXPECT_EQ(joint_clone.position_lower_limits(), + joint_->position_lower_limits()); + EXPECT_EQ(joint_clone.position_upper_limits(), + joint_->position_upper_limits()); + EXPECT_EQ(joint_clone.velocity_lower_limits(), + joint_->velocity_lower_limits()); + EXPECT_EQ(joint_clone.velocity_upper_limits(), + joint_->velocity_upper_limits()); + EXPECT_EQ(joint_clone.acceleration_lower_limits(), + joint_->acceleration_lower_limits()); + EXPECT_EQ(joint_clone.acceleration_upper_limits(), + joint_->acceleration_upper_limits()); + EXPECT_EQ(joint_clone.damping(), joint_->damping()); +} + +TEST_F(UniversalJointTest, SetVelocityAndAccelerationLimits) { + const double new_lower = -0.2; + const double new_upper = 0.2; + // Check for velocity limits. + mutable_joint_->set_velocity_limits(Vector2d::Constant(new_lower), + Vector2d::Constant(new_upper)); + EXPECT_EQ(joint_->velocity_lower_limits(), Vector2d::Constant(new_lower)); + EXPECT_EQ(joint_->velocity_upper_limits(), Vector2d::Constant(new_upper)); + // Does not match num_velocities(). + DRAKE_EXPECT_THROWS_MESSAGE( + mutable_joint_->set_velocity_limits(VectorX(2), + VectorX()), + std::runtime_error, + ".* 'lower_limits.size\\(\\) == upper_limits.size\\(\\)' failed."); + DRAKE_EXPECT_THROWS_MESSAGE( + mutable_joint_->set_velocity_limits(VectorX(), + VectorX(2)), + std::runtime_error, + ".* 'lower_limits.size\\(\\) == upper_limits.size\\(\\)' failed."); + // Lower limit is larger than upper limit. + DRAKE_EXPECT_THROWS_MESSAGE(mutable_joint_->set_velocity_limits( + Vector2d::Constant(2), Vector2d::Constant(0)), + std::runtime_error, + ".* '\\(lower_limits.array\\(\\) <= " + "upper_limits.array\\(\\)\\).all\\(\\)' failed."); + + // Check for acceleration limits. + mutable_joint_->set_acceleration_limits(Vector2d::Constant(new_lower), + Vector2d::Constant(new_upper)); + EXPECT_EQ(joint_->acceleration_lower_limits(), Vector2d::Constant(new_lower)); + EXPECT_EQ(joint_->acceleration_upper_limits(), Vector2d::Constant(new_upper)); + // Does not match num_velocities(). + DRAKE_EXPECT_THROWS_MESSAGE( + mutable_joint_->set_acceleration_limits(VectorX(2), + VectorX()), + std::runtime_error, + ".* 'lower_limits.size\\(\\) == upper_limits.size\\(\\)' failed."); + DRAKE_EXPECT_THROWS_MESSAGE( + mutable_joint_->set_acceleration_limits(VectorX(), + VectorX(2)), + std::runtime_error, + ".* 'lower_limits.size\\(\\) == upper_limits.size\\(\\)' failed."); + // Lower limit is larger than upper limit. + DRAKE_EXPECT_THROWS_MESSAGE(mutable_joint_->set_acceleration_limits( + Vector2d::Constant(2), Vector2d::Constant(0)), + std::runtime_error, + ".* '\\(lower_limits.array\\(\\) <= " + "upper_limits.array\\(\\)\\).all\\(\\)' failed."); +} + +TEST_F(UniversalJointTest, DefaultAngles) { + const Vector2d lower_limit_angles = Vector2d::Constant(kPositionLowerLimit); + const Vector2d upper_limit_angles = Vector2d::Constant(kPositionUpperLimit); + + const Vector2d default_angles = Vector2d::Zero(); + + const Vector2d new_default_angles = + Vector2d::Constant(kPositionNonZeroDefault); + + const Vector2d out_of_bounds_low_angles = + lower_limit_angles - Vector2d::Constant(1); + const Vector2d out_of_bounds_high_angles = + upper_limit_angles + Vector2d::Constant(1); + + // Constructor should set the default angle to Vector2d::Zero() + EXPECT_EQ(joint_->get_default_angles(), default_angles); + + // Setting a new default angle should propagate so that `get_default_angle()` + // remains correct. + mutable_joint_->set_default_angles(new_default_angles); + EXPECT_EQ(joint_->get_default_angles(), new_default_angles); + + // Setting the default angle out of the bounds of the position limits + // should NOT throw an exception. + EXPECT_NO_THROW( + mutable_joint_->set_default_angles(out_of_bounds_low_angles)); + EXPECT_NO_THROW( + mutable_joint_->set_default_angles(out_of_bounds_high_angles)); +} + +} // namespace +} // namespace multibody +} // namespace drake diff --git a/multibody/tree/universal_joint.cc b/multibody/tree/universal_joint.cc new file mode 100644 index 000000000000..ec44819451cb --- /dev/null +++ b/multibody/tree/universal_joint.cc @@ -0,0 +1,69 @@ +#include "drake/multibody/tree/universal_joint.h" + +#include + +#include "drake/multibody/tree/multibody_tree.h" + +namespace drake { +namespace multibody { + +template +template +std::unique_ptr> UniversalJoint::TemplatedDoCloneToScalar( + const internal::MultibodyTree& tree_clone) const { + const Frame& frame_on_parent_body_clone = + tree_clone.get_variant(this->frame_on_parent()); + const Frame& frame_on_child_body_clone = + tree_clone.get_variant(this->frame_on_child()); + + // Make the Joint clone. + auto joint_clone = std::make_unique>( + this->name(), frame_on_parent_body_clone, frame_on_child_body_clone, + this->damping()); + joint_clone->set_position_limits(this->position_lower_limits(), + this->position_upper_limits()); + joint_clone->set_velocity_limits(this->velocity_lower_limits(), + this->velocity_upper_limits()); + joint_clone->set_acceleration_limits(this->acceleration_lower_limits(), + this->acceleration_upper_limits()); + + return std::move(joint_clone); +} + +template +std::unique_ptr> UniversalJoint::DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> UniversalJoint::DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> UniversalJoint::DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +// N.B. Due to esoteric linking errors on Mac (see #9345) involving +// `MobilizerImpl`, we must place this implementation in the source file, not +// in the header file. +template +std::unique_ptr::BluePrint> +UniversalJoint::MakeImplementationBlueprint() const { + auto blue_print = std::make_unique::BluePrint>(); + auto univeral_mobilizer = std::make_unique>( + this->frame_on_parent(), this->frame_on_child()); + univeral_mobilizer->set_default_position(this->default_positions()); + blue_print->mobilizers_.push_back(std::move(univeral_mobilizer)); + return std::move(blue_print); +} + +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::multibody::UniversalJoint) diff --git a/multibody/tree/universal_joint.h b/multibody/tree/universal_joint.h new file mode 100644 index 000000000000..96010de4bd39 --- /dev/null +++ b/multibody/tree/universal_joint.h @@ -0,0 +1,284 @@ +#pragma once + +#include +#include +#include +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/drake_copyable.h" +#include "drake/multibody/tree/joint.h" +#include "drake/multibody/tree/multibody_forces.h" +#include "drake/multibody/tree/universal_mobilizer.h" + +namespace drake { +namespace multibody { + +/// This joint models a universal joint allowing two bodies to rotate relative +/// to one another with two degrees of freedom. +/// A universal joint can be thought of as a mechanism consisting of three +/// bodies; the parent body P, an intermediate cross-shaped body I, and the +/// child body B. In a physical universal joint, body I has a significantly +/// smaller mass than P or B. This universal joint model corresponds to the +/// mathematical limit of having a body I of negligible mass. Given a frame F +/// attached to the parent body P and a frame M attached to the child body B +/// (see the Joint class's documentation), the orientation of M in F can then +/// naturally be defined as follows using a body fixed rotation sequence. A +/// first rotation of θ₁ about Fx defines the orientation R_FI of the +/// intermediate frame I attached to body I (notice that by definition Ix = Fx +/// at all times). A second rotation of θ₂ about Iy defines the orientation R_IM +/// of frame M (notice that by definition My = Iy at all times). Mathematically, +/// the orientation of frame M in F is given by 
+///   R_FM(q) = R_FI(θ₁) * R_IM(θ₂)
+///
+/// No translational motion of M in F is allowed and the origins, `Mo` and `Fo`, +/// of frames M and F respectively remain coincident. The angles of rotation +/// about F's x-axis and M's y-axis, along with their rates, specify the state +/// of the joint. +/// Zero θ₁, θ₂ angles corresponds to frames F, I, and M being coincident. +/// Angles (θ₁, θ₂) are defined to be positive according to the +/// right-hand-rule with the thumb aligned in the direction of their +/// respective axes. +/// +/// @tparam_default_scalar +template +class UniversalJoint final : public Joint { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(UniversalJoint) + + template + using Context = systems::Context; + + /// The name for this Joint type. It resolves to "universal". + static const char kTypeName[]; + + /// Constructor to create a universal joint between two bodies so that frame F + /// attached to the parent body P and frame M attached to the child body B + /// rotate as described in the class's documentation. See class documentation + /// for details on the angles defining orientation. + /// This constructor signature creates a joint with no joint limits, i.e. the + /// joint position, velocity and acceleration limits are the pair `(-∞, ∞)`. + /// These can be set using the Joint methods set_position_limits(), + /// set_velocity_limits() and set_acceleration_limits(). + /// The first three arguments to this constructor are those of the Joint class + /// constructor. See the Joint class's documentation for details. + /// The additional parameters are: + /// @param[in] damping + /// Viscous damping coefficient, in N⋅m⋅s, used to model losses within the + /// joint. See documentation of damping() for details on modelling of the + /// damping torque. + /// @throws std::exception if damping is negative. + UniversalJoint(const std::string& name, const Frame& frame_on_parent, + const Frame& frame_on_child, double damping = 0) + : Joint(name, frame_on_parent, frame_on_child, + VectorX::Constant( + 2, -std::numeric_limits::infinity()), + VectorX::Constant( + 2, std::numeric_limits::infinity()), + VectorX::Constant( + 2, -std::numeric_limits::infinity()), + VectorX::Constant( + 2, std::numeric_limits::infinity()), + VectorX::Constant( + 2, -std::numeric_limits::infinity()), + VectorX::Constant( + 2, std::numeric_limits::infinity())) { + DRAKE_THROW_UNLESS(damping >= 0); + damping_ = damping; + } + + const std::string& type_name() const override { + static const never_destroyed name{kTypeName}; + return name.access(); + } + + /// Returns `this` joint's damping constant in N⋅m⋅s. The damping torque + /// (in N⋅m) is modeled as `τᵢ = -damping⋅ωᵢ, i = 1, 2` i.e. opposing motion, + /// with ωᵢ the angular rates about the i-th axis for `this` joint (see + /// get_angular_rates())and τᵢ the torque on child body B about the same i-th + /// axis. + double damping() const { return damping_; } + + /// @name Context-dependent value access + /// @{ + + /// Gets the rotation angles of `this` joint from `context`. See class + /// documentation for the definition of these angles. + /// + /// @param[in] context The context of the model this joint belongs to. + /// @returns The angle coordinates of `this` joint stored in the `context` + /// ordered as (θ₁, θ₂). + Vector2 get_angles(const Context& context) const { + return get_mobilizer()->get_angles(context); + } + + /// Sets the `context` so that the generalized coordinates corresponding to + /// the rotation angles of `this` joint equals `angles`. + /// @param[in] context The context of the model this joint belongs to. + /// @param[in] angles The desired angles in radians to be stored in `context` + /// ordered as (θ₁, θ₂). See class documentation for + /// details. + /// @returns a constant reference to `this` joint. + const UniversalJoint& set_angles(Context* context, + const Vector2& angles) const { + get_mobilizer()->set_angles(context, angles); + return *this; + } + + /// Gets the rates of change, in radians per second, of `this` joint's + /// angles (see class documentation) from `context`. + /// @param[in] context The context of the model this joint belongs to. + /// @returns The rates of change of `this` joint's angles as stored in the + /// `context`. + Vector2 get_angular_rates(const systems::Context& context) const { + return get_mobilizer()->get_angular_rates(context); + } + + /// Sets the rates of change, in radians per second, of this `this` joint's + /// angles (see class documentation) to `theta_dot`. The new rates of change + /// get stored in `context`. + /// @param[in] context The context of the model this joint belongs to. + /// @param[in] theta_dot The desired rates of change of `this` joints's angles + /// in radians per second. + /// @returns a constant reference to `this` joint. + const UniversalJoint& set_angular_rates( + systems::Context* context, const Vector2& theta_dot) const { + get_mobilizer()->set_angular_rates(context, theta_dot); + return *this; + } + + /// @} + + /// Gets the default angles for `this` joint. Wrapper for the more general + /// `Joint::default_positions()`. + /// @returns The default angles of `this` stored in `default_positions_` + Vector2 get_default_angles() const { + return this->default_positions(); + } + + /// Sets the default angles of this joint. + /// @param[in] angles The desired default angles of the joint + void set_default_angles(const Vector2& angles) { + this->set_default_positions(angles); + if (this->has_implementation()) { + get_mutable_mobilizer()->set_default_position(this->default_positions()); + } + } + + /// Sets the random distribution that angles of this joint will be randomly + /// sampled from. See class documentation for details on the definition of the + /// angles. + void set_random_angles_distribution( + const Vector2& angles) { + get_mutable_mobilizer()->set_random_position_distribution( + Vector2{angles}); + } + + protected: + /// Joint override called through public NVI, Joint::AddInForce(). + /// Therefore arguments were already checked to be valid. + /// For a UniversalJoint, we must always have `joint_dof < 2` since there are + /// two degrees of freedom (num_velocities() == 2). `joint_tau` is the torque + /// applied about the axis specified by `joint_dof`, the x-axis of the parent + /// frame F if `joint_dof = 0` or the y-axis of the child frame M if + /// `joint_dof = 1`. The torque is applied to the body declared as child + /// (according to the universal joint's constructor) at the origin of the + /// child frame M (which is coincident with the origin of the parent frame F + /// at all times). The torque is defined to be positive according to + /// the right-hand-rule with the thumb aligned in the direction of the + /// selected axis. That is, a positive torque causes a positive rotational + /// acceleration (of the child body frame). + void DoAddInOneForce(const systems::Context&, int joint_dof, + const T& joint_tau, + MultibodyForces* forces) const override { + DRAKE_DEMAND(joint_dof < 2); + Eigen::VectorBlock>> tau_mob = + get_mobilizer()->get_mutable_generalized_forces_from_array( + &forces->mutable_generalized_forces()); + tau_mob(joint_dof) += joint_tau; + } + + /// Joint override called through public NVI, Joint::AddInDamping(). + /// Therefore arguments were already checked to be valid. + /// This method adds into `forces` a dissipative torque according to the + /// viscous law `τ = -d⋅ω`, with d the damping coefficient (see damping()). + void DoAddInDamping(const systems::Context& context, + MultibodyForces* forces) const override { + Eigen::VectorBlock>> tau = + get_mobilizer()->get_mutable_generalized_forces_from_array( + &forces->mutable_generalized_forces()); + const Vector2& theta_dot = get_angular_rates(context); + tau = -damping() * theta_dot; + } + + private: + int do_get_velocity_start() const override { + return get_mobilizer()->velocity_start_in_v(); + } + + int do_get_num_velocities() const override { return 2; } + + int do_get_position_start() const override { + return get_mobilizer()->position_start_in_q(); + } + + int do_get_num_positions() const override { return 2; } + + // Joint overrides: + std::unique_ptr::BluePrint> MakeImplementationBlueprint() + const override; + + std::unique_ptr> DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const override; + + std::unique_ptr> DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const override; + + std::unique_ptr> DoCloneToScalar( + const internal::MultibodyTree&) const override; + + // Make UniversalJoint templated on every other scalar type a friend of + // UniversalJoint so that CloneToScalar() can access + // private members of UniversalJoint. + template + friend class UniversalJoint; + + // 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. + const internal::UniversalMobilizer* get_mobilizer() const { + // This implementation should only have one mobilizer. + DRAKE_DEMAND(this->get_implementation().num_mobilizers() == 1); + const internal::UniversalMobilizer* mobilizer = + dynamic_cast*>( + this->get_implementation().mobilizers_[0]); + DRAKE_DEMAND(mobilizer != nullptr); + return mobilizer; + } + + internal::UniversalMobilizer* get_mutable_mobilizer() { + // This implementation should only have one mobilizer. + DRAKE_DEMAND(this->get_implementation().num_mobilizers() == 1); + auto* mobilizer = dynamic_cast*>( + this->get_implementation().mobilizers_[0]); + DRAKE_DEMAND(mobilizer != nullptr); + return mobilizer; + } + + // Helper method to make a clone templated on ToScalar. + template + std::unique_ptr> TemplatedDoCloneToScalar( + const internal::MultibodyTree& tree_clone) const; + + // This joint's damping constant in N⋅m⋅s. + double damping_{0}; +}; + +template +const char UniversalJoint::kTypeName[] = "universal"; + +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::multibody::UniversalJoint) From 42e9533e9d73c36f98b6047bd763077101a5b985 Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Wed, 6 May 2020 13:09:43 -0400 Subject: [PATCH 4/4] Add bindings for UniversalJoint --- bindings/pydrake/multibody/test/plant_test.py | 17 +++++++++++++ bindings/pydrake/multibody/tree_py.cc | 25 +++++++++++++++++++ 2 files changed, 42 insertions(+) diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index 113204ff2fc2..118de09e36e8 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/bindings/pydrake/multibody/test/plant_test.py @@ -33,6 +33,7 @@ SpatialInertia_, UniformGravityFieldElement_, UnitInertia_, + UniversalJoint_, WeldJoint_, world_index, world_model_instance, @@ -1065,6 +1066,14 @@ def make_revolute_joint(plant, P, C): damping=2., ) + 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. @@ -1079,6 +1088,7 @@ def make_weld_joint(plant, P, C): make_ball_rpy_joint, make_prismatic_joint, make_revolute_joint, + make_universal_joint, make_weld_joint, ] @@ -1123,6 +1133,13 @@ def make_weld_joint(plant, P, C): 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 diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc index b72d7b90328a..6cf31e438095 100644 --- a/bindings/pydrake/multibody/tree_py.cc +++ b/bindings/pydrake/multibody/tree_py.cc @@ -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 { @@ -320,6 +321,30 @@ void DoScalarDependentDefinitions(py::module m, T) { cls_doc.set_random_angle_distribution.doc); } + // UniversalJoint + { + using Class = UniversalJoint; + constexpr auto& cls_doc = doc.UniversalJoint; + auto cls = DefineTemplateClassWithDefault>( + m, "UniversalJoint", param, cls_doc.doc); + cls // BR + .def( + py::init&, const Frame&, 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;