Make joint store default value

[joemasterjohn]

This is a PR to have the Joint class store a default value and to pass the value to its mobilizer implementation upon construction of the joint implementation.

• Solves the issue of mobilizers storing default joint positions (Joint default values need to be stored in Joint #13065)

\cc @amcastro-tri @EricCousineau-TRI @sherm1

---

This change is 

[joemasterjohn]

I'm aware that the test coverage is lacking in terms of passing the default values to the mobilizer both in set_default_(angle/translation/angles) and in MakeImplementationBlueprint(). I'm just not sure where the best place to put those tests would be.

Another detail, I had to modify an existing test in for the PrismaticJoint that made an assumption that the default position would be 0. The changes here to the Joint class make the default value the mean of the provided upper and lower position limits. If this behavior is undesirable, let me know.

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

[EricCousineau-TRI]

Can you edit this to remove the reference to initial_position? It will be deprecated when SDFormat 1.8 comes out:
https://github.com/osrf/sdformat/blame/master/Changelog.md#L13
@scpeters, @azeey, and I discussed it (offline, unfortunately), and feel that is redundant to storing some level of state (e.g. in some sort of //state tag).

For more info, see PR link in changelog.

[jwnimmer-tri]

... [initial_position] will be deprecated when SDFormat 1.8 comes out ...

Doesn't that make #13065 moot? It's an issue requesting parsing for a feature we're about to deprecate?

[EricCousineau-TRI]

Ah, yup. I commented on the syntax, w/o reading the issue... d'oh!

[sherm1]

Doesn't that make #13065 moot? It's an issue requesting parsing for a feature we're about to deprecate?

I don't think the idea of specifying initial positions for a joint is being deprecated, just the particular syntax. Is that right @EricCousineau-TRI ? A Drake user should be able to specify default coordinate values for a joint, regardless of where that default value comes from (e.g. sdf or direct calls to the API).

[EricCousineau-TRI]

That is correct. I would also advocate that we either (a) fully decide on how to specify the syntax or (b) defer the decision for now.

I don't really support defining initial positions within models themselves. I think the model should just specify its "zero configuration", and the downstream apps can figure out what "home configuration" they want (e.g. in the world state or what not).

[sherm1]

I don't really support defining initial positions within models themselves.

The modeler is often in the best position to do that. Consider the four-bar linkage: it is nice to specify the zero positions of all the joints the same way, typically so that zero means the connected links line up ======*====== which aligns body frames and has other nice properties for uniform construction. But then the intent is that the bodies should form a loop structure, meaning that setting the joints to 90 degrees by default is a good initial configuration. That information is present in the modeler's head at the time s/he makes the model.

If that's not convincing, consider the Strandbeest. That consists of identical repeated substructures which should be specified identically in the sdf for modeling sanity. But for operation the leg positions need to be offset by particular angles, while also forming loop structures. I don't think it makes sense to leave that to the application(s) to figure out -- the model builder (Theo Jansen?) is best positioned to provide that information. Many apps might make use of it.

[EricCousineau-TRI]

Will move discussion to issue (just to avoid cluttering PR further)

[joemasterjohn]

+@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)

[EricCousineau-TRI]

First pass done, PTAL!

Reviewed 8 of 10 files at r1.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI and @joemasterjohn)

a discussion (no related file):
nit (minor) The current commit is in the form [component]: Description.
I've seen:

• Description (most popular?)
• component: Description (my preference, and maybe 2nd place format?)
• [component] Description (Sean's style, I think?)

Can you choose one of these 3, rather than introducing a new one?

---

---

multibody/tree/joint.h, line 143 at r1 (raw file):

    // initialize the default positions to 0.
    default_positions_ = VectorX<double>::Zero(pos_lower_limits.size());

Can you state why num_positions() is not used, and perhaps use a better variable name?
e.g.

// N.B. We cannot use `num_positions()` here because it is virtual.
const int num_positions = pos_lower_limits.size();

// intialize the default positions.
default_positions_ = VectorX<double>::Zero(num_positions);

---

multibody/tree/prismatic_joint.h, line 262 at r1 (raw file):

  }

  int do_get_num_velocities() const override { return 1; }

