Skip to content

Commit

Permalink
first fixed offset frame parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
@joemasterjohn
joemasterjohn committed Sep 29, 2020
1 parent dba012d commit 1ac10f6
Show file tree
Hide file tree
Showing 4 changed files with 130 additions and 12 deletions.
13 changes: 13 additions & 0 deletions bindings/pydrake/multibody/test/plant_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1451,6 +1451,19 @@ 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(),
np.array([[1., 0., 0., 1.],
[0., 1., 0., 2.],
[0., 0., 1., 3.]])
)

@numpy_compare.check_all_types
def test_multibody_dynamics(self, T):
MultibodyPlant = MultibodyPlant_[T]
Expand Down
12 changes: 10 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,12 @@ 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)
.def("CalcRotationMatrixInBodyFrame",
&Frame<T>::CalcRotationMatrixInBodyFrame,
py::arg("context"), cls_doc.CalcRotationMatrixInBodyFrame.doc);
}

{
Expand Down Expand Up @@ -195,7 +200,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
72 changes: 66 additions & 6 deletions multibody/plant/test/multibody_plant_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3264,20 +3264,23 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) {

// 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));
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;

// 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 @@ -3294,6 +3297,17 @@ GTEST_TEST(MultibodyPlantTest, AutoDiffAcrobotParameters) {
plant_autodiff->GetRigidBodyByName(params.link2_name())
.SetSpatialInertiaInBodyFrame(context_autodiff.get(), M2_L2o);

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 +3363,64 @@ 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) {
// Add a plant with a few rigid bodies.
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 FixedOffsetFrame<double>& frame_F =
plant.AddFrame(std::make_unique<FixedOffsetFrame<double>>(
"frame_F", plant.world_frame(), X_WF));

plant.Finalize();

// Create a default context.
auto context = plant.CreateDefaultContext();

// Verify default parameters are set.
const RigidTransformd& X_WF_context = frame_F.CalcPoseInBodyFrame(*context);

EXPECT_TRUE(
CompareMatrices(X_WF.GetAsMatrix34(), X_WF_context.GetAsMatrix34()));

// Set new parameters and verify they propogate.
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);
EXPECT_TRUE(CompareMatrices(X_WF_new.GetAsMatrix34(),
X_WF_context_new.GetAsMatrix34()));
}

} // namespace
} // namespace multibody
} // namespace drake
45 changes: 41 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,43 @@ 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_);
const Eigen::Matrix<T, 3, 4> X_PF =
Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data());
return parent_frame_.CalcOffsetPoseInBody(context,
math::RigidTransform<T>(X_PF));
}

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_);
const Eigen::Matrix<T, 3, 4> X_PF =
Eigen::Map<const Eigen::Matrix<T, 3, 4>>(
X_PF_parameter.get_value().data());
return parent_frame_.CalcOffsetRotationMatrixInBody(
context, math::RotationMatrix<T>(X_PF.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_);
const VectorX<T> X_PF_vector =
Eigen::Map<const VectorX<T>>(X_PF.GetAsMatrix34().data(), 12, 1);
X_PF_parameter.SetFromVector(X_PF_vector);
}

/// @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 @@ -125,6 +146,18 @@ class FixedOffsetFrame final : public Frame<T> {
const internal::MultibodyTree<symbolic::Expression>&) const override;
/// @}

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

private:
// Helper method to make a clone templated on ToScalar.
template <typename ToScalar>
Expand All @@ -137,6 +170,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 1ac10f6

Please sign in to comment.