Skip to content

Commit 8087d9a

Browse files
committed
Inverse Dynamics in M proto + cassiebench test
1 parent d243930 commit 8087d9a

30 files changed

+1221
-120
lines changed

math/rotation_matrix.h

Lines changed: 17 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -101,20 +101,14 @@ class RotationMatrix {
101101
/// @throws std::exception in debug builds if the rotation matrix
102102
/// R that is built from `theta_lambda` fails IsValid(R). For example, an
103103
/// exception is thrown if `lambda` is zero or contains a NaN or infinity.
104-
explicit RotationMatrix(const Eigen::AngleAxis<T>& theta_lambda) {
105-
using std::cos;
106-
using std::sin;
107-
// TODO(mitiguy) Consider adding an optional second argument if lambda is
108-
// known to be normalized apriori or the caller does not want normalization.
109-
const T& theta = theta_lambda.angle();
110-
const Vector3<T>& lambda = theta_lambda.axis();
111-
// We won't use AngleAxis<T>::toRotationMatrix because somtimes it is
112-
// miscompiled by Clang 15. Instead, we'll follow the derivation here:
113-
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
114-
const T norm = lambda.norm();
115-
const T x = lambda.x() / norm;
116-
const T y = lambda.y() / norm;
117-
const T z = lambda.z() / norm;
104+
explicit RotationMatrix(const Eigen::AngleAxis<T>& theta_lambda)
105+
: RotationMatrix(theta_lambda.angle(), theta_lambda.axis().normalized()) {
106+
}
107+
108+
RotationMatrix(const T& theta, const Eigen::Vector3<T>& unit_lambda) {
109+
const T x = unit_lambda.x();
110+
const T y = unit_lambda.y();
111+
const T z = unit_lambda.z();
118112
const T s = sin(theta);
119113
const T c = cos(theta);
120114
const T t = 1 - c; // 1 - cos(θ)
@@ -131,15 +125,15 @@ class RotationMatrix {
131125
const T txz = tx * z; // (1 - cos(θ)) x z
132126
const T tyz = ty * z; // (1 - cos(θ)) y z
133127
Matrix3<T> R;
134-
R.coeffRef(0, 0) = txx + c; // (1 - cos(θ)) x² + cos(θ)
135-
R.coeffRef(1, 1) = tyy + c; // (1 - cos(θ)) y² + cos(θ)
136-
R.coeffRef(2, 2) = tzz + c; // (1 - cos(θ)) z² + cos(θ)
137-
R.coeffRef(0, 1) = txy - sz; // (1 - cos(θ)) x y - sin(θ) z
138-
R.coeffRef(1, 0) = txy + sz; // (1 - cos(θ)) x y + sin(θ) z
139-
R.coeffRef(0, 2) = txz + sy; // (1 - cos(θ)) x z + sin(θ) y
140-
R.coeffRef(2, 0) = txz - sy; // (1 - cos(θ)) x z - sin(θ) y
141-
R.coeffRef(1, 2) = tyz - sx; // (1 - cos(θ)) y z - sin(θ) x
142-
R.coeffRef(2, 1) = tyz + sx; // (1 - cos(θ)) y z + sin(θ) x
128+
R(0, 0) = txx + c; // (1 - cos(θ)) x² + cos(θ)
129+
R(1, 1) = tyy + c; // (1 - cos(θ)) y² + cos(θ)
130+
R(2, 2) = tzz + c; // (1 - cos(θ)) z² + cos(θ)
131+
R(0, 1) = txy - sz; // (1 - cos(θ)) x y - sin(θ) z
132+
R(1, 0) = txy + sz; // (1 - cos(θ)) x y + sin(θ) z
133+
R(0, 2) = txz + sy; // (1 - cos(θ)) x z + sin(θ) y
134+
R(2, 0) = txz - sy; // (1 - cos(θ)) x z - sin(θ) y
135+
R(1, 2) = tyz - sx; // (1 - cos(θ)) y z - sin(θ) x
136+
R(2, 1) = tyz + sx; // (1 - cos(θ)) y z + sin(θ) x
143137
set(R);
144138
}
145139

multibody/benchmarking/cassie.cc

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,17 @@ class Cassie : public benchmark::Fixture {
146146
}
147147
}
148148

149+
// Runs the InverseDynamicsInM benchmark.
150+
// NOLINTNEXTLINE(runtime/references)
151+
void DoInverseDynamicsInM(benchmark::State& state) {
152+
DRAKE_DEMAND(want_grad_u(state) == false);
153+
for (auto _ : state) {
154+
InvalidateState();
155+
plant_->CalcInverseDynamicsInM(*context_, desired_vdot_,
156+
external_forces_);
157+
}
158+
}
159+
149160
// Runs the ForwardDynamics benchmark.
150161
// NOLINTNEXTLINE(runtime/references)
151162
void DoForwardDynamics(benchmark::State& state) {
@@ -327,6 +338,14 @@ BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics)
327338
->Unit(benchmark::kMicrosecond)
328339
->Arg(kWantNoGrad);
329340

341+
// NOLINTNEXTLINE(runtime/references)
342+
BENCHMARK_DEFINE_F(CassieDouble, InverseDynamics2)(benchmark::State& state) {
343+
DoInverseDynamicsInM(state);
344+
}
345+
BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics2)
346+
->Unit(benchmark::kMicrosecond)
347+
->Arg(kWantNoGrad);
348+
330349
// NOLINTNEXTLINE(runtime/references)
331350
BENCHMARK_DEFINE_F(CassieDouble, ForwardDynamics)(benchmark::State& state) {
332351
DoForwardDynamics(state);
@@ -383,6 +402,21 @@ BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics)
383402
->Arg(kWantGradV|kWantGradVdot)
384403
->Arg(kWantGradX|kWantGradVdot);
385404

405+
// NOLINTNEXTLINE(runtime/references)
406+
BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamics2)(benchmark::State& state) {
407+
DoInverseDynamicsInM(state);
408+
}
409+
BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics2)
410+
->Unit(benchmark::kMicrosecond)
411+
->Arg(kWantNoGrad)
412+
->Arg(kWantGradQ)
413+
->Arg(kWantGradV)
414+
->Arg(kWantGradX)
415+
->Arg(kWantGradVdot)
416+
->Arg(kWantGradQ|kWantGradVdot)
417+
->Arg(kWantGradV|kWantGradVdot)
418+
->Arg(kWantGradX|kWantGradVdot);
419+
386420
// NOLINTNEXTLINE(runtime/references)
387421
BENCHMARK_DEFINE_F(CassieAutoDiff, ForwardDynamics)(benchmark::State& state) {
388422
DoForwardDynamics(state);

multibody/math/spatial_acceleration.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ class SpatialAcceleration : public SpatialVector<SpatialAcceleration, T> {
134134
const Vector3<T>& alpha_MB_E = this->rotational();
135135
// Calculate point Co's translational acceleration measured in M.
136136
Vector3<T>& a_MCo_E = this->translational();
137-
a_MCo_E += (alpha_MB_E.cross(p_BoCo_E)
137+
a_MCo_E += (alpha_MB_E.cross(p_BoCo_E) // 33 flops total
138138
+ w_MB_E.cross(w_MB_E.cross(p_BoCo_E)));
139139
}
140140

multibody/plant/multibody_plant.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3868,6 +3868,14 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
38683868
external_forces);
38693869
}
38703870

