-
Notifications
You must be signed in to change notification settings - Fork 1.3k
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.
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
Adds parameters for FixedOffsetFrame #14137
Adds parameters for FixedOffsetFrame #14137
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
+@EricCousineau-TRI for feature review, please.
Reviewable status: LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
with a request to collapse to just setting SE(3), not SO(3) + R3.
Reviewed 2 of 2 files at r1.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers (waiting on @joemasterjohn)
multibody/tree/fixed_offset_frame.h, line 117 at r1 (raw file):
p_PoFo_P_parameter.SetFromVector(X_PF.translation()); }
nit I guess it makes sense to just compute rotation, but I'm not sure if I understand a useful reason for only setting rotation.
Is there anyone who asked for this?
If not, can this be removed?
multibody/tree/fixed_offset_frame.h, line 190 at r1 (raw file):
const math::RigidTransform<double> X_PF_; // System parameter indices for `this` frame's RigidTransform stored in a
nit Why not store this as 1 parameter, a 3x4 matrix, in X_PF_parameter_index_
?
I doubt there's any meaningful gain for (ticket-based) caching between rotation / translation?
b8c8ff2
to
b3e035c
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewable status: needs at least two assigned reviewers (waiting on @EricCousineau-TRI)
multibody/tree/fixed_offset_frame.h, line 117 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit I guess it makes sense to just compute rotation, but I'm not sure if I understand a useful reason for only setting rotation.
Is there anyone who asked for this?
If not, can this be removed?
Done. No explicit ask, I was just mirroring GetPose/GetRotation. I'll remove it.
multibody/tree/fixed_offset_frame.h, line 190 at r1 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
nit Why not store this as 1 parameter, a 3x4 matrix, in
X_PF_parameter_index_
?I doubt there's any meaningful gain for (ticket-based) caching between rotation / translation?
Done. No real reason. I think it might avoid an extra copy (from GetAsMatrix34()
) when flattening the RigidTransform into a BasicVector. But I think it's minuscule.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewed 2 of 2 files at r2.
Reviewable status: needs at least two assigned reviewers
multibody/tree/fixed_offset_frame.h, line 117 at r1 (raw file):
Previously, joemasterjohn (Joe Masterjohn) wrote…
Done. No explicit ask, I was just mirroring GetPose/GetRotation. I'll remove it.
OK Thanks!
multibody/tree/fixed_offset_frame.h, line 190 at r1 (raw file):
Previously, joemasterjohn (Joe Masterjohn) wrote…
Done. No real reason. I think it might avoid an extra copy (from
GetAsMatrix34()
) when flattening the RigidTransform into a BasicVector. But I think it's minuscule.
OK Awesome, thanks!
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
+@sherm1 for platform review on Tuesday, please!
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @sherm1)
a discussion (no related file):
BTW Might I be able to request some good ol' Python bindings too?
I can handle it if it's outta scope for ya.
b3e035c
to
1ac10f6
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)
a discussion (no related file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
BTW Might I be able to request some good ol' Python bindings too?
I can handle it if it's outta scope for ya.
No problem. Added them in the latest revision. I think I added everything you need, but let me know if not.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewed 2 of 2 files at r3.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @sherm1)
a discussion (no related file):
Previously, joemasterjohn (Joe Masterjohn) wrote…
No problem. Added them in the latest revision. I think I added everything you need, but let me know if not.
OK Awesome, thanks!
bindings/pydrake/multibody/test/plant_test.py, line 1462 at r3 (raw file):
numpy_compare.assert_float_equal( frame.CalcPoseInBodyFrame(context).GetAsMatrix34(), np.array([[1., 0., 0., 1.],
BTW This may be clearer if you just check numpy_compare.assert_float_equal(actual, X_PF.GetAsMatrix34())
.
9e802c1
to
72a1186
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)
bindings/pydrake/multibody/test/plant_test.py, line 1462 at r3 (raw file):
Previously, EricCousineau-TRI (Eric Cousineau) wrote…
BTW This may be clearer if you just check
numpy_compare.assert_float_equal(actual, X_PF.GetAsMatrix34())
.
Done.
72a1186
to
40f0479
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewed 2 of 2 files at r4.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @sherm1)
40f0479
to
79fa96f
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Platform with a few minor comments.
Reviewed 2 of 2 files at r2, 1 of 2 files at r4, 1 of 1 files at r5.
Reviewable status: 7 unresolved discussions (waiting on @joemasterjohn)
multibody/plant/test/multibody_plant_test.cc, line 3300 at r5 (raw file):
.SetSpatialInertiaInBodyFrame(context_autodiff.get(), M2_L2o); const RevoluteJoint<AutoDiffXd>& elbow_joint_ad =
minor: please add a comment explaining that you added this test to make sure fixed offset frame parameter differentiation works, and how this tests that.
multibody/plant/test/multibody_plant_test.cc, line 3391 at r5 (raw file):
GTEST_TEST(MultibodyPlantTests, FixedOffsetFrameParameters) { // Add a plant with a few rigid bodies. MultibodyPlant<double> plant(0.0);
minor: it looks like the only occupant of this MBPlant is World? (Fine, but the comment isn't right then.)
multibody/tree/fixed_offset_frame.h, line 86 at r5 (raw file):
const systems::BasicVector<T>& X_PF_parameter = context.get_numeric_parameter(X_PF_parameter_index_); const Eigen::Matrix<T, 3, 4> X_PF =
minor: this is performing a copy in order to go from a Map to a Matrix. That shouldn't be necessary.
multibody/tree/fixed_offset_frame.h, line 100 at r5 (raw file):
const Eigen::Matrix<T, 3, 4> X_PF = Eigen::Map<const Eigen::Matrix<T, 3, 4>>( X_PF_parameter.get_value().data());
BTW consider collecting the above lines into an inline private method like get_X_PF_as_Matrix34(context)
rather than duplicating them here. (And see if you can avoid the extra copy.)
multibody/tree/fixed_offset_frame.h, line 110 at r5 (raw file):
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);
minor: this is copying 12 elements from the Map to the VectorX. That should not be necessary.
multibody/tree/fixed_offset_frame.h, line 151 at r5 (raw file):
// Implementation for MultibodyElement::DoDeclareParameters(). // FixedOffsetFrame declares a single parameter for its RigidTransform. void DoDeclareParameters(
minor: this method should be private if possible so it doesn't show up in Doxygen (maybe final also?). If you want to leave it in protected
it needs a doxygen comment directed to someone who is overriding it in a further derived class.
multibody/tree/fixed_offset_frame.h, line 156 at r5 (raw file):
Frame<T>::DoDeclareParameters(tree_system); const VectorX<T> X_PF = Eigen::Map<const VectorX<T>>( X_PF_.template cast<T>().GetAsMatrix34().data(), 12, 1);
BTW this copy should not be necessary either, although there isn't a performance issue here since this is just happening during construction. BasicVector's constructor takes an Eigen::Ref
and that should be happy to accept the Map directly.
79fa96f
to
0f74f54
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)
multibody/plant/test/multibody_plant_test.cc, line 3300 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: please add a comment explaining that you added this test to make sure fixed offset frame parameter differentiation works, and how this tests that.
Done.
multibody/plant/test/multibody_plant_test.cc, line 3391 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: it looks like the only occupant of this MBPlant is World? (Fine, but the comment isn't right then.)
Done. Copy/paste error.
multibody/tree/fixed_offset_frame.h, line 86 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: this is performing a copy in order to go from a Map to a Matrix. That shouldn't be necessary.
Done.
multibody/tree/fixed_offset_frame.h, line 100 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW consider collecting the above lines into an inline private method like
get_X_PF_as_Matrix34(context)
rather than duplicating them here. (And see if you can avoid the extra copy.)
Done. I can avoid explicitly creating a Matrix34 altogether by just grabbing the 3x3 block from the Map itself.
multibody/tree/fixed_offset_frame.h, line 110 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: this is copying 12 elements from the Map to the VectorX. That should not be necessary.
Done.
multibody/tree/fixed_offset_frame.h, line 151 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: this method should be private if possible so it doesn't show up in Doxygen (maybe final also?). If you want to leave it in
protected
it needs a doxygen comment directed to someone who is overriding it in a further derived class.
Done. The pattern I used forDoDeclareParameters
requires a derived class to call DoDeclareParameters
of it's parent class, which is why it's usually declared protected. But since FixedOffsetFrame
is final itself, I think it suffices for it to be private (and final too).
multibody/tree/fixed_offset_frame.h, line 156 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW this copy should not be necessary either, although there isn't a performance issue here since this is just happening during construction. BasicVector's constructor takes an
Eigen::Ref
and that should be happy to accept the Map directly.
Done. Removed the unnecessary copy.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Per f2f please verify that this works for weld joints (the primary use case I believe) and drop some TODOs and/or an issue if there are problems there.
Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform) (waiting on @EricCousineau-TRI and @sherm1)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks, looks great. Just a few remaining nits ...
Reviewed 2 of 2 files at r6.
Reviewable status: 3 unresolved discussions (waiting on @joemasterjohn)
multibody/plant/test/multibody_plant_test.cc, line 3300 at r5 (raw file):
Previously, joemasterjohn (Joe Masterjohn) wrote…
Done.
Nice, thanks.
multibody/plant/test/multibody_plant_test.cc, line 3391 at r5 (raw file):
Previously, joemasterjohn (Joe Masterjohn) wrote…
Done. Copy/paste error.
Still there?
multibody/plant/test/multibody_plant_test.cc, line 3267 at r6 (raw file):
// 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. We must update multibody components that
nit: to differentiate against (or with respect to)? Not sure the best wording -- we differentiate functions with respect to variables so "variables to differentiate" seems wrong.
multibody/plant/test/multibody_plant_test.cc, line 3280 at r6 (raw file):
const AutoDiffXd Gc2_ad = (1.0 / 12.0) * l2_ad * l2_ad; // Differentiable parameters for the acrobots mass/inertia parameters.
nit: acrobot's
multibody/plant/test/multibody_plant_test.cc, line 3413 at r6 (raw file):
plant.Finalize(); // Create a default context.override
nit: ".override" looks spurious?
0f74f54
to
e3f7fb7
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ok, I've added to the test to show that updating the parameters on a FixedOffsetFrame
that is the frame_on_parent
of a WeldJoint
correctly affects the pose of the child body.
I guess it should be noted that to achieve the effect of changing the offset between two welded bodies, one of the frames of the joint needs to be a FixedOffsetFrame
. So if for instance someone welded a body's origin to the world origin using the optional parameters to MultibodyPlant::AddJoint
:
plant.AddJoint<WeldJoint>("weld", plant.world_body(), {}, body, {});
Both the frame_on_parent
and frame_on_child
of the weld joint will be BodyFrame`s of their respective bodies, and thus won't have parameters to change.
This might seem obvious, but might not be to a user. Especially true for one using the parser since the urdf parser makes frame_on_child
of a weld joint to be the child's BodyFrame
(a URDF convention I think), while the SDF parser creates two FixedOffsetFrame
s. Compare:
detail_sdf_parser.cc:
case sdf::JointType::FIXED: {
plant->AddJoint<WeldJoint>(
joint_spec.Name(),
parent_body, X_PJ,
child_body, X_CJ,
RigidTransformd::Identity() /* X_JpJc */);
break;
}
detail_urdf_parser.cc
} else if (type.compare("fixed") == 0) {
throw_on_custom_joint(false);
plant->AddJoint<WeldJoint>(name, parent_body, X_PJ,
child_body, std::nullopt,
RigidTransformd::Identity());
}
Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform) (waiting on @sherm1)
multibody/plant/test/multibody_plant_test.cc, line 3391 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Still there?
Done.
multibody/plant/test/multibody_plant_test.cc, line 3267 at r6 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: to differentiate against (or with respect to)? Not sure the best wording -- we differentiate functions with respect to variables so "variables to differentiate" seems wrong.
Done. Bad English on my part.
multibody/plant/test/multibody_plant_test.cc, line 3280 at r6 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: acrobot's
Done.
multibody/plant/test/multibody_plant_test.cc, line 3413 at r6 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit: ".override" looks spurious?
Done.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Interesting -- thanks for investigating. I suspect it won't be immediately obvious to users how to move the weld frames around! But at least it can be done now. We might find that we need to add some sugar to make this easier at some point -- maybe like Joint::SetParentFramePose(&context, X_PJp)
and Joint::SetChildFramePose(&context, X_CJc)
?
Reviewed 1 of 1 files at r7.
Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),sherm1(platform)
Towards #13289.
Implements MultibodyElement Parameterization (started in #13860) for FixedOffsetFrame.
Introduces API for mutating FixedOffsetFrame given a context:
FixedOffsetFrame::SetPoseInBodyFrame
Adds python binding for:
Frame::CalcPoseInBodyFrame()
Frame::CalcRotationMatrixInBodyFrame()
FixedOffsetFrame::SetPoseInBodyFrame()
Relevant issues that this may partially/completely solve:
#14084
#13520
#13517
This change is