Skip to content

Commit

Permalink
Adds inline calc_A_FM(), calc_tau() ∀ mobilizers & calls them. (Robot…
Browse files Browse the repository at this point in the history 
…Locomotion#22014)

Removes passed-in context from some calls.
Cleaned up CalcInverseDynamics_TipToBase() and CalcSpatialAccelerations_BaseToTip().
  • Loading branch information
@sherm1
sherm1 authored Oct 22, 2024
1 parent ac4d16f commit 3439d53
Show file tree
Hide file tree
Showing 27 changed files with 754 additions and 740 deletions.
43 changes: 0 additions & 43 deletions multibody/tree/body_node.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,49 +86,6 @@ void BodyNode<T>::CalcCompositeBodyInertia_TipToBase(
}
}

// This method computes the total force Ftot_BBo on body B that must be
// applied for it to incur in a spatial acceleration A_WB. Mathematically:
// Ftot_BBo = M_B_W * A_WB + b_Bo
// where b_Bo contains the velocity dependent gyroscopic terms (see Eq. 2.26,
// p. 27, in A. Jain's book). The above balance is performed at the origin
// Bo of the body frame B, which does not necessarily need to coincide with
// the body center of mass.
// Notes:
// 1. Ftot_BBo = b_Bo when A_WB = 0.
// 2. b_Bo = 0 when w_WB = 0.
// 3. b_Bo.translational() = 0 when Bo = Bcm (p_BoBcm = 0).
// When Fb_Bo_W_cache is nullptr velocities are considered to be zero.
// Therefore, from (2), the bias term is assumed to be zero and is not
// computed.
template <typename T>
void BodyNode<T>::CalcBodySpatialForceGivenItsSpatialAcceleration(
const std::vector<SpatialInertia<T>>& M_B_W_cache,
const std::vector<SpatialForce<T>>* Fb_Bo_W_cache,
const SpatialAcceleration<T>& A_WB, SpatialForce<T>* Ftot_BBo_W_ptr) const {
DRAKE_DEMAND(Ftot_BBo_W_ptr != nullptr);

// Output spatial force applied on mobilized body B, at Bo, measured in W.
SpatialForce<T>& Ftot_BBo_W = *Ftot_BBo_W_ptr;

// RigidBody for this node.
const RigidBody<T>& body_B = body();

// Mobilized body B spatial inertia about Bo expressed in world W.
const SpatialInertia<T>& M_B_W = M_B_W_cache[body_B.mobod_index()];

// Equations of motion for a rigid body written at a generic point Bo not
// necessarily coincident with the body's center of mass. This corresponds
// to Eq. 2.26 (p. 27) in A. Jain's book.
Ftot_BBo_W = M_B_W * A_WB;

// If velocities are zero, then Fb_Bo_W is zero and does not contribute.
if (Fb_Bo_W_cache != nullptr) {
// Dynamic bias for body B.
const SpatialForce<T>& Fb_Bo_W = (*Fb_Bo_W_cache)[body_B.mobod_index()];
Ftot_BBo_W += Fb_Bo_W;
}
}

