Skip to content

Commit 257e3ed

Browse files
Meow404xuanhien070594ebianchi
authored
feat(osc): refactor - bring remaining changes from sampling_based_c3 (#406)
* Resolve merge conflicts with main (#386) * Resolve merge conflicts with main * Remove obsolete codes * Remove accidentally commited files * Add external force tracking result to lcm * Add back the deleted files in Cassie example * Add back deleted files in pydairlib/cassie * Revert code formatting * Revert code formatting * Revert code formatting * Include workspace and input limits as constraints in C3 QP step * Add parameters for different contact pair configurations * Fix intended checks for resolving contact pairs and maintaining sample buffer * fix: removed depricated functions --------- Co-authored-by: Hien Bui <xuanhien070594@gmail.com> Co-authored-by: Bibit Bianchini <ebianchini.mit@gmail.com>
1 parent 55dda62 commit 257e3ed

27 files changed

Lines changed: 1404 additions & 1297 deletions

‎examples/Cassie/cassie_state_estimator.cc‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1102,6 +1102,9 @@ void CassieStateEstimator::DoCalcNextUpdateTime(
11021102
const auto& self = dynamic_cast<const CassieStateEstimator&>(system);
11031103
return self.Update(c, s);
11041104
};
1105+
auto& uu_events = events->get_mutable_unrestricted_update_events();
1106+
uu_events.AddEvent(UnrestrictedUpdateEvent<double>(
1107+
drake::systems::TriggerType::kTimed, callback));
11051108
} else {
11061109
*time = INFINITY;
11071110
}

‎examples/Cassie/diagrams/osc_running_controller_diagram.cc‎

Lines changed: 41 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,8 @@ namespace controllers {
6464

6565
OSCRunningControllerDiagram::OSCRunningControllerDiagram(
6666
drake::multibody::MultibodyPlant<double>& plant,
67-
const string& osc_running_gains_filename, const string& osqp_settings_filename)
67+
const string& osc_running_gains_filename,
68+
const string& osqp_settings_filename)
6869
: plant_(&plant),
6970
pos_map(multibody::MakeNameToPositionsMap(plant)),
7071
vel_map(multibody::MakeNameToVelocitiesMap(plant)),
@@ -155,19 +156,20 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
155156
auto state_receiver = builder.AddSystem<systems::RobotOutputReceiver>(plant);
156157
auto command_sender = builder.AddSystem<systems::RobotCommandSender>(plant);
157158
auto osc = builder.AddSystem<systems::controllers::OperationalSpaceControl>(
158-
plant, plant, plant_context.get(), plant_context.get(), true);
159+
plant, plant_context.get(), true);
159160
auto radio_parser = builder.AddSystem<systems::RadioParser>();
160161
auto failure_aggregator =
161162
builder.AddSystem<systems::ControllerFailureAggregator>(
162163
control_channel_name_, 1);
163164

164165
/**** OSC setup ****/
165166
/// REGULARIZATION COSTS
166-
osc->SetAccelerationCostWeights(osc_running_gains.w_accel * osc_running_gains.W_acceleration);
167+
osc->SetAccelerationCostWeights(osc_running_gains.w_accel *
168+
osc_running_gains.W_acceleration);
167169
osc->SetInputSmoothingCostWeights(osc_running_gains.w_input_reg *
168-
osc_running_gains.W_input_regularization);
170+
osc_running_gains.W_input_regularization);
169171
osc->SetInputCostWeights(osc_running_gains.w_input *
170-
osc_running_gains.W_input_regularization);
172+
osc_running_gains.W_input_regularization);
171173
osc->SetLambdaContactRegularizationWeight(
172174
osc_running_gains.w_lambda * osc_running_gains.W_lambda_c_regularization);
173175
osc->SetLambdaHolonomicRegularizationWeight(
@@ -179,14 +181,22 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
179181
// Contact information for OSC
180182
osc->SetContactFriction(osc_running_gains.mu);
181183

182-
osc->AddStateAndContactPoint(RunningFsmState::kLeftStance,
183-
&left_toe_evaluator);
184-
osc->AddStateAndContactPoint(RunningFsmState::kLeftStance,
185-
&left_heel_evaluator);
186-
osc->AddStateAndContactPoint(RunningFsmState::kRightStance,
187-
&right_toe_evaluator);
188-
osc->AddStateAndContactPoint(RunningFsmState::kRightStance,
189-
&right_heel_evaluator);
184+
osc->AddContactPoint("left_toe",
185+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
186+
&left_toe_evaluator),
187+
{RunningFsmState::kLeftStance});
188+
osc->AddContactPoint("left_heel",
189+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
190+
&left_heel_evaluator),
191+
{RunningFsmState::kLeftStance});
192+
osc->AddContactPoint("right_toe",
193+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
194+
&right_toe_evaluator),
195+
{RunningFsmState::kRightStance});
196+
osc->AddContactPoint("right_heel",
197+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
198+
&right_heel_evaluator),
199+
{RunningFsmState::kRightStance});
190200