Can you revert all unnecessary whitespace changes?

---

multibody/tree/test/ball_rpy_joint_test.cc, line 208 at r1 (raw file):

  const Vector3d new_default_angles =
      0.5 * lower_limit_angles + 0.5 * upper_limit_angles;

nit Can you make this a constant above, instead of relying on an indirection relationship here? e.g.

const double kPositionNonZeroDefault = (kPositionLowerLimit + kPositionUpperLimit) / 2;

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

               std::runtime_error);
  EXPECT_THROW(mutable_joint_->set_default_angles(out_of_bounds_high_angles),
               std::runtime_error);

What happens if set_position_limits is set to a value that then invalidates its default value?

Conservative approach: It should fail. (easy to relax later)
Less conservative approach: Clamp the defaults to be within the limits.

[joemasterjohn]

Thanks for the quick review!

Reviewable status: 4 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI and @joemasterjohn)

a discussion (no related file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit (minor) The current commit is in the form [component]: Description.
I've seen:

• Description (most popular?)
• component: Description (my preference, and maybe 2nd place format?)
• [component] Description (Sean's style, I think?)

Can you choose one of these 3, rather than introducing a new one?

Ok thanks, I was looking for a doc where that might have been specified. I'll make it the first.

---

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

What happens if set_position_limits is set to a value that then invalidates its default value?

Conservative approach: It should fail. (easy to relax later)
Less conservative approach: Clamp the defaults to be within the limits.

Right, good point. I had a previous commit that tried to address this, but I reverted it because it caused a lot of tests to crash.

Related: there is the same issue technically in setting the default configuration to the zero configuration in the Joint constructor. If the zero configuration is not within the positional limits provided, what should the joint do? Right now the Mobilizer takes the convention that a default state for a default context means all of the join angles are 0. For instance in the following snippet:

  auto plant = std::make_unique<MultibodyPlant<double>>(0.0);

  // Add a rigid body to model each link.
  const RigidBody<double>& link1 = plant->AddRigidBody("link1", SpatialInertia<double>());
  const RigidBody<double>& link2 = plant->AddRigidBody("link2", SpatialInertia<double>());
  const RevoluteJoint<double>& joint = plant->AddJoint<RevoluteJoint>(
      std::make_unique<RevoluteJoint<double>>("joint", link1.body_frame(), link2.body_frame(), Vector3d::UnitZ(), -5.0, -2.0));

  plant->Finalize();

  // create a default context
  auto context = plant->CreateDefaultContext();

  std::cout << "joint angle: " << joint.get_angle(*context) << std::endl;

The angle reported for the default context is 0, although it is not within the joint limits. I'm not sure if this is considered a bug, since I'm not so familiar yet with MBP.

With the changes in this PR, the joints inform the mobilizers (upon their construction in Joint::MakeImplementationBlueprint()) of the default value. If MobilizerImpl::set_default_position() is never explicitly called on a MobilizerImpl, it takes the convention that the default configuration is the zero configuration. This has consequences in Mobilizer::set_default_state(const systems::Context<T>&, systems::State<T>* state), which calls get_default_position().

There are a few places I've seen that expect a creating a default context to set all joints angles to zero, which is why my previous commit that clamped the default angle cause regressions. I didn't want to PR something that could totally blow things up, so for now my plan is to have the Joint class mirror the Mobilizer in implicitly setting defaults to 0.

[sherm1]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI and @joemasterjohn)

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Right, good point. I had a previous commit that tried to address this, but I reverted it because it caused a lot of tests to crash.

Related: there is the same issue technically in setting the default configuration to the zero configuration in the Joint constructor. If the zero configuration is not within the positional limits provided, what should the joint do? Right now the Mobilizer takes the convention that a default state for a default context means all of the join angles are 0. For instance in the following snippet:

  auto plant = std::make_unique<MultibodyPlant<double>>(0.0);

  // Add a rigid body to model each link.
  const RigidBody<double>& link1 = plant->AddRigidBody("link1", SpatialInertia<double>());
  const RigidBody<double>& link2 = plant->AddRigidBody("link2", SpatialInertia<double>());
  const RevoluteJoint<double>& joint = plant->AddJoint<RevoluteJoint>(
      std::make_unique<RevoluteJoint<double>>("joint", link1.body_frame(), link2.body_frame(), Vector3d::UnitZ(), -5.0, -2.0));

  plant->Finalize();

  // create a default context
  auto context = plant->CreateDefaultContext();

  std::cout << "joint angle: " << joint.get_angle(*context) << std::endl;

The angle reported for the default context is 0, although it is not within the joint limits. I'm not sure if this is considered a bug, since I'm not so familiar yet with MBP.

With the changes in this PR, the joints inform the mobilizers (upon their construction in Joint::MakeImplementationBlueprint()) of the default value. If MobilizerImpl::set_default_position() is never explicitly called on a MobilizerImpl, it takes the convention that the default configuration is the zero configuration. This has consequences in Mobilizer::set_default_state(const systems::Context<T>&, systems::State<T>* state), which calls get_default_position().

There are a few places I've seen that expect a creating a default context to set all joints angles to zero, which is why my previous commit that clamped the default angle cause regressions. I didn't want to PR something that could totally blow things up, so for now my plan is to have the Joint class mirror the Mobilizer in implicitly setting defaults to 0.

BTW it is generally considered fine to set generalized coordinates q and velocities v to values that don't satisfy specified constraints (joint limits are essentially unilateral constraints). In general it requires a substantial computation to find a set of q's and v's that satisfy all constraints since they often conflict (in a relative-coordinate system like ours, changing a q at one joint moves everything outboard of that joint also, and that can easily violate some other constraint, say non-penetration).

OTOH ideally we would try to obey easily satisfied constraints at initialization if we can do it. And usually that's possible for joint limits. But it would be fine to note that as a TODO and/or file an issue.

[EricCousineau-TRI]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers (waiting on @EricCousineau-TRI, @joemasterjohn, and @sherm1)

a discussion (no related file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Ok thanks, I was looking for a doc where that might have been specified. I'll make it the first.

OK Sweet! And yeah, it's more a rule of consistency than of a prescription.

---

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW it is generally considered fine to set generalized coordinates q and velocities v to values that don't satisfy specified constraints (joint limits are essentially unilateral constraints). In general it requires a substantial computation to find a set of q's and v's that satisfy all constraints since they often conflict (in a relative-coordinate system like ours, changing a q at one joint moves everything outboard of that joint also, and that can easily violate some other constraint, say non-penetration).

OTOH ideally we would try to obey easily satisfied constraints at initialization if we can do it. And usually that's possible for joint limits. But it would be fine to note that as a TODO and/or file an issue.

Hm... Another solution is to use optional<> to know if a value was set. Then, if a user only sets limits but not the default value, then there's no conflict, but if the user sets both, then there should be an error. (Granted, it may be painful if it's set by different things, e.g. parsing and another call site, but still better than nothing?)

Not sure at what level it should be encapsulated (e.g. should you let things access the optional<>, or only the value, and return 0. if it's nullopt), but mayhaps it's feasible?

Regarding constraint satisfaction, I agree with Sherm that it should be OK to set an infeasible configuration at some point (e.g. intialization), just as long as anything that requires a valid configuration flags it as such such.

[joemasterjohn]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI, @joemasterjohn, and @sherm1)

---

multibody/tree/joint.h, line 143 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Can you state why num_positions() is not used, and perhaps use a better variable name?
e.g.

// N.B. We cannot use `num_positions()` here because it is virtual.
const int num_positions = pos_lower_limits.size();

// intialize the default positions.
default_positions_ = VectorX<double>::Zero(num_positions);

Done.

---

multibody/tree/prismatic_joint.h, line 262 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Can you revert all unnecessary whitespace changes?

Done.

[joemasterjohn]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI)

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Hm... Another solution is to use optional<> to know if a value was set. Then, if a user only sets limits but not the default value, then there's no conflict, but if the user sets both, then there should be an error. (Granted, it may be painful if it's set by different things, e.g. parsing and another call site, but still better than nothing?)

