Change the OSC structure to make it fully compliant with the systems framework. #362
Conversation
There was a problem hiding this comment.
I gave a look to all items except for the meat of the changes to operational_space_control (which I don't know well enough to understand just from the diff, I'll need to dive into it). Note that @yangwill is requested specifically to look at that logic.
Most of these are either nits, request for clarification, or requests to document changes in the PR description.
In general, it would be good to get into the habit of adding testing directions to the PR description.
| @@ -372,6 +372,7 @@ cc_binary( | |||
| cc_binary( | |||
| name = "run_osc_walking_controller", | |||
| srcs = ["run_osc_walking_controller.cc"], | |||
| tags = ["manual"], | |||
There was a problem hiding this comment.
Noting this is the only actual change in this file, and it makes wildcards ignore this rule.
| @@ -16,13 +16,49 @@ namespace controllers { | |||
| static constexpr int kSpaceDim = 3; | |||
| static constexpr int kQuaternionDim = 4; | |||
|
|
|||
| struct OscTrackingDataState { | |||
There was a problem hiding this comment.
I would definitely consider moving this struct into the relevant data class, e.g., OSCTrackingData::State.
Especially since you want all instances to be created with AllocateState. Move this into the class, make it private (so the constructor can only be accessed within the outer class).
| Eigen::MatrixXd time_varying_weight_; | ||
| double last_timestamp_ = -1; | ||
|
|
||
| Eigen::VectorXd CalcYddotCommandSol(const Eigen::VectorXd& dv) const { |
There was a problem hiding this comment.
Add docstring (this should be a quick one).
| name_(name), | ||
| n_y_(n_y), | ||
| n_ydot_(n_ydot), | ||
| K_p_(K_p), | ||
| K_d_(K_d), | ||
| W_(W) {} | ||
| W_(W) { |
There was a problem hiding this comment.
Nit: do we have consistent formatting software we want to use? This is inconsistent with other files where {} is kept on the same line.
|
|
||
| // Create state receiver. | ||
| auto state_receiver = | ||
| builder.AddSystem<systems::RobotOutputReceiver>(plant_w_spr); | ||
| auto pelvis_filt = |
There was a problem hiding this comment.
Note: the removal of this filter is not included in the PR description.
| OscTrackingDataState& tracking_data_state) const { | ||
| tracking_data_state.JdotV_ = | ||
| plant_.CalcBiasCenterOfMassTranslationalAcceleration( | ||
| context_wo_spr, JacobianWrtVariable::kV, world_, world_); | ||
| } | ||
|
|
||
| } |
| const drake::systems::Context<double>& context, double t, | ||
| double t_since_last_state_switch, int fsm_state, int next_fsm_state, | ||
| const Eigen::MatrixXd& M) const; | ||
| // In the IROS 2021 paper, the problem was unconstrained and could be solved |
There was a problem hiding this comment.
Cite specific IROS paper.
Presumably Yang, Posa "
Impact Invariant Control with Applications to Bipedal Locomotion"?
| @@ -265,45 +259,56 @@ class OperationalSpaceControl : public drake::systems::LeafSystem<double> { | |||
| .GetAsSolverOptions(drake::solvers::OsqpSolver::id()) | |||
| ); | |||
| }; | |||
|
|
|||
| std::unique_ptr<std::vector<OscTrackingDataState>> | |||
| void SolveIDQP(const drake::systems::Context<double>& context, | ||
| id_qp_solution* solution) const; | ||
|
|
||
| void UpdateTrackingData(const drake::systems::Context<double>& context, |
There was a problem hiding this comment.
Nit: I'd put this near Allocate, as they are used together in the caching procedure.
…ere not cuaght during Drake upgrade (#363)
There was a problem hiding this comment.
Reviewable status: 0 of 27 files reviewed, 15 unresolved discussions (waiting on @egordon and @yangwill)
WORKSPACE line 6 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Just documenting this is direct from: https://github.com/bazelbuild/apple_support/releases
Done.
examples/Cassie/BUILD.bazel line 375 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Noting this is the only actual change in this file, and it makes wildcards ignore this rule.
Correct. This particular walking controller has gone unmaintained since yu-ming left and will likely be removed once my more recent controllers are merged in. All the other changes are just my bazel linter auto-formatting this file
examples/Cassie/run_osc_walking_controller_alip.cc line 0 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
(Reviewable was unable to map this GitHub inline comment thread to the right spot — sorry!)
Note: the removal of this filter is not included in the PR description.
Reverted to keep this PR to a minimal set of changes
systems/controllers/osc/operational_space_control.h line 295 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Nit: I'd put this near Allocate, as they are used together in the caching procedure.
Made allocate private, near this one in the header
systems/controllers/osc/operational_space_control.h line 303 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Cite specific IROS paper.
Presumably Yang, Posa "
Impact Invariant Control with Applications to Bipedal Locomotion"?
Done.
systems/controllers/osc/operational_space_control.h line 416 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Flagging a change in the default constraint here un-documented in the PR description.
Noted
systems/controllers/osc/options_tracking_data.cc line 75 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Nit: replace magic number with if dimension of td_state.y_ == td_state.filtered_y_
Unfortunately I didn't write these filters, I would need to dig deeper into why this exists to replace it. n_y_ == 4 is only true for rot_space_tracking data at the moment, but still I'm not sure why that would need it's own logic.
systems/controllers/osc/osc_tracking_data.h line 124 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Is this not also a const function?
Done.
systems/controllers/osc/osc_tracking_data.h line 175 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Do you still want
w_sprwo_sprin the arguments for these update methods if you're removing the functionality?
Done.
systems/controllers/osc/osc_tracking_data.cc line 42 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Nit: do we have consistent formatting software we want to use? This is inconsistent with other files where
{}is kept on the same line.
It should be {}. Technically we should be following Drake's style guide (with appropriate linters detailed here): https://drake.mit.edu/code_style_tools.html
There was a problem hiding this comment.
Reviewable status: 0 of 27 files reviewed, 15 unresolved discussions (waiting on @egordon and @yangwill)
systems/controllers/osc/operational_space_control.h line 263 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Can this be protected or private?
Done.
There was a problem hiding this comment.
Reviewable status: 0 of 27 files reviewed, 13 unresolved discussions (waiting on @egordon and @yangwill)
systems/controllers/osc/osc_tracking_data.h line 49 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Add docstring (this should be a quick one).
Done.
systems/controllers/osc/osc_tracking_data.h line 167 at r1 (raw file):
Previously, egordon (Ethan K. Gordon) wrote…
Flagging: could this be const too? It seems to be just set in constructor.
Done.
Some notes:
@yangwill, can you test the correctness of the impact invariant implementation?
This change is