191201
// Fix the springs in the dynamics
192202
evaluators.add_evaluator(&left_fixed_knee_spring);
@@ -196,8 +206,9 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
196206
evaluators.add_evaluator(&left_loop);
197207
evaluators.add_evaluator(&right_loop);
198208

199-
200-
osc->AddKinematicConstraint(&evaluators);
209+
osc->AddKinematicConstraint(
210+
std::unique_ptr<const multibody::KinematicEvaluatorSet<double>>(
211+
&evaluators));
201212

202213
/**** Tracking Data *****/
203214

@@ -208,8 +219,8 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
208219
osc_running_gains.vel_scale_trans_lateral);
209220

210221
auto pelvis_trans_traj_generator =
211-
builder.AddSystem<PelvisTransTrajGenerator>(
212-
plant, plant_context.get(), feet_contact_points);
222+
builder.AddSystem<PelvisTransTrajGenerator>(plant, plant_context.get(),
223+
feet_contact_points);
213224
pelvis_trans_traj_generator->SetSLIPParams(
214225
osc_running_gains.rest_length, osc_running_gains.rest_length_offset);
215226
auto l_foot_traj_generator = builder.AddSystem<FootTrajGenerator>(
@@ -261,8 +272,8 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
261272
right_foot_tracking_data->AddStateAndPointToTrack(
262273
RunningFsmState::kLeftFlight, "toe_right");
263274

264-
left_foot_tracking_data->AddStateAndPointToTrack(
265-
RunningFsmState::kLeftFlight, "toe_left");
275+
left_foot_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight,
276+
"toe_left");
266277
right_foot_tracking_data->AddStateAndPointToTrack(
267278
RunningFsmState::kRightFlight, "toe_right");
268279

@@ -274,17 +285,17 @@ OSCRunningControllerDiagram::OSCRunningControllerDiagram(
274285
"right_hip_traj", osc_running_gains.K_p_swing_foot,
275286
osc_running_gains.K_d_swing_foot, osc_running_gains.W_swing_foot, plant,
276287
plant);
277-
left_hip_tracking_data->AddStateAndPointToTrack(
278-
RunningFsmState::kRightStance, "pelvis");
279-
right_hip_tracking_data->AddStateAndPointToTrack(
280-
RunningFsmState::kLeftStance, "pelvis");
281-
right_hip_tracking_data->AddStateAndPointToTrack(
282-
RunningFsmState::kLeftFlight, "pelvis");
283-
left_hip_tracking_data->AddStateAndPointToTrack(
284-
RunningFsmState::kRightFlight, "pelvis");
285-
286-
left_hip_tracking_data->AddStateAndPointToTrack(
287-
RunningFsmState::kLeftFlight, "pelvis");
288+
left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightStance,
289+
"pelvis");
290+
right_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftStance,
291+
"pelvis");
292+
right_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight,
293+
"pelvis");
294+
left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kRightFlight,
295+
"pelvis");
296+
297+
left_hip_tracking_data->AddStateAndPointToTrack(RunningFsmState::kLeftFlight,
298+
"pelvis");
288299
right_hip_tracking_data->AddStateAndPointToTrack(
289300
RunningFsmState::kRightFlight, "pelvis");
290301

‎examples/Cassie/diagrams/osc_walking_controller_diagram.cc‎