Not sure at what level it should be encapsulated (e.g. should you let things access the optional<>, or only the value, and return 0. if it's nullopt), but mayhaps it's feasible?

Regarding constraint satisfaction, I agree with Sherm that it should be OK to set an infeasible configuration at some point (e.g. intialization), just as long as anything that requires a valid configuration flags it as such such.

I played around with making the default values optional<> as they are in MobilizerImpl. I didn't like exposing an optional return type to the user of Joint and encapsulating it seems like a waste. If we were to return 0 upon nullopt in that encapsulation it would be the same as just setting the defaults to 0 anyway, in the users eyes. I think there are many places where 0 is assumed to be the default angle of the joint anyway, so mirroring that in the Joint class should be fine.

[EricCousineau-TRI]

Reviewed 4 of 7 files at r3.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI, @joemasterjohn, and @sherm1)

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

I played around with making the default values optional<> as they are in MobilizerImpl. I didn't like exposing an optional return type to the user of Joint and encapsulating it seems like a waste. If we were to return 0 upon nullopt in that encapsulation it would be the same as just setting the defaults to 0 anyway, in the users eyes. I think there are many places where 0 is assumed to be the default angle of the joint anyway, so mirroring that in the Joint class should be fine.

Aye, sounds good for to let the two step on each other's feet, then.