3871+
VectorX<T> CalcInverseDynamicsInM(
3872+
const systems::Context<T>& context, const VectorX<T>& known_vdot,
3873+
const MultibodyForces<T>& external_forces) const {
3874+
this->ValidateContext(context);
3875+
return internal_tree().CalcInverseDynamicsInM(context, known_vdot,
3876+
external_forces);
3877+
}
3878+
38713879
#ifdef DRAKE_DOXYGEN_CXX
38723880
// MultibodyPlant uses the NVI implementation of
38733881
// CalcImplicitTimeDerivativesResidual from

multibody/plant/test/external_forces_test.cc

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,12 @@
11
#include <limits>
22

3-
#include <gmock/gmock.h>
43
#include <gtest/gtest.h>
54

6-
#include "drake/common/autodiff.h"
5+
#include "drake/common/fmt_eigen.h"
76
#include "drake/common/test_utilities/eigen_matrix_compare.h"
87
#include "drake/common/test_utilities/limit_malloc.h"
98
#include "drake/multibody/plant/test/kuka_iiwa_model_tests.h"
109
#include "drake/multibody/tree/frame.h"
11-
#include "drake/multibody/tree/rigid_body.h"
1210

1311
namespace drake {
1412

@@ -41,6 +39,9 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
4139
end_effector_link_->body_frame(), &forces);
4240
const VectorX<double> tau_id =
4341
plant_->CalcInverseDynamics(*context_, vdot, forces);
42+
const VectorX<double> tau_id2 =
43+
plant_->CalcInverseDynamicsInM(*context_, vdot, forces);
44+
4445
{ // Repeat the computation to confirm the heap behavior. We allow the
4546
// method to heap-allocate 2 temporaries and 1 return value.
4647
drake::test::LimitMalloc guard({.max_num_allocations = 3});
@@ -75,6 +76,10 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
7576
const double kTolerance = 50 * std::numeric_limits<double>::epsilon();
7677
EXPECT_TRUE(CompareMatrices(tau_id, tau_id_expected, kTolerance,
7778
MatrixCompareType::relative));
79+
// W & M frame ID should be very close.
80+
EXPECT_TRUE(CompareMatrices(tau_id2, tau_id,
81+
16 * std::numeric_limits<double>::epsilon(),
82+
MatrixCompareType::relative));
7883
}
7984