Lines changed: 33 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -92,8 +92,8 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
9292
left_right_foot({left_toe_origin, right_toe_origin}),
9393
left_foot_points({left_heel, left_toe}),
9494
right_foot_points({right_heel, right_toe}),
95-
view_frame_(
96-
std::make_shared<multibody::WorldYawViewFrame<double>>(plant.GetBodyByName("pelvis"))),
95+
view_frame_(std::make_shared<multibody::WorldYawViewFrame<double>>(
96+
plant.GetBodyByName("pelvis"))),
9797
left_toe_evaluator(multibody::WorldPointEvaluator(
9898
plant, left_toe.first, left_toe.second, *view_frame_,
9999
Matrix3d::Identity(), Vector3d::Zero(), {1, 2})),
@@ -198,16 +198,17 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
198198
swing_ft_accel_gain_multiplier_samples[2](2, 2) *= 0;
199199
swing_ft_accel_gain_multiplier_samples[3](2, 2) *= 0;
200200
auto swing_ft_accel_gain_multiplier_gain_multiplier =
201-
std::make_shared<PiecewisePolynomial<double>>(PiecewisePolynomial<double>::FirstOrderHold(
202-
swing_ft_accel_gain_multiplier_breaks,
203-
swing_ft_accel_gain_multiplier_samples));
201+
std::make_shared<PiecewisePolynomial<double>>(
202+
PiecewisePolynomial<double>::FirstOrderHold(
203+
swing_ft_accel_gain_multiplier_breaks,
204+
swing_ft_accel_gain_multiplier_samples));
204205

205206
/**** Initialize all the leaf systems ****/
206207

207208
auto state_receiver = builder.AddSystem<systems::RobotOutputReceiver>(plant);
208209
auto command_sender = builder.AddSystem<systems::RobotCommandSender>(plant);
209210
auto osc = builder.AddSystem<systems::controllers::OperationalSpaceControl>(
210-
plant, plant, plant_context.get(), plant_context.get(), true);
211+
plant, plant_context.get(), true);
211212
auto fsm = builder.AddSystem<systems::TimeBasedFiniteStateMachine>(
212213
plant, fsm_states, state_durations);
213214
auto liftoff_event_time =
@@ -234,28 +235,26 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
234235
// Contact information for OSC
235236
osc->SetContactFriction(osc_walking_gains.mu);
236237

237-
osc->AddStateAndContactPoint(left_stance_state, &left_toe_evaluator);
238-
osc->AddStateAndContactPoint(left_stance_state, &left_heel_evaluator);
239-
osc->AddStateAndContactPoint(right_stance_state, &right_toe_evaluator);
240-
osc->AddStateAndContactPoint(right_stance_state, &right_heel_evaluator);
241-
if (has_double_stance) {
242-
osc->AddStateAndContactPoint(post_left_double_support_state,
243-
&left_toe_evaluator);
244-
osc->AddStateAndContactPoint(post_left_double_support_state,
245-
&left_heel_evaluator);
246-
osc->AddStateAndContactPoint(post_left_double_support_state,
247-
&right_toe_evaluator);
248-
osc->AddStateAndContactPoint(post_left_double_support_state,
249-
&right_heel_evaluator);
250-
osc->AddStateAndContactPoint(post_right_double_support_state,
251-
&left_toe_evaluator);
252-
osc->AddStateAndContactPoint(post_right_double_support_state,
253-
&left_heel_evaluator);
254-
osc->AddStateAndContactPoint(post_right_double_support_state,
255-
&right_toe_evaluator);
256-
osc->AddStateAndContactPoint(post_right_double_support_state,
257-
&right_heel_evaluator);
258-
}
238+
osc->AddContactPoint("left_toe",
239+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
240+
&left_toe_evaluator),
241+
{left_stance_state, post_left_double_support_state,
242+
post_right_double_support_state});
243+
osc->AddContactPoint("left_heel",
244+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
245+
&left_heel_evaluator),
246+
{left_stance_state, post_left_double_support_state,
247+
post_right_double_support_state});
248+
osc->AddContactPoint("right_toe",
249+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
250+
&right_toe_evaluator),
251+
{right_stance_state, post_left_double_support_state,
252+
post_right_double_support_state});
253+
osc->AddContactPoint("right_heel",
254+
std::unique_ptr<multibody::WorldPointEvaluator<double>>(
255+
&right_heel_evaluator),
256+
{right_stance_state, post_left_double_support_state,
257+
post_right_double_support_state});
259258

260259
evaluators.add_evaluator(&left_loop);
261260
evaluators.add_evaluator(&right_loop);
@@ -266,7 +265,8 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
266265
evaluators.add_evaluator(&left_fixed_ankle_spring);
267266
evaluators.add_evaluator(&right_fixed_ankle_spring);
268267

