Skip to content

Commit

Permalink
Adds parameters for FixedOffsetFrame (#14137)
Browse files Browse the repository at this point in the history
  • Loading branch information
@joemasterjohn
joemasterjohn authored Sep 30, 2020
1 parent a823874 commit 596354e
Show file tree
Hide file tree
Showing 4 changed files with 148 additions and 14 deletions.
10 changes: 10 additions & 0 deletions bindings/pydrake/multibody/test/plant_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1454,6 +1454,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]
Expand Down
9 changes: 7 additions & 2 deletions bindings/pydrake/multibody/tree_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<T>::GetFixedPoseInBodyFrame,
cls_doc.GetFixedPoseInBodyFrame.doc);
cls_doc.GetFixedPoseInBodyFrame.doc)
.def("CalcPoseInBodyFrame", &Frame<T>::CalcPoseInBodyFrame,
py::arg("context"), cls_doc.CalcPoseInBodyFrame.doc);
}

{
Expand Down Expand Up @@ -195,7 +197,10 @@ void DoScalarDependentDefinitions(py::module m, T) {
name, P, RigidTransform<double>(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.
Expand Down
100 changes: 92 additions & 8 deletions multibody/plant/test/multibody_plant_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>(1, 0, 0, 0));
const AutoDiffXd m2_ad(m2, Vector4<double>(0, 1, 0, 0));
const AutoDiffXd l1_ad(l1, Vector4<double>(0, 0, 1, 0));
const AutoDiffXd l2_ad(l2, Vector4<double>(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<AutoDiffXd> p_L1L1cm = -lc1 * Vector3d::UnitZ();
const Vector3<AutoDiffXd> p_L1L1cm = -lc1_ad * Vector3d::UnitZ();
// Frame L2's origin is located at the elbow outboard frame.
const Vector3<AutoDiffXd> p_L2L2cm = -lc2_ad * Vector3d::UnitZ();

// Define each link's spatial inertia about their respective COM.
UnitInertia<AutoDiffXd> Gc1_Bcm =
UnitInertia<AutoDiffXd>::StraightLine(Gc1, Vector3d::UnitZ());
UnitInertia<AutoDiffXd>::StraightLine(Gc1_ad, Vector3d::UnitZ());
SpatialInertia<AutoDiffXd> M1_L1o =
SpatialInertia<AutoDiffXd>::MakeFromCentralInertia(m1_ad, p_L1L1cm,
Gc1_Bcm * m1_ad);
Expand All @@ -3288,12 +3296,28 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) {
SpatialInertia<AutoDiffXd>::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<AutoDiffXd>& elbow_joint_ad =
plant_autodiff->GetJointByName<RevoluteJoint>(params.elbow_joint_name());
const FixedOffsetFrame<AutoDiffXd>& elbow_joint_parent_frame =
dynamic_cast<const FixedOffsetFrame<AutoDiffXd>&>(
elbow_joint_ad.frame_on_parent());

const RigidTransform<AutoDiffXd> 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<AutoDiffXd> mass_matrix;
plant_autodiff->CalcMassMatrix(*context_autodiff, &mass_matrix);
Expand Down Expand Up @@ -3349,18 +3373,78 @@ 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<double> 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₂ ]
Vector4<double> analytic_mass_matrix_partial_l2;
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<double> plant(0.0);

const Vector3d p_WF(0, 0, 0);
const RotationMatrixd R_WF{};
const RigidTransformd X_WF(R_WF, p_WF);

const Body<double>& body = plant.AddRigidBody("B", SpatialInertia<double>{});;

const Joint<double>& weld_joint =
plant.AddJoint<WeldJoint>("weld_WB", plant.world_body(), X_WF, body, {},
RigidTransformd::Identity());

const FixedOffsetFrame<double>& frame_F =
dynamic_cast<const FixedOffsetFrame<double>&>(
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
43 changes: 39 additions & 4 deletions multibody/tree/fixed_offset_frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,22 +81,41 @@ class FixedOffsetFrame final : public Frame<T> {
math::RigidTransform<T> CalcPoseInBodyFrame(
const systems::Context<T>& context) const override {
// X_BF = X_BP * X_PF
return parent_frame_.CalcOffsetPoseInBody(context, X_PF_.cast<T>());
const systems::BasicVector<T>& X_PF_parameter =
context.get_numeric_parameter(X_PF_parameter_index_);
return parent_frame_.CalcOffsetPoseInBody(
context,
math::RigidTransform<T>(Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data())));
}

math::RotationMatrix<T> CalcRotationMatrixInBodyFrame(
const systems::Context<T>& context) const override {
// R_BF = R_BP * R_PF
const math::RotationMatrix<double>& R_PF = X_PF_.rotation();
return parent_frame_.CalcOffsetRotationMatrixInBody(context,
R_PF.cast<T>());
const systems::BasicVector<T>& X_PF_parameter =
context.get_numeric_parameter(X_PF_parameter_index_);
return parent_frame_.CalcOffsetRotationMatrixInBody(
context,
math::RotationMatrix<T>(Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data())
.template block<3, 3>(0, 0)));
}

void SetPoseInBodyFrame(systems::Context<T>* context,
const math::RigidTransform<T>& X_PF) const {
systems::BasicVector<T>& X_PF_parameter =
context->get_mutable_numeric_parameter(X_PF_parameter_index_);
X_PF_parameter.set_value(
Eigen::Map<const VectorX<T>>(X_PF.GetAsMatrix34().data(), 12, 1));
}

/// @returns The default fixed pose in the body frame.
math::RigidTransform<T> GetFixedPoseInBodyFrame() const override {
// X_BF = X_BP * X_PF
return parent_frame_.GetFixedOffsetPoseInBody(X_PF_.cast<T>());
}

/// @returns The default rotation matrix of this fixed pose in the body frame.
math::RotationMatrix<T> GetFixedRotationMatrixInBodyFrame() const override {
// R_BF = R_BP * R_PF
const math::RotationMatrix<double>& R_PF = X_PF_.rotation();
Expand Down Expand Up @@ -126,6 +145,18 @@ class FixedOffsetFrame final : public Frame<T> {
/// @}

private:
// Implementation for MultibodyElement::DoDeclareParameters().
// FixedOffsetFrame declares a single parameter for its RigidTransform.
void DoDeclareParameters(
internal::MultibodyTreeSystem<T>* tree_system) final {
// Declare parent classes' parameters
Frame<T>::DoDeclareParameters(tree_system);
X_PF_parameter_index_ = this->DeclareNumericParameter(
tree_system,
systems::BasicVector<T>(Eigen::Map<const VectorX<T>>(
X_PF_.template cast<T>().GetAsMatrix34().data(), 12, 1)));
}

// Helper method to make a clone templated on ToScalar.
template <typename ToScalar>
std::unique_ptr<Frame<ToScalar>> TemplatedDoCloneToScalar(
Expand All @@ -137,6 +168,10 @@ class FixedOffsetFrame final : public Frame<T> {
// Spatial transform giving the fixed pose of this frame F measured in the
// parent frame P.
const math::RigidTransform<double> X_PF_;

// System parameter indices for `this` frame's RigidTransform stored in a
// context.
systems::NumericParameterIndex X_PF_parameter_index_;
};

} // namespace multibody
Expand Down

0 comments on commit 596354e

Please sign in to comment.