diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index 2e6c5c5d458a..d98c01361b46 100644 --- a/multibody/tree/BUILD.bazel +++ b/multibody/tree/BUILD.bazel @@ -121,6 +121,7 @@ drake_cc_library( "revolute_mobilizer.cc", "revolute_spring.cc", "rigid_body.cc", + "screw_mobilizer.cc", "space_xyz_floating_mobilizer.cc", "space_xyz_mobilizer.cc", "uniform_gravity_field_element.cc", @@ -161,6 +162,7 @@ drake_cc_library( "revolute_mobilizer.h", "revolute_spring.h", "rigid_body.h", + "screw_mobilizer.h", "space_xyz_floating_mobilizer.h", "space_xyz_mobilizer.h", "uniform_gravity_field_element.h", @@ -427,6 +429,15 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "screw_mobilizer_test", + deps = [ + ":mobilizer_tester", + ":tree", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "space_xyz_mobilizer_test", deps = [ diff --git a/multibody/tree/screw_mobilizer.cc b/multibody/tree/screw_mobilizer.cc new file mode 100644 index 000000000000..743d9c7186a9 --- /dev/null +++ b/multibody/tree/screw_mobilizer.cc @@ -0,0 +1,205 @@ +#include "drake/multibody/tree/screw_mobilizer.h" + +#include +#include + +namespace drake { +namespace multibody { +namespace internal { + +template +double ScrewMobilizer::screw_pitch() const { + return screw_pitch_; +} + +template +T ScrewMobilizer::get_translation( + const systems::Context& context) const { + auto q = this->get_positions(context); + DRAKE_ASSERT(q.size() == kNq); + return get_screw_translation_from_rotation(q[0], screw_pitch_); +} + +template +const ScrewMobilizer& ScrewMobilizer::set_translation( + systems::Context* context, + const T& translation) const { + const double kEpsilon = std::sqrt(std::numeric_limits::epsilon()); + using std::abs; + DRAKE_THROW_UNLESS(abs(screw_pitch_) > kEpsilon || + abs(translation) < kEpsilon); + auto q = this->get_mutable_positions(&*context); + DRAKE_ASSERT(q.size() == kNq); + q[0] = get_screw_rotation_from_translation(translation, screw_pitch_); + return *this; +} + +template +T ScrewMobilizer::get_angle( + const systems::Context& context) const { + auto q = this->get_positions(context); + DRAKE_ASSERT(q.size() == kNq); + return q[0]; +} + +template +const ScrewMobilizer& ScrewMobilizer::set_angle( + systems::Context* context, const T& angle) const { + auto q = this->get_mutable_positions(&*context); + DRAKE_ASSERT(q.size() == kNq); + q[0] = angle; + return *this; +} + +template +T ScrewMobilizer::get_translation_rate( + const systems::Context& context) const { + auto v = this->get_velocities(context); + DRAKE_ASSERT(v.size() == kNv); + return get_screw_translation_from_rotation(v[0], screw_pitch_); +} + +template +const ScrewMobilizer& ScrewMobilizer::set_translation_rate( + systems::Context* context, + const T& vz) const { + const double kEpsilon = std::sqrt(std::numeric_limits::epsilon()); + using std::abs; + DRAKE_THROW_UNLESS(abs(screw_pitch_) > kEpsilon || abs(vz) < kEpsilon); + + auto v = this->get_mutable_velocities(&*context); + DRAKE_ASSERT(v.size() == kNv); + v[0] = get_screw_rotation_from_translation(vz, screw_pitch_); + return *this; +} + +template +T ScrewMobilizer::get_angular_rate( + const systems::Context& context) const { + auto v = this->get_velocities(context); + DRAKE_ASSERT(v.size() == kNv); + return v[0]; +} + +template +const ScrewMobilizer& ScrewMobilizer::set_angular_rate( + systems::Context* context, const T& theta_dot) const { + auto v = this->get_mutable_velocities(&*context); + DRAKE_ASSERT(v.size() == kNv); + v[0] = theta_dot; + return *this; +} + +template +math::RigidTransform ScrewMobilizer::CalcAcrossMobilizerTransform( + const systems::Context& context) const { + const auto& q = this->get_positions(context); + DRAKE_ASSERT(q.size() == kNq); + const Vector3 X_FM_translation(0.0, 0.0, + get_screw_translation_from_rotation(q[0], screw_pitch_)); + return math::RigidTransform(math::RotationMatrix::MakeZRotation(q[0]), + X_FM_translation); +} + +template +SpatialVelocity ScrewMobilizer::CalcAcrossMobilizerSpatialVelocity( + const systems::Context&, const Eigen::Ref>& v) const { + DRAKE_ASSERT(v.size() == kNv); + Vector6 V_FM_vector; + V_FM_vector << + 0.0, 0.0, get_screw_rotation_from_translation(v[0], screw_pitch_), + 0.0, 0.0, v[0]; + return SpatialVelocity(V_FM_vector); +} + +template +SpatialAcceleration +ScrewMobilizer::CalcAcrossMobilizerSpatialAcceleration( + const systems::Context&, + const Eigen::Ref>& vdot) const { + DRAKE_ASSERT(vdot.size() == kNv); + Vector6 A_FM_vector; + A_FM_vector << 0.0, 0.0, vdot[0], 0.0, 0.0, + get_screw_translation_from_rotation(vdot[0], screw_pitch_); + return SpatialAcceleration(A_FM_vector); +} + +template +void ScrewMobilizer::ProjectSpatialForce(const systems::Context&, + const SpatialForce& F_Mo_F, + Eigen::Ref> tau) const { + DRAKE_ASSERT(tau.size() == kNv); + tau[0] = F_Mo_F.rotational()[2] + screw_pitch() * F_Mo_F.translational()[2]; +} + +template +void ScrewMobilizer::DoCalcNMatrix(const systems::Context&, + EigenPtr> N) const { + *N = Eigen::Matrix::Identity(); +} + +template +void ScrewMobilizer::DoCalcNplusMatrix(const systems::Context&, + EigenPtr> Nplus) const { + *Nplus = Eigen::Matrix::Identity(); +} + +template +void ScrewMobilizer::MapVelocityToQDot( + const systems::Context&, const Eigen::Ref>& v, + EigenPtr> qdot) const { + DRAKE_ASSERT(v.size() == kNv); + DRAKE_ASSERT(qdot != nullptr); + DRAKE_ASSERT(qdot->size() == kNq); + *qdot = v; +} + +template +void ScrewMobilizer::MapQDotToVelocity( + const systems::Context&, const Eigen::Ref>& qdot, + EigenPtr> v) const { + DRAKE_ASSERT(qdot.size() == kNq); + DRAKE_ASSERT(v != nullptr); + DRAKE_ASSERT(v->size() == kNv); + *v = qdot; +} + +template +template +std::unique_ptr> +ScrewMobilizer::TemplatedDoCloneToScalar( + const MultibodyTree& tree_clone) const { + const Frame& inboard_frame_clone = + tree_clone.get_variant(this->inboard_frame()); + const Frame& outboard_frame_clone = + tree_clone.get_variant(this->outboard_frame()); + return std::make_unique>(inboard_frame_clone, + outboard_frame_clone, + this->screw_pitch()); +} + +template +std::unique_ptr> ScrewMobilizer::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> ScrewMobilizer::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> +ScrewMobilizer::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +} // namespace internal +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::multibody::internal::ScrewMobilizer) diff --git a/multibody/tree/screw_mobilizer.h b/multibody/tree/screw_mobilizer.h new file mode 100644 index 000000000000..244896f8612d --- /dev/null +++ b/multibody/tree/screw_mobilizer.h @@ -0,0 +1,244 @@ +#pragma once + +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/multibody/tree/frame.h" +#include "drake/multibody/tree/mobilizer_impl.h" +#include "drake/multibody/tree/multibody_tree_topology.h" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace multibody { +namespace internal { + +/* This mobilizer models a screw joint between an inboard frame F and an + outboard frame M that enables translation along F's z axis while + rotating about it. + + The generalized coordinate for this mobilizer, θ corresponds to + rotation about the z-axis of frame F. + Zero θ defines the "zero configuration", which corresponds to frames F and + M being coincident (axes are aligned and origins are co-located), + see set_zero_state(). The translation (z) depends on and is proportional to + the rotation θ. Their relation is defined by a screw pitch. + The translation (z) is defined to be positive in the direction of + frame F's z-axis and the rotation θ is defined to be positive according + to the right-hand-rule with the thumb aligned in the direction of + frame F's z-axis. The frame F's z-axis and the frame M's z-axis are aligned + at all times for this mobilizer. The generalized velocity for this mobilizer + is the rate of change of the coordinate, ω =˙θ (θ_dot). + + @tparam_default_scalar */ +template +class ScrewMobilizer final : public MobilizerImpl { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ScrewMobilizer) + + /* Constructor for a %ScrewMobilizer between an inboard frame F and + an outboard frame M granting one translational and one rotational degrees + of freedom as described in this class's documentation. + @param[in] screw_pitch The amount of translation along F's z-axis in meters + for a one full revolution about F's z-axis. + When set to zero, the mobilizer behaves like a + revolute joint, i.e. producing zero translation for + any value of the generalized coordinate. */ + ScrewMobilizer(const Frame& inboard_frame_F, + const Frame& outboard_frame_M, + double screw_pitch) + : MobilizerBase(inboard_frame_F, outboard_frame_M) + , screw_pitch_(screw_pitch) {} + + /* @returns the screw pitch, which is used to relate rotational + to translational motion for `this` mobilizer as documented + in this class's documentation. */ + double screw_pitch() const; + + /* Retrieves from `context` the translation (z) which describes the + position for `this` mobilizer as documented in this class's documentation. + + @param[in] context The context of the model this mobilizer belongs to. + @returns The translation (z) of the mobilizer */ + T get_translation(const systems::Context& context) const; + + /* Sets in `context` the position for `this` mobilizer to the translation z + provided by the input argument `translation`. + The generalized coordinate θ will change proportionally, depending on + screw_pitch. + + @param[in] context The context of the model this mobilizer belongs to. + @param[in] translation The desired translation in meters. + @returns A constant reference to `this` mobilizer. + @throws std::exception if the screw_pitch is very near zero and + |translation| > kEpsilon. */ + const ScrewMobilizer& set_translation( + systems::Context* context, + const T& translation) const; + + /* Retrieves from `context` the angle θ which describes the orientation for + `this` mobilizer as documented in this class's documentation. + + @param[in] context The context of the model this mobilizer belongs to. + @returns The angle θ of the mobilizer. */ + T get_angle(const systems::Context& context) const; + + /* Sets in `context` the orientation for `this` mobilizer to the angle θ + provided by the input argument `angle`. + + @param[in] context The context of the model this mobilizer belongs to. + @param[in] angle The desired angle in radians. + @returns a constant reference to `this` mobilizer. */ + const ScrewMobilizer& set_angle(systems::Context* context, + const T& angle) const; + + /* Retrieves from `context` the rate of change, in meters per second, of + `this` mobilizer's translation (see get_translation()). + @param[in] context The context of the model this mobilizer belongs to. + @returns The rate of change of the translation (ż)*/ + T get_translation_rate(const systems::Context& context) const; + + /* Sets in `context` the rate of change, in meters per second, of `this` + mobilizer's translation (see get_translation()) to `vz`. + The generalized velocity ˙θ (θ_dot) will change proportionally, depending on + screw_pitch. + + @param[in] context The context of the model this mobilizer belongs to. + @param[in] vz The desired rate of change of `this` mobilizer's + translation, ż. + @returns A constant reference to `this` mobilizer. + @throws std::exception if the screw_pitch is very near zero and + |vz| > kEpsilon. */ + const ScrewMobilizer& set_translation_rate( + systems::Context* context, + const T& vz) const; + + /* Retrieves from `context` the rate of change, in radians per second, of + `this` mobilizer's angle (see get_angle()). + @param[in] context The context of the model this mobilizer belongs to. + @returns The rate of change of `this` mobilizer's angle. */ + T get_angular_rate(const systems::Context& context) const; + + /* Sets in `context` the rate of change, in radians per second, of `this` + mobilizer's angle (see angle()) to `theta_dot`. + @param[in] context The context of the model this mobilizer belongs to. + @param[in] theta_dot The desired rate of change of `this` mobilizer's angle + in radians per second. + @returns A constant reference to `this` mobilizer. */ + const ScrewMobilizer& set_angular_rate(systems::Context* context, + const T& theta_dot) const; + + /* Computes the across-mobilizer transform `X_FM(q)` between the inboard + frame F and the outboard frame M as a function of the configuration q stored + in `context`. */ + math::RigidTransform CalcAcrossMobilizerTransform( + const systems::Context& context) const final; + + /* Computes the across-mobilizer velocity `V_FM(q, v)` of the outboard frame + M measured and expressed in frame F as a function of the configuration q + stored in `context` and of the input velocity v, formatted as described in + the class documentation. + This method aborts in Debug builds if `v.size()` is not one. + @pre v.size() == 1 */ + SpatialVelocity CalcAcrossMobilizerSpatialVelocity( + const systems::Context& context, + const Eigen::Ref>& v) const final; + + /* Computes the across-mobilizer acceleration `A_FM(q, v, v̇)` of the outboard + frame M in the inboard frame F. + By definition `A_FM = d_F(V_FM)/dt = H_FM(q) * v̇ + Ḣ_FM * v`. The + acceleration `A_FM` will be a function of the configuration q and the + velocity v from the `context` as well as the generalized accelerations + `v̇ = dv/dt`, the rates of change of v. + This method aborts in Debug builds if `vdot.size()` is not one. + @pre vdot.size() == 1. */ + SpatialAcceleration CalcAcrossMobilizerSpatialAcceleration( + const systems::Context& context, + const Eigen::Ref>& vdot) const final; + + /* Projects the spatial force `F_Mo = [τ_Mo, f_Mo]` on `this` mobilizer's + outboard frame M onto the rotational axis, z. Mathematically: + 
+      tau = [τ_Mo⋅Fz]
+
+ Therefore, the result of this method is the vector of torques for + each degree of freedom of `this` mobilizer. + This method aborts in Debug builds if `tau.size()` is not one. + @pre tau.size() == 1 */ + void ProjectSpatialForce(const systems::Context& context, + const SpatialForce& F_Mo_F, + Eigen::Ref> tau) const final; + + /* Performs the identity mapping from v to qdot since, for this mobilizer, + v = q̇. */ + void MapVelocityToQDot(const systems::Context& context, + const Eigen::Ref>& v, + EigenPtr> qdot) const final; + + /* Performs the identity mapping from qdot to v since, for this mobilizer, + v = q̇. */ + void MapQDotToVelocity(const systems::Context& context, + const Eigen::Ref>& qdot, + EigenPtr> v) const final; + + protected: + void DoCalcNMatrix(const systems::Context& context, + EigenPtr> N) const final; + + void DoCalcNplusMatrix(const systems::Context& context, + EigenPtr> Nplus) const final; + + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const final; + + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const final; + + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const final; + + private: + /* Helper method to make a clone templated on ToScalar. */ + template + std::unique_ptr> TemplatedDoCloneToScalar( + const MultibodyTree& tree_clone) const; + + typedef MobilizerImpl MobilizerBase; + /* Bring the handy number of position and velocities MobilizerImpl enums into + this class' scope. This is useful when writing mathematical expressions with + fixed-sized vectors since we can do things like Vector. + Operations with fixed-sized quantities can be optimized at compile time and + therefore they are highly preferred compared to the very slow dynamic sized + quantities. */ + using MobilizerBase::kNq; + using MobilizerBase::kNv; + + const double screw_pitch_; +}; + + /* `get_screw_translation_from_rotation`, + `get_screw_rotation_from_translation` are used for position, velocity, + and acceleration conversions. All of these are governed by + the same relation, depended on the `screw_pitch` of a screw mobilizer. */ +template +inline T get_screw_translation_from_rotation(const T& theta, + double screw_pitch) { + const T revolution_amount{theta / (2 * M_PI)}; + return screw_pitch * revolution_amount; +} + +template +inline T get_screw_rotation_from_translation(const T& z, + double screw_pitch) { + const T revolution_amount{z / screw_pitch}; + return revolution_amount * 2 * M_PI; +} + +} // namespace internal +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::multibody::internal::ScrewMobilizer) diff --git a/multibody/tree/test/screw_mobilizer_test.cc b/multibody/tree/test/screw_mobilizer_test.cc new file mode 100644 index 000000000000..c47bd3cc229b --- /dev/null +++ b/multibody/tree/test/screw_mobilizer_test.cc @@ -0,0 +1,295 @@ +#include "drake/multibody/tree/screw_mobilizer.h" + +#include + +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/tree/multibody_tree-inl.h" +#include "drake/multibody/tree/multibody_tree_system.h" +#include "drake/multibody/tree/test/mobilizer_tester.h" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace multibody { +namespace internal { +namespace { + +using Matrix1d = Eigen::Matrix; +using Eigen::Vector3d; +using Eigen::VectorXd; +using math::RigidTransformd; +using math::RotationMatrixd; + +constexpr double kTolerance = 10 * std::numeric_limits::epsilon(); +constexpr double kScrewPitch = 1.e-2; + +// Fixture to setup a simple MBT model containing a screw mobilizer. +class ScrewMobilizerTest : public MobilizerTester { + public: + // Creates a simple model consisting of a single body with a screw + // mobilizer connecting it to the world. + void SetUp() override { + mobilizer_ = + &AddMobilizerAndFinalize(std::make_unique>( + tree().world_body().body_frame(), + body_->body_frame(), kScrewPitch)); + } + + protected: + const ScrewMobilizer* mobilizer_{nullptr}; +}; + +TEST_F(ScrewMobilizerTest, ScrewPitchAccess) { + EXPECT_EQ(mobilizer_->screw_pitch(), kScrewPitch); +} + +TEST_F(ScrewMobilizerTest, ExceptionRaisingWhenZeroPitch) { + const double zero_screw_pitch{0}; + ScrewMobilizer zero_pitch_screw_mobilizer( + tree().world_body().body_frame(), body_->body_frame(), zero_screw_pitch); + + const double translation_z{1.}; + const double velocity_z{2.}; + EXPECT_THROW( + zero_pitch_screw_mobilizer.set_translation(context_.get(), translation_z), + std::exception); + EXPECT_THROW(zero_pitch_screw_mobilizer.set_translation_rate( + context_.get(), velocity_z), + std::exception); +} + +TEST_F(ScrewMobilizerTest, StateAccess) { + // Verify we can set a screw mobilizer configuration given the model's + // context. + const double translation_z_first{1.}; + const double translation_z_second{2.}; + const double angle_z_second{translation_z_second * 2 * M_PI / kScrewPitch}; + + mobilizer_->set_translation(context_.get(), translation_z_first); + EXPECT_EQ(mobilizer_->get_translation(*context_), translation_z_first); + + mobilizer_->set_translation(context_.get(), translation_z_second); + EXPECT_EQ(mobilizer_->get_translation(*context_), translation_z_second); + EXPECT_EQ(mobilizer_->get_angle(*context_), angle_z_second); + + const double angle_z_first{1. * 180. / M_PI}; + + mobilizer_->set_angle(context_.get(), angle_z_first); + EXPECT_EQ(mobilizer_->get_angle(*context_), angle_z_first); + + mobilizer_->set_angle(context_.get(), angle_z_second); + EXPECT_EQ(mobilizer_->get_angle(*context_), angle_z_second); + + const double velocity_z_first{1.}; + const double velocity_z_second{2.}; + const double angular_velocity_z_second{ + velocity_z_second * 2 * M_PI / kScrewPitch}; + + // Verify we can set a screw mobilizer velocities given the model's context. + mobilizer_->set_translation_rate(context_.get(), velocity_z_first); + EXPECT_EQ(mobilizer_->get_translation_rate(*context_), velocity_z_first); + + mobilizer_->set_translation_rate(context_.get(), velocity_z_second); + EXPECT_EQ(mobilizer_->get_translation_rate(*context_), velocity_z_second); + EXPECT_EQ(mobilizer_->get_angular_rate(*context_), angular_velocity_z_second); + + const double angular_velocity_z_first{1. * 180. / M_PI}; + + mobilizer_->set_angular_rate(context_.get(), angular_velocity_z_first); + EXPECT_EQ(mobilizer_->get_angular_rate(*context_), angular_velocity_z_first); + + mobilizer_->set_angular_rate(context_.get(), angular_velocity_z_second); + EXPECT_EQ(mobilizer_->get_angular_rate(*context_), angular_velocity_z_second); +} + +TEST_F(ScrewMobilizerTest, ZeroState) { + const double angle_z{1. * 180. / M_PI}; + const double angular_velocity_z{2. * 180. / M_PI}; + + const double translation_z{angle_z * kScrewPitch / (2. * M_PI)}; + const double velocity_z{angular_velocity_z * kScrewPitch / (2. * M_PI)}; + + // Set the state to some arbitrary non-zero value. + mobilizer_->set_angle(context_.get(), angle_z); + mobilizer_->set_angular_rate(context_.get(), angular_velocity_z); + EXPECT_EQ(mobilizer_->get_angle(*context_), angle_z); + EXPECT_EQ(mobilizer_->get_angular_rate(*context_), angular_velocity_z); + EXPECT_LE(std::fabs(mobilizer_->get_translation(*context_) - translation_z), + kTolerance); + EXPECT_LE(std::fabs(mobilizer_->get_translation_rate(*context_) - velocity_z), + kTolerance); + + // Set the mobilizer state to zero. + mobilizer_->set_zero_state(*context_, &context_->get_mutable_state()); + EXPECT_EQ(mobilizer_->get_translation(*context_), 0.0); + EXPECT_EQ(mobilizer_->get_translation_rate(*context_), 0.0); + EXPECT_EQ(mobilizer_->get_angle(*context_), 0.0); + EXPECT_EQ(mobilizer_->get_angular_rate(*context_), 0.0); +} + +TEST_F(ScrewMobilizerTest, DefaultPosition) { + ScrewMobilizer* mutable_mobilizer = + &mutable_tree().get_mutable_variant(*mobilizer_); + + EXPECT_EQ(mobilizer_->get_translation(*context_), 0.0); + EXPECT_EQ(mobilizer_->get_angle(*context_), 0.0); + + const Vector1d new_default(.4); + mutable_mobilizer->set_default_position(new_default); + mobilizer_->set_default_state(*context_, &context_->get_mutable_state()); + + EXPECT_EQ(mobilizer_->get_angle(*context_), new_default(0)); + EXPECT_EQ(mobilizer_->get_translation(*context_), + new_default(0) * kScrewPitch / (2. * M_PI)); +} + +TEST_F(ScrewMobilizerTest, RandomState) { + RandomGenerator generator; + std::uniform_real_distribution uniform; + + ScrewMobilizer* mutable_mobilizer = + &mutable_tree().get_mutable_variant(*mobilizer_); + + // Default behavior is to set to zero. + mobilizer_->set_random_state(*context_, &context_->get_mutable_state(), + &generator); + EXPECT_EQ(mobilizer_->get_angle(*context_), 0.0); + EXPECT_EQ(mobilizer_->get_angular_rate(*context_), 0.0); + + // Set position to be random, but not velocity (yet). + mutable_mobilizer->set_random_position_distribution( + Vector1(uniform(generator) + 1.0)); + mobilizer_->set_random_state(*context_, &context_->get_mutable_state(), + &generator); + EXPECT_GE(mobilizer_->get_angle(*context_), 1.0); + EXPECT_EQ(mobilizer_->get_angular_rate(*context_), 0.0); + + // Set the velocity distribution. Now both should be random. + mutable_mobilizer->set_random_velocity_distribution( + Vector1(uniform(generator) - 1.0)); + mobilizer_->set_random_state(*context_, &context_->get_mutable_state(), + &generator); + EXPECT_GE(mobilizer_->get_angle(*context_), 1.0); + EXPECT_GE(mobilizer_->get_angular_rate(*context_), -1.0); + + // Check that they change on a second draw from the distribution. + const double last_translation = mobilizer_->get_translation(*context_); + const double last_translational_rate = mobilizer_->get_translation_rate( + *context_); + const double last_angle = mobilizer_->get_angle(*context_); + const double last_angular_rate = mobilizer_->get_angular_rate(*context_); + mobilizer_->set_random_state(*context_, &context_->get_mutable_state(), + &generator); + EXPECT_NE(mobilizer_->get_translation(*context_), last_translation); + EXPECT_NE(mobilizer_->get_translation_rate(*context_), + last_translational_rate); + EXPECT_NE(mobilizer_->get_angle(*context_), last_angle); + EXPECT_NE(mobilizer_->get_angular_rate(*context_), last_angular_rate); +} + +TEST_F(ScrewMobilizerTest, CalcAcrossMobilizerTransform) { + const double angle = 1.5; + mobilizer_->set_angle(context_.get(), angle); + const RigidTransformd X_FM( + mobilizer_->CalcAcrossMobilizerTransform(*context_)); + + Vector3d X_FM_translation; + X_FM_translation << 0.0, 0.0, angle / (2 * M_PI) * kScrewPitch; + const RigidTransformd X_FM_expected(RotationMatrixd::MakeZRotation(angle), + X_FM_translation); + + EXPECT_TRUE(CompareMatrices(X_FM.GetAsMatrix34(), + X_FM_expected.GetAsMatrix34(), kTolerance, + MatrixCompareType::relative)); +} + +TEST_F(ScrewMobilizerTest, CalcAcrossMobilizerSpatialVeloctiy) { + const double angle = 1.5; + const double angular_velocity = 0.1; + const Vector1d spatial_velocity(angular_velocity / (2 * M_PI) * kScrewPitch); + mobilizer_->set_angle(context_.get(), angle); + const SpatialVelocity V_FM = + mobilizer_->CalcAcrossMobilizerSpatialVelocity(*context_, + spatial_velocity); + + VectorXd v_expected(6); + v_expected << 0.0, 0.0, angular_velocity, 0.0, 0.0, spatial_velocity(0); + const SpatialVelocity V_FM_expected(v_expected); + + EXPECT_TRUE(V_FM.IsApprox(V_FM_expected, kTolerance)); +} + +TEST_F(ScrewMobilizerTest, CalcAcrossMobilizerSpatialAcceleration) { + const double angle = 1.5; + const double angle_rate = 3; + const Vector1d angle_acceleration(4.5); + mobilizer_->set_angle(context_.get(), angle); + mobilizer_->set_angular_rate(context_.get(), angle_rate); + + const SpatialAcceleration A_FM = + mobilizer_->CalcAcrossMobilizerSpatialAcceleration(*context_, + angle_acceleration); + + VectorXd a_expected(6); + a_expected << 0.0, 0.0, angle_acceleration[0], 0.0, 0.0, + angle_acceleration[0] / (2 * M_PI) * kScrewPitch; + const SpatialAcceleration A_FM_expected(a_expected); + + EXPECT_TRUE(A_FM.IsApprox(A_FM_expected, kTolerance)); +} + +TEST_F(ScrewMobilizerTest, ProjectSpatialForce) { + const double translation(0.5); + const double angle = 1.5; + mobilizer_->set_translation(context_.get(), translation); + mobilizer_->set_angle(context_.get(), angle); + + const Vector3d torque_Mo_F(1.0, 2.0, 3.0); + const Vector3d force_Mo_F(1.0, 2.0, 3.0); + const SpatialForce F_Mo_F(torque_Mo_F, force_Mo_F); + Vector1d tau; + mobilizer_->ProjectSpatialForce(*context_, F_Mo_F, tau); + + const Vector1d tau_expected(torque_Mo_F[2] + kScrewPitch * force_Mo_F[2]); + EXPECT_TRUE(CompareMatrices(tau, tau_expected, kTolerance, + MatrixCompareType::relative)); +} + +TEST_F(ScrewMobilizerTest, MapVelocityToQDotAndBack) { + Vector1d v(1.5); + Vector1d qdot; + mobilizer_->MapVelocityToQDot(*context_, v, &qdot); + EXPECT_TRUE( + CompareMatrices(qdot, v, kTolerance, MatrixCompareType::relative)); + + qdot(0) = -std::sqrt(2); + mobilizer_->MapQDotToVelocity(*context_, qdot, &v); + EXPECT_TRUE( + CompareMatrices(v, qdot, kTolerance, MatrixCompareType::relative)); +} + +TEST_F(ScrewMobilizerTest, KinematicMapping) { + // For this joint, Nplus = I independently of the state. We therefore set the + // state to NaN in order to verify this. + tree() + .GetMutablePositionsAndVelocities(context_.get()) + .setConstant(std::numeric_limits::quiet_NaN()); + + // Compute N. + MatrixX N(1, 1); + mobilizer_->CalcNMatrix(*context_, &N); + EXPECT_EQ(N, Matrix1d::Identity()); + + // Compute Nplus. + MatrixX Nplus(1, 1); + mobilizer_->CalcNplusMatrix(*context_, &Nplus); + EXPECT_EQ(Nplus, Matrix1d::Identity()); +} + +} // namespace +} // namespace internal +} // namespace multibody +} // namespace drake