269-
osc->AddKinematicConstraint(&evaluators);
268+
osc->AddKinematicConstraint(
269+
std::unique_ptr<multibody::KinematicEvaluatorSet<double>>(&evaluators));
270270

271271
/**** Trajectory Generators ****/
272272

@@ -346,8 +346,10 @@ OSCWalkingControllerDiagram::OSCWalkingControllerDiagram(
346346
"swing_ft_traj", osc_walking_gains.K_p_swing_foot,
347347
osc_walking_gains.K_d_swing_foot, osc_walking_gains.W_swing_foot, plant,
348348
plant);
349-
swing_ft_traj_global_->AddStateAndPointToTrack(left_stance_state, "toe_right");
350-
swing_ft_traj_global_->AddStateAndPointToTrack(right_stance_state, "toe_left");
349+
swing_ft_traj_global_->AddStateAndPointToTrack(left_stance_state,
350+
"toe_right");
351+
swing_ft_traj_global_->AddStateAndPointToTrack(right_stance_state,
352+
"toe_left");
351353
swing_ft_traj_local_->SetTimeVaryingPDGainMultiplier(
352354
swing_ft_gain_multiplier_gain_multiplier);
353355
swing_ft_traj_local_->SetTimerVaryingFeedForwardAccelMultiplier(

‎examples/Cassie/networking/cassie_udp_publisher.cc‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ std::string CassieUDPPublisher::make_name(const std::string& address,
9292
// period or publish period = 0.0 was passed to the constructor).
9393
drake::systems::EventStatus CassieUDPPublisher::PublishInputAsUDPMessage(
9494
const drake::systems::Context<double>& context) const {
95-
SPDLOG_TRACE(drake::log(), "Publishing UDP {} message", address_);
95+
SPDLOG_TRACE("Publishing UDP {} message", address_);
9696

9797
// Converts the input into message bytes.
9898
const drake::AbstractValue* const input_value =

‎examples/Cassie/run_osc_jumping_controller.cc‎

Lines changed: 26 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ using std::map;
3636
using std::pair;
3737
using std::string;
3838
using std::vector;
39+
using std::unique_ptr;
3940

4041
using Eigen::Matrix3d;
4142
using Eigen::MatrixXd;
@@ -267,7 +268,7 @@ int DoMain(int argc, char* argv[]) {
267268
auto command_sender =
268269
builder.AddSystem<systems::RobotCommandSender>(plant_w_spr);
269270
auto osc = builder.AddSystem<systems::controllers::OperationalSpaceControl>(
270-
plant_w_spr, plant_w_spr, context_w_spr.get(), context_w_spr.get(), true);
271+
plant_w_spr, context_w_spr.get(), true);
271272
auto osc_debug_pub =
272273
builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_osc_output>(
273274
"OSC_DEBUG_JUMPING", &lcm, TriggerTypeSet({TriggerType::kForced})));
@@ -326,14 +327,29 @@ int DoMain(int argc, char* argv[]) {
326327
auto right_heel_evaluator = multibody::WorldPointEvaluator(
327328
plant_w_spr, right_heel.first, right_heel.second, Matrix3d::Identity(),
328329
Vector3d::Zero(), {0, 1, 2});
329-
vector<osc_jump::JUMPING_FSM_STATE> stance_modes = {
330+
vector<int> stance_modes = {
330331
osc_jump::BALANCE, osc_jump::CROUCH, osc_jump::LAND};
331-
for (auto mode : stance_modes) {
332-
osc->AddStateAndContactPoint(mode, &left_toe_evaluator);
333-
osc->AddStateAndContactPoint(mode, &left_heel_evaluator);
334-
osc->AddStateAndContactPoint(mode, &right_toe_evaluator);
335-
osc->AddStateAndContactPoint(mode, &right_heel_evaluator);
336-
}
332+
333+
osc->AddContactPoint(
334+
"left_toe",
335+
unique_ptr<multibody::WorldPointEvaluator<double>>(&left_toe_evaluator),
336+
stance_modes
337+
);
338+
osc->AddContactPoint(
339+
"left_heel",
340+
unique_ptr<multibody::WorldPointEvaluator<double>>(&left_heel_evaluator),
341+
stance_modes
342+
);
343+
osc->AddContactPoint(
344+
"right_toe",
345+
unique_ptr<multibody::WorldPointEvaluator<double>>(&right_toe_evaluator),
346+
stance_modes
347+
);
348+
osc->AddContactPoint(
349+
"right_heel",
350+
unique_ptr<multibody::WorldPointEvaluator<double>>(&right_heel_evaluator),
351+
stance_modes
352+
);
337353

338354
multibody::KinematicEvaluatorSet<double> evaluators(plant_w_spr);
339355

@@ -362,7 +378,8 @@ int DoMain(int argc, char* argv[]) {
362378
evaluators.add_evaluator(&left_loop);
363379
evaluators.add_evaluator(&right_loop);
364380

365-
osc->AddKinematicConstraint(&evaluators);
381+
osc->AddKinematicConstraint(
382+
std::unique_ptr<multibody::KinematicEvaluatorSet<double>>(&evaluators));
366383

367384
/**** Tracking Data for OSC *****/
368385
auto pelvis_tracking_data = std::make_unique<TransTaskSpaceTrackingData>(

‎examples/Cassie/run_osc_running_controller.cc‎

Lines changed: 24 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ using std::map;
4040
using std::pair;
4141
using std::string;
4242
using std::vector;
43+
using std::unique_ptr;
4344

4445
using Eigen::Matrix3d;
4546
using Eigen::MatrixXd;
@@ -173,7 +174,7 @@ int DoMain(int argc, char* argv[]) {
173174
FLAGS_channel_u, &lcm, TriggerTypeSet({TriggerType::kForced})));
174175
auto command_sender = builder.AddSystem<systems::RobotCommandSender>(plant);
175176
auto osc = builder.AddSystem<systems::controllers::OperationalSpaceControl>(
176-
plant, plant, plant_context.get(), plant_context.get(), true);
177+
plant, plant_context.get(), true);
177178
auto osc_debug_pub =
178179
builder.AddSystem(LcmPublisherSystem::Make<dairlib::lcmt_osc_output>(
179180
"OSC_DEBUG_RUNNING", &lcm, TriggerTypeSet({TriggerType::kForced})));
@@ -223,14 +224,26 @@ int DoMain(int argc, char* argv[]) {
223224
plant, right_heel.first, right_heel.second, *pelvis_view_frame,
224225
Matrix3d::Identity(), Vector3d::Zero(), {0, 1, 2});
225226

226-
osc->AddStateAndContactPoint(RunningFsmState::kLeftStance,
227-
&left_toe_evaluator);
228-
osc->AddStateAndContactPoint(RunningFsmState::kLeftStance,
229-
&left_heel_evaluator);
230-
osc->AddStateAndContactPoint(RunningFsmState::kRightStance,
231-
&right_toe_evaluator);
232-
osc->AddStateAndContactPoint(RunningFsmState::kRightStance,
233-
&right_heel_evaluator);
227+
osc->AddContactPoint(
228+
"left_toe",
229+
unique_ptr<multibody::WorldPointEvaluator<double>>(&left_toe_evaluator),
230+
{RunningFsmState::kLeftStance}
231+
);
232+
osc->AddContactPoint(
233+
"left_heel",
234+
unique_ptr<multibody::WorldPointEvaluator<double>>(&left_heel_evaluator),
235+
{RunningFsmState::kLeftStance}
236+
);
237+
osc->AddContactPoint(
238+
"right_toe",
239+
unique_ptr<multibody::WorldPointEvaluator<double>>(&right_toe_evaluator),
240+
{RunningFsmState::kRightStance}
241+
);
242+
osc->AddContactPoint(
243+
"right_heel",
244+
unique_ptr<multibody::WorldPointEvaluator<double>>(&right_heel_evaluator),
245+
{RunningFsmState::kRightStance}
246+
);
234247

235248
multibody::KinematicEvaluatorSet<double> evaluators(plant);
236249

@@ -259,7 +272,8 @@ int DoMain(int argc, char* argv[]) {
259272
evaluators.add_evaluator(&left_loop);
260273
evaluators.add_evaluator(&right_loop);
261274

262-
osc->AddKinematicConstraint(&evaluators);
275+
osc->AddKinematicConstraint(
276+
unique_ptr<const multibody::KinematicEvaluatorSet<double>>(&evaluators));
263277

264278
/**** Tracking Data *****/
265279

0 commit comments

Comments
 (0)