template <typename T>
void BodyNode<T>::CalcArticulatedBodyHingeInertiaMatrixFactorization(
const MatrixUpTo6<T>& D_B,
Expand Down
230 changes: 95 additions & 135 deletions multibody/tree/body_node.h
View file

Large diffs are not rendered by default.

467 changes: 229 additions & 238 deletions multibody/tree/body_node_impl.cc
View file

Large diffs are not rendered by default.

35 changes: 16 additions & 19 deletions multibody/tree/body_node_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,21 +64,17 @@ class BodyNodeImpl final : public BodyNode<T> {

~BodyNodeImpl() final;

// TODO(sherm1) Just a warm up -- move the rest of the kernel computations
// here also.

void CalcPositionKinematicsCache_BaseToTip(
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
PositionKinematicsCache<T>* pc) const final;

void CalcAcrossNodeJacobianWrtVExpressedInWorld(
const systems::Context<T>& context,
const FrameBodyPoseCache<T>& frame_body_pose_cache,
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
const PositionKinematicsCache<T>& pc,
std::vector<Vector6<T>>* H_PB_W_cache) const final;

void CalcVelocityKinematicsCache_BaseToTip(
const systems::Context<T>& context, const PositionKinematicsCache<T>& pc,
const T* positions, const PositionKinematicsCache<T>& pc,
const std::vector<Vector6<T>>& H_PB_W_cache, const T* velocities,
VelocityKinematicsCache<T>* vc) const final;

Expand Down Expand Up @@ -106,22 +102,20 @@ class BodyNodeImpl final : public BodyNode<T> {
#undef DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK

void CalcSpatialAcceleration_BaseToTip(
const systems::Context<T>& context,
const FrameBodyPoseCache<T>& frame_body_poses_cache,
const PositionKinematicsCache<T>& pc,
const VelocityKinematicsCache<T>* vc, const VectorX<T>& mbt_vdot,
std::vector<SpatialAcceleration<T>>* A_WB_array_ptr) const final;
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
const PositionKinematicsCache<T>& pc, const T* velocities,
const VelocityKinematicsCache<T>* vc, const T* accelerations,
std::vector<SpatialAcceleration<T>>* A_WB_array) const final;

void CalcInverseDynamics_TipToBase(
const systems::Context<T>& context,
const FrameBodyPoseCache<T>& frame_body_pose_cache,
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
const PositionKinematicsCache<T>& pc,
const std::vector<SpatialInertia<T>>& M_B_W_cache,
const std::vector<SpatialForce<T>>* Fb_Bo_W_cache,
const std::vector<SpatialAcceleration<T>>& A_WB_array,
const SpatialForce<T>& Fapplied_Bo_W,
const std::vector<SpatialForce<T>>& Fapplied_Bo_W,
const Eigen::Ref<const VectorX<T>>& tau_applied,
std::vector<SpatialForce<T>>* F_BMo_W_array_ptr,
std::vector<SpatialForce<T>>* F_BMo_W_array,
EigenPtr<VectorX<T>> tau_array) const final;

void CalcArticulatedBodyInertiaCache_TipToBase(
Expand All @@ -148,11 +142,10 @@ class BodyNodeImpl final : public BodyNode<T> {
AccelerationKinematicsCache<T>* ac) const final;

void CalcSpatialAccelerationBias(
const systems::Context<T>& context,
const FrameBodyPoseCache<T>& frame_body_pose_cache,
const PositionKinematicsCache<T>& pc,
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
const PositionKinematicsCache<T>& pc, const T* velocities,
const VelocityKinematicsCache<T>& vc,
SpatialAcceleration<T>* Ab_WB) const final;
std::vector<SpatialAcceleration<T>>* Ab_WB_array) const final;

private:
// Given a pointer to the contiguous array of all q's in this system, returns
Expand All @@ -174,6 +167,10 @@ class BodyNodeImpl final : public BodyNode<T> {
return &velocities[mobilizer().velocity_start_in_v()];
}

T* get_mutable_v(T* velocities) const {
return &velocities[mobilizer().velocity_start_in_v()];
}

// Returns this mobilizer's vs as a fixed-size VVector.
Eigen::Map<const VVector> get_vvector(const T* velocities) const {
return Eigen::Map<const VVector>(get_v(velocities));
Expand Down
37 changes: 19 additions & 18 deletions multibody/tree/body_node_world.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,13 @@ class BodyNodeWorld final : public BodyNode<T> {
}

void CalcAcrossNodeJacobianWrtVExpressedInWorld(
const systems::Context<T>&, const FrameBodyPoseCache<T>&,
const PositionKinematicsCache<T>&, std::vector<Vector6<T>>*) const final {
const FrameBodyPoseCache<T>&, const T*, const PositionKinematicsCache<T>&,
std::vector<Vector6<T>>*) const final {
DRAKE_UNREACHABLE();
}

void CalcVelocityKinematicsCache_BaseToTip(
const systems::Context<T>&, const PositionKinematicsCache<T>&,
const T*, const PositionKinematicsCache<T>&,
const std::vector<Vector6<T>>&, const T*,
VelocityKinematicsCache<T>*) const final {
DRAKE_UNREACHABLE();
Expand Down Expand Up @@ -67,19 +67,21 @@ class BodyNodeWorld final : public BodyNode<T> {
#undef DEFINE_DUMMY_OFF_DIAGONAL_BLOCK

void CalcSpatialAcceleration_BaseToTip(
const systems::Context<T>&, const FrameBodyPoseCache<T>&,
const PositionKinematicsCache<T>&, const VelocityKinematicsCache<T>*,
const VectorX<T>&, std::vector<SpatialAcceleration<T>>*) const final {
const FrameBodyPoseCache<T>&, const T*, const PositionKinematicsCache<T>&,
const T*, const VelocityKinematicsCache<T>*, const T*,
std::vector<SpatialAcceleration<T>>*) const final {
DRAKE_UNREACHABLE();
}

void CalcInverseDynamics_TipToBase(
const systems::Context<T>&, const FrameBodyPoseCache<T>&,
const PositionKinematicsCache<T>&, const std::vector<SpatialInertia<T>>&,
const std::vector<SpatialForce<T>>*,
const std::vector<SpatialAcceleration<T>>&, const SpatialForce<T>&,
const Eigen::Ref<const VectorX<T>>&, std::vector<SpatialForce<T>>*,
EigenPtr<VectorX<T>>) const final {
void CalcInverseDynamics_TipToBase(const FrameBodyPoseCache<T>&, const T*,
const PositionKinematicsCache<T>&,
const std::vector<SpatialInertia<T>>&,
const std::vector<SpatialForce<T>>*,
const std::vector<SpatialAcceleration<T>>&,
const std::vector<SpatialForce<T>>&,
const Eigen::Ref<const VectorX<T>>&,
std::vector<SpatialForce<T>>*,
EigenPtr<VectorX<T>>) const final {
DRAKE_UNREACHABLE();
}

Expand Down Expand Up @@ -115,11 +117,10 @@ class BodyNodeWorld final : public BodyNode<T> {
DRAKE_UNREACHABLE();
}

void CalcSpatialAccelerationBias(const systems::Context<T>&,
const FrameBodyPoseCache<T>&,
const PositionKinematicsCache<T>&,
const VelocityKinematicsCache<T>&,
SpatialAcceleration<T>*) const final {
void CalcSpatialAccelerationBias(
const FrameBodyPoseCache<T>&, const T*, const PositionKinematicsCache<T>&,
const T*, const VelocityKinematicsCache<T>&,
std::vector<SpatialAcceleration<T>>*) const final {
DRAKE_UNREACHABLE();
}
};
Expand Down
21 changes: 13 additions & 8 deletions multibody/tree/mobilizer_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,23 +29,28 @@ of dynamic-sized Eigen matrices that would otherwise lead to run-time
dynamic memory allocations.
Every concrete Mobilizer derived from MobilizerImpl must implement the
following (ideally inline) methods:
following (ideally inline) methods.
// Returns X_FM(q)
math::RigidTransform<T> calc_X_FM(const T* q) const;
SpatialVelocity<T> calc_V_FM(const systems::Context<T>&,
// Returns H_FM(q)⋅v
SpatialVelocity<T> calc_V_FM(const T* q,
const T* v) const;
// Returns H_FM(q)⋅vdot + Hdot_FM(q,v)⋅v
SpatialAcceleration<T> calc_A_FM(const T* q,
const T* v,
const T* vdot) const;
// Returns tau = H_FMᵀ(q)⋅F_BMo_F
void calc_tau(const T* q, const SpatialForce<T>& F_BMo_F, T* tau) const;
The coordinate pointers are guaranteed to point to the kNq or kNv state
variables for the particular mobilizer. They are only 8-byte aligned so
be careful when interpreting them as Eigen vectors for computation purposes.
TODO(sherm1) The above signatures should _not_ include a Context; all the
low-level methods should be purely numerical. Anything needed from the
Context should be extracted once prior to the tree recursion and passed
directly to the low-level methods.
%MobilizerImpl also provides a number of size specific methods to retrieve
MobilizerImpl also provides a number of size specific methods to retrieve
multibody quantities of interest from caching structures. These are common
to all mobilizer implementations and therefore they live in this class.
Users should not need to interact with this class directly unless they need
Expand Down
Loading

0 comments on commit 3439d53

Please sign in to comment.