Skip to content

Commit

Permalink
Inverse Dynamics 2 (in M) proto + cassiebench test
Browse files Browse the repository at this point in the history
  • Loading branch information
sherm1 committed Dec 4, 2024
1 parent f4c19e7 commit a730a7d
Show file tree
Hide file tree
Showing 27 changed files with 1,154 additions and 80 deletions.
33 changes: 33 additions & 0 deletions multibody/benchmarking/cassie.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
8 changes: 8 additions & 0 deletions multibody/plant/multibody_plant.h
Original file line number Diff line number Diff line change
Expand Up @@ -3871,6 +3871,14 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
external_forces);
}

VectorX<T> CalcInverseDynamics2(
const systems::Context<T>& context, const VectorX<T>& known_vdot,
const MultibodyForces<T>& 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
Expand Down
11 changes: 8 additions & 3 deletions multibody/plant/test/external_forces_test.cc
Original file line number Diff line number Diff line change
@@ -1,14 +1,12 @@
#include <limits>

#include <gmock/gmock.h>
#include <gtest/gtest.h>

#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 {

Expand Down Expand Up @@ -41,6 +39,9 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
end_effector_link_->body_frame(), &forces);
const VectorX<double> tau_id =
plant_->CalcInverseDynamics(*context_, vdot, forces);
const VectorX<double> 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});
Expand Down Expand Up @@ -75,6 +76,10 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
const double kTolerance = 50 * std::numeric_limits<double>::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<double>::epsilon(),
MatrixCompareType::relative));
}

TEST_F(KukaIiwaModelTests, BodyForceApi) {
Expand Down
25 changes: 25 additions & 0 deletions multibody/tree/body_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -241,6 +241,10 @@ class BodyNode : public MultibodyElement<T> {
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
PositionKinematicsCache<T>* pc) const = 0;

virtual void CalcPositionKinematicsCache2_BaseToTip(
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
PositionKinematicsCache2<T>* 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
Expand Down Expand Up @@ -290,6 +294,10 @@ class BodyNode : public MultibodyElement<T> {
const std::vector<Vector6<T>>& H_PB_W_cache, const T* velocities,
VelocityKinematicsCache<T>* vc) const = 0;

virtual void CalcVelocityKinematicsCache2_BaseToTip(
const T* positions, const PositionKinematicsCache2<T>& pc2,
const T* velocities, VelocityKinematicsCache2<T>* 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
Expand Down Expand Up @@ -373,6 +381,12 @@ class BodyNode : public MultibodyElement<T> {
const VelocityKinematicsCache<T>* vc, const T* accelerations,
std::vector<SpatialAcceleration<T>>* A_WB_array) const = 0;

virtual void CalcSpatialAcceleration2_BaseToTip(
const T* positions, const PositionKinematicsCache2<T>& pc2,
const T* velocities, const VelocityKinematicsCache2<T>& vc2,
const T* accelerations,
std::vector<SpatialAcceleration<T>>* 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
Expand Down Expand Up @@ -432,6 +446,17 @@ class BodyNode : public MultibodyElement<T> {
std::vector<SpatialForce<T>>* F_BMo_W_array,
EigenPtr<VectorX<T>> tau_array) const = 0;

virtual void CalcInverseDynamics2_TipToBase(
const FrameBodyPoseCache<T>& frame_body_pose_cache, // M_BMo_M, X_BM
const T* positions,
const PositionKinematicsCache2<T>& pc, // X_MpM, X_WB
const VelocityKinematicsCache2<T>& vc, // V_WM_M
const std::vector<SpatialAcceleration<T>>& A_WM_M_array,
const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array, // Bo, W !
const Eigen::Ref<const VectorX<T>>& tau_applied_array,
std::vector<SpatialForce<T>>* F_BMo_M_array,
EigenPtr<VectorX<T>> 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.
Expand Down
Loading

0 comments on commit a730a7d

Please sign in to comment.