Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adds parameters for FixedOffsetFrame #14137

Merged
merged 1 commit into from
Sep 30, 2020
Merged

Adds parameters for FixedOffsetFrame #14137

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions bindings/pydrake/multibody/test/plant_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
9 changes: 7 additions & 2 deletions bindings/pydrake/multibody/tree_py.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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