From e3f7fb79e0a8a45333ee9881378f3796887b22a9 Mon Sep 17 00:00:00 2001 From: Joe Masterjohn Date: Tue, 1 Sep 2020 12:50:21 -0400 Subject: [PATCH] first fixed offset frame parameters --- bindings/pydrake/multibody/test/plant_test.py | 10 ++ bindings/pydrake/multibody/tree_py.cc | 9 +- multibody/plant/test/multibody_plant_test.cc | 100 ++++++++++++++++-- multibody/tree/fixed_offset_frame.h | 43 +++++++- 4 files changed, 148 insertions(+), 14 deletions(-) diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index 667b814c98f8..f476b838c4d3 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/bindings/pydrake/multibody/test/plant_test.py @@ -1451,6 +1451,16 @@ def test_multibody_add_frame(self, T): frame.GetFixedPoseInBodyFrame().GetAsMatrix4(), np.eye(4)) + plant.Finalize() + context = plant.CreateDefaultContext() + + X_PF = RigidTransform_[T](p=[1., 2., 3.]) + frame.SetPoseInBodyFrame(context=context, X_PF=X_PF) + + numpy_compare.assert_float_equal( + frame.CalcPoseInBodyFrame(context).GetAsMatrix34(), + numpy_compare.to_float(X_PF.GetAsMatrix34())) + @numpy_compare.check_all_types def test_multibody_dynamics(self, T): MultibodyPlant = MultibodyPlant_[T] diff --git a/bindings/pydrake/multibody/tree_py.cc b/bindings/pydrake/multibody/tree_py.cc index 659ceee6a477..5d35eb402169 100644 --- a/bindings/pydrake/multibody/tree_py.cc +++ b/bindings/pydrake/multibody/tree_py.cc @@ -165,7 +165,9 @@ void DoScalarDependentDefinitions(py::module m, T) { .def("name", &Class::name, cls_doc.name.doc) .def("body", &Class::body, py_rvp::reference_internal, cls_doc.body.doc) .def("GetFixedPoseInBodyFrame", &Frame::GetFixedPoseInBodyFrame, - cls_doc.GetFixedPoseInBodyFrame.doc); + cls_doc.GetFixedPoseInBodyFrame.doc) + .def("CalcPoseInBodyFrame", &Frame::CalcPoseInBodyFrame, + py::arg("context"), cls_doc.CalcPoseInBodyFrame.doc); } { @@ -195,7 +197,10 @@ void DoScalarDependentDefinitions(py::module m, T) { name, P, RigidTransform(X_PF), model_instance); }), py::arg("name"), py::arg("P"), py::arg("X_PF"), - py::arg("model_instance") = std::nullopt, doc_iso3_deprecation); + py::arg("model_instance") = std::nullopt, doc_iso3_deprecation) + .def("SetPoseInBodyFrame", &Class::SetPoseInBodyFrame, + py::arg("context"), py::arg("X_PF"), + cls_doc.SetPoseInBodyFrame.doc); } // Bodies. diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index 1f8e0be8734c..77c14bba5cfe 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -3262,22 +3262,30 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) { plant_autodiff->CreateDefaultContext(); context_autodiff->SetTimeStateAndParametersFrom(*context); - // Set up our parameters as the independent variables. - // Differentiable parameters for the acrobot using only inertial parameters. - const AutoDiffXd m1_ad(m1, Vector3d(1, 0, 0)); - const AutoDiffXd m2_ad(m2, Vector3d(0, 1, 0)); - const AutoDiffXd l2_ad(l2, Vector3d(0, 0, 1)); + // Set up our parameters as independent variables. + // We will use mass and length of the two links of the acrobot as our + // variables to differentiate with respect to. We must update multibody + // components that depend on these parameters so that their derivatives will + // propagate. For this example the multibody elements that require parameter + // updates are the bodies' mass and inertia, and the joints' offset frames. + const AutoDiffXd m1_ad(m1, Vector4(1, 0, 0, 0)); + const AutoDiffXd m2_ad(m2, Vector4(0, 1, 0, 0)); + const AutoDiffXd l1_ad(l1, Vector4(0, 0, 1, 0)); + const AutoDiffXd l2_ad(l2, Vector4(0, 0, 0, 1)); + const AutoDiffXd lc1_ad = 0.5 * l1_ad; const AutoDiffXd lc2_ad = 0.5 * l2_ad; + const AutoDiffXd Gc1_ad = (1.0 / 12.0) * l1_ad * l1_ad; const AutoDiffXd Gc2_ad = (1.0 / 12.0) * l2_ad * l2_ad; + // Differentiable parameters for the acrobot's mass/inertia parameters. // Frame L1's origin is located at the shoulder outboard frame. - const Vector3 p_L1L1cm = -lc1 * Vector3d::UnitZ(); + const Vector3 p_L1L1cm = -lc1_ad * Vector3d::UnitZ(); // Frame L2's origin is located at the elbow outboard frame. const Vector3 p_L2L2cm = -lc2_ad * Vector3d::UnitZ(); // Define each link's spatial inertia about their respective COM. UnitInertia Gc1_Bcm = - UnitInertia::StraightLine(Gc1, Vector3d::UnitZ()); + UnitInertia::StraightLine(Gc1_ad, Vector3d::UnitZ()); SpatialInertia M1_L1o = SpatialInertia::MakeFromCentralInertia(m1_ad, p_L1L1cm, Gc1_Bcm * m1_ad); @@ -3288,12 +3296,28 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) { SpatialInertia::MakeFromCentralInertia(m2_ad, p_L2L2cm, Gc2_Bcm * m2_ad); + // Update each body's inertial parameters. plant_autodiff->GetRigidBodyByName(params.link1_name()) .SetSpatialInertiaInBodyFrame(context_autodiff.get(), M1_L1o); plant_autodiff->GetRigidBodyByName(params.link2_name()) .SetSpatialInertiaInBodyFrame(context_autodiff.get(), M2_L2o); + // The parent frame of the elbow joint is an offset frame that depends on the + // length of link1. Therefore, we must update this offset frame's parameter + // with the autodiff variable containing the proper partial derivatives for + // them to propagate through dynamics computations. + const RevoluteJoint& elbow_joint_ad = + plant_autodiff->GetJointByName(params.elbow_joint_name()); + const FixedOffsetFrame& elbow_joint_parent_frame = + dynamic_cast&>( + elbow_joint_ad.frame_on_parent()); + + const RigidTransform X_link1_Ei(-l1_ad * Vector3d::UnitZ()); + + elbow_joint_parent_frame.SetPoseInBodyFrame(context_autodiff.get(), + X_link1_Ei); + // Take the derivative of the mass matrix w.r.t. length. Matrix2 mass_matrix; plant_autodiff->CalcMassMatrix(*context_autodiff, &mass_matrix); @@ -3349,6 +3373,17 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) { analytic_mass_matrix_partial_m2, kTolerance, MatrixCompareType::relative)); + // Analytic ∂M/∂l₁: + // [ (2/3)m₁l₁ + 2m₂l₁ + 2m₂lc₂c₂ m₂lc₂c₂ ] + // [ m₂lc₂c₂ 0 ] + Vector4 analytic_mass_matrix_partial_l1; + analytic_mass_matrix_partial_l1 << + (2.0/3.0)*m1*l1 + 2*m2*l1 + 2*m2*lc2, m2*lc2, + m2*lc2, 0; + EXPECT_TRUE(CompareMatrices(mass_matrix_grad.col(2), + analytic_mass_matrix_partial_l1, kTolerance, + MatrixCompareType::relative)); + // Analytic ∂M/∂l₂: // [ (2/3)m₂l₂ + m₂l₁ (2/3)m₂l₂ + (1/2)m₂l₁ ] // [ (2/3)m₂l₂ + (1/2)m₂l₁ (2/3)m₂l₂ ] @@ -3356,11 +3391,60 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) { analytic_mass_matrix_partial_l2 << (2.0/3.0)*m2*l2 + m2*l1, (2.0/3.0)*m2*l2 + 0.5*m2*l1, (2.0/3.0)*m2*l2 + 0.5*m2*l1, (2.0/3.0)*m2*l2; - EXPECT_TRUE(CompareMatrices(mass_matrix_grad.col(2), + EXPECT_TRUE(CompareMatrices(mass_matrix_grad.col(3), analytic_mass_matrix_partial_l2, kTolerance, MatrixCompareType::relative)); } +GTEST_TEST(MultibodyPlantTests, FixedOffsetFrameParameters) { + MultibodyPlant plant(0.0); + + const Vector3d p_WF(0, 0, 0); + const RotationMatrixd R_WF{}; + const RigidTransformd X_WF(R_WF, p_WF); + + const Body& body = plant.AddRigidBody("B", SpatialInertia{});; + + const Joint& weld_joint = + plant.AddJoint("weld_WB", plant.world_body(), X_WF, body, {}, + RigidTransformd::Identity()); + + const FixedOffsetFrame& frame_F = + dynamic_cast&>( + weld_joint.frame_on_parent()); + + plant.Finalize(); + + // Create a default context. + auto context = plant.CreateDefaultContext(); + + // Verify default parameters are set for the frame and that they've propagated + // to the welded body's pose. + const RigidTransformd& X_WF_context = frame_F.CalcPoseInBodyFrame(*context); + const math::RigidTransformd& X_WF_body = + plant.EvalBodyPoseInWorld(*context, body); + + EXPECT_TRUE( + CompareMatrices(X_WF.GetAsMatrix34(), X_WF_context.GetAsMatrix34())); + EXPECT_TRUE(CompareMatrices(X_WF.GetAsMatrix34(), X_WF_body.GetAsMatrix34())); + + // Set new parameters and verify they propagate. + const Vector3d p_WF_new(1, 1, 1); + const RotationMatrixd R_WF_new = RotationMatrixd::MakeXRotation(3); + const RigidTransformd X_WF_new(R_WF_new, p_WF_new); + + frame_F.SetPoseInBodyFrame(context.get(), X_WF_new); + + const RigidTransformd& X_WF_context_new = + frame_F.CalcPoseInBodyFrame(*context); + const math::RigidTransformd& X_WF_body_new = + plant.EvalBodyPoseInWorld(*context, body); + EXPECT_TRUE(CompareMatrices(X_WF_new.GetAsMatrix34(), + X_WF_context_new.GetAsMatrix34())); + EXPECT_TRUE( + CompareMatrices(X_WF_new.GetAsMatrix34(), X_WF_body_new.GetAsMatrix34())); +} + } // namespace } // namespace multibody } // namespace drake diff --git a/multibody/tree/fixed_offset_frame.h b/multibody/tree/fixed_offset_frame.h index 609c870f3953..289dde9850dd 100644 --- a/multibody/tree/fixed_offset_frame.h +++ b/multibody/tree/fixed_offset_frame.h @@ -81,22 +81,41 @@ class FixedOffsetFrame final : public Frame { math::RigidTransform CalcPoseInBodyFrame( const systems::Context& context) const override { // X_BF = X_BP * X_PF - return parent_frame_.CalcOffsetPoseInBody(context, X_PF_.cast()); + const systems::BasicVector& X_PF_parameter = + context.get_numeric_parameter(X_PF_parameter_index_); + return parent_frame_.CalcOffsetPoseInBody( + context, + math::RigidTransform(Eigen::Map>( + X_PF_parameter.get_value().data()))); } math::RotationMatrix CalcRotationMatrixInBodyFrame( const systems::Context& context) const override { // R_BF = R_BP * R_PF - const math::RotationMatrix& R_PF = X_PF_.rotation(); - return parent_frame_.CalcOffsetRotationMatrixInBody(context, - R_PF.cast()); + const systems::BasicVector& X_PF_parameter = + context.get_numeric_parameter(X_PF_parameter_index_); + return parent_frame_.CalcOffsetRotationMatrixInBody( + context, + math::RotationMatrix(Eigen::Map>( + X_PF_parameter.get_value().data()) + .template block<3, 3>(0, 0))); + } + + void SetPoseInBodyFrame(systems::Context* context, + const math::RigidTransform& X_PF) const { + systems::BasicVector& X_PF_parameter = + context->get_mutable_numeric_parameter(X_PF_parameter_index_); + X_PF_parameter.set_value( + Eigen::Map>(X_PF.GetAsMatrix34().data(), 12, 1)); } + /// @returns The default fixed pose in the body frame. math::RigidTransform GetFixedPoseInBodyFrame() const override { // X_BF = X_BP * X_PF return parent_frame_.GetFixedOffsetPoseInBody(X_PF_.cast()); } + /// @returns The default rotation matrix of this fixed pose in the body frame. math::RotationMatrix GetFixedRotationMatrixInBodyFrame() const override { // R_BF = R_BP * R_PF const math::RotationMatrix& R_PF = X_PF_.rotation(); @@ -126,6 +145,18 @@ class FixedOffsetFrame final : public Frame { /// @} private: + // Implementation for MultibodyElement::DoDeclareParameters(). + // FixedOffsetFrame declares a single parameter for its RigidTransform. + void DoDeclareParameters( + internal::MultibodyTreeSystem* tree_system) final { + // Declare parent classes' parameters + Frame::DoDeclareParameters(tree_system); + X_PF_parameter_index_ = this->DeclareNumericParameter( + tree_system, + systems::BasicVector(Eigen::Map>( + X_PF_.template cast().GetAsMatrix34().data(), 12, 1))); + } + // Helper method to make a clone templated on ToScalar. template std::unique_ptr> TemplatedDoCloneToScalar( @@ -137,6 +168,10 @@ class FixedOffsetFrame final : public Frame { // Spatial transform giving the fixed pose of this frame F measured in the // parent frame P. const math::RigidTransform X_PF_; + + // System parameter indices for `this` frame's RigidTransform stored in a + // context. + systems::NumericParameterIndex X_PF_parameter_index_; }; } // namespace multibody