Skip to content

Commit 5e09704

Browse files
cuie23cuie23
committed
Working with xyz + revolute joints
1 parent 24c419a commit 5e09704

File tree

6 files changed

+604
-4
lines changed

6 files changed

+604
-4
lines changed

‎core/c3_options.h‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,8 @@ struct C3Options {
8383
std::optional<std::vector<double>> u_eta_n;
8484
std::optional<std::vector<double>> u_eta_t;
8585
std::optional<std::vector<double>> u_eta;
86+
//std::optional<std::vector<double>> x_init;
87+
8688

8789
template <typename Archive>
8890
void Serialize(Archive* a) {
@@ -103,6 +105,8 @@ struct C3Options {
103105
a->Visit(DRAKE_NVP(rho_scale));
104106
a->Visit(DRAKE_NVP(gamma));
105107

108+
//a->Visit(DRAKE_NVP(x_init));
109+
106110
a->Visit(DRAKE_NVP(w_Q));
107111
a->Visit(DRAKE_NVP(w_R));
108112
a->Visit(DRAKE_NVP(w_G));
@@ -161,12 +165,12 @@ struct C3Options {
161165
u_vector.insert(u_vector.end(), u_u.begin(), u_u.end());
162166
if (u_eta != std::nullopt || u_eta_slack != std::nullopt) {
163167
if (u_eta == std::nullopt || u_eta->empty()) {
164-
g_vector.insert(g_vector.end(), u_eta_slack->begin(),
168+
u_vector.insert(u_vector.end(), u_eta_slack->begin(),
165169
u_eta_slack->end());
166-
g_vector.insert(g_vector.end(), u_eta_n->begin(), u_eta_n->end());
167-
g_vector.insert(g_vector.end(), u_eta_t->begin(), u_eta_t->end());
170+
u_vector.insert(u_vector.end(), u_eta_n->begin(), u_eta_n->end());
171+
u_vector.insert(u_vector.end(), u_eta_t->begin(), u_eta_t->end());
168172
} else {
169-
g_vector.insert(g_vector.end(), u_eta->begin(), u_eta->end());
173+
u_vector.insert(u_vector.end(), u_eta->begin(), u_eta->end());
170174
}
171175
}
172176

@@ -179,6 +183,7 @@ struct C3Options {
179183
Eigen::VectorXd u = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
180184
this->u_vector.data(), this->u_vector.size());
181185

186+
//std::cout << "g size: " << g.size() << " u size: " << u.size() << std::endl;
182187
DRAKE_DEMAND(g.size() == u.size());
183188

184189
Q = w_Q * q.asDiagonal();

‎examples/lcs_factory_system_example.cc‎

Lines changed: 222 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,8 @@ using c3::systems::LCSFactorySystem;
4242
using c3::systems::LCSSimulator;
4343

4444
using drake::SortedPair;
45+
using drake::math::RigidTransformd;
46+
using drake::math::RotationMatrixd;
4547
using drake::geometry::GeometryId;
4648
using drake::geometry::SceneGraph;
4749
using drake::multibody::AddMultibodyPlantSceneGraph;
@@ -435,6 +437,223 @@ int RunPivotingTest() {
435437
return 0;
436438
}
437439

440+
int RunPlateTest() {
441+
// Build the plant and scene graph for the pivoting system.
442+
DiagramBuilder<double> plant_builder;
443+
auto [plant_for_lcs, scene_graph_for_lcs] =
444+
AddMultibodyPlantSceneGraph(&plant_builder, 0.0);
445+
Parser parser_for_lcs(&plant_for_lcs, &scene_graph_for_lcs);
446+
447+
const std::string plate_file_lcs = "examples/resources/plate/plate.sdf";
448+
const std::string cube_file_lcs = "examples/resources/plate/cube.sdf";
449+
450+
parser_for_lcs.AddModels(plate_file_lcs);
451+
parser_for_lcs.AddModels(cube_file_lcs);
452+
453+
plant_for_lcs.Finalize();
454+
455+
// Build the plant diagram.
456+
auto plant_diagram = plant_builder.Build();
457+
458+
// Retrieve collision geometries for relevant bodies.
459+
drake::geometry::GeometryId plate_collision_geom =
460+
plant_for_lcs.GetCollisionGeometriesForBody(
461+
plant_for_lcs.GetBodyByName("plate"))[0];
462+
std::vector<drake::geometry::GeometryId> cube_collision_geoms;
463+
for (int i = 1; i <= 8; i++) {
464+
cube_collision_geoms.push_back(
465+
plant_for_lcs.GetCollisionGeometriesForBody(
466+
plant_for_lcs.GetBodyByName("cube"))[i]);
467+
}
468+
469+
// Define contact pairs for the LCS system.
470+
std::vector<SortedPair<GeometryId>> contact_pairs;
471+
472+
for (GeometryId geom_id : cube_collision_geoms) {
473+
contact_pairs.emplace_back(plate_collision_geom, geom_id);
474+
}
475+
476+
// Build the main diagram.
477+
DiagramBuilder<double> builder;
478+
auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0.01);
479+
Parser parser(&plant, &scene_graph);
480+
const std::string plate_file = "examples/resources/plate/plate.sdf";
481+
const std::string cube_file = "examples/resources/plate/cube.sdf";
482+
483+
parser.AddModels(plate_file);
484+
parser.AddModels(cube_file);
485+
486+
plant.Finalize();
487+
488+
// Load controller options and cost matrices.
489+
C3ControllerOptions options = drake::yaml::LoadYamlFile<C3ControllerOptions>(
490+
"examples/resources/plate/c3_controller_plate_options.yaml");
491+
C3::CostMatrices cost = C3::CreateCostMatricesFromC3Options(
492+
options.c3_options, options.lcs_factory_options.N);
493+
494+
// Create contexts for the plant and LCS factory system.
495+
std::unique_ptr<drake::systems::Context<double>> plant_diagram_context =
496+
plant_diagram->CreateDefaultContext();
497+
auto plant_autodiff =
498+
drake::systems::System<double>::ToAutoDiffXd(plant_for_lcs);
499+
auto& plant_for_lcs_context = plant_diagram->GetMutableSubsystemContext(
500+
plant_for_lcs, plant_diagram_context.get());
501+
auto plant_context_autodiff = plant_autodiff->CreateDefaultContext();
502+
503+
// Add the LCS factory system.
504+
auto lcs_factory_system = builder.AddSystem<LCSFactorySystem>(
505+
plant_for_lcs, plant_for_lcs_context, *plant_autodiff,
506+
*plant_context_autodiff, contact_pairs, options.lcs_factory_options);
507+
508+
std::cout << "Before add C3 controller" << std::endl;
509+
510+
// Add the C3 controller.
511+
auto c3_controller =
512+
builder.AddSystem<C3Controller>(plant_for_lcs, cost, options);
513+
c3_controller->set_name("c3_controller");
514+
515+
std::cout << "After add C3 controller" << std::endl;
516+
517+
// Add linear constraints to the controller.
518+
// Eigen::MatrixXd A = Eigen::MatrixXd::Zero(26, 26);
519+
// A(4, 4) = 1;
520+
// A(5, 5) = 1;
521+
// A(6, 6) = 1;
522+
// A(11, 11) = 1;
523+
// A(12, 12) = 1;
524+
// A(13, 13) = 1;
525+
// Eigen::VectorXd lower_bound(26);
526+
// Eigen::VectorXd upper_bound(26);
527+
// lower_bound << 0, 0, 0, 0, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
528+
// upper_bound << 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
529+
// c3_controller->AddLinearConstraint(A, lower_bound, upper_bound,
530+
// ConstraintVariable::STATE);
531+
532+
// Add a constant vector source for the desired state = ee rot, ee pos, cube rot, cube pos
533+
Eigen::VectorXd xd(23);
534+
//xd << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
535+
xd << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
536+
537+
std::vector<double> x_des = *options.x_des;
538+
xd = Eigen::Map<Eigen::VectorXd>(x_des.data(), x_des.size());
539+
540+
std::cout << "xd: " << xd.transpose() << std::endl;
541+
542+
auto xdes =
543+
builder.AddSystem<drake::systems::ConstantVectorSource<double>>(xd);
544+
545+
// Add a vector-to-timestamped-vector converter.
546+
auto vector_to_timestamped_vector =
547+
builder.AddSystem<Vector2TimestampedVector>(23);
548+
builder.Connect(plant.get_state_output_port(),
549+
vector_to_timestamped_vector->get_input_port_state());
550+
551+
// Connect controller inputs.
552+
builder.Connect(
553+
vector_to_timestamped_vector->get_output_port_timestamped_state(),
554+
c3_controller->get_input_port_lcs_state());
555+
builder.Connect(lcs_factory_system->get_output_port_lcs(),
556+
c3_controller->get_input_port_lcs());
557+
builder.Connect(xdes->get_output_port(),
558+
c3_controller->get_input_port_target());
559+
560+
// Add and connect the C3 solution input system.
561+
auto c3_input = builder.AddSystem<C3Solution2Input>(5);
562+
builder.Connect(c3_controller->get_output_port_c3_solution(),
563+
c3_input->get_input_port_c3_solution());
564+
builder.Connect(c3_input->get_output_port_c3_input(),
565+
plant.get_actuation_input_port());
566+
567+
// Add a ZeroOrderHold system for state updates.
568+
auto input_zero_order_hold =
569+
builder.AddSystem<drake::systems::ZeroOrderHold<double>>(
570+
1 / options.publish_frequency, 5);
571+
builder.Connect(c3_input->get_output_port_c3_input(),
572+
input_zero_order_hold->get_input_port());
573+
builder.Connect(
574+
vector_to_timestamped_vector->get_output_port_timestamped_state(),
575+
lcs_factory_system->get_input_port_lcs_state());
576+
builder.Connect(input_zero_order_hold->get_output_port(),
577+
lcs_factory_system->get_input_port_lcs_input());
578+
579+
580+
581+
Eigen::Vector4d q_vec = xd.segment(5, 4);
582+
Eigen::Quaterniond q(q_vec(0), q_vec(1), q_vec(2), q_vec(3));
583+
q.normalize();
584+
RotationMatrixd R_target(q);
585+
RigidTransformd X_WF(R_target, xd.segment(9, 3));
586+
587+
// Set up Meshcat visualizer.
588+
auto meshcat = std::make_shared<drake::geometry::Meshcat>();
589+
drake::geometry::MeshcatVisualizerParams params;
590+
drake::geometry::MeshcatVisualizer<double>::AddToBuilder(
591+
&builder, scene_graph, meshcat, std::move(params));
592+
drake::multibody::meshcat::ContactVisualizer<double>::AddToBuilder(
593+
&builder, plant, meshcat,
594+
drake::multibody::meshcat::ContactVisualizerParams());
595+
596+
const double axis_len = 0.2;
597+
const double radius = 0.01;
598+
599+
meshcat->SetObject("target_pose/x_axis", drake::geometry::Cylinder(radius, axis_len), drake::geometry::Rgba(1, 0, 0, 1));
600+
RigidTransformd X_FX(
601+
RotationMatrixd::MakeYRotation(-M_PI / 2.0),
602+
Eigen::Vector3d(axis_len / 2.0, 0, 0));
603+
meshcat->SetTransform("target_pose/x_axis", X_WF * X_FX);
604+
605+
meshcat->SetObject("target_pose/y_axis", drake::geometry::Cylinder(radius, axis_len), drake::geometry::Rgba(0, 1, 0, 1));
606+
RigidTransformd X_FY(
607+
RotationMatrixd::MakeXRotation(M_PI / 2.0),
608+
Eigen::Vector3d(0, axis_len / 2.0, 0));
609+
meshcat->SetTransform("target_pose/y_axis", X_WF * X_FY);
610+
611+
meshcat->SetObject("target_pose/z_axis", drake::geometry::Cylinder(radius, axis_len), drake::geometry::Rgba(0, 0, 1, 1));
612+
RigidTransformd X_FZ(
613+
RotationMatrixd::Identity(),
614+
Eigen::Vector3d(0, 0, axis_len / 2.0));
615+
meshcat->SetTransform("target_pose/z_axis", X_WF * X_FZ);
616+
617+
618+
// Build the diagram.
619+
auto diagram = builder.Build();
620+
621+
if (!FLAGS_diagram_path.empty())
622+
c3::systems::common::DrawAndSaveDiagramGraph(*diagram, FLAGS_diagram_path);
623+
624+
// Create a default context for the diagram.
625+
auto diagram_context = diagram->CreateDefaultContext();
626+
627+
// Set the initial state of the system.
628+
Eigen::VectorXd x0(23);
629+
x0 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
630+
631+
// x0 << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
632+
std::vector<double> x_init = *options.x_init;
633+
x0 = Eigen::Map<Eigen::VectorXd>(x_init.data(), x_init.size());
634+
std::cout << "x0: " << x0.transpose() << std::endl;
635+
636+
// for (const auto& pname : plant.GetPositionNames()) {
637+
// std::cout << pname << std::endl;
638+
// }
639+
// for (const auto& vname : plant.GetVelocityNames()) {
640+
// std::cout << vname << std::endl;
641+
// }
642+
643+
auto& plant_context =
644+
diagram->GetMutableSubsystemContext(plant, diagram_context.get());
645+
plant.SetPositionsAndVelocities(&plant_context, x0);
646+
647+
// Create and configure the simulator.
648+
drake::systems::Simulator<double> simulator(*diagram,
649+
std::move(diagram_context));
650+
simulator.set_target_realtime_rate(1); // Run simulation at real-time speed.
651+
simulator.Initialize();
652+
simulator.AdvanceTo(120.0); // Run simulation for 10 seconds.
653+
654+
return 0;
655+
}
656+
438657
int main(int argc, char* argv[]) {
439658
gflags::ParseCommandLineFlags(&argc, &argv, true);
440659
if (FLAGS_experiment_type == "cartpole_softwalls") {
@@ -443,6 +662,9 @@ int main(int argc, char* argv[]) {
443662
} else if (FLAGS_experiment_type == "cube_pivoting") {
444663
std::cout << "Running Cube Pivoting Test..." << std::endl;
445664
return RunPivotingTest();
665+
} else if (FLAGS_experiment_type == "plate") {
666+
std::cout << "Running Plate Test..." << std::endl;
667+
return RunPlateTest();
446668
} else {
447669
std::cerr
448670
<< "Unknown experiment type: " << FLAGS_experiment_type

‎examples/resources/plate/c3_controller_plate_options.yaml‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
projection_type: "C3+"
2+
3+
solve_time_filter_alpha: 0.0
4+
#set to 0 to publish as fast as possible
5+
publish_frequency: 100
6+
state_prediction_joints: []
7+
8+
x_init: [0, 0, -1.1,
9+
0, 0,
10+
1, 0, 0, 0,
11+
0, 0, -1,
12+
0, 0, 0,
13+
0, 0,
14+
0, 0, 0,
15+
0, 0, 0]
16+
17+
x_des: [0, 0, 0,
18+
0, 0,
19+
0.707, 0, 0.707, 0,
20+
0.2, 0, 0.5,
21+
0, 0, 0,
22+
0, 0,
23+
0, 0, 0,
24+
0, 0, 0]
25+
26+
lcs_factory_options:
27+
#options are 'stewart_and_trinkle' or 'anitescu'
28+
# contact_model : 'stewart_and_trinkle'
29+
contact_model: "anitescu"
30+
num_friction_directions: 2
31+
num_contacts: 8
32+
spring_stiffness: 0
33+
mu: [0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4]
34+
N: 10
35+
dt: 0.02
36+
37+
c3_options:
38+
warm_start: false
39+
end_on_qp_step: true
40+
scale_lcs: false
41+
42+
num_threads: 10
43+
delta_option: 1
44+
45+
M: 1000
46+
47+
admm_iter: 3
48+
49+
gamma: 1.0
50+
rho_scale: 1.1 #matrix scaling
51+
w_Q: 5
52+
w_R: 1 #Penalty on all decision variables, assuming scalar
53+
w_G: 0.2 #Penalty on all decision variables, assuming scalar
54+
w_U: 1 #State Tracking Error, assuming diagonal
55+
56+
57+
q_vector: [1, 1, 1, # plate pos
58+
1, 1, # plate rot
59+
5000, 5000, 5000, 5000, # cube rot
60+
3000, 3000, 3000, # cube pos
61+
50, 50, 50, # plate velo
62+
50, 50, # plate ang velo
63+
0.1, 0.1, 0.1, # cube ang velo
64+
10, 10, 10] # cube velo
65+
66+
r_vector: [0.01, 0.01, 0.01, 0.01, 0.01]
67+
#r_vector: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
68+
69+
#Penalty on matching projected variables
70+
g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
71+
g_gamma: [1, 1, 1, 1, 1, 1, 1, 1]
72+
g_lambda_n: [1, 1, 1, 1, 1, 1, 1, 1]
73+
g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
74+
g_lambda: [2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]
75+
g_u: [0, 0, 0, 0, 0]
76+
77+
g_eta_slack: [1, 1, 1, 1, 1, 1, 1, 1]
78+
g_eta_n: [1, 1, 1, 1, 1, 1, 1, 1]
79+
g_eta_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
80+
g_eta: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
81+
82+
83+
#Penalty on matching the QP variables
84+
u_x: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100]
85+
u_gamma: [1, 1, 1, 1, 1, 1, 1, 1]
86+
u_lambda_n: [1, 1, 1, 1, 1, 1, 1, 1]
87+
u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
88+
u_lambda: [2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2]
89+
u_u: [0, 0, 0, 0, 0]
90+
91+
u_eta_slack: [1, 1, 1, 1, 1, 1, 1, 1]
92+
u_eta_n: [1, 1, 1, 1, 1, 1, 1, 1]
93+
u_eta_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
94+
u_eta: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]

0 commit comments

Comments
 (0)