Skip to content

Commit

Permalink
Templatize internal types QVector, VVector, HMatrix (RobotLocomotion#…
Browse files Browse the repository at this point in the history
  • Loading branch information
sherm1 authored and RussTedrake committed Dec 15, 2024
1 parent 4fc7389 commit 332e9d4
Show file tree
Hide file tree
Showing 13 changed files with 95 additions and 52 deletions.
14 changes: 7 additions & 7 deletions multibody/tree/body_node_impl.cc
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ void BodyNodeImpl<T, ConcreteMobilizer>::
const Vector3<T> p_MB_F = R_FM * p_MB_M;

const T* q = get_q(positions); // just this mobilizer's q's
VVector v = VVector::Zero();
VVector<T> v = VVector<T>::Zero();
auto H_PB_W = get_mutable_H(H_PB_W_cache);
// We compute H_FM(q) one column at a time by calling the multiplication by
// H_FM operation on a vector of generalized velocities which is zero except
Expand Down Expand Up @@ -190,7 +190,7 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcVelocityKinematicsCache_BaseToTip(
// Hinge matrix for this node. H_PB_W ∈ ℝ⁶ˣⁿᵛ with nv ∈ [0; 6] the
// number of mobilities for this node.
const auto H_PB_W = get_H(H_PB_W_cache); // 6 x kNv fixed-size Map.
const Eigen::Map<const VVector> v(v_ptr);
const Eigen::Map<const VVector<T>> v(v_ptr);
V_PB_W.get_coeffs() = H_PB_W * v;
} else {
V_PB_W.get_coeffs().setZero();
Expand Down Expand Up @@ -305,7 +305,7 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAcceleration_BaseToTip(
// Early return if we don't have to deal with velocity.
if (vc == nullptr) {
DRAKE_ASSERT(velocities == nullptr);
const VVector v = VVector::Zero();
const VVector<T> v = VVector<T>::Zero();
// Operator A_FM = H_FM * vmdot + Hdot_FM * 0
const SpatialAcceleration<T> A_FM =
mobilizer_->calc_A_FM(get_q(positions), v.data(), get_v(accelerations));
Expand Down Expand Up @@ -505,11 +505,11 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcInverseDynamics_TipToBase(

// Output generalized forces due to the mobilizer reaction (must remove any
// applied taus). Indexing is the same as generalized velocities.
Eigen::Map<VVector> tau(get_mutable_v(tau_array->data()));
Eigen::Map<VVector<T>> tau(get_mutable_v(tau_array->data()));
// Be careful not to overwrite tau_app before we use it!
if (is_tau_applied) {
const Eigen::Map<const VVector> tau_app(get_v(tau_applied_array.data()));
VVector tau_projection;
const Eigen::Map<const VVector<T>> tau_app(get_v(tau_applied_array.data()));
VVector<T> tau_projection;
mobilizer_->calc_tau(get_q(positions), F_BMo_F, tau_projection.data());
tau = tau_projection - tau_app;
} else {
Expand Down Expand Up @@ -821,7 +821,7 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAccelerationBias(

// We first compute the acceleration bias Ab_FM = Hdot * vm.
// Note, A_FM = H_FM(qm) * vmdot + Ab_FM(qm, vm).
const VVector vmdot_zero = VVector::Zero();
const VVector<T> vmdot_zero = VVector<T>::Zero();
const SpatialAcceleration<T> Ab_FM = mobilizer_->calc_A_FM(
get_q(positions), get_v(velocities), vmdot_zero.data());

Expand Down
25 changes: 14 additions & 11 deletions multibody/tree/body_node_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,12 @@ class BodyNodeImpl final : public BodyNode<T> {
kNv = ConcreteMobilizer<T>::kNv,
kNx = ConcreteMobilizer<T>::kNx
};
using QVector = typename ConcreteMobilizer<T>::QVector;
using VVector = typename ConcreteMobilizer<T>::VVector;
using HMatrix = typename ConcreteMobilizer<T>::HMatrix;
template <typename U>
using QVector = typename ConcreteMobilizer<T>::template QVector<U>;
template <typename U>
using VVector = typename ConcreteMobilizer<T>::template VVector<U>;
template <typename U>
using HMatrix = typename ConcreteMobilizer<T>::template HMatrix<U>;

using BodyNode<T>::body;
using BodyNode<T>::child_nodes;
Expand Down Expand Up @@ -156,8 +159,8 @@ class BodyNodeImpl final : public BodyNode<T> {
}

// Returns this mobilizer's qs as a fixed-size QVector.
Eigen::Map<const QVector> get_qvector(const T* positions) const {
return Eigen::Map<const QVector>(get_q(positions));
Eigen::Map<const QVector<T>> get_qvector(const T* positions) const {
return Eigen::Map<const QVector<T>>(get_q(positions));
}

// Given a pointer to the contiguous array of all v's in this system, returns
Expand All @@ -172,28 +175,28 @@ class BodyNodeImpl final : public BodyNode<T> {
}

// 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));
Eigen::Map<const VVector<T>> get_vvector(const T* velocities) const {
return Eigen::Map<const VVector<T>>(get_v(velocities));
}

// Given a complete array of hinge matrices H stored by contiguous columns,
// returns a const reference to H for this mobilizer, as a 6xnv fixed-size
// matrix. This matrix is 16-byte aligned because it is composed of
// columns of Eigen::Vector6 objects which Eigen aligns.
Eigen::Map<const HMatrix, Eigen::Aligned16> get_H(
Eigen::Map<const HMatrix<T>, Eigen::Aligned16> get_H(
const std::vector<Vector6<T>>& H_cache) const {
return Eigen::Map<const HMatrix, Eigen::Aligned16>(
return Eigen::Map<const HMatrix<T>, Eigen::Aligned16>(
H_cache[mobilizer().velocity_start_in_v()].data());
}

// Given a pointer to a mutable complete array of hinge matrices H stored by
// contiguous columns, returns a mutable reference to H for this mobilizer,
// as a 6xnv fixed-size matrix. This matrix is 16-byte aligned because it is
// composed of columns of Eigen::Vector6 objects which Eigen aligns.
Eigen::Map<HMatrix, Eigen::Aligned16> get_mutable_H(
Eigen::Map<HMatrix<T>, Eigen::Aligned16> get_mutable_H(
std::vector<Vector6<T>>* H_cache) const {
DRAKE_ASSERT(H_cache != nullptr);
return Eigen::Map<HMatrix, Eigen::Aligned16>(
return Eigen::Map<HMatrix<T>, Eigen::Aligned16>(
(*H_cache)[mobilizer().velocity_start_in_v()].data());
}

Expand Down
9 changes: 6 additions & 3 deletions multibody/tree/mobilizer_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -71,9 +71,12 @@ class MobilizerImpl : public Mobilizer<T> {
kNv = compile_time_num_velocities,
kNx = compile_time_num_positions + compile_time_num_velocities
};
using QVector = Eigen::Matrix<T, kNq, 1>;
using VVector = Eigen::Matrix<T, kNv, 1>;
using HMatrix = Eigen::Matrix<T, 6, kNv>;
template <typename U>
using QVector = Eigen::Matrix<U, kNq, 1>;
template <typename U>
using VVector = Eigen::Matrix<U, kNv, 1>;
template <typename U>
using HMatrix = Eigen::Matrix<U, 6, kNv>;

// As with Mobilizer this the only constructor available for this base class.
// The minimum amount of information that we need to define a mobilizer is
Expand Down
8 changes: 6 additions & 2 deletions multibody/tree/planar_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,12 @@ class PlanarMobilizer final : public MobilizerImpl<T, 3, 3> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PlanarMobilizer);
using MobilizerBase = MobilizerImpl<T, 3, 3>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

/* Constructor for a %PlanarMobilizer between an inboard frame F
`inboard_frame_F` and an outboard frame M `outboard_frame_M` granting two
Expand Down
8 changes: 6 additions & 2 deletions multibody/tree/prismatic_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,12 @@ class PrismaticMobilizer final : public MobilizerImpl<T, 1, 1> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PrismaticMobilizer);
using MobilizerBase = MobilizerImpl<T, 1, 1>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

// Constructor for a %PrismaticMobilizer between the `inboard_frame_F` and
// `outboard_frame_M` granting a single translational degree of freedom along
Expand Down
5 changes: 3 additions & 2 deletions multibody/tree/quaternion_floating_mobilizer.cc
Original file line number Diff line number Diff line change
Expand Up @@ -223,8 +223,9 @@ QuaternionFloatingMobilizer<T>::SetTranslationalVelocity(
}

template <typename T>
Vector<double, 7> QuaternionFloatingMobilizer<T>::get_zero_position() const {
Vector<double, 7> q = Vector<double, 7>::Zero();
auto QuaternionFloatingMobilizer<T>::get_zero_position() const
-> QVector<double> {
QVector<double> q = QVector<double>::Zero();
const Quaternion<double> quaternion = Quaternion<double>::Identity();
q[0] = quaternion.w();
q.template segment<3>(1) = quaternion.vec();
Expand Down
16 changes: 10 additions & 6 deletions multibody/tree/quaternion_floating_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,12 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(QuaternionFloatingMobilizer);
using MobilizerBase = MobilizerImpl<T, 7, 6>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

// Constructor for a %QuaternionFloatingMobilizer granting six degrees of
// freedom to an outboard frame M with respect to an inboard frame F. The
Expand Down Expand Up @@ -218,22 +222,22 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {

SpatialVelocity<T> calc_V_FM(const T*, const T* v) const {
DRAKE_ASSERT(v != nullptr);
const Eigen::Map<const VVector> V_FM(v);
const Eigen::Map<const VVector<T>> V_FM(v);
return SpatialVelocity<T>(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.
SpatialAcceleration<T> calc_A_FM(const T*, const T*, const T* vdot) const {
DRAKE_ASSERT(vdot != nullptr);
const Eigen::Map<const VVector> A_FM(vdot);
const Eigen::Map<const VVector<T>> A_FM(vdot);
return SpatialAcceleration<T>(A_FM);
}

// Returns tau = H_FMᵀ⋅F. H is identity for this mobilizer.
void calc_tau(const T*, const SpatialForce<T>& F_BMo_F, T* tau) const {
DRAKE_ASSERT(tau != nullptr);
Eigen::Map<VVector> tau_as_vector(tau);
Eigen::Map<VVector<T>> tau_as_vector(tau);
tau_as_vector = F_BMo_F.get_coeffs();
}

Expand Down Expand Up @@ -266,7 +270,7 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
protected:
// Sets `state` to store a configuration in which M coincides with F (i.e.
// q_FM is the identity quaternion).
Vector<double, 7> get_zero_position() const final;
QVector<double> get_zero_position() const final;

void DoCalcNMatrix(const systems::Context<T>& context,
EigenPtr<MatrixX<T>> N) const final;
Expand Down
8 changes: 6 additions & 2 deletions multibody/tree/revolute_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,12 @@ class RevoluteMobilizer final : public MobilizerImpl<T, 1, 1> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RevoluteMobilizer);
using MobilizerBase = MobilizerImpl<T, 1, 1>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

// Constructor for a %RevoluteMobilizer between the inboard frame F
// `inboard_frame_F` and the outboard frame M `outboard_frame_F` granting a
Expand Down
10 changes: 7 additions & 3 deletions multibody/tree/rpy_ball_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,12 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RpyBallMobilizer);
using MobilizerBase = MobilizerImpl<T, 3, 3>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

// Constructor for an RpyBallMobilizer between an inboard frame F
// inboard_frame_F and an outboard frame M outboard_frame_M granting
Expand Down Expand Up @@ -201,7 +205,7 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
// the rest is zero.
void calc_tau(const T*, const SpatialForce<T>& F_BMo_F, T* tau) const {
DRAKE_ASSERT(tau != nullptr);
Eigen::Map<VVector> tau_as_vector(tau);
Eigen::Map<VVector<T>> tau_as_vector(tau);
const Vector3<T>& t_BMo_F = F_BMo_F.rotational();
tau_as_vector = t_BMo_F;
}
Expand Down
14 changes: 9 additions & 5 deletions multibody/tree/rpy_floating_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,12 @@ class RpyFloatingMobilizer final : public MobilizerImpl<T, 6, 6> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RpyFloatingMobilizer);
using MobilizerBase = MobilizerImpl<T, 6, 6>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

// Constructor for an RpyFloatingMobilizer between an inboard frame F
// inboard_frame_F and an outboard frame M outboard_frame_M.
Expand Down Expand Up @@ -236,21 +240,21 @@ class RpyFloatingMobilizer final : public MobilizerImpl<T, 6, 6> {
// velocity v, packed as documented in get_generalized_velocities(). (That's
// conveniently just V_FM already.)
SpatialVelocity<T> calc_V_FM(const T*, const T* v) const {
const Eigen::Map<const VVector> V_FM(v);
const Eigen::Map<const VVector<T>> V_FM(v);
return SpatialVelocity<T>(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.
SpatialAcceleration<T> calc_A_FM(const T*, const T*, const T* vdot) const {
const Eigen::Map<const VVector> A_FM(vdot);
const Eigen::Map<const VVector<T>> A_FM(vdot);
return SpatialAcceleration<T>(A_FM);
}

// Returns tau = H_FMᵀ⋅F. H is identity for this mobilizer.
void calc_tau(const T*, const SpatialForce<T>& F_BMo_F, T* tau) const {
DRAKE_ASSERT(tau != nullptr);
Eigen::Map<VVector> tau_as_vector(tau);
Eigen::Map<VVector<T>> tau_as_vector(tau);
tau_as_vector = F_BMo_F.get_coeffs();
}

Expand Down
8 changes: 6 additions & 2 deletions multibody/tree/screw_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,12 @@ class ScrewMobilizer final : public MobilizerImpl<T, 1, 1> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ScrewMobilizer);
using MobilizerBase = MobilizerImpl<T, 1, 1>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

/* Constructor for a %ScrewMobilizer between an inboard frame F and
an outboard frame M granting one translational and one rotational degrees
Expand Down
14 changes: 9 additions & 5 deletions multibody/tree/universal_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,12 @@ class UniversalMobilizer final : public MobilizerImpl<T, 2, 2> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(UniversalMobilizer);
using MobilizerBase = MobilizerImpl<T, 2, 2>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

// Constructor for a %UniversalMobilizer between an inboard frame F
// `inboard_frame_F` and an outboard frame M `outboard_frame_M` granting
Expand Down Expand Up @@ -143,7 +147,7 @@ class UniversalMobilizer final : public MobilizerImpl<T, 2, 2> {
// in get_angular_rates().
// TODO(sherm1) Should not have to recalculate H_FM(q) here.
SpatialVelocity<T> calc_V_FM(const T* q, const T* v) const {
const Eigen::Map<const VVector> w(v);
const Eigen::Map<const VVector<T>> w(v);
const Eigen::Matrix<T, 3, 2> Hw = this->CalcHwMatrix(q);
return SpatialVelocity<T>(Hw * w, Vector3<T>::Zero());
}
Expand All @@ -157,15 +161,15 @@ class UniversalMobilizer final : public MobilizerImpl<T, 2, 2> {
const T* vdot) const {
Vector3<T> Hw_dot_col1;
const Eigen::Matrix<T, 3, 2> Hw = this->CalcHwMatrix(q, v, &Hw_dot_col1);
const Eigen::Map<const VVector> wdot(vdot);
const Eigen::Map<const VVector<T>> wdot(vdot);
return SpatialAcceleration<T>(Hw * wdot + Hw_dot_col1 * v[1],
Vector3<T>::Zero());
}

// Returns tau = H_FMᵀ⋅F. See above for the structure of H.
void calc_tau(const T* q, const SpatialForce<T>& F_BMo_F, T* tau) const {
DRAKE_ASSERT(tau != nullptr);
Eigen::Map<VVector> tau_as_vector(tau);
Eigen::Map<VVector<T>> tau_as_vector(tau);
const Vector3<T>& t_B_F = F_BMo_F.rotational(); // torque
const Eigen::Matrix<T, 3, 2> Hw_FM = this->CalcHwMatrix(q);
tau_as_vector = Hw_FM.transpose() * t_B_F;
Expand Down
8 changes: 6 additions & 2 deletions multibody/tree/weld_mobilizer.h
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,12 @@ class WeldMobilizer final : public MobilizerImpl<T, 0, 0> {
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(WeldMobilizer);
using MobilizerBase = MobilizerImpl<T, 0, 0>;
using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
using typename MobilizerBase::HMatrix;
using typename MobilizerBase::QVector, typename MobilizerBase::VVector;
template <typename U>
using QVector = typename MobilizerBase::template QVector<U>;
template <typename U>
using VVector = typename MobilizerBase::template VVector<U>;
template <typename U>
using HMatrix = typename MobilizerBase::template HMatrix<U>;

// Constructor for a %WeldMobilizer between the `inboard_frame_F` and
// `outboard_frame_M`.
Expand Down

0 comments on commit 332e9d4

Please sign in to comment.