From c793ef06525d658497a6e6025a18ef2bf7aec245 Mon Sep 17 00:00:00 2001 From: Mathew Halm Date: Tue, 24 Dec 2024 13:55:41 -0800 Subject: [PATCH 1/2] [multibody] Add CurvilinearJoint (#22214) Adds a Joint and corresponding Mobilizer which model a single-DoF joint along a path composed of circular arcs and line segments within a plane. --- .../piecewise_constant_curvature_trajectory.h | 17 +- multibody/tree/BUILD.bazel | 23 + multibody/tree/body_node_impl.cc | 2 + multibody/tree/body_node_impl_mass_matrix.cc | 2 + multibody/tree/curvilinear_joint.cc | 94 ++++ multibody/tree/curvilinear_joint.h | 403 ++++++++++++++++++ multibody/tree/curvilinear_mobilizer.cc | 185 ++++++++ multibody/tree/curvilinear_mobilizer.h | 329 ++++++++++++++ multibody/tree/test/curvilinear_joint_test.cc | 294 +++++++++++++ .../tree/test/curvilinear_mobilizer_test.cc | 242 +++++++++++ 10 files changed, 1584 insertions(+), 7 deletions(-) create mode 100644 multibody/tree/curvilinear_joint.cc create mode 100644 multibody/tree/curvilinear_joint.h create mode 100644 multibody/tree/curvilinear_mobilizer.cc create mode 100644 multibody/tree/curvilinear_mobilizer.h create mode 100644 multibody/tree/test/curvilinear_joint_test.cc create mode 100644 multibody/tree/test/curvilinear_mobilizer_test.cc diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.h b/common/trajectories/piecewise_constant_curvature_trajectory.h index 8ac88bf949ef..4e5ddfa9b4c6 100644 --- a/common/trajectories/piecewise_constant_curvature_trajectory.h +++ b/common/trajectories/piecewise_constant_curvature_trajectory.h @@ -40,8 +40,8 @@ namespace trajectories { For constant curvature paths on a plane, the Frenet–Serret formulas simplify and we can write: 
-     dFx/ds(s) =  ρ(s)⋅ Fy(s)
-     dFy/ds(s) = -ρ(s)⋅ Fx(s)
+     dFx/ds(s) =  ρ(s)⋅Fy(s)
+     dFy/ds(s) = -ρ(s)⋅Fx(s)
      dFz/ds(s) =  0
for the entire trajectory. @@ -69,6 +69,9 @@ class PiecewiseConstantCurvatureTrajectory final /** An empty piecewise constant curvature trajectory. */ PiecewiseConstantCurvatureTrajectory() = default; + template + using ScalarValueConverter = typename systems::scalar_conversion::template ValueConverter; + /** Constructs a piecewise constant curvature trajectory. Endpoints of each constant-curvature segments are defined by n breaks @@ -90,7 +93,7 @@ class PiecewiseConstantCurvatureTrajectory final @param turning_rates A vector of n-1 turning rates ρᵢ for each segment. @param initial_curve_tangent The initial tangent of the curve expressed in the parent frame, t̂_A(s₀). - @param The normal axis of the 2D plane in which the curve + @param plane_normal The normal axis of the 2D plane in which the curve lies, expressed in the parent frame, p̂_A. @param initial_position The initial position of the curve expressed in the parent frame, p_AoFo_A(s₀). @@ -118,12 +121,12 @@ class PiecewiseConstantCurvatureTrajectory final other.get_initial_pose() .rotation() .col(kCurveTangentIndex) - .template cast(), + .unaryExpr(ScalarValueConverter{}), other.get_initial_pose() .rotation() .col(kPlaneNormalIndex) - .template cast(), - other.get_initial_pose().translation().template cast()) {} + .unaryExpr(ScalarValueConverter{}), + other.get_initial_pose().translation().unaryExpr(ScalarValueConverter{})) {} /** @returns the number of rows in the output of value(). */ Eigen::Index rows() const override { return 3; } @@ -221,7 +224,7 @@ class PiecewiseConstantCurvatureTrajectory final static std::vector ScalarConvertStdVector( const std::vector& segment_data) { std::vector converted_segment_data; - systems::scalar_conversion::ValueConverter converter; + ScalarValueConverter converter; for (const U& segment : segment_data) { converted_segment_data.push_back(converter(segment)); } diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index 1c9354a1fccb..b55eaa85061d 100644 --- a/multibody/tree/BUILD.bazel +++ b/multibody/tree/BUILD.bazel @@ -87,6 +87,8 @@ drake_cc_library( "body_node.cc", "body_node_impl.cc", "body_node_impl_mass_matrix.cc", + "curvilinear_joint.cc", + "curvilinear_mobilizer.cc", "door_hinge.cc", "element_collection.cc", "fixed_offset_frame.cc", @@ -131,6 +133,8 @@ drake_cc_library( "body_node.h", "body_node_impl.h", "body_node_world.h", + "curvilinear_joint.h", + "curvilinear_mobilizer.h", "door_hinge.h", "element_collection.h", "fixed_offset_frame.h", @@ -185,7 +189,9 @@ drake_cc_library( "//common:nice_type_name", "//common:string_container", "//common:unused", + "//common/trajectories:piecewise_constant_curvature_trajectory", "//math:geometric_transform", + "//math:wrap_to", "//multibody/topology", "//systems/framework:leaf_system", ], @@ -446,6 +452,15 @@ drake_cc_library( ], ) +drake_cc_googletest( + name = "curvilinear_mobilizer_test", + deps = [ + ":mobilizer_tester", + ":tree", + "//common/test_utilities:eigen_matrix_compare", + ], +) + drake_cc_googletest( name = "planar_mobilizer_test", deps = [ @@ -611,6 +626,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "curvilinear_joint_test", + deps = [ + ":tree", + "//common/test_utilities:expect_throws_message", + ], +) + drake_cc_googletest( name = "revolute_joint_test", deps = [ diff --git a/multibody/tree/body_node_impl.cc b/multibody/tree/body_node_impl.cc index 877f9068fb48..b7d65757a7cd 100644 --- a/multibody/tree/body_node_impl.cc +++ b/multibody/tree/body_node_impl.cc @@ -1,6 +1,7 @@ #include "drake/multibody/tree/body_node_impl.h" #include "drake/common/default_scalars.h" +#include "drake/multibody/tree/curvilinear_mobilizer.h" #include "drake/multibody/tree/planar_mobilizer.h" #include "drake/multibody/tree/prismatic_mobilizer.h" #include "drake/multibody/tree/quaternion_floating_mobilizer.h" @@ -864,6 +865,7 @@ void BodyNodeImpl::CalcSpatialAccelerationBias( // Macro used to explicitly instantiate implementations for every mobilizer. #define EXPLICITLY_INSTANTIATE_IMPLS(T) \ + template class BodyNodeImpl; \ template class BodyNodeImpl; \ template class BodyNodeImpl; \ template class BodyNodeImpl; \ diff --git a/multibody/tree/body_node_impl_mass_matrix.cc b/multibody/tree/body_node_impl_mass_matrix.cc index 2a5abdabc705..63eb1585eb9b 100644 --- a/multibody/tree/body_node_impl_mass_matrix.cc +++ b/multibody/tree/body_node_impl_mass_matrix.cc @@ -4,6 +4,7 @@ /* clang-format on */ #include "drake/common/default_scalars.h" +#include "drake/multibody/tree/curvilinear_mobilizer.h" #include "drake/multibody/tree/planar_mobilizer.h" #include "drake/multibody/tree/prismatic_mobilizer.h" #include "drake/multibody/tree/quaternion_floating_mobilizer.h" @@ -139,6 +140,7 @@ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(6) // Macro used to explicitly instantiate implementations for every mobilizer. #define EXPLICITLY_INSTANTIATE_IMPLS(T) \ + template class BodyNodeImpl; \ template class BodyNodeImpl; \ template class BodyNodeImpl; \ template class BodyNodeImpl; \ diff --git a/multibody/tree/curvilinear_joint.cc b/multibody/tree/curvilinear_joint.cc new file mode 100644 index 000000000000..ef1f0d5f28f6 --- /dev/null +++ b/multibody/tree/curvilinear_joint.cc @@ -0,0 +1,94 @@ +#include "drake/multibody/tree/curvilinear_joint.h" + +#include +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/never_destroyed.h" +#include "drake/multibody/tree/multibody_tree.h" + +using drake::never_destroyed; + +namespace drake { +namespace multibody { + +template +CurvilinearJoint::CurvilinearJoint( + const std::string& name, const Frame& frame_on_parent, + const Frame& frame_on_child, + const trajectories::PiecewiseConstantCurvatureTrajectory + curvilinear_path, + bool is_periodic, double pos_lower_limit, double pos_upper_limit, + double damping) + : Joint( + name, frame_on_parent, frame_on_child, + VectorX::Constant(1, damping), + VectorX::Constant(1, pos_lower_limit), + VectorX::Constant(1, pos_upper_limit), + VectorX::Constant(1, + -std::numeric_limits::infinity()), + VectorX::Constant(1, std::numeric_limits::infinity()), + VectorX::Constant(1, + -std::numeric_limits::infinity()), + VectorX::Constant(1, + std::numeric_limits::infinity())), + curvilinear_path_(curvilinear_path), + is_periodic_(is_periodic) {} + +template +CurvilinearJoint::~CurvilinearJoint() = default; + +template +const std::string& CurvilinearJoint::type_name() const { + static const never_destroyed name{kTypeName}; + return name.access(); +} + +template +template +std::unique_ptr> CurvilinearJoint::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, + curvilinear_path_, is_periodic_, this->position_lower_limit(), + this->position_upper_limit(), this->default_damping()); + + 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()); + joint_clone->set_default_positions(this->default_positions()); + + return joint_clone; +} + +template +std::unique_ptr> CurvilinearJoint::DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> CurvilinearJoint::DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> +CurvilinearJoint::DoCloneToScalar( + const internal::MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::multibody::CurvilinearJoint); diff --git a/multibody/tree/curvilinear_joint.h b/multibody/tree/curvilinear_joint.h new file mode 100644 index 000000000000..bfe011b1172a --- /dev/null +++ b/multibody/tree/curvilinear_joint.h @@ -0,0 +1,403 @@ +#pragma once + +#include +#include +#include +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/trajectories/piecewise_constant_curvature_trajectory.h" +#include "drake/multibody/tree/curvilinear_mobilizer.h" +#include "drake/multibody/tree/joint.h" +#include "drake/multibody/tree/multibody_forces.h" + +namespace drake { +namespace multibody { + +/** A Joint which allows a child body to move relative to its parent along a + curvilinear path composed of line segments and circular arcs within a plane. + + Given a frame F attached to the parent body P and a frame M attached to the + child body B, this Joint allows frames F and M to have a relative transform + X_FM defined by a PiecewiseConstantCurvatureTrajectory. + + The joint may be specified to be periodic, representing a path shaped as a + closed loop. In this case, the path must return to the starting pose at its end + distance s_f [m], and the transfrom X_FM(q) is then periodic with period + s_f (X_FM(q) = X_FM(q + s_f)). + + This Joint has a single degree of freedom defined as the distance of travel + along the path, with the transform X_FM defined as the path-aligned frame + available through PiecewiseConstantCurvatureTrajectory::CalcPose. + + The path lies within a plane with normal axis p̂, equal to the z axis of frame + M. p̂ is not necessarily equal to Fz, but p̂_F is constant. + + By default, the joint limits are the endpoints for aperiodic paths, and `(-∞, + ∞)` for periodic paths. + + @tparam_default_scalar */ +template +class CurvilinearJoint final : public Joint { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CurvilinearJoint); + + template + using Context = systems::Context; + + static const char kTypeName[]; + + /** Constructor to create a curvilinear joint between two bodies so that + frame F attached to the parent body P and frame M attached to the child + body B, move relatively to one another along a planar curvilinear path. + See this class's documentation for further details on the definition of + these frames and the curvilinear path. + + This constructor signature creates a joint with natural joint limits, i.e. + the joint velocity and acceleration limits are the pair `(-∞, ∞)`; and the + position limits are `(-∞, ∞)` if the path is a periodic loop, or its + endpoints if not. + + 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] curvilinear_path The curvilinear path for this joint, along which + the child frame M moves relative to the parent frame F. + @param[in] is_periodic A boolean indicating whether the curvilinear path is + periodic. + @param[in] damping Viscous damping coefficient, in N⋅s/m, used to model + losses within the joint. The damping force (in N) is modeled as `f = + -damping⋅v`, i.e. opposing motion, with v the tangential velocity for `this` + joint (see get_tangential_velocity()). + @throws std::exception if damping is negative */ + CurvilinearJoint( + const std::string& name, const Frame& frame_on_parent, + const Frame& frame_on_child, + const trajectories::PiecewiseConstantCurvatureTrajectory + curvilinear_path, + bool is_periodic, double damping = 0) + : CurvilinearJoint( + name, frame_on_parent, frame_on_child, curvilinear_path, + is_periodic, + is_periodic ? -std::numeric_limits::infinity() : 0., + is_periodic ? std::numeric_limits::infinity() + : curvilinear_path.length(), + damping) {} + + /** Constructor to create a curvilinear joint between two bodies so that + frame F attached to the parent body P and frame M attached to the child + body B, move relatively to one another along a planar curvilinear path. + See this class's documentation for further details on the definition of + these frames and the path. + 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] curvilinear_path The curvilinear path for this joint, along which + the child frame M moves relative to the parent frame F. + @param[in] is_periodic A boolean indicating whether the curvilinear path is + periodic. + @param[in] pos_lower_limit Lower position limit, in meters, for the distance + coordinate (see get_distance()). + @param[in] pos_upper_limit Upper position limit, in meters, for the distance + coordinate (see get_distance()). + @param[in] damping Viscous damping coefficient, in N⋅s/m, used to model + losses within the joint. The damping force (in N) is modeled as `f = + -damping⋅v`, i.e. opposing motion, with v the tangential velocity for `this` + joint (see get_tangential_velocity()). + @throws std::exception if damping is negative. + @throws std::exception if pos_lower_limit > pos_upper_limit. */ + CurvilinearJoint( + const std::string& name, const Frame& frame_on_parent, + const Frame& frame_on_child, + const trajectories::PiecewiseConstantCurvatureTrajectory + curvilinear_path, + bool is_periodic, double pos_lower_limit, double pos_upper_limit, + double damping = 0); + + ~CurvilinearJoint() override; + + const std::string& type_name() const override; + + /** Returns `this` joint's default damping constant in N⋅s/m. */ + double default_damping() const { return this->default_damping_vector()[0]; } + + /** Sets the default value of viscous damping for this joint, in N⋅s/m. + @throws std::exception if damping is negative. + @pre the MultibodyPlant must not be finalized. */ + void set_default_damping(double damping) { + DRAKE_THROW_UNLESS(damping >= 0); + DRAKE_DEMAND(!this->get_parent_tree().topology_is_valid()); + this->set_default_damping_vector(Vector1d(damping)); + } + + /** Returns the position lower limit for `this` joint in m. */ + double position_lower_limit() const { + return this->position_lower_limits()[0]; + } + + /** Returns the position upper limit for `this` joint in m. */ + double position_upper_limit() const { + return this->position_upper_limits()[0]; + } + + /** Returns the velocity lower limit for `this` joint in m/s. */ + double velocity_lower_limit() const { + return this->velocity_lower_limits()[0]; + } + + /** Returns the velocity upper limit for `this` joint in m/s. */ + double velocity_upper_limit() const { + return this->velocity_upper_limits()[0]; + } + + /** Returns the acceleration lower limit for `this` joint in m/s². */ + double acceleration_lower_limit() const { + return this->acceleration_lower_limits()[0]; + } + + /** Returns the acceleration upper limit for `this` joint in m/s². */ + double acceleration_upper_limit() const { + return this->acceleration_upper_limits()[0]; + } + /** Context-dependent joint coordinate value access. + + Gets the travel distance of the joint from the provided context. + @param[in] context The context of the model this joint belongs to. + @returns The distance coordinate of the joint stored in the context. */ + const T& get_distance(const systems::Context& context) const { + return get_mobilizer().get_distance(context); + } + + /** Context-dependent joint coordinate setter. + + Sets the travel distance of the joint from the provided context. + @param[in] context The context of the model this joint belongs to. + @param[in] distance The travel distance to be set in the context. + @returns const reference to this joint. + */ + const CurvilinearJoint& set_distance(systems::Context* context, + const T& distance) const { + get_mobilizer().SetDistance(context, distance); + return *this; + } + /** Sets the random distribution for the distance along the path. + @param[in] distance Expression defining the random distance distribution. */ + void set_random_distance_distribution(const symbolic::Expression& distance) { + get_mutable_mobilizer().set_random_position_distribution( + Vector1{distance}); + } + + /** Context-dependent joint velocity value access. + + Gets the tangential velocity in meters per second, i.e. the rate of change of + this joint's travel distance (see get_distance()) from the provided context. + @param[in] context The context of the model this joint belongs to. + @returns The tangential velocity as stored in the provided context. */ + const T& get_tangential_velocity(const systems::Context& context) const { + return get_mobilizer().get_tangential_velocity(context); + } + + /** Context-dependent joint velocity setter. + + Sets the tangential velocity of the joint from the provided context. + @param[in] context The context of the model this joint belongs to. + @param[in] tangential_velocity The tangential velocity to be set in the + context. + @returns const reference to this joint. */ + const CurvilinearJoint& set_tangential_velocity( + systems::Context* context, const T& tangential_velocity) const { + get_mobilizer().SetTangentialVelocity(context, tangential_velocity); + return *this; + } + + /** Context-dependent damping coefficient getter. + + Refer to default_damping() for details. + @param[in] context The context of the model this joint belongs to. + @returns The damping coefficient stored in the context, in N⋅s/m. */ + const T& GetDamping(const systems::Context& context) const { + return this->GetDampingVector(context)[0]; + } + + /** Context-dependent damping coefficient setter. + + Refer to default_damping() for details. + @param[out] context The context of the model this joint belongs to. + @param[in] damping The damping coefficient to be set in N⋅s/m. + @throws std::exception if `damping` is negative. */ + void SetDamping(systems::Context* context, const T& damping) const { + DRAKE_THROW_UNLESS(damping >= 0); + this->SetDampingVector(context, Vector1(damping)); + } + + /** Gets the default travel distance along the path. + + Wrapper for the more general Joint::default_positions. + + @returns The default distance along the path of `this` stored in + `default_positions_` */ + double get_default_distance() const { return this->default_positions()[0]; } + + /** Sets the default travel distance of this joint. + @param[in] distance The desired default distance of the joint in meters. */ + void set_default_distance(double distance) { + this->set_default_positions(Vector1d{distance}); + } + + /** Adds into a MultibodyForces a generalized force on this joint. + + A generalized force for a curvilinear joint is equivalent to a force in + Newtons applied on the path, in the path tangent direction (x-axis of the + mobilized frame M). + @param[in] context The context of the model this joint belongs to. + @param[in] force The force to be applied, in Newtons. + @param[out] forces The MultibodyForces object to which the generalized force + is added. + */ + void AddInForce(const systems::Context& context, const T& force, + MultibodyForces* forces) const { + DRAKE_DEMAND(forces != nullptr); + DRAKE_DEMAND(forces->CheckHasRightSizeForModel(this->get_parent_tree())); + this->AddInOneForce(context, 0, force, forces); + } + + protected: + /** Joint override called through Joint::AddInForce. + + Arguments already checked to be valid by Joint::AddInForce. + + @param[in] joint_dof The joint degree of freedom index, on which the force is + added, which must be 0. + @param[in] joint_tau The force along the path's tangential axis to be added, + in Newtons, applied to the child body. + @param[out] forces The MultibodyForces object to which the force is added. + @see The public NVI AddInOneForce() for details. */ + void DoAddInOneForce(const systems::Context&, int joint_dof, + const T& joint_tau, + MultibodyForces* forces) const override { + DRAKE_DEMAND(joint_dof == 0); + Eigen::Ref> tau_mob = + get_mobilizer().get_mutable_generalized_forces_from_array( + &forces->mutable_generalized_forces()); + tau_mob(joint_dof) += joint_tau; + } + + /** Joint override called through Joint::AddInDamping. + + Arguments already checked to be valid by Joint::AddInDamping. + + Adds a dissipative force according to the viscous law `F = -d⋅v`, where + d is the damping coefficient (see default_damping()) and v the tangential + velocity along the path. + + @param[in] context The context of the model this joint belongs to. + @param[out] forces The MultibodyForces object to which the damping force is + added. */ + void DoAddInDamping(const systems::Context& context, + MultibodyForces* forces) const override { + const T damping_force = + -this->GetDamping(context) * get_tangential_velocity(context); + AddInForce(context, damping_force, forces); + } + + private: + int do_get_velocity_start() const override { + return get_mobilizer().velocity_start_in_v(); + } + + int do_get_num_velocities() const override { return 1; } + + int do_get_position_start() const override { + return get_mobilizer().position_start_in_q(); + } + + int do_get_num_positions() const override { return 1; } + + std::string do_get_position_suffix(int index) const override { + return get_mobilizer().position_suffix(index); + } + + std::string do_get_velocity_suffix(int index) const override { + return get_mobilizer().velocity_suffix(index); + } + + void do_set_default_positions( + const VectorX& default_positions) override { + if (this->has_implementation()) { + get_mutable_mobilizer().set_default_position(default_positions); + } + } + + const T& DoGetOnePosition(const systems::Context& context) const override { + return get_distance(context); + } + + const T& DoGetOneVelocity(const systems::Context& context) const override { + return get_tangential_velocity(context); + } + + // Joint overrides: + std::unique_ptr::BluePrint> MakeImplementationBlueprint( + const internal::SpanningForest::Mobod& mobod) const override { + auto blue_print = std::make_unique::BluePrint>(); + const auto [inboard_frame, outboard_frame] = + this->tree_frames(mobod.is_reversed()); + // TODO(sherm1) The mobilizer needs to be reversed, not just the frames. + auto curvilinear_mobilizer = + std::make_unique>( + mobod, *inboard_frame, *outboard_frame, curvilinear_path_, + is_periodic_); + curvilinear_mobilizer->set_default_position(this->default_positions()); + blue_print->mobilizer = std::move(curvilinear_mobilizer); + return blue_print; + } + + 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 CurvilinearJoint templated on every other scalar type a friend of + // CurvilinearJoint so that CloneToScalar() can access + // private members of CurvilinearJoint. + template + friend class CurvilinearJoint; + + // 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. + const internal::CurvilinearMobilizer& get_mobilizer() const { + return this + ->template get_mobilizer_downcast(); + } + + internal::CurvilinearMobilizer& get_mutable_mobilizer() { + return this->template get_mutable_mobilizer_downcast< + internal::CurvilinearMobilizer>(); + } + + // Helper method to make a clone templated on ToScalar. + template + std::unique_ptr> TemplatedDoCloneToScalar( + const internal::MultibodyTree& tree_clone) const; + + trajectories::PiecewiseConstantCurvatureTrajectory curvilinear_path_; + bool is_periodic_; +}; + +template +const char CurvilinearJoint::kTypeName[] = "curvilinear"; + +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::multibody::CurvilinearJoint); diff --git a/multibody/tree/curvilinear_mobilizer.cc b/multibody/tree/curvilinear_mobilizer.cc new file mode 100644 index 000000000000..2c180f6d90d0 --- /dev/null +++ b/multibody/tree/curvilinear_mobilizer.cc @@ -0,0 +1,185 @@ +#include "drake/multibody/tree/curvilinear_mobilizer.h" + +#include +#include + +#include "drake/multibody/tree/body_node_impl.h" +#include "drake/multibody/tree/multibody_tree.h" + +namespace drake { +namespace multibody { +namespace internal { + +template +CurvilinearMobilizer::~CurvilinearMobilizer() = default; + +template +std::unique_ptr> CurvilinearMobilizer::CreateBodyNode( + const internal::BodyNode* parent_node, const RigidBody* body, + const Mobilizer* mobilizer) const { + return std::make_unique>( + parent_node, body, mobilizer); +} + +template +std::string CurvilinearMobilizer::position_suffix( + int position_index_in_mobilizer) const { + if (position_index_in_mobilizer == 0) { + return "q"; + } + throw std::runtime_error("CurvilinearMobilizer has only 1 position."); +} + +template +std::string CurvilinearMobilizer::velocity_suffix( + int velocity_index_in_mobilizer) const { + if (velocity_index_in_mobilizer == 0) { + return "v"; + } + throw std::runtime_error("CurvilinearMobilizer has only 1 velocity."); +} + +template +const T& CurvilinearMobilizer::get_distance( + const drake::systems::Context& context) const { + auto q = this->get_positions(context); + DRAKE_ASSERT(q.size() == kNq); + return q.coeffRef(0); +} + +template +const CurvilinearMobilizer& CurvilinearMobilizer::SetDistance( + drake::systems::Context* context, const T& distance) const { + auto q = this->GetMutablePositions(context); + DRAKE_ASSERT(q.size() == kNq); + q[0] = distance; + return *this; +} + +template +const T& CurvilinearMobilizer::get_tangential_velocity( + const drake::systems::Context& context) const { + const auto& v = this->get_velocities(context); + DRAKE_ASSERT(v.size() == kNv); + return v.coeffRef(0); +} + +template +const CurvilinearMobilizer& CurvilinearMobilizer::SetTangentialVelocity( + drake::systems::Context* context, const T& tangential_velocity) const { + auto v = this->GetMutableVelocities(context); + DRAKE_ASSERT(v.size() == kNv); + v[0] = tangential_velocity; + return *this; +} + +template +math::RigidTransform CurvilinearMobilizer::CalcAcrossMobilizerTransform( + const drake::systems::Context& context) const { + const auto& q = this->get_positions(context); + DRAKE_ASSERT(q.size() == kNq); + return calc_X_FM(q.data()); +} + +template +SpatialVelocity CurvilinearMobilizer::CalcAcrossMobilizerSpatialVelocity( + const drake::systems::Context& context, + const Eigen::Ref>& v) const { + DRAKE_ASSERT(v.size() == kNv); + const auto& q = this->get_positions(context); + return calc_V_FM(q.data(), v.data()); +} + +template +SpatialAcceleration +CurvilinearMobilizer::CalcAcrossMobilizerSpatialAcceleration( + const drake::systems::Context& context, + const Eigen::Ref>& vdot) const { + DRAKE_ASSERT(vdot.size() == kNv); + const auto& q = this->get_positions(context); + const auto& v = this->get_velocities(context); + return calc_A_FM(q.data(), v.data(), vdot.data()); +} + +template +void CurvilinearMobilizer::ProjectSpatialForce( + const drake::systems::Context& context, const SpatialForce& F_BMo_F, + Eigen::Ref> tau) const { + DRAKE_ASSERT(tau.size() == kNv); + const auto& q = this->get_positions(context); + calc_tau(q.data(), F_BMo_F, tau.data()); +} + +template +void CurvilinearMobilizer::DoCalcNMatrix(const systems::Context&, + EigenPtr> N) const { + (*N)(0, 0) = 1.0; +} + +template +void CurvilinearMobilizer::DoCalcNplusMatrix( + const systems::Context&, EigenPtr> Nplus) const { + (*Nplus)(0, 0) = 1.0; +} + +template +void CurvilinearMobilizer::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 CurvilinearMobilizer::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> +CurvilinearMobilizer::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>( + tree_clone.get_mobod(this->mobod().index()), inboard_frame_clone, + outboard_frame_clone, + PiecewiseConstantCurvatureTrajectory(curvilinear_path_), + is_periodic_); +} + +template +std::unique_ptr> CurvilinearMobilizer::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> CurvilinearMobilizer::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> +CurvilinearMobilizer::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::CurvilinearMobilizer); diff --git a/multibody/tree/curvilinear_mobilizer.h b/multibody/tree/curvilinear_mobilizer.h new file mode 100644 index 000000000000..2038d34e6b15 --- /dev/null +++ b/multibody/tree/curvilinear_mobilizer.h @@ -0,0 +1,329 @@ +#pragma once + +#include +#include +#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/common/trajectories/piecewise_constant_curvature_trajectory.h" +#include "drake/math/wrap_to.h" +#include "drake/multibody/math/spatial_algebra.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" + +using drake::trajectories::PiecewiseConstantCurvatureTrajectory; + +namespace drake { +namespace multibody { +namespace internal { + +// Tolerance used to check joint periodicity. +static constexpr double kCurvilinearJointPeriodicityTolerance = + 1e3 * std::numeric_limits::epsilon(); + +/** A Mobilizer which allows two frames to translate and rotate relatively to + one another along a curvilinear path composed of line segments and circular + arcs within a plane. + + The path is specified as a PiecewiseConstantCurvatureTrajectory. + + The mobilizer may be specified to be periodic, representing a path shaped as a + closed loop. In this case, the path must return to the starting pose at its end + distance s_f [m]. The transfrom X_FM(q) from the CurvilinearMobilizer's inboard + frame F to the outboard frame M in this case is periodic with period s_f + (X_FM(q) = X_FM(q + s_f)). + + The single generalized coordinate q [m] introduced by this mobilizer + corresponds to the distance of travel along the path. The transfrom X_FM(q) + from the CurvilinearMobilizer's inboard frame F to the outboard frame M is set + to be the path-aligned pose at distance s(q) [m], available through + PiecewiseConstantCurvatureTrajectory::CalcPose. For aperiodic trajectories, + s(q) = q. For periodic trajectories, s(q) is wrapped using modular arithmetic, + i.e. s(q) = q - s_f⋅floor(q/s_f) [m]. The generalized velocity v = q̇ [m/s] is + the tangential velocity along the path. + + The path lies within a plane with normal axis p̂, equal to the z axis of the + mobilized frame Mz. p̂ is not necessarily equal to Fz, but p̂_F is constant. + + At any given point along the path, a scalar turning rate ρ(q) [1/m] is defined + via right-hand rule about the planar axis p̂, such that the angular velocity + across the mobilizer is w_FM = ρ⋅p̂⋅v. The hinge matrix and time derivative + are + + H_FM₆ₓ₁(q) = [ Mx_F(q), Hdot_FM₆ₓ₁(q) = [ρ(q)⋅My_F(q)⋅v, + ρ(q)⋅Mz_F(q)], 0]. + + @tparam_default_scalar */ +template +class CurvilinearMobilizer final : public MobilizerImpl { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CurvilinearMobilizer); + using MobilizerBase = MobilizerImpl; + using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx; + template + using QVector = typename MobilizerBase::template QVector; + template + using VVector = typename MobilizerBase::template VVector; + template + using HMatrix = typename MobilizerBase::template HMatrix; + + /** Constructor for a CurvilinearMobilizer between the inboard frame F + and the outboard frame M granting a single degree of freedom expressed by the + pose X_FM(q) of the path `curvilinear_path` at distance q [m] along the path. + The path is required to be a closed loop if the parameter `is_periodic` is + set to `true`. + + @param mobod information for the mobilized body attached to frame M + @param inboard_frame_F the inboard frame F + @param outboard_frame_M the outboard frame M + @param curvilinear_path the curvilinear path defining X_FM + @param is_periodic if true, the mobilizer is periodic, and the path must be a + closed loop. + + @throws std::exception if `is_periodic` is true, but `curvilinear_path` is + not (nearly) periodic. */ + CurvilinearMobilizer( + const SpanningForest::Mobod& mobod, const Frame& inboard_frame_F, + const Frame& outboard_frame_M, + const PiecewiseConstantCurvatureTrajectory& curvilinear_path, + bool is_periodic) + : MobilizerBase(mobod, inboard_frame_F, outboard_frame_M), + curvilinear_path_(curvilinear_path), + is_periodic_(is_periodic) { + bool path_is_periodic = curvilinear_path.IsNearlyPeriodic( + kCurvilinearJointPeriodicityTolerance); + if (is_periodic_ && !path_is_periodic) { + // The path not periodic, but the mobilizer is. + throw std::runtime_error( + "CurvilinearMobilizer: Periodic mobilizer must be constructed with " + "periodic path."); + } + } + + ~CurvilinearMobilizer() final; + + std::unique_ptr> CreateBodyNode( + const internal::BodyNode* parent_node, const RigidBody* body, + const Mobilizer* mobilizer) const final; + + std::string position_suffix(int position_index_in_mobilizer) const final; + std::string velocity_suffix(int velocity_index_in_mobilizer) const final; + + 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 + 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 + 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. + @returns a const reference to this mobilizer */ + const CurvilinearMobilizer& SetDistance(systems::Context* context, + const T& distance) const; + + /** 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 + meters per second. + @param context The context of the model this mobilizer belongs to. + @param tangential_velocity The desired velocity coordinate of the mobilizer. + @returns a const reference to this mobilizer */ + const CurvilinearMobilizer& SetTangentialVelocity( + systems::Context* context, const T& tangential_velocity) const; + + /** Calculates the distance along the path s(q) [m] associated with a + generalized coordinate q [m]. + + For aperiodic paths, this function is the identity (s(q) = q). + + For periodic paths of length s_f [m], the mobilizer returns to the start of + the path after each full length of travel, meaning that s must be "wrapped" + to the path domain [0, s_f): + + 
+      s(q) = q mod s_f.
+
+ + @param q The generalized coordinate of the mobilizer. + @returns The position s along the path associated with q. */ + T calc_s(const T& q) const { + return is_periodic_ ? math::wrap_to(q, T(0.), curvilinear_path_.length()) + : q; + } + + /** 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 { + const T s = calc_s(q[0]); + return curvilinear_path_.CalcPose(s); + } + + /** 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 + second. + @returns The across-mobilizer spatial velocity V_FM(q, v). */ + SpatialVelocity calc_V_FM(const T* q, const T* v) const { + const T s = calc_s(q[0]); + return curvilinear_path_.CalcSpatialVelocity(s, v[0]); + } + + /** 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. + @param v The tangential velocity along the mobilizer's path in meters per + second. + @param vdot The tangential acceleration along the mobilizer's path in meters + per second squared. + @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 { + const T s = calc_s(q[0]); + return curvilinear_path_.CalcSpatialAcceleration(s, v[0], vdot[0]); + } + + /** Projects the spatial force `F_Mo_F` on `this` mobilizer's outboard + frame M onto the path tangent. Mathematically, + this is the dot product of `F_Mo_F` with the spatial derivative of + the path w.r.t. distance traveled q: F_Mo_F⋅H_FM. + + For this mobilizer, H_FM(q) = d/dv V_FM_F(q, v) is numerically equal to the + spatial velocity V_FM_F(q, 1) evaluated at the unit velocity v = 1 [m/s]: + + 
+      tau = F_BMo_F.dot(V_FM_F(q, 1))
+
+ + The result of this method is the scalar equivalent to + a force applied on the path, pointed along the path's tangential axis and + measured in Newtons. + + + @param q The distance traveled along the mobilizer's path in meters. + @param F_BMo_F The spatial force applied to the child body at Mo, expressed + in F. + @param[out] tau A pointer to store the resulting generalized force in + Newtons. + */ + void calc_tau(const T* q, const SpatialForce& F_BMo_F, T* tau) const { + DRAKE_ASSERT(tau != nullptr); + const T v(1.); + // Computes tau = H_FM(q)⋅F_Mo_F, equivalent to V_FM(q, 1)⋅F_Mo_F. + tau[0] = calc_V_FM(q, &v).dot(F_BMo_F); + } + + /** Calculates and stores the across-mobilizer transform X_FM given + the generalized coordinate stored in a given context. + @param context The context of the model this mobilizer belongs to. + @returns The across-mobilizer transform X_FM(q). */ + math::RigidTransform CalcAcrossMobilizerTransform( + const systems::Context& context) const final; + + /** Calculates and stores the across-mobilizer spatial velocity V_FM given + the generalized coordinate stored in a given context and a provided + tangential velocity. + @param context The context of the model this mobilizer belongs to. + @param v The tangential velocity along the mobilizer's path in meters per + second. + @returns The across-mobilizer spatial velocity V_FM(q, v). + @note This method aborts in Debug builds if v.size() is not one. */ + SpatialVelocity CalcAcrossMobilizerSpatialVelocity( + const systems::Context& context, + const Eigen::Ref>& v) const final; + + /** Calculates and stores the across-mobilizer spatial acceleration A_FM given + the generalized coordinate and velocity stored in a given context and a + provided tangential acceleration. + @param context The context of the model this mobilizer belongs to. + @param vdot The tangential acceleration along the mobilizer's path in meters + per second squared. + @returns The across-mobilizer spatial acceleration A_FM(q, v, v̇). + @note This method aborts in Debug builds if vdot.size() is not one. */ + SpatialAcceleration CalcAcrossMobilizerSpatialAcceleration( + const systems::Context& context, + const Eigen::Ref>& vdot) const override; + + /** Projects the spatial force `F_Mo_F` on `this` mobilizer's outboard + frame M onto the path tangent. Mathematically, + this is the dot product of `F_Mo_F` with the spatial derivative of + the path w.r.t. distance traveled q: F_Mo_F⋅H_FM. + + For this mobilizer, H_FM(q) = d/dv V_FM(q, v) is numerically equal to the + spatial velocity V_FM(q, 1) evaluated at the unit velocity v = 1 [m/s]: + + 
+      tau = F_Mo_F.dot(V_FM(q, 1)).
+
+ + The result of this method is the scalar equivalent to + a force applied on the path, pointed along the path's tangential axis and + measured in Newtons. + @param context The context of the model this mobilizer belongs to. + @param F_BMo_F The spatial force applied to the child body at Mo, expressed + in F. + @param[out] tau A reference to store the resulting generalized force in + Newtons. + @note This method aborts in Debug builds if `tau.size()` is not one.*/ + void ProjectSpatialForce(const systems::Context& context, + const SpatialForce& F_BMo_F, + Eigen::Ref> tau) const override; + + bool is_velocity_equal_to_qdot() const override { return true; } + + void MapVelocityToQDot(const systems::Context& context, + const Eigen::Ref>& v, + EigenPtr> qdot) const override; + + void MapQDotToVelocity(const systems::Context& context, + const Eigen::Ref>& qdot, + EigenPtr> v) const override; + + 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 override; + + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const override; + + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const override; + + private: + template + std::unique_ptr> TemplatedDoCloneToScalar( + const MultibodyTree& tree_clone) const; + + PiecewiseConstantCurvatureTrajectory curvilinear_path_; + bool is_periodic_; +}; + +} // namespace internal +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::multibody::internal::CurvilinearMobilizer); diff --git a/multibody/tree/test/curvilinear_joint_test.cc b/multibody/tree/test/curvilinear_joint_test.cc new file mode 100644 index 000000000000..48e8330bfecf --- /dev/null +++ b/multibody/tree/test/curvilinear_joint_test.cc @@ -0,0 +1,294 @@ +#include "drake/multibody/tree/curvilinear_joint.h" + +#include + +#include "drake/common/eigen_types.h" +#include "drake/multibody/tree/multibody_tree-inl.h" +#include "drake/multibody/tree/rigid_body.h" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace multibody { +namespace { + +const double kEpsilon = std::numeric_limits::epsilon(); + +using Eigen::Vector3d; +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 bool kIsPeriodic = true; + +class CurvilinearJointTest : public ::testing::Test { + public: + // Creates a simple model consisting of a single body with a prismatic joint + // with the sole purpose of testing the CurvilinearJoint user facing API. + void SetUp() override { + std::unique_ptr> model = MakeModel(); + + // We are done adding modeling elements. Transfer tree to system and get + // a Context. + system_ = std::make_unique>( + std::move(model), true /* is_discrete */); + context_ = system_->CreateDefaultContext(); + } + + std::unique_ptr> MakeModel() { + // Spatial inertia for adding body. The actual value is not important for + // these tests and therefore we do not initialize it. + const auto M_B = SpatialInertia::NaN(); + + // Create an empty model. + auto model = std::make_unique>(); + + // Add a body so we can add joint to it. + body1_ = &model->AddRigidBody("Body", M_B); + + // Add a prismatic joint between the world and body1. + joint1_ = &model->AddJoint( + "Joint1", model->world_body(), std::nullopt, *body1_, std::nullopt, + trajectory_, kIsPeriodic, kDamping); + Joint& mutable_joint = model->get_mutable_joint(joint1_->index()); + mutable_joint1_ = dynamic_cast*>(&mutable_joint); + mutable_joint1_->set_position_limits( + Vector1::Constant(kPositionLowerLimit), + Vector1::Constant(kPositionUpperLimit)); + mutable_joint1_->set_velocity_limits( + Vector1::Constant(kVelocityLowerLimit), + Vector1::Constant(kVelocityUpperLimit)); + mutable_joint1_->set_acceleration_limits( + Vector1::Constant(kAccelerationLowerLimit), + Vector1::Constant(kAccelerationUpperLimit)); + + return model; + } + + const internal::MultibodyTree& tree() const { + return internal::GetInternalTree(*system_); + } + + protected: + std::unique_ptr> system_; + std::unique_ptr> context_; + + const RigidBody* body1_{nullptr}; + const CurvilinearJoint* joint1_{nullptr}; + CurvilinearJoint* mutable_joint1_{nullptr}; + + protected: + const double k_ = 1.0; + const double l_ = 1.0; + const std::vector breaks_{0, M_PI / k_, l_ + M_PI / k_, + l_ + 2 * M_PI / k_, 2 * l_ + 2 * M_PI / k_}; + const std::vector turning_rates_{k_, 0, k_, 0}; + const Vector3d tangent_axis_{1., -std::sqrt(2.), 1.}; + const Vector3d plane_axis_{1., std::sqrt(2.), 1.}; + const Vector3d initial_position_{1., 2., 3.}; + const PiecewiseConstantCurvatureTrajectory trajectory_{ + breaks_, turning_rates_, tangent_axis_, plane_axis_, initial_position_}; +}; + +TEST_F(CurvilinearJointTest, Type) { + const Joint& base = *joint1_; + EXPECT_EQ(base.type_name(), CurvilinearJoint::kTypeName); +} + +// Verify the expected number of dofs. +TEST_F(CurvilinearJointTest, NumDOFs) { + EXPECT_EQ(tree().num_positions(), 1); + EXPECT_EQ(tree().num_velocities(), 1); + EXPECT_EQ(joint1_->num_positions(), 1); + EXPECT_EQ(joint1_->num_velocities(), 1); + EXPECT_EQ(joint1_->position_start(), 0); + EXPECT_EQ(joint1_->velocity_start(), 0); +} + +TEST_F(CurvilinearJointTest, GetJointLimits) { + EXPECT_EQ(joint1_->position_lower_limits().size(), 1); + EXPECT_EQ(joint1_->position_upper_limits().size(), 1); + EXPECT_EQ(joint1_->velocity_lower_limits().size(), 1); + EXPECT_EQ(joint1_->velocity_upper_limits().size(), 1); + EXPECT_EQ(joint1_->acceleration_lower_limits().size(), 1); + EXPECT_EQ(joint1_->acceleration_upper_limits().size(), 1); + + EXPECT_EQ(joint1_->position_lower_limit(), kPositionLowerLimit); + EXPECT_EQ(joint1_->position_upper_limit(), kPositionUpperLimit); + EXPECT_EQ(joint1_->velocity_lower_limit(), kVelocityLowerLimit); + EXPECT_EQ(joint1_->velocity_upper_limit(), kVelocityUpperLimit); + EXPECT_EQ(joint1_->acceleration_lower_limit(), kAccelerationLowerLimit); + EXPECT_EQ(joint1_->acceleration_upper_limit(), kAccelerationUpperLimit); +} + +TEST_F(CurvilinearJointTest, Damping) { + std::unique_ptr> model = MakeModel(); + auto& joint = model->GetMutableJointByName("Joint1"); + EXPECT_EQ(joint.default_damping(), kDamping); + EXPECT_EQ(joint.default_damping_vector(), Vector1d(kDamping)); + const double new_damping = 2.0 * kDamping; + joint.set_default_damping(new_damping); + EXPECT_EQ(joint.default_damping(), new_damping); + EXPECT_EQ(joint.default_damping_vector(), Vector1d(new_damping)); + + // Expect to throw on invalid damping values. + EXPECT_THROW(joint.set_default_damping(-1), std::exception); +} + +// Context-dependent value access. +TEST_F(CurvilinearJointTest, ContextDependentAccess) { + const double some_value = 1.5; + // Translation access. + joint1_->set_distance(context_.get(), some_value); + EXPECT_EQ(joint1_->get_distance(*context_), some_value); + + // Translation rate access. + joint1_->set_tangential_velocity(context_.get(), some_value); + EXPECT_EQ(joint1_->get_tangential_velocity(*context_), some_value); + + // Joint locking. + joint1_->Lock(context_.get()); + EXPECT_EQ(joint1_->get_tangential_velocity(*context_), 0.); + + // Damping. + EXPECT_EQ(joint1_->GetDamping(*context_), kDamping); + EXPECT_EQ(joint1_->GetDampingVector(*context_), Vector1d(kDamping)); + + EXPECT_NO_THROW( + joint1_->SetDampingVector(context_.get(), Vector1d(some_value))); + EXPECT_EQ(joint1_->GetDamping(*context_), some_value); + EXPECT_EQ(joint1_->GetDampingVector(*context_), Vector1d(some_value)); + + EXPECT_NO_THROW(joint1_->SetDamping(context_.get(), kDamping)); + EXPECT_EQ(joint1_->GetDamping(*context_), kDamping); + EXPECT_EQ(joint1_->GetDampingVector(*context_), Vector1d(kDamping)); + + // Expect to throw on invalid damping values. + EXPECT_THROW(joint1_->SetDamping(context_.get(), -1), std::exception); + EXPECT_THROW(joint1_->SetDampingVector(context_.get(), Vector1d(-1)), + std::exception); +} + +// Tests API to apply torques to a joint. +TEST_F(CurvilinearJointTest, AddInForces) { + const double some_value = 1.5; + // Default initialized to zero forces. + MultibodyForces forces1(tree()); + + // Add value twice. + joint1_->AddInForce(*context_, some_value, &forces1); + joint1_->AddInForce(*context_, some_value, &forces1); + + MultibodyForces forces2(tree()); + // Add value only once. + joint1_->AddInForce(*context_, 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)); +} + +// Tests API to add in damping forces. +TEST_F(CurvilinearJointTest, AddInDampingForces) { + const double translational_velocity = 0.1; + const double damping = 0.2 * kDamping; + + const Vector1d damping_force_expected(-damping * translational_velocity); + + joint1_->set_tangential_velocity(context_.get(), translational_velocity); + joint1_->SetDamping(context_.get(), damping); + + MultibodyForces forces(tree()); + joint1_->AddInDamping(*context_, &forces); + EXPECT_EQ(forces.generalized_forces(), damping_force_expected); +} + +TEST_F(CurvilinearJointTest, Clone) { + auto model_clone = tree().CloneToScalar(); + const auto& joint1_clone = dynamic_cast&>( + model_clone->get_variant(*joint1_)); + + EXPECT_EQ(joint1_clone.name(), joint1_->name()); + EXPECT_EQ(joint1_clone.frame_on_parent().index(), + joint1_->frame_on_parent().index()); + EXPECT_EQ(joint1_clone.frame_on_child().index(), + joint1_->frame_on_child().index()); + EXPECT_EQ(joint1_clone.position_lower_limits(), + joint1_->position_lower_limits()); + EXPECT_EQ(joint1_clone.position_upper_limits(), + joint1_->position_upper_limits()); + EXPECT_EQ(joint1_clone.velocity_lower_limits(), + joint1_->velocity_lower_limits()); + EXPECT_EQ(joint1_clone.velocity_upper_limits(), + joint1_->velocity_upper_limits()); + EXPECT_EQ(joint1_clone.acceleration_lower_limits(), + joint1_->acceleration_lower_limits()); + EXPECT_EQ(joint1_clone.acceleration_upper_limits(), + joint1_->acceleration_upper_limits()); + EXPECT_EQ(joint1_clone.default_damping(), joint1_->default_damping()); + EXPECT_EQ(joint1_clone.get_default_distance(), + joint1_->get_default_distance()); +} + +TEST_F(CurvilinearJointTest, CanRotateAndTranslate) { + EXPECT_TRUE(joint1_->can_rotate()); + EXPECT_TRUE(joint1_->can_translate()); +} + +TEST_F(CurvilinearJointTest, NameSuffix) { + EXPECT_EQ(joint1_->position_suffix(0), "q"); + EXPECT_EQ(joint1_->velocity_suffix(0), "v"); +} + +TEST_F(CurvilinearJointTest, RandomTranslationTest) { + // Calling SetRandomContext before setting the distribution results in the + // zero state. + RandomGenerator generator; + tree().SetRandomState(*context_, &context_->get_mutable_state(), &generator); + EXPECT_EQ(joint1_->get_distance(*context_), 0.); + + // Setup distribution for random initial conditions. + std::uniform_real_distribution uniform( + 1.0, kPositionUpperLimit); + mutable_joint1_->set_random_distance_distribution(uniform()); + tree().SetRandomState(*context_, &context_->get_mutable_state(), &generator); + EXPECT_LE(1.0, joint1_->get_distance(*context_)); + EXPECT_GE(kPositionUpperLimit, joint1_->get_distance(*context_)); +} + +TEST_F(CurvilinearJointTest, DefaultTranslation) { + const double default_distance = 0.0; + + const double new_default_distance = + 0.5 * kPositionLowerLimit + 0.5 * kPositionUpperLimit; + + const double out_of_bounds_low_translation = kPositionLowerLimit - 1; + const double out_of_bounds_high_translation = kPositionUpperLimit + 1; + + // Constructor should set the default translation to 0.0. + EXPECT_EQ(joint1_->get_default_distance(), default_distance); + + // Setting a new default translation should propagate so that + // get_default_distance() remains correct. + mutable_joint1_->set_default_distance(new_default_distance); + EXPECT_EQ(joint1_->get_default_distance(), new_default_distance); + + // Setting the default angle out of the bounds of the position limits + // should NOT throw an exception. + EXPECT_NO_THROW( + mutable_joint1_->set_default_distance(out_of_bounds_low_translation)); + EXPECT_NO_THROW( + mutable_joint1_->set_default_distance(out_of_bounds_high_translation)); +} + +} // namespace +} // namespace multibody +} // namespace drake diff --git a/multibody/tree/test/curvilinear_mobilizer_test.cc b/multibody/tree/test/curvilinear_mobilizer_test.cc new file mode 100644 index 000000000000..8c85427a7e15 --- /dev/null +++ b/multibody/tree/test/curvilinear_mobilizer_test.cc @@ -0,0 +1,242 @@ +#include "drake/multibody/tree/curvilinear_mobilizer.h" + +#include + +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/trajectories/piecewise_constant_curvature_trajectory.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/tree/curvilinear_joint.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 drake::trajectories::PiecewiseConstantCurvatureTrajectory; +using Eigen::AngleAxisd; +using Eigen::Vector3d; +using math::RigidTransformd; +using math::RotationMatrixd; +using std::make_unique; +using std::sqrt; +using std::unique_ptr; +using systems::Context; + +constexpr double kTolerance = 10 * std::numeric_limits::epsilon(); + +// Fixture to setup a simple model containing a curvilinear mobilizer. +class CurvilinearMobilizerTest : public MobilizerTester { + public: + void SetUp() override { + std::unique_ptr> joint = + std::make_unique>( + "joint1=0", tree().world_body().body_frame(), body_->body_frame(), + trajectory_, true); + mobilizer_ = &AddJointAndFinalize( + std::move(joint)); + } + + protected: + const CurvilinearMobilizer* mobilizer_{nullptr}; + const double k_ = 1.0; + const double l_ = 1.0; + const std::vector breaks_{0, M_PI / k_, l_ + M_PI / k_, + l_ + 2 * M_PI / k_, 2 * l_ + 2 * M_PI / k_}; + const std::vector turning_rates_{k_, 0, k_, 0}; + const Vector3d tangent_axis_{1., -std::sqrt(2.), 1.}; + const Vector3d plane_axis_{1., std::sqrt(2.), 1.}; + const Vector3d initial_position_{1., 2., 3.}; + const PiecewiseConstantCurvatureTrajectory trajectory_{ + breaks_, turning_rates_, tangent_axis_, plane_axis_, initial_position_}; +}; + +TEST_F(CurvilinearMobilizerTest, CanRotateAndTranslate) { + EXPECT_TRUE(mobilizer_->can_rotate()); + EXPECT_TRUE(mobilizer_->can_translate()); +} + +TEST_F(CurvilinearMobilizerTest, StateAccess) { + const double some_value1 = 1.5; + const double some_value2 = std::sqrt(2); + + // Verify we can set position given the model's context. + mobilizer_->SetDistance(context_.get(), some_value1); + EXPECT_EQ(mobilizer_->get_distance(*context_), some_value1); + mobilizer_->SetDistance(context_.get(), some_value2); + EXPECT_EQ(mobilizer_->get_distance(*context_), some_value2); + + // Verify we can set tangential velocity given the model's context. + mobilizer_->SetTangentialVelocity(context_.get(), some_value1); + EXPECT_EQ(mobilizer_->get_tangential_velocity(*context_), some_value1); + mobilizer_->SetTangentialVelocity(context_.get(), some_value2); + EXPECT_EQ(mobilizer_->get_tangential_velocity(*context_), some_value2); +} + +TEST_F(CurvilinearMobilizerTest, ZeroState) { + const double some_value1 = 1.5; + const double some_value2 = std::sqrt(2); + + // Set the state to some arbitrary non-zero value. + mobilizer_->SetDistance(context_.get(), some_value1); + EXPECT_EQ(mobilizer_->get_distance(*context_), some_value1); + mobilizer_->SetTangentialVelocity(context_.get(), some_value2); + EXPECT_EQ(mobilizer_->get_tangential_velocity(*context_), some_value2); + + // Set the "zero state" for this mobilizer, which does happen to be that of + // zero position and velocity. + mobilizer_->SetZeroState(*context_, &context_->get_mutable_state()); + EXPECT_EQ(mobilizer_->get_distance(*context_), 0); + EXPECT_EQ(mobilizer_->get_tangential_velocity(*context_), 0); +} + +TEST_F(CurvilinearMobilizerTest, CalcAcrossMobilizerTransform) { + const double wrapped_distance = 0.5 * M_PI / k_; + const double distance = wrapped_distance + trajectory_.end_time(); + + mobilizer_->SetDistance(context_.get(), distance); + const RigidTransformd X_FM( + mobilizer_->CalcAcrossMobilizerTransform(*context_)); + + // Expect the mobilizer pose to be the trajectory pose, modulo curve length. + const RigidTransformd X_FM_expected = trajectory_.CalcPose(wrapped_distance); + EXPECT_TRUE(CompareMatrices(X_FM.GetAsMatrix34(), + X_FM_expected.GetAsMatrix34(), kTolerance, + MatrixCompareType::relative)); +} + +TEST_F(CurvilinearMobilizerTest, CalcAcrossMobilizerSpatialVelocity) { + const double tangential_velocity = 1.5; + const double wrapped_distance = 0.5 * M_PI / k_; + const double distance = wrapped_distance + trajectory_.end_time(); + mobilizer_->SetDistance(context_.get(), distance); + const SpatialVelocity V_FM = + mobilizer_->CalcAcrossMobilizerSpatialVelocity( + *context_, Vector1d(tangential_velocity)); + + // Expect the mobilizer spatial velocity to be the trajectory spatial + // velocity, modulo curve length. + const SpatialVelocity V_FM_expected = + trajectory_.CalcSpatialVelocity(wrapped_distance, tangential_velocity); + EXPECT_TRUE(V_FM.IsApprox(V_FM_expected, kTolerance)); +} + +TEST_F(CurvilinearMobilizerTest, CalcAcrossMobilizerSpatialAcceleration) { + const double tangential_acceleration = 1.5; + const double tangential_velocity = 1.5; + const double wrapped_distance = 0.5 * M_PI / k_; + const double distance = wrapped_distance + trajectory_.end_time(); + mobilizer_->SetDistance(context_.get(), distance); + mobilizer_->SetTangentialVelocity(context_.get(), tangential_velocity); + const SpatialAcceleration A_FM = + mobilizer_->CalcAcrossMobilizerSpatialAcceleration( + *context_, Vector1d(tangential_acceleration)); + + const SpatialAcceleration A_FM_expected = + trajectory_.CalcSpatialAcceleration(wrapped_distance, tangential_velocity, + tangential_acceleration); + + // Expect the mobilizer spatial acceleration to be the trajectory spatial + // acceleration, modulo curve length. + EXPECT_TRUE(A_FM.IsApprox(A_FM_expected, kTolerance)); +} + +TEST_F(CurvilinearMobilizerTest, ProjectSpatialForce) { + 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; + const double distance = breaks_[1] / 2.; + // Set mobilizer to initial position. + mobilizer_->SetDistance(context_.get(), distance); + mobilizer_->ProjectSpatialForce(*context_, F_Mo_F, tau); + + // Only the torque along affects joint. + // tau_rotational = (torque⋅(rotation axis))⋅turning rate. + const double rotational_component_expected = + k_ * torque_Mo_F.dot(plane_axis_.normalized()); + + // The force should be projected along the tangent (x) axis. + const Vector3d tangent_axis = + trajectory_.CalcPose(distance).rotation().col(0); + const double translational_component_expected = force_Mo_F.dot(tangent_axis); + const double tau_expected = + rotational_component_expected + translational_component_expected; + EXPECT_NEAR(tau(0), tau_expected, kTolerance); +} + +TEST_F(CurvilinearMobilizerTest, MapVelocityToQDotAndBack) { + EXPECT_TRUE(mobilizer_->is_velocity_equal_to_qdot()); + + Vector1d v(1.5); + Vector1d qdot; + mobilizer_->MapVelocityToQDot(*context_, v, &qdot); + EXPECT_NEAR(qdot(0), v(0), kTolerance); + + qdot(0) = -std::sqrt(2); + mobilizer_->MapQDotToVelocity(*context_, qdot, &v); + EXPECT_NEAR(v(0), qdot(0), kTolerance); +} + +TEST_F(CurvilinearMobilizerTest, KinematicMapping) { + // For this joint, Nplus = 1 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(0, 0), 1.0); + + // Compute Nplus. + MatrixX Nplus(1, 1); + mobilizer_->CalcNplusMatrix(*context_, &Nplus); + EXPECT_EQ(Nplus(0, 0), 1.0); +} + +TEST_F(CurvilinearMobilizerTest, MapUsesN) { + // Set an arbitrary non-zero state. + mobilizer_->SetDistance(context_.get(), 1.5); + + // Set arbitrary v and MapVelocityToQDot. + Vector1d v(2.5); + Vector1d qdot; + mobilizer_->MapVelocityToQDot(*context_, v, &qdot); + + // Compute N. + MatrixX N(1, 1); + mobilizer_->CalcNMatrix(*context_, &N); + + // Ensure N(q) is used in q̇ = N(q)⋅v. + EXPECT_EQ(qdot, N * v); +} + +TEST_F(CurvilinearMobilizerTest, MapUsesNplus) { + // Set an arbitrary "non-zero" state. + mobilizer_->SetDistance(context_.get(), 1.5); + + // Set arbitrary q̇ and MapQDotToVelocity. + Vector1d qdot(2.5); + Vector1d v; + mobilizer_->MapQDotToVelocity(*context_, qdot, &v); + + // Compute Nplus. + MatrixX Nplus(1, 1); + mobilizer_->CalcNplusMatrix(*context_, &Nplus); + + // Ensure N⁺(q) is used in v = N⁺(q)⋅q̇. + EXPECT_EQ(v, Nplus * qdot); +} + +} // namespace +} // namespace internal +} // namespace multibody +} // namespace drake From f925d21e195f771524c1b3ebf9b7663513984db0 Mon Sep 17 00:00:00 2001 From: amcastro-tri Date: Wed, 8 Jan 2025 15:25:03 -0500 Subject: [PATCH 2/2] Alejandro's edits --- common/trajectories/BUILD.bazel | 1 + ...piecewise_constant_curvature_trajectory.cc | 18 +- .../piecewise_constant_curvature_trajectory.h | 33 ++- ...wise_constant_curvature_trajectory_test.cc | 12 + multibody/tree/BUILD.bazel | 1 - multibody/tree/curvilinear_joint.cc | 8 +- multibody/tree/curvilinear_joint.h | 31 +-- multibody/tree/curvilinear_mobilizer.cc | 45 +++- multibody/tree/curvilinear_mobilizer.h | 222 +++++------------- .../tree/test/curvilinear_mobilizer_test.cc | 64 ++--- 10 files changed, 182 insertions(+), 253 deletions(-) diff --git a/common/trajectories/BUILD.bazel b/common/trajectories/BUILD.bazel index f4fed5796e05..2499d21f81a6 100644 --- a/common/trajectories/BUILD.bazel +++ b/common/trajectories/BUILD.bazel @@ -155,6 +155,7 @@ drake_cc_library( "//common:essential", "//common:pointer_cast", "//math:geometric_transform", + "//math:wrap_to", "//multibody/math:spatial_algebra", "//systems/framework:system_scalar_converter", ], diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.cc b/common/trajectories/piecewise_constant_curvature_trajectory.cc index ed09d822d48d..2894554101c4 100644 --- a/common/trajectories/piecewise_constant_curvature_trajectory.cc +++ b/common/trajectories/piecewise_constant_curvature_trajectory.cc @@ -10,7 +10,7 @@ template PiecewiseConstantCurvatureTrajectory::PiecewiseConstantCurvatureTrajectory( const std::vector& breaks, const std::vector& turning_rates, const Vector3& initial_curve_tangent, const Vector3& plane_normal, - const Vector3& initial_position) + const Vector3& initial_position, double periodicity_tolerance) : PiecewiseTrajectory(breaks), segment_turning_rates_(turning_rates) { if (turning_rates.size() != breaks.size() - 1) { throw std::logic_error( @@ -37,6 +37,7 @@ PiecewiseConstantCurvatureTrajectory::PiecewiseConstantCurvatureTrajectory( segment_start_poses_ = MakeSegmentStartPoses( MakeInitialPose(initial_curve_tangent, plane_normal, initial_position), breaks, turning_rates); + is_periodic_ = IsNearlyPeriodic(periodicity_tolerance); } template @@ -53,10 +54,11 @@ std::unique_ptr> PiecewiseConstantCurvatureTrajectory::Clone() template math::RigidTransform PiecewiseConstantCurvatureTrajectory::CalcPose( const T& s) const { - int segment_index = this->get_segment_index(s); + const T w = maybe_wrap(s); + int segment_index = this->get_segment_index(w); const math::RigidTransform X_FiF = CalcRelativePoseInSegment(segment_turning_rates_[segment_index], - s - this->start_time(segment_index)); + w - this->start_time(segment_index)); return segment_start_poses_[segment_index] * X_FiF; } @@ -64,8 +66,9 @@ template multibody::SpatialVelocity PiecewiseConstantCurvatureTrajectory::CalcSpatialVelocity( const T& s, const T& s_dot) const { - const math::RotationMatrix R_AF = CalcPose(s).rotation(); - const T& rho_i = segment_turning_rates_[this->get_segment_index(s)]; + const T w = maybe_wrap(s); + const math::RotationMatrix R_AF = CalcPose(w).rotation(); + const T& rho_i = segment_turning_rates_[this->get_segment_index(w)]; multibody::SpatialVelocity spatial_velocity; // From Frenet–Serret analysis and the class doc for @@ -84,8 +87,9 @@ template multibody::SpatialAcceleration PiecewiseConstantCurvatureTrajectory::CalcSpatialAcceleration( const T& s, const T& s_dot, const T& s_ddot) const { - const math::RotationMatrix R_AF = CalcPose(s).rotation(); - const T& rho_i = segment_turning_rates_[this->get_segment_index(s)]; + const T w = maybe_wrap(s); + const math::RotationMatrix R_AF = CalcPose(w).rotation(); + const T& rho_i = segment_turning_rates_[this->get_segment_index(w)]; // The spatial acceleration is the time derivative of the spatial velocity. // We compute the acceleration by applying the chain rule to the formulas diff --git a/common/trajectories/piecewise_constant_curvature_trajectory.h b/common/trajectories/piecewise_constant_curvature_trajectory.h index 4e5ddfa9b4c6..aa082778be64 100644 --- a/common/trajectories/piecewise_constant_curvature_trajectory.h +++ b/common/trajectories/piecewise_constant_curvature_trajectory.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -7,6 +8,7 @@ #include "drake/common/eigen_types.h" #include "drake/common/trajectories/piecewise_trajectory.h" #include "drake/math/rigid_transform.h" +#include "drake/math/wrap_to.h" #include "drake/multibody/math/spatial_algebra.h" #include "drake/systems/framework/scalar_conversion_traits.h" @@ -70,7 +72,8 @@ class PiecewiseConstantCurvatureTrajectory final PiecewiseConstantCurvatureTrajectory() = default; template - using ScalarValueConverter = typename systems::scalar_conversion::template ValueConverter; + using ScalarValueConverter = + typename systems::scalar_conversion::template ValueConverter; /** Constructs a piecewise constant curvature trajectory. @@ -97,6 +100,10 @@ class PiecewiseConstantCurvatureTrajectory final lies, expressed in the parent frame, p̂_A. @param initial_position The initial position of the curve expressed in the parent frame, p_AoFo_A(s₀). + @param periodicity_tolerance Tolerance used to determine if the resulting + trajectory is periodic, according to the metric defined by + IsNearlyPeriodic(). If IsNearlyPeriodic(periodicity_tolerance) is true, then + calling is_periodic() on the new object will return `true`. @throws std::exception if the number of turning rates does not match the number of segments @@ -105,11 +112,12 @@ class PiecewiseConstantCurvatureTrajectory final norm. @throws std::exception if initial_curve_tangent is not perpendicular to plane_normal. */ - PiecewiseConstantCurvatureTrajectory(const std::vector& breaks, - const std::vector& turning_rates, - const Vector3& initial_curve_tangent, - const Vector3& plane_normal, - const Vector3& initial_position); + PiecewiseConstantCurvatureTrajectory( + const std::vector& breaks, const std::vector& turning_rates, + const Vector3& initial_curve_tangent, const Vector3& plane_normal, + const Vector3& initial_position, + double periodicity_tolerance = 1.0e3 * + std::numeric_limits::epsilon()); /** Scalar conversion constructor. See @ref system_scalar_conversion. */ template @@ -126,7 +134,8 @@ class PiecewiseConstantCurvatureTrajectory final .rotation() .col(kPlaneNormalIndex) .unaryExpr(ScalarValueConverter{}), - other.get_initial_pose().translation().unaryExpr(ScalarValueConverter{})) {} + other.get_initial_pose().translation().unaryExpr( + ScalarValueConverter{})) {} /** @returns the number of rows in the output of value(). */ Eigen::Index rows() const override { return 3; } @@ -137,6 +146,9 @@ class PiecewiseConstantCurvatureTrajectory final /** @returns the total arclength of the curve in meters. */ T length() const { return this->end_time(); } + /* Returns `true` if `this` trajectory is periodic. */ + boolean is_periodic() const { return is_periodic_; } + /** Calculates the trajectory's pose X_AF(s) at the given arclength s. @note For s < 0 and s > length() the pose is extrapolated as if the curve @@ -231,6 +243,12 @@ class PiecewiseConstantCurvatureTrajectory final return converted_segment_data; } + /* If the trajectory is periodic, this helper wraps the distance coordinate s + to the path's length. If not periodic, then it simply returns s. */ + T maybe_wrap(const T& s) const { + return is_periodic_ ? math::wrap_to(s, T(0.), length()) : s; + } + /* Calculates pose X_FiF of the frame F at distance ds from the start of the i-th segment, relative to frame Fi at the start of the segment. @@ -292,6 +310,7 @@ class PiecewiseConstantCurvatureTrajectory final std::vector segment_turning_rates_; std::vector> segment_start_poses_; + boolean is_periodic_{false}; static inline constexpr size_t kCurveTangentIndex = 0; static inline constexpr size_t kCurveNormalIndex = 1; diff --git a/common/trajectories/test/piecewise_constant_curvature_trajectory_test.cc b/common/trajectories/test/piecewise_constant_curvature_trajectory_test.cc index 06127f017781..01b561e57882 100644 --- a/common/trajectories/test/piecewise_constant_curvature_trajectory_test.cc +++ b/common/trajectories/test/piecewise_constant_curvature_trajectory_test.cc @@ -207,6 +207,7 @@ GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestRandomizedTrajectory) { GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestPeriodicity) { const double r = 2; + const double length = 2 * M_PI * r; const double kappa = 1 / r; const int num_segments = 10; std::vector segment_breaks_periodic(num_segments + 1); @@ -231,8 +232,19 @@ GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestPeriodicity) { segment_breaks_aperiodic, turning_rates, curve_tangent, plane_normal, Vector3d::Zero()); + EXPECT_TRUE(periodic_trajectory.is_periodic()); EXPECT_TRUE(periodic_trajectory.IsNearlyPeriodic(kTolerance)); + EXPECT_FALSE(aperiodic_trajectory.is_periodic()); EXPECT_FALSE(aperiodic_trajectory.IsNearlyPeriodic(kTolerance)); + + // Test periodicity for at arbitrary value. + const double s = 1.5; + const RigidTransformd X_AF_s0 = periodic_trajectory.CalcPose(s); + const RigidTransformd X_AF_s1 = periodic_trajectory.CalcPose(s + length); + const RigidTransformd X_AF_s3 = + periodic_trajectory.CalcPose(s + 3.0 * length); + EXPECT_TRUE(X_AF_s0.IsNearlyEqualTo(X_AF_s1, kTolerance)); + EXPECT_TRUE(X_AF_s0.IsNearlyEqualTo(X_AF_s3, kTolerance)); } GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestScalarConversion) { diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index b55eaa85061d..6aa1fd61785c 100644 --- a/multibody/tree/BUILD.bazel +++ b/multibody/tree/BUILD.bazel @@ -191,7 +191,6 @@ drake_cc_library( "//common:unused", "//common/trajectories:piecewise_constant_curvature_trajectory", "//math:geometric_transform", - "//math:wrap_to", "//multibody/topology", "//systems/framework:leaf_system", ], diff --git a/multibody/tree/curvilinear_joint.cc b/multibody/tree/curvilinear_joint.cc index ef1f0d5f28f6..acb5a1fa1f02 100644 --- a/multibody/tree/curvilinear_joint.cc +++ b/multibody/tree/curvilinear_joint.cc @@ -18,8 +18,7 @@ CurvilinearJoint::CurvilinearJoint( const Frame& frame_on_child, const trajectories::PiecewiseConstantCurvatureTrajectory curvilinear_path, - bool is_periodic, double pos_lower_limit, double pos_upper_limit, - double damping) + double pos_lower_limit, double pos_upper_limit, double damping) : Joint( name, frame_on_parent, frame_on_child, VectorX::Constant(1, damping), @@ -32,8 +31,7 @@ CurvilinearJoint::CurvilinearJoint( -std::numeric_limits::infinity()), VectorX::Constant(1, std::numeric_limits::infinity())), - curvilinear_path_(curvilinear_path), - is_periodic_(is_periodic) {} + curvilinear_path_(curvilinear_path) {} template CurvilinearJoint::~CurvilinearJoint() = default; @@ -56,7 +54,7 @@ std::unique_ptr> CurvilinearJoint::TemplatedDoCloneToScalar( // Make the Joint clone. auto joint_clone = std::make_unique>( this->name(), frame_on_parent_body_clone, frame_on_child_body_clone, - curvilinear_path_, is_periodic_, this->position_lower_limit(), + curvilinear_path_, this->position_lower_limit(), this->position_upper_limit(), this->default_damping()); joint_clone->set_velocity_limits(this->velocity_lower_limits(), diff --git a/multibody/tree/curvilinear_joint.h b/multibody/tree/curvilinear_joint.h index bfe011b1172a..afd6b43aa1a7 100644 --- a/multibody/tree/curvilinear_joint.h +++ b/multibody/tree/curvilinear_joint.h @@ -64,8 +64,6 @@ class CurvilinearJoint final : public Joint { The additional parameters are: @param[in] curvilinear_path The curvilinear path for this joint, along which the child frame M moves relative to the parent frame F. - @param[in] is_periodic A boolean indicating whether the curvilinear path is - periodic. @param[in] damping Viscous damping coefficient, in N⋅s/m, used to model losses within the joint. The damping force (in N) is modeled as `f = -damping⋅v`, i.e. opposing motion, with v the tangential velocity for `this` @@ -74,15 +72,15 @@ class CurvilinearJoint final : public Joint { CurvilinearJoint( const std::string& name, const Frame& frame_on_parent, const Frame& frame_on_child, - const trajectories::PiecewiseConstantCurvatureTrajectory - curvilinear_path, - bool is_periodic, double damping = 0) + const trajectories::PiecewiseConstantCurvatureTrajectory& + trajectory, + double damping = 0) : CurvilinearJoint( - name, frame_on_parent, frame_on_child, curvilinear_path, - is_periodic, - is_periodic ? -std::numeric_limits::infinity() : 0., - is_periodic ? std::numeric_limits::infinity() - : curvilinear_path.length(), + name, frame_on_parent, frame_on_child, trajectory, + trajectory.is_periodic() ? -std::numeric_limits::infinity() + : 0., + trajectory.is_periodic() ? std::numeric_limits::infinity() + : trajectory.length(), damping) {} /** Constructor to create a curvilinear joint between two bodies so that @@ -93,10 +91,8 @@ class CurvilinearJoint final : public Joint { 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] curvilinear_path The curvilinear path for this joint, along which + @param[in] trajectory The curvilinear path for this joint, along which the child frame M moves relative to the parent frame F. - @param[in] is_periodic A boolean indicating whether the curvilinear path is - periodic. @param[in] pos_lower_limit Lower position limit, in meters, for the distance coordinate (see get_distance()). @param[in] pos_upper_limit Upper position limit, in meters, for the distance @@ -111,9 +107,8 @@ class CurvilinearJoint final : public Joint { const std::string& name, const Frame& frame_on_parent, const Frame& frame_on_child, const trajectories::PiecewiseConstantCurvatureTrajectory - curvilinear_path, - bool is_periodic, double pos_lower_limit, double pos_upper_limit, - double damping = 0); + trajectory, + double pos_lower_limit, double pos_upper_limit, double damping = 0); ~CurvilinearJoint() override; @@ -346,8 +341,7 @@ class CurvilinearJoint final : public Joint { // TODO(sherm1) The mobilizer needs to be reversed, not just the frames. auto curvilinear_mobilizer = std::make_unique>( - mobod, *inboard_frame, *outboard_frame, curvilinear_path_, - is_periodic_); + mobod, *inboard_frame, *outboard_frame, curvilinear_path_); curvilinear_mobilizer->set_default_position(this->default_positions()); blue_print->mobilizer = std::move(curvilinear_mobilizer); return blue_print; @@ -390,7 +384,6 @@ class CurvilinearJoint final : public Joint { const internal::MultibodyTree& tree_clone) const; trajectories::PiecewiseConstantCurvatureTrajectory curvilinear_path_; - bool is_periodic_; }; template diff --git a/multibody/tree/curvilinear_mobilizer.cc b/multibody/tree/curvilinear_mobilizer.cc index 2c180f6d90d0..24e54ccdd975 100644 --- a/multibody/tree/curvilinear_mobilizer.cc +++ b/multibody/tree/curvilinear_mobilizer.cc @@ -10,6 +10,14 @@ namespace drake { namespace multibody { namespace internal { +template +CurvilinearMobilizer::CurvilinearMobilizer( + const SpanningForest::Mobod& mobod, const Frame& inboard_frame_F, + const Frame& outboard_frame_M, + const PiecewiseConstantCurvatureTrajectory& curvilinear_path) + : MobilizerBase(mobod, inboard_frame_F, outboard_frame_M), + curvilinear_path_(curvilinear_path) {} + template CurvilinearMobilizer::~CurvilinearMobilizer() = default; @@ -73,6 +81,40 @@ const CurvilinearMobilizer& CurvilinearMobilizer::SetTangentialVelocity( return *this; } +template +math::RigidTransform CurvilinearMobilizer::calc_X_FM(const T* q) const { + return curvilinear_path_.CalcPose(*q); +} + +template +SpatialVelocity CurvilinearMobilizer::calc_V_FM(const T* q, + const T* v) const { + return curvilinear_path_.CalcSpatialVelocity(*q, *v); +} + +template +SpatialAcceleration CurvilinearMobilizer::calc_A_FM(const T* q, + const T* v, + const T* vdot) const { + return curvilinear_path_.CalcSpatialAcceleration(*q, *v, *vdot); +} + +template +void CurvilinearMobilizer::calc_tau(const T* q, + const SpatialForce& F_BMo_F, + T* tau) const { + DRAKE_ASSERT(tau != nullptr); + + /* For this mobilizer, H_FM(q) = d/dv V_FM_F(q, v) is numerically equal to the + spatial velocity V_FM_F(q, 1) evaluated at the unit velocity v = 1 [m/s]: + + tau = F_BMo_F.dot(V_FM_F(q, 1)) + */ + const T v(1.); + // Computes tau = H_FM(q)⋅F_Mo_F, equivalent to V_FM(q, 1)⋅F_Mo_F. + tau[0] = calc_V_FM(q, &v).dot(F_BMo_F); +} + template math::RigidTransform CurvilinearMobilizer::CalcAcrossMobilizerTransform( const drake::systems::Context& context) const { @@ -154,8 +196,7 @@ CurvilinearMobilizer::TemplatedDoCloneToScalar( return std::make_unique>( tree_clone.get_mobod(this->mobod().index()), inboard_frame_clone, outboard_frame_clone, - PiecewiseConstantCurvatureTrajectory(curvilinear_path_), - is_periodic_); + PiecewiseConstantCurvatureTrajectory(curvilinear_path_)); } template diff --git a/multibody/tree/curvilinear_mobilizer.h b/multibody/tree/curvilinear_mobilizer.h index 2038d34e6b15..0b721fc0ea64 100644 --- a/multibody/tree/curvilinear_mobilizer.h +++ b/multibody/tree/curvilinear_mobilizer.h @@ -22,43 +22,38 @@ namespace drake { namespace multibody { namespace internal { -// Tolerance used to check joint periodicity. -static constexpr double kCurvilinearJointPeriodicityTolerance = - 1e3 * std::numeric_limits::epsilon(); - -/** A Mobilizer which allows two frames to translate and rotate relatively to - one another along a curvilinear path composed of line segments and circular - arcs within a plane. - - The path is specified as a PiecewiseConstantCurvatureTrajectory. - - The mobilizer may be specified to be periodic, representing a path shaped as a - closed loop. In this case, the path must return to the starting pose at its end - distance s_f [m]. The transfrom X_FM(q) from the CurvilinearMobilizer's inboard - frame F to the outboard frame M in this case is periodic with period s_f - (X_FM(q) = X_FM(q + s_f)). - - The single generalized coordinate q [m] introduced by this mobilizer - corresponds to the distance of travel along the path. The transfrom X_FM(q) - from the CurvilinearMobilizer's inboard frame F to the outboard frame M is set - to be the path-aligned pose at distance s(q) [m], available through - PiecewiseConstantCurvatureTrajectory::CalcPose. For aperiodic trajectories, - s(q) = q. For periodic trajectories, s(q) is wrapped using modular arithmetic, - i.e. s(q) = q - s_f⋅floor(q/s_f) [m]. The generalized velocity v = q̇ [m/s] is - the tangential velocity along the path. - - The path lies within a plane with normal axis p̂, equal to the z axis of the - mobilized frame Mz. p̂ is not necessarily equal to Fz, but p̂_F is constant. - - At any given point along the path, a scalar turning rate ρ(q) [1/m] is defined - via right-hand rule about the planar axis p̂, such that the angular velocity - across the mobilizer is w_FM = ρ⋅p̂⋅v. The hinge matrix and time derivative - are - - H_FM₆ₓ₁(q) = [ Mx_F(q), Hdot_FM₆ₓ₁(q) = [ρ(q)⋅My_F(q)⋅v, - ρ(q)⋅Mz_F(q)], 0]. - - @tparam_default_scalar */ +/** 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 + class documentation for further details on parameterization, conventions and + 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 + 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. + 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)| + corresponds to the path's curvature and its sign is defined via the right-hand + rule about the plane's normal Mz. + + 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)]. + +where the angular component of Hdot_FM is zero since ρ(q) is constant within +each piecewise segment in PiecewiseConstantCurvatureTrajectory. + + @tparam_default_scalar */ template class CurvilinearMobilizer final : public MobilizerImpl { public: @@ -72,38 +67,22 @@ class CurvilinearMobilizer final : public MobilizerImpl { template using HMatrix = typename MobilizerBase::template HMatrix; - /** Constructor for a CurvilinearMobilizer between the inboard frame F - and the outboard frame M granting a single degree of freedom expressed by the - pose X_FM(q) of the path `curvilinear_path` at distance q [m] along the path. - The path is required to be a closed loop if the parameter `is_periodic` is - set to `true`. - - @param mobod information for the mobilized body attached to frame M - @param inboard_frame_F the inboard frame F - @param outboard_frame_M the outboard frame M - @param curvilinear_path the curvilinear path defining X_FM - @param is_periodic if true, the mobilizer is periodic, and the path must be a - closed loop. - - @throws std::exception if `is_periodic` is true, but `curvilinear_path` is - not (nearly) periodic. */ + /** 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. + + `curvilinear_path` provides the pose X_FM(s) along a piecewise constant + curvature trajectory. In particular, X_FM(s=0) does not need to be identity + matrix. + + @param mobod Topological information for this mobilizer. + @param inboard_frame_F the inboard frame F. + @param outboard_frame_M the outboard frame M. + @param curvilinear_path the curvilinear path defining X_FM(q). */ CurvilinearMobilizer( const SpanningForest::Mobod& mobod, const Frame& inboard_frame_F, const Frame& outboard_frame_M, - const PiecewiseConstantCurvatureTrajectory& curvilinear_path, - bool is_periodic) - : MobilizerBase(mobod, inboard_frame_F, outboard_frame_M), - curvilinear_path_(curvilinear_path), - is_periodic_(is_periodic) { - bool path_is_periodic = curvilinear_path.IsNearlyPeriodic( - kCurvilinearJointPeriodicityTolerance); - if (is_periodic_ && !path_is_periodic) { - // The path not periodic, but the mobilizer is. - throw std::runtime_error( - "CurvilinearMobilizer: Periodic mobilizer must be constructed with " - "periodic path."); - } - } + const PiecewiseConstantCurvatureTrajectory& curvilinear_path); ~CurvilinearMobilizer() final; @@ -145,34 +124,11 @@ class CurvilinearMobilizer final : public MobilizerImpl { const CurvilinearMobilizer& SetTangentialVelocity( systems::Context* context, const T& tangential_velocity) const; - /** Calculates the distance along the path s(q) [m] associated with a - generalized coordinate q [m]. - - For aperiodic paths, this function is the identity (s(q) = q). - - For periodic paths of length s_f [m], the mobilizer returns to the start of - the path after each full length of travel, meaning that s must be "wrapped" - to the path domain [0, s_f): - - 
-      s(q) = q mod s_f.
-
- - @param q The generalized coordinate of the mobilizer. - @returns The position s along the path associated with q. */ - T calc_s(const T& q) const { - return is_periodic_ ? math::wrap_to(q, T(0.), curvilinear_path_.length()) - : q; - } - /** 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 { - const T s = calc_s(q[0]); - return curvilinear_path_.CalcPose(s); - } + math::RigidTransform calc_X_FM(const T* q) const; /** 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. @@ -180,10 +136,7 @@ class CurvilinearMobilizer final : public MobilizerImpl { @param v The tangential velocity along the mobilizer's path in meters per second. @returns The across-mobilizer spatial velocity V_FM(q, v). */ - SpatialVelocity calc_V_FM(const T* q, const T* v) const { - const T s = calc_s(q[0]); - return curvilinear_path_.CalcSpatialVelocity(s, v[0]); - } + SpatialVelocity calc_V_FM(const T* q, const T* v) const; /** Computes the across-mobilizer spatial acceleration A_FM(q, v, v̇) as a function of the distance traveled, tangential velocity, and tangential @@ -194,94 +147,34 @@ class CurvilinearMobilizer final : public MobilizerImpl { @param vdot The tangential acceleration along the mobilizer's path in meters per second squared. @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 { - const T s = calc_s(q[0]); - return curvilinear_path_.CalcSpatialAcceleration(s, v[0], vdot[0]); - } + 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 - frame M onto the path tangent. Mathematically, - this is the dot product of `F_Mo_F` with the spatial derivative of - the path w.r.t. distance traveled q: F_Mo_F⋅H_FM. - - For this mobilizer, H_FM(q) = d/dv V_FM_F(q, v) is numerically equal to the - spatial velocity V_FM_F(q, 1) evaluated at the unit velocity v = 1 [m/s]: - - 
-      tau = F_BMo_F.dot(V_FM_F(q, 1))
-
- - The result of this method is the scalar equivalent to - a force applied on the path, pointed along the path's tangential axis and - measured in Newtons. + 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 + the path that would cause the same acceleration than F_Mo_F. @param q The distance traveled along the mobilizer's path in meters. - @param F_BMo_F The spatial force applied to the child body at Mo, expressed - in F. + @param F_BMo_F The spatial force applied to the mobilized body B at Mo, + expressed in F. @param[out] tau A pointer to store the resulting generalized force in Newtons. - */ - void calc_tau(const T* q, const SpatialForce& F_BMo_F, T* tau) const { - DRAKE_ASSERT(tau != nullptr); - const T v(1.); - // Computes tau = H_FM(q)⋅F_Mo_F, equivalent to V_FM(q, 1)⋅F_Mo_F. - tau[0] = calc_V_FM(q, &v).dot(F_BMo_F); - } - - /** Calculates and stores the across-mobilizer transform X_FM given - the generalized coordinate stored in a given context. - @param context The context of the model this mobilizer belongs to. - @returns The across-mobilizer transform X_FM(q). */ + @pre tau is not the nullptr. + @pre tau->size() equals one. */ + void calc_tau(const T* q, const SpatialForce& F_BMo_F, T* tau) const; + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; - /** Calculates and stores the across-mobilizer spatial velocity V_FM given - the generalized coordinate stored in a given context and a provided - tangential velocity. - @param context The context of the model this mobilizer belongs to. - @param v The tangential velocity along the mobilizer's path in meters per - second. - @returns The across-mobilizer spatial velocity V_FM(q, v). - @note This method aborts in Debug builds if v.size() is not one. */ SpatialVelocity CalcAcrossMobilizerSpatialVelocity( const systems::Context& context, const Eigen::Ref>& v) const final; - /** Calculates and stores the across-mobilizer spatial acceleration A_FM given - the generalized coordinate and velocity stored in a given context and a - provided tangential acceleration. - @param context The context of the model this mobilizer belongs to. - @param vdot The tangential acceleration along the mobilizer's path in meters - per second squared. - @returns The across-mobilizer spatial acceleration A_FM(q, v, v̇). - @note This method aborts in Debug builds if vdot.size() is not one. */ SpatialAcceleration CalcAcrossMobilizerSpatialAcceleration( const systems::Context& context, const Eigen::Ref>& vdot) const override; - /** Projects the spatial force `F_Mo_F` on `this` mobilizer's outboard - frame M onto the path tangent. Mathematically, - this is the dot product of `F_Mo_F` with the spatial derivative of - the path w.r.t. distance traveled q: F_Mo_F⋅H_FM. - - For this mobilizer, H_FM(q) = d/dv V_FM(q, v) is numerically equal to the - spatial velocity V_FM(q, 1) evaluated at the unit velocity v = 1 [m/s]: - - 
-      tau = F_Mo_F.dot(V_FM(q, 1)).
-
- - The result of this method is the scalar equivalent to - a force applied on the path, pointed along the path's tangential axis and - measured in Newtons. - @param context The context of the model this mobilizer belongs to. - @param F_BMo_F The spatial force applied to the child body at Mo, expressed - in F. - @param[out] tau A reference to store the resulting generalized force in - Newtons. - @note This method aborts in Debug builds if `tau.size()` is not one.*/ void ProjectSpatialForce(const systems::Context& context, const SpatialForce& F_BMo_F, Eigen::Ref> tau) const override; @@ -318,7 +211,6 @@ class CurvilinearMobilizer final : public MobilizerImpl { const MultibodyTree& tree_clone) const; PiecewiseConstantCurvatureTrajectory curvilinear_path_; - bool is_periodic_; }; } // namespace internal diff --git a/multibody/tree/test/curvilinear_mobilizer_test.cc b/multibody/tree/test/curvilinear_mobilizer_test.cc index 8c85427a7e15..bf5ef2221bb4 100644 --- a/multibody/tree/test/curvilinear_mobilizer_test.cc +++ b/multibody/tree/test/curvilinear_mobilizer_test.cc @@ -29,7 +29,7 @@ using std::sqrt; using std::unique_ptr; using systems::Context; -constexpr double kTolerance = 10 * std::numeric_limits::epsilon(); +constexpr double kEpsilon = std::numeric_limits::epsilon(); // Fixture to setup a simple model containing a curvilinear mobilizer. class CurvilinearMobilizerTest : public MobilizerTester { @@ -37,16 +37,20 @@ class CurvilinearMobilizerTest : public MobilizerTester { void SetUp() override { std::unique_ptr> joint = std::make_unique>( - "joint1=0", tree().world_body().body_frame(), body_->body_frame(), - trajectory_, true); + "joint1", tree().world_body().body_frame(), body_->body_frame(), + trajectory_); mobilizer_ = &AddJointAndFinalize( std::move(joint)); } protected: const CurvilinearMobilizer* mobilizer_{nullptr}; - const double k_ = 1.0; - const double l_ = 1.0; + // We specify a trajectory that draws a "stadium curve" (a rectangle with two + // semicircle caps). The Rectangular section has length l_ and the circular + // sections have radii 1/k_. This trajectory is periodic, of total length 2*l_ + // + 2*pi/k_. + const double k_ = 1.0; // Rectangular region length. + const double l_ = 1.0; // Radius of the circular end caps. const std::vector breaks_{0, M_PI / k_, l_ + M_PI / k_, l_ + 2 * M_PI / k_, 2 * l_ + 2 * M_PI / k_}; const std::vector turning_rates_{k_, 0, k_, 0}; @@ -107,7 +111,7 @@ TEST_F(CurvilinearMobilizerTest, CalcAcrossMobilizerTransform) { // Expect the mobilizer pose to be the trajectory pose, modulo curve length. const RigidTransformd X_FM_expected = trajectory_.CalcPose(wrapped_distance); EXPECT_TRUE(CompareMatrices(X_FM.GetAsMatrix34(), - X_FM_expected.GetAsMatrix34(), kTolerance, + X_FM_expected.GetAsMatrix34(), kEpsilon, MatrixCompareType::relative)); } @@ -124,7 +128,7 @@ TEST_F(CurvilinearMobilizerTest, CalcAcrossMobilizerSpatialVelocity) { // velocity, modulo curve length. const SpatialVelocity V_FM_expected = trajectory_.CalcSpatialVelocity(wrapped_distance, tangential_velocity); - EXPECT_TRUE(V_FM.IsApprox(V_FM_expected, kTolerance)); + EXPECT_TRUE(V_FM.IsApprox(V_FM_expected, kEpsilon)); } TEST_F(CurvilinearMobilizerTest, CalcAcrossMobilizerSpatialAcceleration) { @@ -144,7 +148,7 @@ TEST_F(CurvilinearMobilizerTest, CalcAcrossMobilizerSpatialAcceleration) { // Expect the mobilizer spatial acceleration to be the trajectory spatial // acceleration, modulo curve length. - EXPECT_TRUE(A_FM.IsApprox(A_FM_expected, kTolerance)); + EXPECT_TRUE(A_FM.IsApprox(A_FM_expected, kEpsilon)); } TEST_F(CurvilinearMobilizerTest, ProjectSpatialForce) { @@ -152,8 +156,8 @@ TEST_F(CurvilinearMobilizerTest, ProjectSpatialForce) { const Vector3d force_Mo_F(1.0, 2.0, 3.0); const SpatialForce F_Mo_F(torque_Mo_F, force_Mo_F); Vector1d tau; - const double distance = breaks_[1] / 2.; - // Set mobilizer to initial position. + const double distance = breaks_[1] / 2.3; + // Set an arbitrary position. mobilizer_->SetDistance(context_.get(), distance); mobilizer_->ProjectSpatialForce(*context_, F_Mo_F, tau); @@ -168,7 +172,7 @@ TEST_F(CurvilinearMobilizerTest, ProjectSpatialForce) { const double translational_component_expected = force_Mo_F.dot(tangent_axis); const double tau_expected = rotational_component_expected + translational_component_expected; - EXPECT_NEAR(tau(0), tau_expected, kTolerance); + EXPECT_NEAR(tau(0), tau_expected, 4.0 * kEpsilon); } TEST_F(CurvilinearMobilizerTest, MapVelocityToQDotAndBack) { @@ -177,11 +181,11 @@ TEST_F(CurvilinearMobilizerTest, MapVelocityToQDotAndBack) { Vector1d v(1.5); Vector1d qdot; mobilizer_->MapVelocityToQDot(*context_, v, &qdot); - EXPECT_NEAR(qdot(0), v(0), kTolerance); + EXPECT_EQ(qdot(0), v(0)); qdot(0) = -std::sqrt(2); mobilizer_->MapQDotToVelocity(*context_, qdot, &v); - EXPECT_NEAR(v(0), qdot(0), kTolerance); + EXPECT_EQ(v(0), qdot(0)); } TEST_F(CurvilinearMobilizerTest, KinematicMapping) { @@ -202,40 +206,6 @@ TEST_F(CurvilinearMobilizerTest, KinematicMapping) { EXPECT_EQ(Nplus(0, 0), 1.0); } -TEST_F(CurvilinearMobilizerTest, MapUsesN) { - // Set an arbitrary non-zero state. - mobilizer_->SetDistance(context_.get(), 1.5); - - // Set arbitrary v and MapVelocityToQDot. - Vector1d v(2.5); - Vector1d qdot; - mobilizer_->MapVelocityToQDot(*context_, v, &qdot); - - // Compute N. - MatrixX N(1, 1); - mobilizer_->CalcNMatrix(*context_, &N); - - // Ensure N(q) is used in q̇ = N(q)⋅v. - EXPECT_EQ(qdot, N * v); -} - -TEST_F(CurvilinearMobilizerTest, MapUsesNplus) { - // Set an arbitrary "non-zero" state. - mobilizer_->SetDistance(context_.get(), 1.5); - - // Set arbitrary q̇ and MapQDotToVelocity. - Vector1d qdot(2.5); - Vector1d v; - mobilizer_->MapQDotToVelocity(*context_, qdot, &v); - - // Compute Nplus. - MatrixX Nplus(1, 1); - mobilizer_->CalcNplusMatrix(*context_, &Nplus); - - // Ensure N⁺(q) is used in v = N⁺(q)⋅q̇. - EXPECT_EQ(v, Nplus * qdot); -} - } // namespace } // namespace internal } // namespace multibody