diff --git a/multibody/tree/curvilinear_mobilizer.h b/multibody/tree/curvilinear_mobilizer.h index 0b721fc0ea64..d4ec059a0b46 100644 --- a/multibody/tree/curvilinear_mobilizer.h +++ b/multibody/tree/curvilinear_mobilizer.h @@ -22,7 +22,7 @@ namespace drake { namespace multibody { namespace internal { -/** A Mobilizer that describes the motion of the mobilized frame M along a +/* A Mobilizer that describes the motion of the mobilized frame M along a piecewise constant curvature path contained in a plane. The path is specified as a PiecewiseConstantCurvatureTrajectory, refer to that @@ -30,28 +30,34 @@ namespace internal { notation used. This mobilizer grants a single degree of freedom q that corresponds to the - length s (in meters) along the path. The generalized velocity v = q̇ - corresponds to the magnitude of the tangential velocity. The mobilized frame M - is defined according to the convention documented in + length s (in meters) along the path. The generalized velocity v = q̇ (= ṡ) + corresponds to the signed magnitude of the tangential velocity. The mobilized + frame M is defined according to the convention documented in PiecewiseConstantCurvatureTrajectory. That is, axis Mx is the tangent to the - trajectory, Mz equals the (constant) normal p̂ to the plane, and My = Mz x Mx. + trajectory, Mz equals the (constant) normal p̂ to the plane, and My = Mz⨯ Mx. It is not required that M coincides with F at distance q = 0. - If the specified trajectory is periodic, the mobilizer describes a trajectory - of length s_f that satisfies X_FM(q) = X_FM(q + s_f). - At any given point along the path, the turning rate ρ(q) (units of 1/m) is - defined such the angular velocity is given by w_FM = ρ⋅v⋅Mz_F, i.e. |ρ(q)| + defined such the angular velocity is given by ω_FM = ρ⋅v⋅Mz_F, i.e. |ρ(q)| corresponds to the path's curvature and its sign is defined via the right-hand rule about the plane's normal Mz. + If the specified trajectory is periodic, the mobilizer describes a trajectory + of length s_f that satisfies X_FM(q) = X_FM(q + s_f) and ρ(q) = ρ(q + s_f). + Therefore the hinge matrix H_FM ∈ ℝ⁶ˣ¹ and its time derivative are - H_FM(q) = [ρ(q)⋅Mz_F(q)] Hdot_FM(q) = [ 0 ] - [ Mx_F(q)], [ρ(q)⋅v⋅My_F(q)]. + H_FM_F(q) = [ρ(q)⋅Mz_F(q)] Hdot_FM_F(q,v) = [ 0 ] + [ Mx_F(q)], [ρ(q)⋅v⋅My_F(q)] + + (Recall that the time derivative taken in F of a vector r⃗ fixed in a frame + M is given by ω_FM ⨯ r⃗, so DtF(Mx) = (ρ⋅v⋅Mz) ⨯ Mx = ρ⋅v⋅My.) + + This is of course much nicer in M: + H_FM_M(q) = [0 0 ρ(q) 1 0 0]ᵀ Hdot_FM_M = [ 0₆ ]ᵀ -where the angular component of Hdot_FM is zero since ρ(q) is constant within -each piecewise segment in PiecewiseConstantCurvatureTrajectory. + The angular component of Hdot_FM is zero since ρ(q) is constant within each + piecewise segment in PiecewiseConstantCurvatureTrajectory. @tparam_default_scalar */ template @@ -67,7 +73,7 @@ class CurvilinearMobilizer final : public MobilizerImpl { template using HMatrix = typename MobilizerBase::template HMatrix; - /** Constructor for a CurvilinearMobilizer describing the motion of outboard + /* Constructor for a CurvilinearMobilizer describing the motion of outboard frame M along `curvilinear_path` with pose X_FM in the inboard frame F. Refer to the class documentation for further details on notation and conventions. @@ -96,13 +102,13 @@ class CurvilinearMobilizer final : public MobilizerImpl { bool can_rotate() const final { return true; } bool can_translate() const final { return true; } - /** Gets the distance of travel along the path of the mobilizer from + /* Gets the distance of travel along the path of the mobilizer from the provided context in meters. @param context The context of the model this mobilizer belongs to. @returns The distance coordinate of the mobilizer in the context. */ const T& get_distance(const systems::Context& context) const; - /** Sets the distance of travel along the path of the mobilizer from + /* Sets the distance of travel along the path of the mobilizer from the provided context in meters. @param context The context of the model this mobilizer belongs to. @param distance The desired distance coordinate of the mobilizer. @@ -110,13 +116,13 @@ class CurvilinearMobilizer final : public MobilizerImpl { const CurvilinearMobilizer& SetDistance(systems::Context* context, const T& distance) const; - /** Gets the tangential velocity of the mobilizer from the provided context in + /* Gets the tangential velocity of the mobilizer from the provided context in meters per second. @param context The context of the model this mobilizer belongs to. @returns The velocity coordinate of the mobilizer in the context. */ const T& get_tangential_velocity(const systems::Context& context) const; - /** Sets the tangential velocity of the mobilizer from the provided context in + /* Sets the tangential velocity of the mobilizer from the provided context in meters per second. @param context The context of the model this mobilizer belongs to. @param tangential_velocity The desired velocity coordinate of the mobilizer. @@ -124,13 +130,13 @@ class CurvilinearMobilizer final : public MobilizerImpl { const CurvilinearMobilizer& SetTangentialVelocity( systems::Context* context, const T& tangential_velocity) const; - /** Computes the across-mobilizer transform X_FM(q) as a function of the + /* Computes the across-mobilizer transform X_FM(q) as a function of the distance traveled along the mobilizer's path. @param q The distance traveled along the mobilizer's path in meters. @returns The across-mobilizer transform X_FM(q). */ math::RigidTransform calc_X_FM(const T* q) const; - /** Computes the across-mobilizer spatial velocity V_FM(q, v) as a function of + /* Computes the across-mobilizer spatial velocity V_FM(q, v) as a function of the distance traveled and tangential velocity along the mobilizer's path. @param q The distance traveled along the mobilizer's path in meters. @param v The tangential velocity along the mobilizer's path in meters per @@ -138,7 +144,7 @@ class CurvilinearMobilizer final : public MobilizerImpl { @returns The across-mobilizer spatial velocity V_FM(q, v). */ SpatialVelocity calc_V_FM(const T* q, const T* v) const; - /** Computes the across-mobilizer spatial acceleration A_FM(q, v, v̇) as a + /* Computes the across-mobilizer spatial acceleration A_FM(q, v, v̇) as a function of the distance traveled, tangential velocity, and tangential acceleration along the mobilizer's path. @param q The distance traveled along the mobilizer's path in meters. @@ -149,7 +155,7 @@ class CurvilinearMobilizer final : public MobilizerImpl { @returns The across-mobilizer spatial acceleration A_FM(q, v, v̇). */ SpatialAcceleration calc_A_FM(const T* q, const T* v, const T* vdot) const; - /** Projects the spatial force `F_Mo_F` on `this` mobilizer's outboard + /* Projects the spatial force `F_Mo_F` on `this` mobilizer's outboard frame M onto the path tangent. Mathematically, tau = F_Mo_F⋅H_FMᵀ. The result of this projection is the magnitude of a force (in N) along diff --git a/multibody/tree/test/prismatic_spring_test.cc b/multibody/tree/test/prismatic_spring_test.cc index f67bcd51d895..12294e3c521a 100644 --- a/multibody/tree/test/prismatic_spring_test.cc +++ b/multibody/tree/test/prismatic_spring_test.cc @@ -29,8 +29,8 @@ class SpringTester : public ::testing::Test { // Create an empty model. auto model = std::make_unique>(); - bodyA_ = &model->AddRigidBody("BodyA", SpatialInertia::Zero()); - bodyB_ = &model->AddRigidBody("BodyB", SpatialInertia::Zero()); + bodyA_ = &model->AddRigidBody("BodyA", SpatialInertia::NaN()); + bodyB_ = &model->AddRigidBody("BodyB", SpatialInertia::NaN()); model->AddJoint("WeldBodyAToWorld", model->world_body(), {}, *bodyA_, {},