8085
TEST_F(KukaIiwaModelTests, BodyForceApi) {

multibody/tree/body_node.h

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -241,6 +241,10 @@ class BodyNode : public MultibodyElement<T> {
241241
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
242242
PositionKinematicsCache<T>* pc) const = 0;
243243

244+
virtual void CalcPositionKinematicsCacheInM_BaseToTip(
245+
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
246+
PositionKinematicsCacheInM<T>* pcm) const = 0;
247+
244248
// Calculates the hinge matrix H_PB_W, the `6 x nm` hinge matrix that relates
245249
// V_PB_W`(body B's spatial velocity in its parent body P, expressed in world
246250
// W) to this node's nm generalized velocities (or mobilities) v_B as
@@ -290,6 +294,10 @@ class BodyNode : public MultibodyElement<T> {
290294
const std::vector<Vector6<T>>& H_PB_W_cache, const T* velocities,
291295
VelocityKinematicsCache<T>* vc) const = 0;
292296

297+
virtual void CalcVelocityKinematicsCacheInM_BaseToTip(
298+
const T* positions, const PositionKinematicsCacheInM<T>& pcm,
299+
const T* velocities, VelocityKinematicsCacheInM<T>* vcm) const = 0;
300+
293301
// The CalcMassMatrix() algorithm invokes this on each body k, serving
294302
// as the composite body R(k) in the outer loop of Jain's algorithm 9.3.
295303
// This node must fill in its nv x nv diagonal block in M, and then
@@ -373,6 +381,12 @@ class BodyNode : public MultibodyElement<T> {
373381
const VelocityKinematicsCache<T>* vc, const T* accelerations,
374382
std::vector<SpatialAcceleration<T>>* A_WB_array) const = 0;
375383

384+
virtual void CalcSpatialAccelerationInM_BaseToTip(
385+
const T* positions, const PositionKinematicsCacheInM<T>& pcm,
386+
const T* velocities, const VelocityKinematicsCacheInM<T>& vcm,
387+
const T* accelerations,
388+
std::vector<SpatialAcceleration<T>>* A_WM_M_array) const = 0;
389+
376390
// Computes the generalized forces `tau` for a single BodyNode.
377391
// This method is used by MultibodyTree within a tip-to-base loop to compute
378392
// the vector of generalized forces `tau` that would correspond with a known
@@ -432,6 +446,17 @@ class BodyNode : public MultibodyElement<T> {
432446
std::vector<SpatialForce<T>>* F_BMo_W_array,
433447
EigenPtr<VectorX<T>> tau_array) const = 0;
434448

449+
virtual void CalcInverseDynamicsInM_TipToBase(
450+
const FrameBodyPoseCache<T>& frame_body_pose_cache, // M_BMo_M, X_BM
451+
const T* positions,
452+
const PositionKinematicsCacheInM<T>& pc, // X_MpM, X_WB
453+
const VelocityKinematicsCacheInM<T>& vc, // V_WM_M
454+
const std::vector<SpatialAcceleration<T>>& A_WM_M_array,
455+
const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array, // Bo, W !
456+
const Eigen::Ref<const VectorX<T>>& tau_applied_array,
457+
std::vector<SpatialForce<T>>* F_BMo_M_array,
458+
EigenPtr<VectorX<T>> tau_array) const = 0;
459+
435460
// This method is used by MultibodyTree within a tip-to-base loop to compute
436461
// this node's articulated body inertia quantities that depend only on the
437462
// generalized positions.

0 commit comments

Comments
 (0)