Can you describe this in set_position_limits as well? (e.g. updating limits could invalidate other configurations, including the default one) or something like that?

And can you still test this part of the contract? i.e. setting set_position_limits after having set set_default_value, with some sort of EXPECT_NO_THROW thing?

[EricCousineau-TRI]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI, @joemasterjohn, and @sherm1)

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Aye, sounds good for to let the two step on each other's feet, then.

Can you describe this in set_position_limits as well? (e.g. updating limits could invalidate other configurations, including the default one) or something like that?

And can you still test this part of the contract? i.e. setting set_position_limits after having set set_default_value, with some sort of EXPECT_NO_THROW thing?

(well, to let setting limits invalidate default value, but not vice versa?)

[EricCousineau-TRI]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI, @joemasterjohn, and @sherm1)

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

(well, to let setting limits invalidate default value, but not vice versa?)

(Sorry for the email spam)
Should this be a symmetric relationship? Should there be no error if the default value is invalid?

[joemasterjohn]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI)

---

multibody/tree/test/ball_rpy_joint_test.cc, line 228 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

(Sorry for the email spam)
Should this be a symmetric relationship? Should there be no error if the default value is invalid?

No worries. And thanks for the suggestion. I agree it should be a symmetric relationship. Took out the exception in Joint and modified all the tests.

[EricCousineau-TRI]

Reviewed 4 of 4 files at r4.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn)

---

multibody/tree/joint.h, line 364 at r4 (raw file):

  /// @throws std::exception if the dimension of @p default_positions does not
  /// match num_positions().
  void set_default_positions(const VectorX<double>& default_positions) {

nit (Last one, I promise!) Is it worth stating here (and in the set_position_limits()) that neither of this check for conflicts?

(I don't have a good suggestion for how to concisely word this, so feel free to ignore!)

[EricCousineau-TRI]

- thank you for your patience!

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @joemasterjohn)

[joemasterjohn]

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI)

---

multibody/tree/joint.h, line 364 at r4 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit (Last one, I promise!) Is it worth stating here (and in the set_position_limits()) that neither of this check for conflicts?

(I don't have a good suggestion for how to concisely word this, so feel free to ignore!)

Easy enough. Done.

[joemasterjohn]

Thanks, @EricCousineau-TRI!

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @EricCousineau-TRI)

[EricCousineau-TRI]

Reviewed 1 of 1 files at r5.
Reviewable status: needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

---

multibody/tree/joint.h, line 364 at r4 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Easy enough. Done.

OK Sweet, thanks!

[EricCousineau-TRI]

Welcome!

Reviewable status: needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

[EricCousineau-TRI]

+@sammy-tri for platform review, please (since I am platform on Monday)

Reviewable status: LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sammy-tri)

[sammy-tri]

Reviewed 3 of 10 files at r1, 2 of 7 files at r3, 3 of 4 files at r4, 1 of 1 files at r5.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

[sammy-tri]

+(status: squashing now)

Reviewable status: complete! all discussions resolved, LGTM from assignees sammy-tri(platform),EricCousineau-TRI(platform)