diff --git a/multibody/benchmarking/cassie.cc b/multibody/benchmarking/cassie.cc index 7e9a7f083335..3f14cd364b4e 100644 --- a/multibody/benchmarking/cassie.cc +++ b/multibody/benchmarking/cassie.cc @@ -146,6 +146,16 @@ class Cassie : public benchmark::Fixture { } } + // Runs the InverseDynamics2 benchmark. + // NOLINTNEXTLINE(runtime/references) + void DoInverseDynamics2(benchmark::State& state) { + DRAKE_DEMAND(want_grad_u(state) == false); + for (auto _ : state) { + InvalidateState(); + plant_->CalcInverseDynamics2(*context_, desired_vdot_, external_forces_); + } + } + // Runs the ForwardDynamics benchmark. // NOLINTNEXTLINE(runtime/references) void DoForwardDynamics(benchmark::State& state) { @@ -327,6 +337,14 @@ BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics) ->Unit(benchmark::kMicrosecond) ->Arg(kWantNoGrad); +// NOLINTNEXTLINE(runtime/references) +BENCHMARK_DEFINE_F(CassieDouble, InverseDynamics2)(benchmark::State& state) { + DoInverseDynamics2(state); +} +BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics2) + ->Unit(benchmark::kMicrosecond) + ->Arg(kWantNoGrad); + // NOLINTNEXTLINE(runtime/references) BENCHMARK_DEFINE_F(CassieDouble, ForwardDynamics)(benchmark::State& state) { DoForwardDynamics(state); @@ -383,6 +401,21 @@ BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics) ->Arg(kWantGradV|kWantGradVdot) ->Arg(kWantGradX|kWantGradVdot); +// NOLINTNEXTLINE(runtime/references) +BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamics2)(benchmark::State& state) { + DoInverseDynamics2(state); +} +BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics2) + ->Unit(benchmark::kMicrosecond) + ->Arg(kWantNoGrad) + ->Arg(kWantGradQ) + ->Arg(kWantGradV) + ->Arg(kWantGradX) + ->Arg(kWantGradVdot) + ->Arg(kWantGradQ|kWantGradVdot) + ->Arg(kWantGradV|kWantGradVdot) + ->Arg(kWantGradX|kWantGradVdot); + // NOLINTNEXTLINE(runtime/references) BENCHMARK_DEFINE_F(CassieAutoDiff, ForwardDynamics)(benchmark::State& state) { DoForwardDynamics(state); diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index 91231a496819..27095b336912 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -3871,6 +3871,14 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { external_forces); } + VectorX CalcInverseDynamics2( + const systems::Context& context, const VectorX& known_vdot, + const MultibodyForces& external_forces) const { + this->ValidateContext(context); + return internal_tree().CalcInverseDynamics2(context, known_vdot, + external_forces); + } + #ifdef DRAKE_DOXYGEN_CXX // MultibodyPlant uses the NVI implementation of // CalcImplicitTimeDerivativesResidual from diff --git a/multibody/plant/test/external_forces_test.cc b/multibody/plant/test/external_forces_test.cc index 668ca74bb715..b290336a96cd 100644 --- a/multibody/plant/test/external_forces_test.cc +++ b/multibody/plant/test/external_forces_test.cc @@ -1,14 +1,12 @@ #include -#include #include -#include "drake/common/autodiff.h" +#include "drake/common/fmt_eigen.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" #include "drake/common/test_utilities/limit_malloc.h" #include "drake/multibody/plant/test/kuka_iiwa_model_tests.h" #include "drake/multibody/tree/frame.h" -#include "drake/multibody/tree/rigid_body.h" namespace drake { @@ -41,6 +39,9 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) { end_effector_link_->body_frame(), &forces); const VectorX tau_id = plant_->CalcInverseDynamics(*context_, vdot, forces); + const VectorX tau_id2 = + plant_->CalcInverseDynamics2(*context_, vdot, forces); + { // Repeat the computation to confirm the heap behavior. We allow the // method to heap-allocate 2 temporaries and 1 return value. drake::test::LimitMalloc guard({.max_num_allocations = 3}); @@ -75,6 +76,10 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) { const double kTolerance = 50 * std::numeric_limits::epsilon(); EXPECT_TRUE(CompareMatrices(tau_id, tau_id_expected, kTolerance, MatrixCompareType::relative)); + // W & M frame ID should be very close. + EXPECT_TRUE(CompareMatrices(tau_id2, tau_id, + 16 * std::numeric_limits::epsilon(), + MatrixCompareType::relative)); } TEST_F(KukaIiwaModelTests, BodyForceApi) { diff --git a/multibody/tree/body_node.h b/multibody/tree/body_node.h index a9e304a2d72e..006405ac6c76 100644 --- a/multibody/tree/body_node.h +++ b/multibody/tree/body_node.h @@ -241,6 +241,10 @@ class BodyNode : public MultibodyElement { const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, PositionKinematicsCache* pc) const = 0; + virtual void CalcPositionKinematicsCache2_BaseToTip( + const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, + PositionKinematicsCache2* pc2) const = 0; + // Calculates the hinge matrix H_PB_W, the `6 x nm` hinge matrix that relates // V_PB_W`(body B's spatial velocity in its parent body P, expressed in world // W) to this node's nm generalized velocities (or mobilities) v_B as @@ -290,6 +294,10 @@ class BodyNode : public MultibodyElement { const std::vector>& H_PB_W_cache, const T* velocities, VelocityKinematicsCache* vc) const = 0; + virtual void CalcVelocityKinematicsCache2_BaseToTip( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, VelocityKinematicsCache2* vc2) const = 0; + // The CalcMassMatrix() algorithm invokes this on each body k, serving // as the composite body R(k) in the outer loop of Jain's algorithm 9.3. // This node must fill in its nv x nv diagonal block in M, and then @@ -373,6 +381,12 @@ class BodyNode : public MultibodyElement { const VelocityKinematicsCache* vc, const T* accelerations, std::vector>* A_WB_array) const = 0; + virtual void CalcSpatialAcceleration2_BaseToTip( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, const VelocityKinematicsCache2& vc2, + const T* accelerations, + std::vector>* A_WM_M_array) const = 0; + // Computes the generalized forces `tau` for a single BodyNode. // This method is used by MultibodyTree within a tip-to-base loop to compute // the vector of generalized forces `tau` that would correspond with a known @@ -432,6 +446,17 @@ class BodyNode : public MultibodyElement { std::vector>* F_BMo_W_array, EigenPtr> tau_array) const = 0; + virtual void CalcInverseDynamics2_TipToBase( + const FrameBodyPoseCache& frame_body_pose_cache, // M_BMo_M, X_BM + const T* positions, + const PositionKinematicsCache2& pc, // X_MpM, X_WB + const VelocityKinematicsCache2& vc, // V_WM_M + const std::vector>& A_WM_M_array, + const std::vector>& Fapplied_Bo_W_array, // Bo, W ! + const Eigen::Ref>& tau_applied_array, + std::vector>* F_BMo_M_array, + EigenPtr> tau_array) const = 0; + // This method is used by MultibodyTree within a tip-to-base loop to compute // this node's articulated body inertia quantities that depend only on the // generalized positions. diff --git a/multibody/tree/body_node_impl.cc b/multibody/tree/body_node_impl.cc index 877f9068fb48..08d388e20725 100644 --- a/multibody/tree/body_node_impl.cc +++ b/multibody/tree/body_node_impl.cc @@ -50,6 +50,40 @@ void BodyNodeImpl::CalcPositionKinematicsCache_BaseToTip( pc); } +// Calculate and save X_FM, X_MpM, X_WM +template class ConcreteMobilizer> +void BodyNodeImpl::CalcPositionKinematicsCache2_BaseToTip( + const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, + PositionKinematicsCache2* pc2) const { + DRAKE_ASSERT(pc2 != nullptr); + const MobodIndex index = mobod_index(); + DRAKE_ASSERT(index != world_mobod_index()); + + const T* q = get_q(positions); + + // Calculate and store X_FM. + math::RigidTransform& X_FM = pc2->get_mutable_X_FM(mobod_index()); + X_FM = mobilizer_->calc_X_FM(q); + + // Get precalculated frame info. + // This mobilizer's inboard frame, fixed on inboard (parent) body. + const FrameIndex index_F = inboard_frame().index(); + const bool X_MpF_is_identity = + frame_body_pose_cache.is_X_MbF_identity(index_F); + const math::RigidTransform& X_MpF = + frame_body_pose_cache.get_X_MbF(index_F); + + // Calculate and store X_MpM. + math::RigidTransform& X_MpM = pc2->get_mutable_X_MpM(mobod_index()); + X_MpM = X_MpF_is_identity ? X_FM : X_MpF * X_FM; // 0 or 63 flops + + // Calculate and store X_WM. + // Inboard's M frame (Mp) pose is known since we're going base to tip. + const math::RigidTransform& X_WMp = pc2->get_X_WM(inboard_mobod_index()); + math::RigidTransform& X_WM = pc2->get_mutable_X_WM(mobod_index()); + X_WM = X_WMp * X_MpM; // 63 flops +} + // TODO(sherm1) Consider combining this with VelocityCache computation // so that we don't have to make a separate pass. Or better, get rid of this // computation altogether by working in better frames. @@ -213,6 +247,41 @@ void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( get_mutable_V_WB(vc) = V_WP.ComposeWithMovingFrameVelocity(p_PB_W, V_PB_W); } +// Calculate V_FM_M and V_WM_M. +template class ConcreteMobilizer> +void BodyNodeImpl::CalcVelocityKinematicsCache2_BaseToTip( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, VelocityKinematicsCache2* vc2) const { + DRAKE_ASSERT(vc2 != nullptr); + const MobodIndex index = mobod_index(); + DRAKE_ASSERT(index != world_mobod_index()); + + // Generalized coordinates local to this node's mobilizer. + const T* q = get_q(positions); + const T* v = get_v(velocities); + + const math::RigidTransform& X_FM = pc2.get_X_FM(index); + + SpatialVelocity& V_FM_M = vc2->get_mutable_V_FM_M(index); + V_FM_M = mobilizer_->calc_V_FM_M(X_FM, q, v); // 3 flops: revolute/prismatic + // 30 flops: floating + + const math::RigidTransform& X_MpM = pc2.get_X_MpM(index); + const Vector3& p_MpM_Mp = X_MpM.translation(); // Shift vector + const math::RotationMatrix R_MMp = X_MpM.rotation().inverse(); + + // We're going base to tip so we know the inboard body's M-frame velocity. + const SpatialVelocity& V_WMp_Mp = vc2->get_V_WM_M(inboard_mobod_index()); + + // Let Mc be a frame fixed to parent body P but coincident with the child + // body's M frame. Then this is the spatial velocity of Mp, shifted to Mc, + // but still expressed in the Mp frame. + const SpatialVelocity V_WMc_Mp = V_WMp_Mp.Shift(p_MpM_Mp); // 15 flops + + SpatialVelocity& V_WM_M = vc2->get_mutable_V_WM_M(index); + V_WM_M = R_MMp * V_WMc_Mp + V_FM_M; // 36 flops +} + // As a guideline for developers, a summary of the computations performed in // this method is provided: // Notation: @@ -319,7 +388,7 @@ void BodyNodeImpl::CalcSpatialAcceleration_BaseToTip( // N.B. It is possible for positions & velocities to be null here if // the system consists only of welds (one of our unit tests does that). - // Operator A_FM = H_FM * vmdot + Hdot_FM * vm + // Operator A_FM = H_FM(qₘ)⋅v̇ₘ + Ḣ_FM(qₘ,vₘ)⋅vₘ const SpatialAcceleration A_FM = mobilizer_->calc_A_FM( get_q(positions), get_v(velocities), get_v(accelerations)); @@ -342,6 +411,59 @@ void BodyNodeImpl::CalcSpatialAcceleration_BaseToTip( V_PB_W, A_PB_W); } +template class ConcreteMobilizer> +void BodyNodeImpl::CalcSpatialAcceleration2_BaseToTip( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, const VelocityKinematicsCache2& vc2, + const T* accelerations, + std::vector>* A_WM_M_array) const { + const MobodIndex index = mobod_index(); // mobod B + + // Collect the inputs. + const math::RigidTransform& X_MpM = pc2.get_X_MpM(index); + const SpatialVelocity& V_WM_M = vc2.get_V_WM_M(index); + const T* q = get_q(positions); + const T* v = get_v(velocities); + const T* vdot = get_v(accelerations); + // Inboard (parent) body's M frame spatial velocity and spatial acceleration + // in World (already computed in base-to-tip ordering). + const SpatialVelocity& V_WMp_Mp = vc2.get_V_WM_M(inboard_mobod_index()); + const SpatialAcceleration& A_WMp_Mp = + (*A_WM_M_array)[inboard_mobod_index()]; + + // This is going to be the output. + SpatialAcceleration& A_WM_M = (*A_WM_M_array)[index]; + + // Shift and re-express parent contribution. + const Vector3& p_MpM_Mp = X_MpM.translation(); + const math::RotationMatrix R_MMp = X_MpM.rotation().inverse(); + const Vector3& w_WP_Mp = V_WMp_Mp.rotational(); // all frames on P same w + + // Start with parent contribution. 42 flops for shift + 30 for re-express + A_WM_M = R_MMp * A_WMp_Mp.Shift(p_MpM_Mp, w_WP_Mp); // parent contribution + + // Next add in the velocity-dependent bias acceleration: + // Ab_M = ω_WP x ω_FM note: ω_WF == ω_WP + // 2 * ω_WP x v_FM + const SpatialVelocity& V_FM_M = vc2.get_V_FM_M(index); + const Vector3& w_FM_M = V_FM_M.rotational(); + const Vector3& v_FM_M = V_FM_M.translational(); + const Vector3& w_WM_M = V_WM_M.rotational(); + + // To avoid re-expressing w_WP_Mp in M, we can use this identity: + // w_WM = w_WP + w_FM, so w_WP = w_WM - w_FM. + const Vector3 w_WP_M = w_WM_M - w_FM_M; // 3 flops + const SpatialAcceleration Ab_WM_M(w_WP_M.cross(w_FM_M), // 12 flops + 2 * w_WP_M.cross(v_FM_M)); // 15 flops + A_WM_M += Ab_WM_M; + + // Calculate cross-mobilizer spatial acceleration. + const math::RigidTransform& X_FM = pc2.get_X_FM(index); + const SpatialAcceleration A_FM_M = + mobilizer_->calc_A_FM_M(X_FM, q, v, vdot); + A_WM_M += A_FM_M; +} + // As a guideline for developers, a summary of the computations performed in // this method is provided: // Notation: @@ -421,11 +543,11 @@ void BodyNodeImpl::CalcInverseDynamics_TipToBase( // Ftot_BBo = M_B * A_WB + Fb_Bo const SpatialInertia& M_B_W = M_B_W_cache[mobod_index()]; const SpatialAcceleration& A_WB = A_WB_array[mobod_index()]; - SpatialForce Ftot_BBo_W = M_B_W * A_WB; + SpatialForce Ftot_BBo_W = M_B_W * A_WB; // 66 flops if (Fb_Bo_W_cache != nullptr) { - // Dynamic bias for body B. + // Dynamic bias for body B (these cost 57 flops elsewhere). const SpatialForce& Fb_Bo_W = (*Fb_Bo_W_cache)[mobod_index()]; - Ftot_BBo_W += Fb_Bo_W; + Ftot_BBo_W += Fb_Bo_W; // 6 flops } // We're looking for forces due to the mobilizer, so remove the applied @@ -433,7 +555,7 @@ void BodyNodeImpl::CalcInverseDynamics_TipToBase( if (is_Fapplied) { // Use the applied force before it gets overwritten below. const SpatialForce& Fapp_Bo_W = Fapplied_Bo_W_array[mobod_index()]; - Ftot_BBo_W -= Fapp_Bo_W; + Ftot_BBo_W -= Fapp_Bo_W; // 6 flops } // Compute shift vector from Bo to Mo expressed in the world frame W. @@ -441,14 +563,14 @@ void BodyNodeImpl::CalcInverseDynamics_TipToBase( outboard_frame().get_X_BF(frame_body_pose_cache); // F==M const Vector3& p_BoMo_B = X_BM.translation(); const math::RotationMatrix& R_WB = get_X_WB(pc).rotation(); - const Vector3 p_BoMo_W = R_WB * p_BoMo_B; + const Vector3 p_BoMo_W = R_WB * p_BoMo_B; // 15 flops // Output spatial force that would need to be exerted by this node's // mobilizer in order to attain the prescribed acceleration A_WB. SpatialForce& F_BMo_W = (*F_BMo_W_array)[mobod_index()]; - F_BMo_W = Ftot_BBo_W.Shift(p_BoMo_W); // May override Fapplied. + F_BMo_W = Ftot_BBo_W.Shift(p_BoMo_W); // May override Fapplied (12 flops). - // Add in contribution to F_B_Mo_W from outboard nodes. + // Add in contribution to F_B_Mo_W from outboard nodes. (39 flops per) for (const BodyNode* child_node : child_nodes()) { const MobodIndex child_node_index = child_node->mobod_index(); @@ -460,13 +582,13 @@ void BodyNodeImpl::CalcInverseDynamics_TipToBase( const math::RotationMatrix& R_WC = pc.get_X_WB(child_node_index).rotation(); const math::RigidTransform& X_CMc = - frame_Mc.get_X_BF(frame_body_pose_cache); // B==C, F==Mc - const Vector3& p_CoMc_W = R_WC * X_CMc.translation(); + frame_Mc.get_X_BF(frame_body_pose_cache); // B==C, F==Mc + const Vector3& p_CoMc_W = R_WC * X_CMc.translation(); // 15 flops // Shift position vector from child C outboard mobilizer frame Mc to body // B outboard mobilizer Mc. p_MoMc_W: // Since p_BoMo = p_BoCo + p_CoMc + p_McMo, we have: - const Vector3 p_McMo_W = p_BoMo_W - p_BoCo_W - p_CoMc_W; + const Vector3 p_McMo_W = p_BoMo_W - p_BoCo_W - p_CoMc_W; // 6 flops // Spatial force on the child body C at the origin Mc of the outboard // mobilizer frame for the child body. @@ -481,10 +603,10 @@ void BodyNodeImpl::CalcInverseDynamics_TipToBase( // Shift to this node's mobilizer origin Mo (still, F_CMo is the force // acting on the child body C): - const SpatialForce F_CMo_W = F_CMc_W.Shift(p_McMo_W); + const SpatialForce F_CMo_W = F_CMc_W.Shift(p_McMo_W); // 12 flops // From Eq. (3), this force is added (with positive sign) to the force // applied by this body's mobilizer: - F_BMo_W += F_CMo_W; + F_BMo_W += F_CMo_W; // 6 flops } // Re-express F_BMo_W in the inboard frame F before projecting it onto the @@ -495,8 +617,8 @@ void BodyNodeImpl::CalcInverseDynamics_TipToBase( const math::RotationMatrix& R_WP = get_R_WP(pc); // TODO(amcastro-tri): consider caching R_WF since also used in position and // velocity kinematics. - const math::RotationMatrix R_WF = R_WP * R_PF; - const SpatialForce F_BMo_F = R_WF.inverse() * F_BMo_W; + const math::RotationMatrix R_WF = R_WP * R_PF; // 45 flops + const SpatialForce F_BMo_F = R_WF.inverse() * F_BMo_W; // 30 flops // The generalized forces on the mobilizer correspond to the active // components of the spatial force performing work. Therefore we need to @@ -517,6 +639,77 @@ void BodyNodeImpl::CalcInverseDynamics_TipToBase( } } +template class ConcreteMobilizer> +void BodyNodeImpl::CalcInverseDynamics2_TipToBase( + const FrameBodyPoseCache& frame_body_pose_cache, // M_BMo_M, X_BM + const T* positions, + const PositionKinematicsCache2& pc2, // X_MpM, X_WM + const VelocityKinematicsCache2& vc2, // V_WM_M + const std::vector>& A_WM_M_array, + const std::vector>& Fapplied_Bo_W_array, // Bo, W ! + const Eigen::Ref>& tau_applied_array, + std::vector>* F_BMo_M_array, + EigenPtr> tau_array) const { + const MobodIndex index = mobod_index(); // mobod B + + // Model parameters: mass properties, frame locations. + const SpatialInertia& M_BMo_M = frame_body_pose_cache.get_M_BMo_M(index); + const T& mass = M_BMo_M.get_mass(); + const Vector3& p_MoBcm_M = M_BMo_M.get_com(); + const UnitInertia& G_BMo_M = M_BMo_M.get_unit_inertia(); + + const math::RigidTransform& X_MB = + outboard_frame().get_X_FB(frame_body_pose_cache); // F==M + const Vector3& p_MoBo_M = X_MB.translation(); + + // Kinematics. + const math::RotationMatrix R_MW = pc2.get_X_WM(index).rotation().inverse(); + const SpatialVelocity& V_WM_M = vc2.get_V_WM_M(index); + const Vector3& w_WM_M = V_WM_M.rotational(); + const SpatialAcceleration& A_WM_M = A_WM_M_array[index]; + + // Unfortunately the applied force is given at Bo and expressed in W. We + // need it applied at Mo and expressed in M. + const SpatialForce& Fapp_BBo_W = Fapplied_Bo_W_array[index]; + const SpatialForce Fapp_BBo_M = R_MW * Fapp_BBo_W; // 30 flops + const SpatialForce Fapp_BMo_M = Fapp_BBo_M.Shift(-p_MoBo_M); // 15 flops + + // Calculate the velocity-dependent bias force on B (57 flops). + const SpatialForce Fbias_BMo_M( + mass * w_WM_M.cross(G_BMo_M * w_WM_M), // bias moment, 30 flops + mass * w_WM_M.cross(w_WM_M.cross(p_MoBcm_M))); // bias force, 27 flops + + // F_BMo_M is an output and will be the total force on B. + SpatialForce& F_BMo_M = (*F_BMo_M_array)[index]; + F_BMo_M = M_BMo_M * A_WM_M + Fbias_BMo_M - Fapp_BMo_M; // 78 flops + + // Next, account for forces applied to B through its outboard joints, treated + // as though they were locked. Since we're going tip to base we already have + // the total force F_CMc_Mc on each outboard body C. 48 flops per + for (const BodyNode* outboard_node : child_nodes()) { + const MobodIndex outboard_index = outboard_node->mobod_index(); + + // Outboard body's kinematics. + const math::RigidTransform& X_MMc = pc2.get_X_MpM(outboard_index); + const math::RotationMatrix& R_MMc = X_MMc.rotation(); + const Vector3& p_MoMc_M = X_MMc.translation(); + + const SpatialForce& F_CMc_Mc = (*F_BMo_M_array)[outboard_index]; + const SpatialForce F_CMc_M = R_MMc * F_CMc_Mc; // 30 flops + const SpatialForce F_CMo_M = F_CMc_M.Shift(-p_MoMc_M); // 15 flops + F_BMo_M += F_CMo_M; // 6 flops + } + + // tau is the primary output. + Eigen::Map> tau(get_mutable_v(tau_array->data())); + const Eigen::Map> tau_app(get_v(tau_applied_array.data())); + VVector tau_projection; // = Hᵀ_FM_M ⋅ F_BMo_M + const math::RigidTransform& X_FM = pc2.get_X_FM(index); + mobilizer_->calc_tau_from_M(X_FM, get_q(positions), F_BMo_M, + tau_projection.data()); + tau = tau_projection - tau_app; +} + template class ConcreteMobilizer> void BodyNodeImpl:: CalcArticulatedBodyInertiaCache_TipToBase( diff --git a/multibody/tree/body_node_impl.h b/multibody/tree/body_node_impl.h index fbc2b1a6ddc9..b9b4eb77bc64 100644 --- a/multibody/tree/body_node_impl.h +++ b/multibody/tree/body_node_impl.h @@ -71,6 +71,10 @@ class BodyNodeImpl final : public BodyNode { const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, PositionKinematicsCache* pc) const final; + void CalcPositionKinematicsCache2_BaseToTip( + const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, + PositionKinematicsCache2* pc2) const final; + void CalcAcrossNodeJacobianWrtVExpressedInWorld( const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, const PositionKinematicsCache& pc, @@ -81,6 +85,10 @@ class BodyNodeImpl final : public BodyNode { const std::vector>& H_PB_W_cache, const T* velocities, VelocityKinematicsCache* vc) const final; + void CalcVelocityKinematicsCache2_BaseToTip( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, VelocityKinematicsCache2* vc2) const final; + void CalcMassMatrixContribution_TipToBase( const PositionKinematicsCache& pc, const std::vector>& Mc_B_W_cache, @@ -110,6 +118,12 @@ class BodyNodeImpl final : public BodyNode { const VelocityKinematicsCache* vc, const T* accelerations, std::vector>* A_WB_array) const final; + void CalcSpatialAcceleration2_BaseToTip( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, const VelocityKinematicsCache2& vc2, + const T* accelerations, + std::vector>* A_WM_M_array) const final; + void CalcInverseDynamics_TipToBase( const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, const PositionKinematicsCache& pc, @@ -121,6 +135,16 @@ class BodyNodeImpl final : public BodyNode { std::vector>* F_BMo_W_array, EigenPtr> tau_array) const final; + void CalcInverseDynamics2_TipToBase( + const FrameBodyPoseCache& frame_body_pose_cache, const T* positions, + const PositionKinematicsCache2& pc2, + const VelocityKinematicsCache2& vc2, + const std::vector>& A_WM_M_array, + const std::vector>& Fapplied_Bo_W_array, + const Eigen::Ref>& tau_applied_array, + std::vector>* F_BMo_M_array, + EigenPtr> tau_array) const final; + void CalcArticulatedBodyInertiaCache_TipToBase( const systems::Context& context, const PositionKinematicsCache& pc, const Eigen::Ref>& H_PB_W, diff --git a/multibody/tree/body_node_world.h b/multibody/tree/body_node_world.h index 8cc2dd66ccd3..1a1da99586c7 100644 --- a/multibody/tree/body_node_world.h +++ b/multibody/tree/body_node_world.h @@ -31,6 +31,12 @@ class BodyNodeWorld final : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcPositionKinematicsCache2_BaseToTip( + const FrameBodyPoseCache&, const T*, + PositionKinematicsCache2*) const final { + DRAKE_UNREACHABLE(); + } + void CalcAcrossNodeJacobianWrtVExpressedInWorld( const FrameBodyPoseCache&, const T*, const PositionKinematicsCache&, std::vector>*) const final { @@ -44,6 +50,12 @@ class BodyNodeWorld final : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcVelocityKinematicsCache2_BaseToTip( + const T*, const PositionKinematicsCache2&, const T*, + VelocityKinematicsCache2*) const final { + DRAKE_UNREACHABLE(); + } + void CalcMassMatrixContribution_TipToBase( const PositionKinematicsCache&, const std::vector>&, const std::vector>&, EigenPtr>) const final { @@ -73,6 +85,13 @@ class BodyNodeWorld final : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcSpatialAcceleration2_BaseToTip( + const T*, const PositionKinematicsCache2&, const T*, + const VelocityKinematicsCache2&, const T*, + std::vector>*) const final { + DRAKE_UNREACHABLE(); + } + void CalcInverseDynamics_TipToBase(const FrameBodyPoseCache&, const T*, const PositionKinematicsCache&, const std::vector>&, @@ -85,6 +104,15 @@ class BodyNodeWorld final : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcInverseDynamics2_TipToBase( + const FrameBodyPoseCache&, const T*, + const PositionKinematicsCache2&, const VelocityKinematicsCache2&, + const std::vector>&, + const std::vector>&, const Eigen::Ref>&, + std::vector>*, EigenPtr>) const final { + DRAKE_UNREACHABLE(); + } + void CalcArticulatedBodyInertiaCache_TipToBase( const systems::Context&, const PositionKinematicsCache&, const Eigen::Ref>&, const SpatialInertia&, diff --git a/multibody/tree/frame.h b/multibody/tree/frame.h index 8ed626df37c9..1e7b18378f44 100644 --- a/multibody/tree/frame.h +++ b/multibody/tree/frame.h @@ -540,6 +540,50 @@ class Frame : public FrameBase { const internal::FrameBodyPoseCache& frame_body_poses) const { return frame_body_poses.get_X_FB(body_pose_index_in_cache_); } + + /// (Internal use only) Given an already up-to-date frame body pose cache, + /// returns whether X_BF is exactly identity. This is precomputed in the + /// cache so is very fast to check. + /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() + /// since the last parameter change; we can't check here. + /// @see get_X_BF() + bool is_X_BF_identity( + const internal::FrameBodyPoseCache& frame_body_poses) { + return frame_body_poses.is_X_BF_identity(body_pose_index_in_cache_); + } + + /// (Internal use only) Given an already up-to-date frame body pose cache, + /// extract X_MbF for this %Frame from it. + /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() + /// since the last parameter change; we can't check here. + /// @retval X_MbF pose of this frame in its body's inboard mobilizer's + /// outboard frame M. + const math::RigidTransform& get_X_MbF( + const internal::FrameBodyPoseCache& frame_body_poses) const { + return frame_body_poses.get_X_MbF(this->index()); + } + + /// (Internal use only) Given an already up-to-date frame body pose cache, + /// extract X_FMb (=X_MbF⁻¹) for this %Frame from it. + /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() + /// since the last parameter change; we can't check here. + /// @retval X_FMb inverse of this frame's pose in its body's inboard + /// mobilizer's outboard frame M. + const math::RigidTransform& get_X_FMb( + const internal::FrameBodyPoseCache& frame_body_poses) const { + return frame_body_poses.get_X_FMb(this->index()); + } + + /// (Internal use only) Given an already up-to-date frame body pose cache, + /// returns whether X_MbF is exactly identity. This is precomputed in the + /// cache so is very fast to check. + /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() + /// since the last parameter change; we can't check here. + /// @see get_X_MbF() + bool is_X_MbF_identity( + const internal::FrameBodyPoseCache& frame_body_poses) { + return frame_body_poses.is_X_MbF_identity(this->index()); + } //@} protected: diff --git a/multibody/tree/frame_body_pose_cache.h b/multibody/tree/frame_body_pose_cache.h index 182642705f52..d1cca019f8dc 100644 --- a/multibody/tree/frame_body_pose_cache.h +++ b/multibody/tree/frame_body_pose_cache.h @@ -6,6 +6,8 @@ #include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" #include "drake/math/rigid_transform.h" +#include "drake/multibody/tree/multibody_tree_indexes.h" +#include "drake/multibody/tree/spatial_inertia.h" namespace drake { namespace multibody { @@ -31,9 +33,14 @@ class FrameBodyPoseCache { public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FrameBodyPoseCache); - explicit FrameBodyPoseCache(int num_frame_body_pose_slots_needed) + explicit FrameBodyPoseCache(int num_mobods, int num_frames, + int num_frame_body_pose_slots_needed) : X_BF_pool_(num_frame_body_pose_slots_needed), - X_FB_pool_(num_frame_body_pose_slots_needed) { + X_FB_pool_(num_frame_body_pose_slots_needed), + X_MbF_pool_(num_frames), + X_FMb_pool_(num_frames), + is_X_MbF_identity_(num_frames), + M_BMo_M_pool_(num_mobods, SpatialInertia::NaN()) { DRAKE_DEMAND(num_frame_body_pose_slots_needed > 0); // All RigidBodyFrames share this body pose. @@ -50,16 +57,70 @@ class FrameBodyPoseCache { return X_FB_pool_[body_pose_index]; } + // Returns true for body frames. + bool is_X_BF_identity(int body_pose_index) const { + return body_pose_index == 0; + } + + const math::RigidTransform& get_X_MbF(FrameIndex index) const { + DRAKE_ASSERT(0 <= index && index < ssize(X_MbF_pool_)); + return X_MbF_pool_[index]; + } + + const math::RigidTransform& get_X_FMb(FrameIndex index) const { + DRAKE_ASSERT(0 <= index && index < ssize(X_FMb_pool_)); + return X_FMb_pool_[index]; + } + + // Returns true for any frame F that is the outboard frame M for some + // mobilizer, as well as any frame exactly coincident with one of those. + bool is_X_MbF_identity(FrameIndex index) const { + DRAKE_ASSERT(0 <= index && index < ssize(is_X_MbF_identity_)); + return static_cast(is_X_MbF_identity_[index]); + } + + const SpatialInertia& get_M_BMo_M(MobodIndex index) const { + DRAKE_ASSERT(0 <= index && index < ssize(M_BMo_M_pool_)); + return M_BMo_M_pool_[index]; + } + void set_X_BF(int body_pose_index, const math::RigidTransform& X_BF) { - DRAKE_ASSERT(0 <= body_pose_index && body_pose_index < ssize(X_BF_pool_)); + DRAKE_DEMAND(0 <= body_pose_index && body_pose_index < ssize(X_BF_pool_)); X_BF_pool_[body_pose_index] = X_BF; X_FB_pool_[body_pose_index] = X_BF.inverse(); } + void set_X_MbF(FrameIndex index, const math::RigidTransform& X_MbF) { + DRAKE_DEMAND(0 <= index && index < ssize(X_MbF_pool_)); + X_MbF_pool_[index] = X_MbF; + X_FMb_pool_[index] = X_MbF.inverse(); + if constexpr (scalar_predicate::is_bool) { + is_X_MbF_identity_[index] = X_MbF.IsExactlyIdentity(); + } else { + is_X_MbF_identity_[index] = static_cast(false); + } + } + + void set_M_BMo_M(MobodIndex index, const SpatialInertia& M_BMo_M) { + DRAKE_DEMAND(0 <= index && index < ssize(M_BMo_M_pool_)); + M_BMo_M_pool_[index] = M_BMo_M; + } + private: - // Size is set on construction. + // Sizes are set on construction. + + // These are indexed by Frame::get_body_pose_index_in_cache(). std::vector> X_BF_pool_; std::vector> X_FB_pool_; + + // These are indexed by Frame::index(). + std::vector> X_MbF_pool_; + std::vector> X_FMb_pool_; + std::vector is_X_MbF_identity_; // fast vector equivalent + + // Spatial inertia of mobilized body B, about its inboard joint frame M's + // origin Mo, expressed in M. These are indexed by MobodIndex. + std::vector> M_BMo_M_pool_; }; } // namespace internal diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 470be2ffd46d..c0eff7b75c72 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -1341,6 +1341,31 @@ void MultibodyTree::CalcPositionKinematicsCache( } } +template +void MultibodyTree::CalcPositionKinematicsCache2( + const systems::Context& context, + PositionKinematicsCache2* pc2) const { + DRAKE_DEMAND(pc2 != nullptr); + + const FrameBodyPoseCache& frame_body_pose_cache = + EvalFrameBodyPoses(context); + + const Eigen::VectorBlock> q_block = get_positions(context); + const T* q = q_block.data(); + + for (int level = 1; level < forest_height(); ++level) { + for (MobodIndex mobod_index : body_node_levels_[level]) { + const BodyNode& node = *body_nodes_[mobod_index]; + + DRAKE_ASSERT(node.get_topology().level == level); + DRAKE_ASSERT(node.mobod_index() == mobod_index); + + node.CalcPositionKinematicsCache2_BaseToTip(frame_body_pose_cache, q, + pc2); + } + } +} + template void MultibodyTree::CalcVelocityKinematicsCache( const systems::Context& context, const PositionKinematicsCache& pc, @@ -1376,6 +1401,31 @@ void MultibodyTree::CalcVelocityKinematicsCache( } } +template +void MultibodyTree::CalcVelocityKinematicsCache2( + const systems::Context& context, const PositionKinematicsCache2& pc2, + VelocityKinematicsCache2* vc2) const { + DRAKE_DEMAND(vc2 != nullptr); + + const T* positions = get_positions(context).data(); + const T* velocities = get_velocities(context).data(); + + // Performs a base-to-tip recursion computing body velocities. + // This skips the world, level = 0. + for (int level = 1; level < forest_height(); ++level) { + for (MobodIndex mobod_index : body_node_levels_[level]) { + const BodyNode& node = *body_nodes_[mobod_index]; + + DRAKE_ASSERT(node.get_topology().level == level); + DRAKE_ASSERT(node.mobod_index() == mobod_index); + + // Update per-mobod kinematics. + node.CalcVelocityKinematicsCache2_BaseToTip(positions, pc2, velocities, + vc2); + } + } +} + // Result is indexed by MobodIndex, not BodyIndex. template void MultibodyTree::CalcSpatialInertiasInWorld( @@ -1444,9 +1494,17 @@ void MultibodyTree::CalcFrameBodyPoses( DRAKE_ASSERT(frame_body_poses->get_X_BF(0).IsExactlyIdentity()); DRAKE_ASSERT(frame_body_poses->get_X_FB(0).IsExactlyIdentity()); - for (const Frame* frame : frames_.elements()) { - const int body_pose_index_in_cache = frame->get_body_pose_index_in_cache(); - if (frame->is_body_frame()) { + // N.B.: we are using "F" for arbitrary frames here; don't confuse it with + // a joint's parent frame F or mobilizer's inboard frame F. (Sure, some of + // these frames are the joint frames but most aren't and it doesn't matter + // here.) + + // The first pass locates every frame F with respect to the body frame B + // of the body to which it is fixed. + for (const Frame* frame_F : frames_.elements()) { + const int body_pose_index_in_cache = + frame_F->get_body_pose_index_in_cache(); + if (frame_F->is_body_frame()) { DRAKE_DEMAND(body_pose_index_in_cache == 0); continue; } @@ -1456,7 +1514,60 @@ void MultibodyTree::CalcFrameBodyPoses( // there is a performance issue, consider doing this in topological // order (or memoizing) so we don't have to recalculate. frame_body_poses->set_X_BF(body_pose_index_in_cache, - frame->CalcPoseInBodyFrame(context)); + frame_F->CalcPoseInBodyFrame(context)); + } + + // The second pass precomputes, for any frame F fixed to a body B, X_MbF which + // is the pose of that frame in body B's unique mobilizer frame Mb. This is + // useful so we can efficiently compute multibody quantities in each body's + // M frame. The first pass will have determined X_BMb (and X_MbB) as well + // as X_BF. We'll compute X_MbF = X_MbB*X_BF and store the result for every + // frame, indexed by FrameIndex. + for (const Frame* frame_F : frames_.elements()) { + // This is the mobilizer that connects body B to its inboard (parent) + // body. The _outboard_ frame of that mobilizer is Mb. + const Mobilizer& mobilizer_B = + get_mobilizer(frame_F->body().mobod_index()); + const Frame& frame_Mb = mobilizer_B.outboard_frame(); + + const RigidTransform& X_MbB = frame_Mb.get_X_FB(*frame_body_poses); + const RigidTransform& X_BF = frame_F->get_X_BF(*frame_body_poses); + + frame_body_poses->set_X_MbF(frame_F->index(), X_MbB * X_BF); + } + + // For every mobilized body, shift its (parameterized) spatial inertia from + // the body origin Bo to the (just determined above) unique inboard mobilizer + // frame M's origin Mo. Then re-express the shifted inertia in M. + for (const SpanningForest::Mobod& mobod : forest().mobods()) { + if (mobod.is_world()) continue; + // TODO(sherm1) Can't handle composites yet. + DRAKE_DEMAND(ssize(mobod.follower_link_ordinals()) == 1); + const Mobilizer& mobilizer = get_mobilizer(mobod.index()); + + // Get the shift and new basis. + const Frame& frame_M = mobilizer.outboard_frame(); + const int pose_index = frame_M.get_body_pose_index_in_cache(); + const Vector3& p_BoMo_B = + frame_body_poses->get_X_BF(pose_index).translation(); + const RotationMatrix& R_MB = + frame_body_poses->get_X_FB(pose_index).rotation(); + + // Get the parameterized spatial inertia. + const RigidBody& body = mobilizer.outboard_body(); + const SpatialInertia& M_BBo_B = + body.CalcSpatialInertiaInBodyFrame(context); + + if (!M_BBo_B.IsNaN()) { + // Shift and re-express. + const SpatialInertia M_BMo_B = M_BBo_B.Shift(p_BoMo_B); + const SpatialInertia M_BMo_M = M_BMo_B.ReExpress(R_MB); + frame_body_poses->set_M_BMo_M(mobod.index(), M_BMo_M); + } else { + // Hopefully no one is planning to use this. If they do it will blow + // up later. + frame_body_poses->set_M_BMo_M(mobod.index(), M_BBo_B); + } } } @@ -1630,6 +1741,32 @@ void MultibodyTree::CalcSpatialAccelerationsFromVdot( } } +template +void MultibodyTree::CalcSpatialAccelerationsInMFromVdot( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, const VelocityKinematicsCache2& vc2, + const T* accelerations, + std::vector>* A_WM_M_array) const { + DRAKE_ASSERT(positions != nullptr && velocities != nullptr && + accelerations != nullptr); + DRAKE_ASSERT(A_WM_M_array != nullptr); + DRAKE_ASSERT(ssize(*A_WM_M_array) == topology_.num_mobods()); + + // The world's spatial acceleration is always zero. + (*A_WM_M_array)[world_mobod_index()] = SpatialAcceleration::Zero(); + + // Performs a base-to-tip recursion computing body accelerations. World was + // handled above so is skipped here. + for (MobodIndex mobod_index{1}; mobod_index < num_mobods(); ++mobod_index) { + const BodyNode& node = *body_nodes_[mobod_index]; + DRAKE_ASSERT(node.mobod_index() == mobod_index); + + // Update per-node kinematics. + node.CalcSpatialAcceleration2_BaseToTip(positions, pc2, velocities, vc2, + accelerations, &*A_WM_M_array); + } +} + template void MultibodyTree::CalcAccelerationKinematicsCache( const systems::Context& context, const PositionKinematicsCache& pc, @@ -1745,6 +1882,58 @@ void MultibodyTree::CalcInverseDynamics( } } +template +VectorX MultibodyTree::CalcInverseDynamics2( + const systems::Context& context, const VectorX& known_vdot, + const MultibodyForces& external_forces) const { + // Temporary storage used in the computation of inverse dynamics. + std::vector> A_WM_M_array(num_bodies()); + std::vector> F_BMo_M_array(num_bodies()); + + const T* const positions = get_positions(context).data(); + const T* const velocities = get_velocities(context).data(); + const T* const accelerations = known_vdot.data(); + + const PositionKinematicsCache2& pc2 = EvalPositionKinematics2(context); + const VelocityKinematicsCache2& vc2 = EvalVelocityKinematics2(context); + + CalcSpatialAccelerationsInMFromVdot(positions, pc2, velocities, vc2, + accelerations, &A_WM_M_array); + + const FrameBodyPoseCache& frame_body_pose_cache = + EvalFrameBodyPoses(context); + + // Note that applied forces are in W. + const std::vector>& Fapp_Bo_W_array = + external_forces.body_forces(); + const VectorX& tau_app_array = external_forces.generalized_forces(); + + VectorX tau(num_velocities()); // The to-be-returned result. + for (int level = forest_height() - 1; level >= 0; --level) { + for (MobodIndex mobod_index : body_node_levels_[level]) { + const BodyNode& node = *body_nodes_[mobod_index]; + + DRAKE_ASSERT(node.get_topology().level == level); + DRAKE_ASSERT(node.mobod_index() == mobod_index); + + // Compute F_BMo_M for the body associated with this node and project it + // onto the space of generalized forces tau for the associated mobilizer. + node.CalcInverseDynamics2_TipToBase(frame_body_pose_cache, positions, pc2, + vc2, A_WM_M_array, Fapp_Bo_W_array, + tau_app_array, &F_BMo_M_array, &tau); + } + } + + // Add the effect of reflected inertias. + // See JointActuator::reflected_inertia(). + const VectorX& reflected_inertia = EvalReflectedInertiaCache(context); + for (int i = 0; i < num_velocities(); ++i) { + tau(i) += reflected_inertia(i) * known_vdot(i); + } + + return tau; +} + template void MultibodyTree::CalcForceElementsContribution( const systems::Context& context, const PositionKinematicsCache& pc, diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h index d50c49a7fe5f..c3a037839018 100644 --- a/multibody/tree/multibody_tree.h +++ b/multibody/tree/multibody_tree.h @@ -1344,6 +1344,9 @@ class MultibodyTree { void CalcPositionKinematicsCache(const systems::Context& context, PositionKinematicsCache* pc) const; + void CalcPositionKinematicsCache2(const systems::Context& context, + PositionKinematicsCache2* pc2) const; + // Computes all the kinematic quantities that depend on the generalized // velocities and stores them in the velocity kinematics cache `vc`. // These include: @@ -1360,6 +1363,10 @@ class MultibodyTree { const PositionKinematicsCache& pc, VelocityKinematicsCache* vc) const; + void CalcVelocityKinematicsCache2(const systems::Context& context, + const PositionKinematicsCache2& pc2, + VelocityKinematicsCache2* vc2) const; + // Computes the spatial inertia M_B_W(q) for each body B in the model about // its frame origin Bo and expressed in the world frame W. // @param[in] context @@ -1470,11 +1477,22 @@ class MultibodyTree { const VelocityKinematicsCache& vc, const VectorX& known_vdot, std::vector>* A_WB_array) const; + void CalcSpatialAccelerationsInMFromVdot( + const T* positions, const PositionKinematicsCache2& pc2, + const T* velocities, const VelocityKinematicsCache2& vc2, + const T* accelerations, + std::vector>* A_WM_M_array) const; + // See MultibodyPlant method. VectorX CalcInverseDynamics( const systems::Context& context, const VectorX& known_vdot, const MultibodyForces& external_forces) const; + // Uses M frame instead of W. + VectorX CalcInverseDynamics2( + const systems::Context& context, const VectorX& known_vdot, + const MultibodyForces& external_forces) const; + // (Advanced) Given the state of this MultibodyTree in context and a // known vector of generalized accelerations vdot, this method computes the // set of generalized forces tau that would need to be applied at each @@ -2235,6 +2253,12 @@ class MultibodyTree { return tree_system_->EvalPositionKinematics(context); } + const PositionKinematicsCache2& EvalPositionKinematics2( + const systems::Context& context) const { + DRAKE_ASSERT(tree_system_ != nullptr); + return tree_system_->EvalPositionKinematics2(context); + } + // Evaluates velocity kinematics cached in context. This will also // force position kinematics to be updated if it hasn't already. // @param context A Context whose velocity kinematics cache will be @@ -2246,6 +2270,12 @@ class MultibodyTree { return tree_system_->EvalVelocityKinematics(context); } + const VelocityKinematicsCache2& EvalVelocityKinematics2( + const systems::Context& context) const { + DRAKE_ASSERT(tree_system_ != nullptr); + return tree_system_->EvalVelocityKinematics2(context); + } + // Evaluates acceleration kinematics cached in context. This will also // force other cache entries to be updated if they haven't already. // @param context A Context whose acceleration kinematics cache will be diff --git a/multibody/tree/multibody_tree_system.cc b/multibody/tree/multibody_tree_system.cc index c1c6f56cb53c..393b986d1cc7 100644 --- a/multibody/tree/multibody_tree_system.cc +++ b/multibody/tree/multibody_tree_system.cc @@ -223,7 +223,9 @@ void MultibodyTreeSystem::Finalize() { cache_indexes_.frame_body_poses = this->DeclareCacheEntry( std::string("frame pose in body frame"), - FrameBodyPoseCache(num_frame_body_poses_needed), + FrameBodyPoseCache(internal_tree().num_mobods(), + internal_tree().num_frames(), + num_frame_body_poses_needed), &MultibodyTreeSystem::CalcFrameBodyPoses, {this->all_parameters_ticket()}) .cache_index(); @@ -244,6 +246,14 @@ void MultibodyTreeSystem::Finalize() { {position_ticket, this->all_parameters_ticket()}) .cache_index(); + cache_indexes_.position_kinematics2 = + this->DeclareCacheEntry( + std::string("position kinematics in M"), + PositionKinematicsCache2(internal_tree().num_mobods()), + &MultibodyTreeSystem::CalcPositionKinematicsCache2, + {position_ticket, this->all_parameters_ticket()}) + .cache_index(); + // Allocate cache entry to store spatial inertia M_B_W(q) for each body. cache_indexes_.spatial_inertia_in_world = this->DeclareCacheEntry( @@ -275,6 +285,14 @@ void MultibodyTreeSystem::Finalize() { {position_ticket, velocity_ticket, this->all_parameters_ticket()}) .cache_index(); + cache_indexes_.velocity_kinematics2 = + this->DeclareCacheEntry( + std::string("velocity kinematics in M"), + VelocityKinematicsCache2(internal_tree().num_mobods()), + &MultibodyTreeSystem::CalcVelocityKinematicsCache2, + {position_ticket, velocity_ticket, this->all_parameters_ticket()}) + .cache_index(); + // Allocate cache entry to store Fb_Bo_W(q, v) for each body. cache_indexes_.dynamic_bias = this->DeclareCacheEntry( diff --git a/multibody/tree/multibody_tree_system.h b/multibody/tree/multibody_tree_system.h index 100f2ace9600..16eec14d799b 100644 --- a/multibody/tree/multibody_tree_system.h +++ b/multibody/tree/multibody_tree_system.h @@ -111,6 +111,13 @@ class MultibodyTreeSystem : public systems::LeafSystem { .template Eval>(context); } + const PositionKinematicsCache2& EvalPositionKinematics2( + const systems::Context& context) const { + this->ValidateContext(context); + return position_kinematics2_cache_entry() + .template Eval>(context); + } + /* Returns a reference to the up-to-date VelocityKinematicsCache in the given Context, recalculating it first if necessary. Also if necessary, the PositionKinematicsCache will be recalculated as well. */ @@ -121,6 +128,13 @@ class MultibodyTreeSystem : public systems::LeafSystem { .template Eval>(context); } + const VelocityKinematicsCache2& EvalVelocityKinematics2( + const systems::Context& context) const { + this->ValidateContext(context); + return velocity_kinematics2_cache_entry() + .template Eval>(context); + } + /* Returns a reference to the up-to-date AccelerationKinematicsCache in the given Context, recalculating it first via forward dynamics if necessary. Also if necessary, other cache entries such as PositionKinematicsCache and @@ -259,11 +273,19 @@ class MultibodyTreeSystem : public systems::LeafSystem { return this->get_cache_entry(cache_indexes_.position_kinematics); } + const systems::CacheEntry& position_kinematics2_cache_entry() const { + return this->get_cache_entry(cache_indexes_.position_kinematics2); + } + /* Returns the cache entry that holds velocity kinematics results. */ const systems::CacheEntry& velocity_kinematics_cache_entry() const { return this->get_cache_entry(cache_indexes_.velocity_kinematics); } + const systems::CacheEntry& velocity_kinematics2_cache_entry() const { + return this->get_cache_entry(cache_indexes_.velocity_kinematics2); + } + /* Returns the cache entry that holds acceleration kinematics results. */ const systems::CacheEntry& acceleration_kinematics_cache_entry() const { return this->get_cache_entry(cache_indexes_.acceleration_kinematics); @@ -404,6 +426,12 @@ class MultibodyTreeSystem : public systems::LeafSystem { internal_tree().CalcPositionKinematicsCache(context, position_cache); } + void CalcPositionKinematicsCache2( + const systems::Context& context, + PositionKinematicsCache2* position_cache2) const { + internal_tree().CalcPositionKinematicsCache2(context, position_cache2); + } + void CalcSpatialInertiasInWorld( const systems::Context& context, std::vector>* spatial_inertias) const { @@ -455,6 +483,13 @@ class MultibodyTreeSystem : public systems::LeafSystem { context, EvalPositionKinematics(context), velocity_cache); } + void CalcVelocityKinematicsCache2( + const systems::Context& context, + VelocityKinematicsCache2* velocity_cache2) const { + internal_tree().CalcVelocityKinematicsCache2( + context, EvalPositionKinematics2(context), velocity_cache2); + } + void CalcDynamicBiasForces( const systems::Context& context, std::vector>* dynamic_bias_forces) const { @@ -546,10 +581,12 @@ class MultibodyTreeSystem : public systems::LeafSystem { systems::CacheIndex articulated_body_force_bias; systems::CacheIndex dynamic_bias; systems::CacheIndex position_kinematics; + systems::CacheIndex position_kinematics2; systems::CacheIndex spatial_inertia_in_world; systems::CacheIndex composite_body_inertia_in_world; systems::CacheIndex spatial_acceleration_bias; systems::CacheIndex velocity_kinematics; + systems::CacheIndex velocity_kinematics2; }; // This is the one real constructor. From the public API, a null tree is diff --git a/multibody/tree/parameter_conversion.h b/multibody/tree/parameter_conversion.h index 40228ca0f671..88e413b7fbb7 100644 --- a/multibody/tree/parameter_conversion.h +++ b/multibody/tree/parameter_conversion.h @@ -76,6 +76,9 @@ SpatialInertia ToSpatialInertia( Vector3(spatial_inertia_vector[SpatialInertiaIndex::k_com_x], spatial_inertia_vector[SpatialInertiaIndex::k_com_y], spatial_inertia_vector[SpatialInertiaIndex::k_com_z]), + // TODO(sherm1) Despite the `true` (don't check validity) below, this + // blows up in Debug mode due to the underlying RotationMatrix + // constructor that gets called from here. Fix. UnitInertia(spatial_inertia_vector[SpatialInertiaIndex::k_Gxx], spatial_inertia_vector[SpatialInertiaIndex::k_Gyy], spatial_inertia_vector[SpatialInertiaIndex::k_Gzz], diff --git a/multibody/tree/planar_mobilizer.h b/multibody/tree/planar_mobilizer.h index b455d3de827a..14779833452e 100644 --- a/multibody/tree/planar_mobilizer.h +++ b/multibody/tree/planar_mobilizer.h @@ -31,12 +31,20 @@ namespace internal { The generalized velocities for this mobilizer are the rate of change of the coordinates, v = q̇. - H_FM₆ₓ₃=[0 0 0] Hdot_FM = 0₆ₓ₃ - [0 0 0] - [0 0 1] - [1 0 0] - [0 1 0] - [0 0 0] + H_FM_F₆ₓ₃=[0 0 0] Hdot_FM_F = 0₆ₓ₃ + [0 0 0] + [0 0 1] R_FM(q₂) = [c -s 0] + [1 0 0] [s c 0] + [0 1 0] [0 0 1] + [0 0 0] + + H_FM_M = R_MF ⋅ H_FM_F Hdot_FM_M + = [ 0 0 0] = [ 0 0 0] + [ 0 0 0] [ 0 0 0] + [ 0 0 1] [ 0 0 1] + [ c s 0] [-s c 0]⋅v₂ + [-s c 0] [-c -s 0]⋅v₂ + [ 0 0 0] [ 0 0 0] @tparam_default_scalar */ template @@ -154,18 +162,42 @@ class PlanarMobilizer final : public MobilizerImpl { } /* Computes the across-mobilizer velocity V_FM(q, v) of the outboard frame - M measured and expressed in frame F as a function of the input velocity v. */ + M measured and expressed in frame F as a function of the input velocity v. */ SpatialVelocity calc_V_FM(const T*, const T* v) const { return SpatialVelocity(Vector3(0.0, 0.0, v[2]), Vector3(v[0], v[1], 0.0)); } - /* Returns H_FM⋅vdot + Hdot_FM⋅v. See class description for definitions. */ + + /* Computes the across-mobilizer velocity V_FM_M(q, v) of the outboard frame + M measured in frame F and expressed in frame M as a function of the input + velocity v. */ + SpatialVelocity calc_V_FM_M(const math::RigidTransform&, const T* q, + const T* v) const { + // TODO(sherm1) These are already available in the passed-in X_FM. + using std::sin, std::cos; + const T s = sin(q[2]), c = cos(q[2]); + return SpatialVelocity( + Vector3(0.0, 0.0, v[2]), + Vector3(c * v[0] + s * v[1], c * v[1] - s * v[0], 0.0)); + } + + /* Returns H_FM_F⋅vdot + Hdot_FM_F⋅v. See class description. */ SpatialAcceleration calc_A_FM(const T*, const T*, const T* vdot) const { return SpatialAcceleration(Vector3(0.0, 0.0, vdot[2]), Vector3(vdot[0], vdot[1], 0.0)); } - /* Returns tau = H_FMᵀ⋅F */ + /* Returns H_FM_M⋅vdot + Hdot_FM_M⋅v. See class description. */ + SpatialAcceleration calc_A_FM_M(const math::RigidTransform& X_FM, + const T*, const T*, const T* vdot) const { + // TODO(sherm1) Use explicit sin & cos from X_FM. + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialAcceleration A_FM_F = calc_A_FM(nullptr, nullptr, vdot); + const SpatialAcceleration A_FM_M = R_FM.inverse() * A_FM_F; // 30 flops + return A_FM_M; + } + + /* Returns tau = H_FM_Fᵀ⋅F_F */ void calc_tau(const T*, const SpatialForce& F_BMo_F, T* tau) const { DRAKE_ASSERT(tau != nullptr); const Vector3& t_B_F = F_BMo_F.rotational(); // torque @@ -175,6 +207,20 @@ class PlanarMobilizer final : public MobilizerImpl { tau[2] = t_B_F[2]; // torque about z } + /* Returns tau = H_FM_Mᵀ⋅F_M. See class comments. */ + void calc_tau_from_M(const math::RigidTransform&, const T* q, + const SpatialForce& F_BMo_M, T* tau) const { + // TODO(sherm1) These are already available in the passed-in X_FM. + using std::sin, std::cos; + DRAKE_ASSERT(tau != nullptr); + const T s = sin(q[2]), c = cos(q[2]); + const Vector3& t_B_M = F_BMo_M.rotational(); // torque + const Vector3& f_BMo_M = F_BMo_M.translational(); // force + tau[0] = c * f_BMo_M[0] - s * f_BMo_M[1]; // force along x + tau[1] = s * f_BMo_M[0] + c * f_BMo_M[1]; // force along y + tau[2] = t_B_M[2]; // torque about z + } + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; diff --git a/multibody/tree/position_kinematics_cache.h b/multibody/tree/position_kinematics_cache.h index cadd5f7599b2..99fcf6a1d737 100644 --- a/multibody/tree/position_kinematics_cache.h +++ b/multibody/tree/position_kinematics_cache.h @@ -174,6 +174,47 @@ class PositionKinematicsCache { Vector3PoolType p_PoBo_W_pool_; }; +template +class PositionKinematicsCache2 { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PositionKinematicsCache2); + + explicit PositionKinematicsCache2(int num_mobods) + : X_FM_pool_(num_mobods), + X_MpM_pool_(num_mobods), + X_WM_pool_(num_mobods) {} + + const math::RigidTransform& get_X_FM(MobodIndex index) const { + return X_FM_pool_[index]; + } + + const math::RigidTransform& get_X_MpM(MobodIndex index) const { + return X_MpM_pool_[index]; + } + + const math::RigidTransform& get_X_WM(MobodIndex index) const { + return X_WM_pool_[index]; + } + + math::RigidTransform& get_mutable_X_FM(MobodIndex index) { + return X_FM_pool_[index]; + } + + math::RigidTransform& get_mutable_X_MpM(MobodIndex index) { + return X_MpM_pool_[index]; + } + + math::RigidTransform& get_mutable_X_WM(MobodIndex index) { + return X_WM_pool_[index]; + } + + private: + // Index by MobodIndex. + std::vector> X_FM_pool_; + std::vector> X_MpM_pool_; + std::vector> X_WM_pool_; +}; + } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/tree/prismatic_mobilizer.h b/multibody/tree/prismatic_mobilizer.h index f2f1aa0b9186..4aa330a0092d 100644 --- a/multibody/tree/prismatic_mobilizer.h +++ b/multibody/tree/prismatic_mobilizer.h @@ -23,14 +23,18 @@ namespace internal { // rotation between the inboard and outboard frames, just translation. // To fully specify this mobilizer, a user must provide the inboard frame F, // the outboard (or "mobilized") frame M and the axis `axis_F` (expressed in -// frame F) along which frame M translates with respect to frame F. +// frame F or equivalently in frame M) along which frame M translates with +// respect to frame F. // The single generalized coordinate q introduced by this mobilizer // corresponds to the translation distance (in meters) of the origin `Mo` of // frame M with respect to frame F along `axis_F`. When `q = 0`, frames F and M // are coincident. The translation distance is defined to be positive in the // direction of `axis_F`. // -// H_FM₆ₓ₁=[0₃, axis_F]ᵀ Hdot_FM₆ₓ₁ = 0 +// The measure numbers of axis_F and axis_M are identical and constant. +// +// H_FM_F₆ₓ₁=[0₃, axis_F]ᵀ Hdot_FM_F₆ₓ₁ = 0 +// H_FM_M₆ₓ₁=[0₃, axis_M]ᵀ Hdot_FM_M₆ₓ₁ = 0 // // @tparam_default_scalar template @@ -135,18 +139,39 @@ class PrismaticMobilizer final : public MobilizerImpl { return SpatialVelocity(Vector3::Zero(), v[0] * translation_axis()); } + SpatialVelocity calc_V_FM_M(const math::RigidTransform&, const T*, + const T* v) const { + // axis_M == axis_F so this is the same as V_FM_F. + return SpatialVelocity(v[0] * axis_F_, Vector3::Zero()); + } + SpatialAcceleration calc_A_FM(const T*, const T*, const T* vdot) const { return SpatialAcceleration(Vector3::Zero(), - vdot[0] * translation_axis()); + vdot[0] * translation_axis()); // axis_F } - // Returns tau = H_FMᵀ⋅F, where H_FMᵀ = [0₃ᵀ axis_Fᵀ]. + SpatialAcceleration calc_A_FM_M(const math::RigidTransform&, const T*, + const T*, const T* vdot) const { + return SpatialAcceleration(Vector3::Zero(), + vdot[0] * translation_axis()); // axis_M + } + + // Returns tau = H_FM_Fᵀ⋅F_F, where H_FM_Fᵀ = [0₃ᵀ axis_Fᵀ]. void calc_tau(const T*, const SpatialForce& F_BMo_F, T* tau) const { DRAKE_ASSERT(tau != nullptr); const Vector3& f_BMo_F = F_BMo_F.translational(); tau[0] = axis_F_.dot(f_BMo_F); } + // Returns tau = H_FM_Mᵀ⋅F_M, where H_FM_Mᵀ = [0₃ᵀ axis_Mᵀ], and + // axis_M == axis_F (see class comments). + void calc_tau_from_M(const math::RigidTransform&, const T*, + const SpatialForce& F_BMo_M, T* tau) const { + DRAKE_ASSERT(tau != nullptr); + const Vector3& f_BMo_M = F_BMo_M.translational(); + tau[0] = axis_F_.dot(f_BMo_M); // This is axis_M. + } + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; @@ -215,7 +240,7 @@ class PrismaticMobilizer final : public MobilizerImpl { const MultibodyTree& tree_clone) const; // Default axis expressed in the inboard frame F. It is a unit vector. - Vector3 axis_F_; + Vector3 axis_F_; // This is also axis_M. }; } // namespace internal diff --git a/multibody/tree/quaternion_floating_mobilizer.h b/multibody/tree/quaternion_floating_mobilizer.h index f49ed8295726..631978331175 100644 --- a/multibody/tree/quaternion_floating_mobilizer.h +++ b/multibody/tree/quaternion_floating_mobilizer.h @@ -31,7 +31,10 @@ namespace internal { // introduces the angular velocity w_FM of frame M in F and the linear // velocity v_FM of frame M's origin in frame F, ordered (w_FM, v_FM). // -// H_FM₆ₓ₆ = I₆ₓ₆ Hdot_FM₆ₓ₆ = 0₆ₓ₆ +// H_FM_F₆ₓ₆ = I₆ₓ₆ Hdot_FM_F₆ₓ₆ = 0₆ₓ₆ +// +// H_FM_M = R_MF ⋅ H_FM_F = [ R_MF 0 ] +// [ 0 R_MF ] // // @tparam_default_scalar template @@ -226,21 +229,54 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl { return SpatialVelocity(V_FM); // w_FM, v_FM } - // We chose the generalized velocities for this mobilizer so that H=I, Hdot=0. - // Therefore A_FM = H⋅vdot + Hdot⋅v = vdot. + // The generalized velocities v are in F so we have to re-express. + // TODO(sherm1) Consider switching coordinates to M, or make another joint. + SpatialVelocity calc_V_FM_M(const math::RigidTransform& X_FM, + const T* q, const T* v) const { + DRAKE_ASSERT(q != nullptr && v != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const Eigen::Map> v_vector(v); + const SpatialVelocity V_FM_F(v_vector); + const SpatialVelocity V_FM_M = R_FM.inverse() * V_FM_F; // 30 flops + return V_FM_M; // w_FM_M, v_FM_M + } + + // We chose the generalized velocities for this mobilizer so that H_F=I, + // Hdot_F=0. Therefore A_FM_F = H_F⋅vdot + Hdot_F⋅v = vdot. 0 flops SpatialAcceleration calc_A_FM(const T*, const T*, const T* vdot) const { DRAKE_ASSERT(vdot != nullptr); const Eigen::Map> A_FM(vdot); return SpatialAcceleration(A_FM); } - // Returns tau = H_FMᵀ⋅F. H is identity for this mobilizer. + // Sadly, reporting this in M is much more difficult because our generalized + // coordinates are in F. We'll just calculate in F and then re-express. + SpatialAcceleration calc_A_FM_M(const math::RigidTransform& X_FM, + const T* q, const T*, + const T* vdot) const { + DRAKE_ASSERT(q != nullptr && vdot != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialAcceleration A_FM_F = calc_A_FM(nullptr, nullptr, vdot); + const SpatialAcceleration A_FM_M = R_FM.inverse() * A_FM_F; // 30 flops + return A_FM_M; + } + + // Returns tau = H_FM_Fᵀ⋅F_F. H is identity for this mobilizer. void calc_tau(const T*, const SpatialForce& F_BMo_F, T* tau) const { DRAKE_ASSERT(tau != nullptr); Eigen::Map> tau_as_vector(tau); tau_as_vector = F_BMo_F.get_coeffs(); } + // Returns tau = H_FM_Mᵀ⋅F_M. See class comments. + void calc_tau_from_M(const math::RigidTransform& X_FM, const T* q, + const SpatialForce& F_BMo_M, T* tau) const { + DRAKE_ASSERT(q != nullptr && tau != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialForce F_BMo_F = R_FM * F_BMo_M; // 30 flops + calc_tau(nullptr, F_BMo_F, &*tau); + } + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; diff --git a/multibody/tree/revolute_mobilizer.h b/multibody/tree/revolute_mobilizer.h index e89a9a3db49b..f16f73c74acb 100644 --- a/multibody/tree/revolute_mobilizer.h +++ b/multibody/tree/revolute_mobilizer.h @@ -22,7 +22,9 @@ namespace internal { // outboard frames, while the distance between the two frames does not vary. // To fully specify this mobilizer a user must provide the inboard frame F, // the outboard (or "mobilized") frame M and the axis `axis_F` (expressed in -// frame F) about which frame M rotates with respect to F. +// frame F or equivalently in frame M since the measure numbers are identical +// in either frame) about which frame M rotates with respect to F. +// // The single generalized coordinate q introduced by this mobilizer // corresponds to the rotation angle in radians of frame M with respect to // frame F about the rotation axis `axis_F`. When `q = 0`, frames F and M are @@ -30,9 +32,11 @@ namespace internal { // right-hand-rule with the thumb aligned in the direction of the `axis_F`. // Notice that the components of the rotation axis as expressed in // either frame F or M are constant. That is, `axis_F` and `axis_M` remain -// unchanged w.r.t. both frames by this mobilizer's motion. +// identical and unchanged w.r.t. both frames by this mobilizer's motion. // -// H_FM₆ₓ₁=[axis_F 0₃]ᵀ Hdot_FM₆ₓ₁ = 0₆ +// H_FM_F₆ₓ₁=[axis_F 0₃]ᵀ Hdot_FM_F₆ₓ₁ = 0₆ +// H_FM_M₆ₓ₁=[axis_M 0₃]ᵀ Hdot_FM_M₆ₓ₁ = 0₆ +// where axis_M == axis_F // // @tparam_default_scalar template @@ -135,20 +139,46 @@ class RevoluteMobilizer final : public MobilizerImpl { // frame M measured and expressed in frame F as a function of the input // angular velocity `v` about this mobilizer's axis (@see revolute_axis()). SpatialVelocity calc_V_FM(const T*, const T* v) const { - return SpatialVelocity(v[0] * axis_F_, Vector3::Zero()); + return SpatialVelocity(v[0] * axis_F_, // 3 flops + Vector3::Zero()); + } + + SpatialVelocity calc_V_FM_M(const math::RigidTransform&, const T*, + const T* v) const { + return SpatialVelocity(v[0] * axis_F_, // really axis_M, 3 flops + Vector3::Zero()); } - // Here H₆ₓ₁=[axis, 0₃]ᵀ so Hdot = 0 and - // A_FM = H⋅vdot + Hdot⋅v = [axis⋅vdot, 0₃]ᵀ + // Here H_F₆ₓ₁=[axis_F, 0₃]ᵀ so Hdot_F = 0 and + // A_FM_F = H_F⋅vdot + Hdot_F⋅v = [axis_F⋅vdot, 0₃]ᵀ SpatialAcceleration calc_A_FM(const T*, const T*, const T* vdot) const { - return SpatialAcceleration(vdot[0] * axis_F_, Vector3::Zero()); + return SpatialAcceleration(vdot[0] * axis_F_, // 3 flops + Vector3::Zero()); + } + + // Here H_M₆ₓ₁=[axis_M, 0₃]ᵀ so Hdot_M = 0 and + // A_FM_M = H_M⋅vdot + Hdot_M⋅v = [axis_M⋅vdot, 0₃]ᵀ + // But axis_M == axis_F. + SpatialAcceleration calc_A_FM_M(const math::RigidTransform&, const T*, + const T*, const T* vdot) const { + return SpatialAcceleration(vdot[0] * axis_F_, // really axis_M, 3 flops + Vector3::Zero()); } - // Returns tau = H_FMᵀ⋅F, where H_FMᵀ = [axis_Fᵀ 0₃ᵀ]. + // Returns tau = H_FM_Fᵀ⋅F_F, where H_FM_Fᵀ = [axis_Fᵀ 0₃ᵀ]. void calc_tau(const T*, const SpatialForce& F_BMo_F, T* tau) const { DRAKE_ASSERT(tau != nullptr); const Vector3& t_BMo_F = F_BMo_F.rotational(); - tau[0] = axis_F_.dot(t_BMo_F); + tau[0] = axis_F_.dot(t_BMo_F); // 5 flops + } + + // Returns tau = H_FM_Mᵀ⋅F_M, where H_FM_Mᵀ = [axis_Mᵀ 0₃ᵀ] and + // axis_M == axis_F (see class comments). + void calc_tau_from_M(const math::RigidTransform&, const T*, + const SpatialForce& F_BMo_M, T* tau) const { + DRAKE_ASSERT(tau != nullptr); + const Vector3& t_BMo_M = F_BMo_M.rotational(); + tau[0] = axis_F_.dot(t_BMo_M); // This is axis_M also, 5 flops } math::RigidTransform CalcAcrossMobilizerTransform( @@ -215,7 +245,7 @@ class RevoluteMobilizer final : public MobilizerImpl { const MultibodyTree& tree_clone) const; // Default joint axis expressed in the inboard frame F. - Vector3 axis_F_; + Vector3 axis_F_; // This is also axis_M. }; } // namespace internal diff --git a/multibody/tree/rotational_inertia.h b/multibody/tree/rotational_inertia.h index cd9f758e7bc1..8b6b408f3358 100644 --- a/multibody/tree/rotational_inertia.h +++ b/multibody/tree/rotational_inertia.h @@ -189,7 +189,7 @@ class RotationalInertia { RotationalInertia(const T& Ixx, const T& Iyy, const T& Izz, const T& Ixy, const T& Ixz, const T& Iyz) { set_moments_and_products_no_validity_check(Ixx, Iyy, Izz, Ixy, Ixz, Iyz); - DRAKE_ASSERT_VOID(ThrowIfNotPhysicallyValid(__func__)); + // DRAKE_ASSERT_VOID(ThrowIfNotPhysicallyValid(__func__)); } /// Constructs a rotational inertia for a particle Q of mass `mass`, whose diff --git a/multibody/tree/rpy_ball_mobilizer.h b/multibody/tree/rpy_ball_mobilizer.h index 74f42b29184c..530a648a8636 100644 --- a/multibody/tree/rpy_ball_mobilizer.h +++ b/multibody/tree/rpy_ball_mobilizer.h @@ -57,8 +57,11 @@ namespace internal { // @note The roll-pitch-yaw (space x-y-z) Euler sequence is also known as the // Tait-Bryan angles or Cardan angles. // -// H_FM₆ₓ₃=[ I₃ₓ₃ ] Hdot_FM = 0₆ₓ₃ -// [ 0₃ₓ₃ ] +// H_FM_F₆ₓ₃=[ I₃ₓ₃ ] Hdot_FM_F = 0₆ₓ₃ +// [ 0₃ₓ₃ ] +// +// H_FM_M = R_MF ⋅ H_FM_F = [ R_MF ] +// [ 0 ] // // @tparam_default_scalar template @@ -194,13 +197,32 @@ class RpyBallMobilizer final : public MobilizerImpl { return SpatialVelocity(w_FM, Vector3::Zero()); } - // Here H₆ₓ₃=[I₃ₓ₃ 0₃ₓ₃]ᵀ so Hdot=0 and - // A_FM = H⋅vdot + Hdot⋅v = [vdot, 0₃]ᵀ + // Angular velocities v are in F. We'll just re-express. + SpatialVelocity calc_V_FM_M(const math::RigidTransform& X_FM, const T*, + const T* v) const { + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialVelocity V_FM_F = calc_V_FM(nullptr, v); + const SpatialVelocity V_FM_M = R_FM.inverse() * V_FM_F; // 30 flops + return V_FM_M; + } + + // Here H_F₆ₓ₃=[I₃ₓ₃ 0₃ₓ₃]ᵀ so Hdot_F=0 and + // A_FM_F = H_F⋅vdot + Hdot_F⋅v = [vdot, 0₃]ᵀ SpatialAcceleration calc_A_FM(const T*, const T*, const T* vdot) const { const Eigen::Map> alpha_FM(vdot); return SpatialAcceleration(alpha_FM, Vector3::Zero()); } + // Angular velocities v and accelerations vdot are in F. We'll just + // re-express. + SpatialAcceleration calc_A_FM_M(const math::RigidTransform& X_FM, + const T*, const T*, const T* vdot) const { + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialAcceleration A_FM_F = calc_A_FM(nullptr, nullptr, vdot); + const SpatialAcceleration A_FM_M = R_FM.inverse() * A_FM_F; // 30 flops + return A_FM_M; + } + // Returns tau = H_FMᵀ⋅F. The rotational part of H is identity here and // the rest is zero. void calc_tau(const T*, const SpatialForce& F_BMo_F, T* tau) const { @@ -210,6 +232,16 @@ class RpyBallMobilizer final : public MobilizerImpl { tau_as_vector = t_BMo_F; } + // Returns tau = H_FM_Mᵀ⋅F_M. See class comments. + void calc_tau_from_M(const math::RigidTransform& X_FM, const T* q, + const SpatialForce& F_BMo_M, T* tau) const { + DRAKE_ASSERT(q != nullptr && tau != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const Vector3& t_BMo_M = F_BMo_M.rotational(); + Eigen::Map> tau_as_vector(tau); + tau_as_vector = R_FM * t_BMo_M; // 15 flops + } + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; diff --git a/multibody/tree/rpy_floating_mobilizer.h b/multibody/tree/rpy_floating_mobilizer.h index abf27dcc49db..04c8052f55ca 100644 --- a/multibody/tree/rpy_floating_mobilizer.h +++ b/multibody/tree/rpy_floating_mobilizer.h @@ -61,6 +61,9 @@ namespace internal { // // H_FM₆ₓ₆ = I₆ₓ₆ Hdot_FM₆ₓ₆ = 0₆ₓ₆ // +// H_FM_M = R_MF ⋅ H_FM_F = [ R_MF 0 ] +// [ 0 R_MF ] +// // @tparam_default_scalar template class RpyFloatingMobilizer final : public MobilizerImpl { @@ -243,6 +246,18 @@ class RpyFloatingMobilizer final : public MobilizerImpl { return SpatialVelocity(V_FM); // w_FM, v_FM } + // The generalized velocities v are in F so we have to re-express. + // TODO(sherm1) Consider switching coordinates to M, or make another joint. + SpatialVelocity calc_V_FM_M(const math::RigidTransform& X_FM, + const T* q, const T* v) const { + DRAKE_ASSERT(q != nullptr && v != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const Eigen::Map> v_vector(v); + const SpatialVelocity V_FM_F(v_vector); + const SpatialVelocity V_FM_M = R_FM.inverse() * V_FM_F; // 30 flops + return V_FM_M; // w_FM_M, v_FM_M + } + // We chose the generalized velocities for this mobilizer so that H=I, Hdot=0. // Therefore A_FM = H⋅vdot + Hdot⋅v = vdot. SpatialAcceleration calc_A_FM(const T*, const T*, const T* vdot) const { @@ -250,6 +265,18 @@ class RpyFloatingMobilizer final : public MobilizerImpl { return SpatialAcceleration(A_FM); } + // Sadly, reporting this in M is much more difficult because our generalized + // velocities are in F. We'll just calculate in F and then re-express. + SpatialAcceleration calc_A_FM_M(const math::RigidTransform& X_FM, + const T* q, const T*, + const T* vdot) const { + DRAKE_ASSERT(q != nullptr && vdot != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialAcceleration A_FM_F = calc_A_FM(nullptr, nullptr, vdot); + const SpatialAcceleration A_FM_M = R_FM.inverse() * A_FM_F; // 30 flops + return A_FM_M; + } + // Returns tau = H_FMᵀ⋅F. H is identity for this mobilizer. void calc_tau(const T*, const SpatialForce& F_BMo_F, T* tau) const { DRAKE_ASSERT(tau != nullptr); @@ -257,6 +284,15 @@ class RpyFloatingMobilizer final : public MobilizerImpl { tau_as_vector = F_BMo_F.get_coeffs(); } + // Returns tau = H_FM_Mᵀ⋅F_M. See class comments. + void calc_tau_from_M(const math::RigidTransform& X_FM, const T* q, + const SpatialForce& F_BMo_M, T* tau) const { + DRAKE_ASSERT(q != nullptr && tau != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialForce F_BMo_F = R_FM * F_BMo_M; // 30 flops + calc_tau(nullptr, F_BMo_F, &*tau); + } + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; diff --git a/multibody/tree/screw_mobilizer.h b/multibody/tree/screw_mobilizer.h index ba539fad454b..fcdee2a0097a 100644 --- a/multibody/tree/screw_mobilizer.h +++ b/multibody/tree/screw_mobilizer.h @@ -34,7 +34,7 @@ T GetScrewRotationFromTranslation(const T& z, double screw_pitch) { return revolution_amount * 2 * M_PI; } -/* This mobilizer models a screw joint between an inboard frame F and an +/* This Mobilizer models a screw joint between an inboard frame F and an outboard frame M that enables translation along an axis while rotating about it, such that the axis is constant when measured in either this mobilizer's inboard or outboard frames. @@ -52,8 +52,9 @@ T GetScrewRotationFromTranslation(const T& z, double screw_pitch) { at all times for this mobilizer. The generalized velocity for this mobilizer is the rate of change of the coordinate, ω =˙θ (θ_dot). - H_FM₆ₓ₁ = [axisᵀ f⋅axisᵀ]ᵀ where f=pitch/2π - Hdot_FM = 0₆ₓ₁ + H_FM_F₆ₓ₁ = [axis_Fᵀ f⋅axis_Fᵀ]ᵀ with f=pitch/2π Hdot_FM_F = 0₆ₓ₁ + H_FM_M₆ₓ₁ = [axis_Mᵀ f⋅axis_Mᵀ]ᵀ Hdot_FM_M = 0₆ₓ₁ + where axis_M == axis_F @tparam_default_scalar */ template @@ -205,23 +206,39 @@ class ScrewMobilizer final : public MobilizerImpl { /* Computes the across-mobilizer velocity V_FM(q, v) of the outboard frame M measured and expressed in frame F as a function of the input velocity v, which is the angular velocity. We scale that by the pitch to find the - related translational velocity. */ + related translational velocity. 8 flops */ SpatialVelocity calc_V_FM(const T*, const T* v) const { DRAKE_ASSERT(v != nullptr); const T f_v = GetScrewTranslationFromRotation(v[0], screw_pitch_); - return SpatialVelocity(v[0] * axis_, f_v * axis_); + return SpatialVelocity(v[0] * axis_, f_v * axis_); // axis_F + } + + /* Same as V_FM_F. 8 flops */ + SpatialVelocity calc_V_FM_M(const math::RigidTransform&, const T*, + const T* v) const { + DRAKE_ASSERT(v != nullptr); + const T f_v = GetScrewTranslationFromRotation(v[0], screw_pitch_); + return SpatialVelocity(v[0] * axis_, f_v * axis_); // axis_M } /* Our lone generalized acceleration is the angular acceleration θdotdot about the screw axis. Therefore we have H₆ₓ₁=[axis f⋅axis] where f=pitch/2π, and - Hdot=0, so A_FM = H⋅vdot + Hdot⋅v = [axis⋅vdot, f⋅axis⋅vdot]ᵀ */ + Hdot=0, so A_FM = H⋅vdot + Hdot⋅v = [axis⋅vdot, f⋅axis⋅vdot]ᵀ. 8 flops */ SpatialAcceleration calc_A_FM(const T*, const T*, const T* vdot) const { DRAKE_ASSERT(vdot != nullptr); const T f_vdot = GetScrewTranslationFromRotation(vdot[0], screw_pitch_); return SpatialAcceleration(vdot[0] * axis_, f_vdot * axis_); } - /* Returns tau = H_FMᵀ⋅F. See above for H. */ + /* Same as A_FM_F. 8 flops */ + SpatialAcceleration calc_A_FM_M(const math::RigidTransform&, const T*, + const T*, const T* vdot) const { + DRAKE_ASSERT(vdot != nullptr); + const T f_vdot = GetScrewTranslationFromRotation(vdot[0], screw_pitch_); + return SpatialAcceleration(vdot[0] * axis_, f_vdot * axis_); + } + + /* Returns tau = H_FMᵀ⋅F. See above for H. 12 flops */ void calc_tau(const T*, const SpatialForce& F_BMo_F, T* tau) const { DRAKE_ASSERT(tau != nullptr); const T f = screw_pitch_ / (2 * M_PI); @@ -230,6 +247,17 @@ class ScrewMobilizer final : public MobilizerImpl { tau[0] = axis_.dot(t_B_F) + f * axis_.dot(f_BMo_F); } + /* Returns tau = H_FM_Mᵀ⋅F_M, where H_FM_Mᵀ = [axis_Mᵀ f⋅axis_Mᵀ] and + axis_M == axis_F (see class comments). 12 flops */ + void calc_tau_from_M(const math::RigidTransform&, const T*, + const SpatialForce& F_BMo_M, T* tau) const { + DRAKE_ASSERT(tau != nullptr); + const T f = screw_pitch_ / (2 * M_PI); + const Vector3& t_B_M = F_BMo_M.rotational(); // torque + const Vector3& f_BMo_M = F_BMo_M.translational(); // force + tau[0] = axis_.dot(t_B_M) + f * axis_.dot(f_BMo_M); // This is axis_M. + } + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; diff --git a/multibody/tree/test/body_node_test.cc b/multibody/tree/test/body_node_test.cc index ad4a33eb98d8..548c9a7bc33a 100644 --- a/multibody/tree/test/body_node_test.cc +++ b/multibody/tree/test/body_node_test.cc @@ -73,6 +73,12 @@ class DummyBodyNode : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcPositionKinematicsCache2_BaseToTip( + const FrameBodyPoseCache&, const T*, + PositionKinematicsCache2*) const final { + DRAKE_UNREACHABLE(); + } + void CalcAcrossNodeJacobianWrtVExpressedInWorld( const FrameBodyPoseCache&, const T*, const PositionKinematicsCache&, std::vector>*) const final { @@ -86,6 +92,12 @@ class DummyBodyNode : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcVelocityKinematicsCache2_BaseToTip( + const T*, const PositionKinematicsCache2&, const T*, + VelocityKinematicsCache2*) const final { + DRAKE_UNREACHABLE(); + } + void CalcMassMatrixContribution_TipToBase( const PositionKinematicsCache&, const std::vector>&, const std::vector>&, EigenPtr>) const final { @@ -115,6 +127,13 @@ class DummyBodyNode : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcSpatialAcceleration2_BaseToTip( + const T*, const PositionKinematicsCache2&, const T*, + const VelocityKinematicsCache2&, const T*, + std::vector>*) const final { + DRAKE_UNREACHABLE(); + } + void CalcInverseDynamics_TipToBase(const FrameBodyPoseCache&, const T*, const PositionKinematicsCache&, const std::vector>&, @@ -127,6 +146,15 @@ class DummyBodyNode : public BodyNode { DRAKE_UNREACHABLE(); } + void CalcInverseDynamics2_TipToBase( + const FrameBodyPoseCache&, const T*, + const PositionKinematicsCache2&, const VelocityKinematicsCache2&, + const std::vector>&, + const std::vector>&, const Eigen::Ref>&, + std::vector>*, EigenPtr>) const final { + DRAKE_UNREACHABLE(); + } + void CalcArticulatedBodyInertiaCache_TipToBase( const systems::Context&, const PositionKinematicsCache&, const Eigen::Ref>&, const SpatialInertia&, diff --git a/multibody/tree/universal_mobilizer.h b/multibody/tree/universal_mobilizer.h index 85c3e187533f..d322559ccf71 100644 --- a/multibody/tree/universal_mobilizer.h +++ b/multibody/tree/universal_mobilizer.h @@ -43,13 +43,13 @@ namespace internal { // aligned in the direction of their respective axes. The generalized // velocities for this mobilizer are the rate of change of the angles, v = q̇. // -// H_FM₆ₓ₂ = [Hw_FM₃ₓ₂] Hw_FM = [ 1 0 ] Hv_FM = 0₃ₓ₂ -// [Hv_FM₃ₓ₂] [ 0 c(q₀) ] -// [ 0 s(q₀) ] +// H_FM_F₆ₓ₂ = [Hw_FM₃ₓ₂] Hw_FM_F = [ 1 0 ] Hv_FM_F = 0₃ₓ₂ +// [Hv_FM₃ₓ₂] [ 0 c(q₀) ] +// [ 0 s(q₀) ] // -// Hdot_FM₆ₓ₂ = [Hwdot_FM₃ₓ₂] Hwdot_FM = [ 0 0 ] Hvdot_FM = 0₃ₓ₂ -// [Hvdot_FM₃ₓ₂] [ 0 -v₀s(q₀) ] -// [ 0 v₀c(q₀) ] +// Hdot_FM_F₆ₓ₂ = [Hwdot_FM₃ₓ₂] Hwdot_FM_F = [ 0 0 ] Hvdot_FM_F = 0₃ₓ₂ +// [Hvdot_FM₃ₓ₂] [ 0 -v₀s(q₀) ] +// [ 0 v₀c(q₀) ] // // @tparam_default_scalar template @@ -128,13 +128,13 @@ class UniversalMobilizer final : public MobilizerImpl { // frame F and the outboard frame M as a function of the angles (θ₀, θ₁) // stored in `context`. math::RigidTransform calc_X_FM(const T* q) const { - const T s1 = sin(q[0]), c1 = cos(q[0]); - const T s2 = sin(q[1]), c2 = cos(q[1]); + const T s0 = sin(q[0]), c0 = cos(q[0]); + const T s1 = sin(q[1]), c1 = cos(q[1]); Matrix3 R_FM_matrix; // clang-format off - R_FM_matrix << c2, 0.0, s2, - s1 * s2, c1, -s1 * c2, - -c1 * s2, s1, c1 * c2; + R_FM_matrix << c1, 0.0, s1, + s0 * s1, c0, -s0 * c1, + -c0 * s1, s0, c0 * c1; // clang-format on return math::RigidTransform( math::RotationMatrix::MakeUnchecked(R_FM_matrix), @@ -152,6 +152,15 @@ class UniversalMobilizer final : public MobilizerImpl { return SpatialVelocity(Hw * w, Vector3::Zero()); } + // TODO(sherm1) Could do better than this if anyone cares. + SpatialVelocity calc_V_FM_M(const math::RigidTransform& X_FM, + const T* q, const T* v) const { + const SpatialVelocity V_FM_F = calc_V_FM(q, v); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialVelocity V_FM_M = R_FM.inverse() * V_FM_F; // 30 flops + return V_FM_M; + } + // Hwᵀ Hvᵀ Hw_dotᵀ Hv_dotᵀ // Here H₆ₓ₂=[1 0 0 | 0₃]ᵀ Hdot = [ 0₃ | 0₃ ] // [0 c(q₀) s(q₀) | 0₃] [ 0 -v₀s(q₀) v₀c(q₀) | 0₃ ] @@ -166,15 +175,35 @@ class UniversalMobilizer final : public MobilizerImpl { Vector3::Zero()); } - // Returns tau = H_FMᵀ⋅F. See above for the structure of H. + // TODO(sherm1) Could do better than this if anyone cares. + SpatialAcceleration calc_A_FM_M(const math::RigidTransform& X_FM, + const T* q, const T* v, + const T* vdot) const { + const SpatialAcceleration A_FM_F = calc_A_FM(q, v, vdot); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialAcceleration A_FM_M = R_FM.inverse() * A_FM_F; + return A_FM_M; + } + + // Returns tau = H_FM_Fᵀ⋅F_F. See above for the structure of H. void calc_tau(const T* q, const SpatialForce& F_BMo_F, T* tau) const { - DRAKE_ASSERT(tau != nullptr); + DRAKE_ASSERT(q != nullptr && tau != nullptr); Eigen::Map> tau_as_vector(tau); const Vector3& t_B_F = F_BMo_F.rotational(); // torque const Eigen::Matrix Hw_FM = this->CalcHwMatrix(q); tau_as_vector = Hw_FM.transpose() * t_B_F; } + // Returns tau = H_FM_Mᵀ⋅F_M. + // TODO(sherm1) Just re-expressing here; could likely do better. + void calc_tau_from_M(const math::RigidTransform& X_FM, const T* q, + const SpatialForce& F_BMo_M, T* tau) const { + DRAKE_ASSERT(q != nullptr && tau != nullptr); + const math::RotationMatrix& R_FM = X_FM.rotation(); + const SpatialForce F_BMo_F = R_FM * F_BMo_M; + return calc_tau(q, F_BMo_F, &*tau); + } + math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const final; diff --git a/multibody/tree/velocity_kinematics_cache.h b/multibody/tree/velocity_kinematics_cache.h index 12496bdaec99..d5e9e52c93c3 100644 --- a/multibody/tree/velocity_kinematics_cache.h +++ b/multibody/tree/velocity_kinematics_cache.h @@ -139,6 +139,39 @@ class VelocityKinematicsCache { SpatialVelocity_PoolType V_PB_W_pool_; }; +template +class VelocityKinematicsCache2 { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(VelocityKinematicsCache2); + + explicit VelocityKinematicsCache2(int num_mobods) + : V_FM_M_pool_(num_mobods), V_WM_M_pool_(num_mobods) { + V_FM_M_pool_[0].SetZero(); // World velocity is zero. + V_WM_M_pool_[0].SetZero(); + } + + const SpatialVelocity& get_V_FM_M(MobodIndex index) const { + return V_FM_M_pool_[index]; + } + + const SpatialVelocity& get_V_WM_M(MobodIndex index) const { + return V_WM_M_pool_[index]; + } + + SpatialVelocity& get_mutable_V_FM_M(MobodIndex index) { + return V_FM_M_pool_[index]; + } + + SpatialVelocity& get_mutable_V_WM_M(MobodIndex index) { + return V_WM_M_pool_[index]; + } + + private: + // Index by MobodIndex. + std::vector> V_FM_M_pool_; + std::vector> V_WM_M_pool_; +}; + } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/tree/weld_mobilizer.h b/multibody/tree/weld_mobilizer.h index 0de03deae2ba..808b32ddd4b5 100644 --- a/multibody/tree/weld_mobilizer.h +++ b/multibody/tree/weld_mobilizer.h @@ -62,12 +62,24 @@ class WeldMobilizer final : public MobilizerImpl { return SpatialVelocity::Zero(); } + SpatialVelocity calc_V_FM_M(const math::RigidTransform&, const T*, + const T*) const { + return SpatialVelocity::Zero(); + } + SpatialAcceleration calc_A_FM(const T*, const T*, const T*) const { return SpatialAcceleration::Zero(); } + SpatialAcceleration calc_A_FM_M(const math::RigidTransform&, const T*, + const T*, const T*) const { + return SpatialAcceleration::Zero(); + } + // Does nothing since there are no taus. void calc_tau(const T*, const SpatialForce&, T*) const {} + void calc_tau_from_M(const math::RigidTransform&, const T*, + const SpatialForce&, T*) const {} math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context&) const final;