From 4ca00222c9f7a2938633b87632808b8a089d4252 Mon Sep 17 00:00:00 2001 From: Michael Posa Date: Sun, 29 Jun 2025 23:59:05 -0400 Subject: [PATCH 1/9] Fixes to bazel build files --- examples/sampling_c3/BUILD.bazel | 2 +- examples/sampling_c3/jacktoy/BUILD.bazel | 13 +------------ 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/examples/sampling_c3/BUILD.bazel b/examples/sampling_c3/BUILD.bazel index aa2f0f5a3..af48fccb4 100644 --- a/examples/sampling_c3/BUILD.bazel +++ b/examples/sampling_c3/BUILD.bazel @@ -9,8 +9,8 @@ cc_library( filegroup( name = "all_example_params_yamls", - srcs = glob(["*/parameters/*.yaml"]), visibility = ["//visibility:public"], + data = ["//examples/sampling_c3/jacktoy:parameters"], ) filegroup( diff --git a/examples/sampling_c3/jacktoy/BUILD.bazel b/examples/sampling_c3/jacktoy/BUILD.bazel index c27b0f44c..aea0f689a 100644 --- a/examples/sampling_c3/jacktoy/BUILD.bazel +++ b/examples/sampling_c3/jacktoy/BUILD.bazel @@ -3,24 +3,13 @@ package(default_visibility = ["//visibility:public"]) -cc_library( - name = "urdfs", - data = glob([ - "urdf/**", - ]), -) - cc_library( name = "parameters", data = glob([ - "*yaml", + "parameters/*.yaml", ]), ) -cc_library( - name = "franka_hardware", - deps = [], -) cc_library( name = "lcm_channels_jacktoy", From 565464d67526413f9b35da532d464153362bd932 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 2 Jul 2025 14:27:10 -0400 Subject: [PATCH 2/9] Push t example; working in sim --- .../franka_sampling_c3_controller.cc | 46 +++++ examples/sampling_c3/generate_samples.cc | 50 ++---- examples/sampling_c3/generate_samples.h | 1 - examples/sampling_c3/goal_generator.cc | 8 +- examples/sampling_c3/goal_generator.h | 17 ++ ..._hardware.pmd => franka_hardware_jack.pmd} | 0 ...ka_sim_jacktoy.pmd => franka_sim_jack.pmd} | 0 .../sampling_c3_controller_params.yaml | 2 +- .../jacktoy/parameters/sampling_params.yaml | 2 +- .../jacktoy/parameters/sim_params.yaml | 1 - examples/sampling_c3/push_t/BUILD.bazel | 88 ++++++++++ .../sampling_c3/push_t/franka_hardware_t.pmd | 103 +++++++++++ examples/sampling_c3/push_t/franka_sim_t.pmd | 71 ++++++++ .../push_t/parameters/goal_params.yaml | 36 ++++ .../push_t/parameters/progress_params.yaml | 64 +++++++ .../push_t/parameters/reposition_params.yaml | 32 ++++ .../sampling_c3_controller_params.yaml | 23 +++ .../parameters/sampling_c3_options.yaml | 163 ++++++++++++++++++ .../push_t/parameters/sampling_params.yaml | 44 +++++ .../push_t/parameters/sim_params.yaml | 14 ++ .../push_t/parameters/vis_params.yaml | 37 ++++ .../{jack_ground.sdf => jack_control.sdf} | 0 examples/sampling_c3/urdf/push_t.sdf | 93 ++++++++++ examples/sampling_c3/urdf/push_t_control.sdf | 117 +++++++++++++ 24 files changed, 973 insertions(+), 39 deletions(-) rename examples/sampling_c3/jacktoy/{franka_hardware.pmd => franka_hardware_jack.pmd} (100%) rename examples/sampling_c3/jacktoy/{franka_sim_jacktoy.pmd => franka_sim_jack.pmd} (100%) create mode 100644 examples/sampling_c3/push_t/BUILD.bazel create mode 100644 examples/sampling_c3/push_t/franka_hardware_t.pmd create mode 100644 examples/sampling_c3/push_t/franka_sim_t.pmd create mode 100644 examples/sampling_c3/push_t/parameters/goal_params.yaml create mode 100644 examples/sampling_c3/push_t/parameters/progress_params.yaml create mode 100644 examples/sampling_c3/push_t/parameters/reposition_params.yaml create mode 100644 examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml create mode 100644 examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml create mode 100644 examples/sampling_c3/push_t/parameters/sampling_params.yaml create mode 100644 examples/sampling_c3/push_t/parameters/sim_params.yaml create mode 100644 examples/sampling_c3/push_t/parameters/vis_params.yaml rename examples/sampling_c3/urdf/{jack_ground.sdf => jack_control.sdf} (100%) create mode 100644 examples/sampling_c3/urdf/push_t.sdf create mode 100644 examples/sampling_c3/urdf/push_t_control.sdf diff --git a/examples/sampling_c3/franka_sampling_c3_controller.cc b/examples/sampling_c3/franka_sampling_c3_controller.cc index 7d8f6ddc3..0226e390e 100644 --- a/examples/sampling_c3/franka_sampling_c3_controller.cc +++ b/examples/sampling_c3/franka_sampling_c3_controller.cc @@ -184,6 +184,48 @@ int DoMain(int argc, char* argv[]) { contact_geoms["CAPSULE_3_SPHERE_2"], contact_geoms["GROUND"])); contact_pairs.push_back(ground_object_contact_pairs); } + else if (FLAGS_demo_name == "push_t") { + drake::geometry::GeometryId vertical_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[0]; + drake::geometry::GeometryId horizontal_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("horizontal_link"))[0]; + + drake::geometry::GeometryId top_left_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[1]; + drake::geometry::GeometryId top_right_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[2]; + drake::geometry::GeometryId bottom_sphere_geoms = + plant_lcs.GetCollisionGeometriesForBody( + plant_lcs.GetBodyByName("vertical_link"))[3]; + + contact_geoms["VERTICAL_LINK"] = vertical_geoms; + contact_geoms["HORIZONTAL_LINK"] = horizontal_geoms; + contact_geoms["TOP_LEFT_SPHERE"] = top_left_sphere_geoms; + contact_geoms["TOP_RIGHT_SPHERE"] = top_right_sphere_geoms; + contact_geoms["BOTTOM_SPHERE"] = bottom_sphere_geoms; + + // EE-object contact pairs second. + std::vector> ee_contact_pairs; + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["HORIZONTAL_LINK"])); + ee_contact_pairs.push_back( + SortedPair(contact_geoms["EE"], contact_geoms["VERTICAL_LINK"])); + contact_pairs.push_back(ee_contact_pairs); + + // Object-ground contact pairs third. + std::vector> ground_object_contact_pairs; + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["TOP_LEFT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["TOP_RIGHT_SPHERE"], contact_geoms["GROUND"])); + ground_object_contact_pairs.push_back(SortedPair( + contact_geoms["BOTTOM_SPHERE"], contact_geoms["GROUND"])); + contact_pairs.push_back(ground_object_contact_pairs); + } else { throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); } @@ -214,6 +256,10 @@ int DoMain(int argc, char* argv[]) { target_generator = std::make_unique( plant_object, controller_params.goal_params); + } else if (FLAGS_demo_name == "push_t") { + target_generator = + std::make_unique( + plant_object, controller_params.goal_params); } else { throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); } diff --git a/examples/sampling_c3/generate_samples.cc b/examples/sampling_c3/generate_samples.cc index aef817362..104bbb09f 100644 --- a/examples/sampling_c3/generate_samples.cc +++ b/examples/sampling_c3/generate_samples.cc @@ -238,7 +238,6 @@ Eigen::Vector3d PerimeterSampling( double z_sample = 0; // Convert to world frame using the current object state. - Eigen::VectorXd x_lcs_world = x_lcs; Eigen::Quaterniond quat_object(x_lcs(3), x_lcs(4), x_lcs(5), x_lcs(6)); Eigen::Vector3d object_position = x_lcs.segment(7, 3); candidate_state = x_lcs; @@ -249,19 +248,19 @@ Eigen::Vector3d PerimeterSampling( // Project samples to specified sampling height in world frame. candidate_state[2] = sampling_params.sampling_height; } while (!IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, 0.0, false, candidate_state, plant, context, plant_ad, - context_ad, contact_geoms, sampling_c3_options, min_distance_index)); + n_q, n_v, n_u, 0.0, candidate_state, plant, context, plant_ad, context_ad, + contact_geoms, sampling_c3_options, min_distance_index)); // Project the sample past the surface of the object with clearance. Eigen::VectorXd projected_state = ProjectSampleOutsideObject( - candidate_state, min_distance_index, sampling_params, plant, *context, - contact_geoms); + candidate_state, min_distance_index, sampling_params, plant, *context, + contact_geoms); // Check the desired clearance is satisfied; otherwise try again. UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, projected_state); if (IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, sampling_params.sample_projection_clearance, true, + n_q, n_v, n_u, sampling_params.sample_projection_clearance, projected_state, plant, context, plant_ad, context_ad, contact_geoms, sampling_c3_options, min_distance_index)) { continue; @@ -333,8 +332,8 @@ Eigen::Vector3d ShellSampling( y_samplec + sampling_radius * sin(theta) * sin(elevation_theta); candidate_state[2] = z_samplec + sampling_radius * cos(elevation_theta); } while (!IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, 0.0, false, candidate_state, plant, context, plant_ad, - context_ad, contact_geoms, sampling_c3_options, min_distance_index)); + n_q, n_v, n_u, 0.0, candidate_state, plant, context, plant_ad, context_ad, + contact_geoms, sampling_c3_options, min_distance_index)); // Project the sample past the surface of the object with clearance. Eigen::VectorXd projected_state = ProjectSampleOutsideObject( @@ -345,7 +344,7 @@ Eigen::Vector3d ShellSampling( UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad, projected_state); if (IsSampleWithinDistanceOfSurface( - n_q, n_v, n_u, sampling_params.sample_projection_clearance, true, + n_q, n_v, n_u, sampling_params.sample_projection_clearance, projected_state, plant, context, plant_ad, context_ad, contact_geoms, sampling_c3_options, min_distance_index)) { continue; @@ -390,7 +389,8 @@ double GetEERadiusFromPlant( query_port.template Eval>(context); const auto& inspector = query_object.inspector(); - // Locate the EE and obtain its radius. + // Locate the EE and obtain its radius. The first set of contact geoms has + // the EE and ground. GeometryId ee_geom_id = contact_geoms.at(0).at(0).first(); const drake::geometry::Shape& shape = inspector.GetShape(ee_geom_id); const auto* sphere = dynamic_cast(&shape); @@ -403,15 +403,14 @@ double GetEERadiusFromPlant( bool IsSampleWithinDistanceOfSurface( const int& n_q, const int& n_v, const int& n_u, const double& clearance_distance, - const bool& factor_in_ee_radius, const Eigen::VectorXd& candidate_state, drake::multibody::MultibodyPlant& plant, drake::systems::Context* context, drake::multibody::MultibodyPlant& plant_ad, drake::systems::Context* context_ad, const std::vector< - std::vector>>& - contact_geoms, + std::vector>>& + contact_geoms, SamplingC3Options sampling_c3_options, int& min_distance_index) { @@ -421,14 +420,12 @@ bool IsSampleWithinDistanceOfSurface( // Find the closest pair if there are multiple pairs std::vector distances; - - for (int i = 0; i < contact_geoms.at(0).size(); i++) { - // Evaluate the distance for each pair - SortedPair pair{(contact_geoms.at(0)).at(i)}; + for (int i = 0; i < contact_geoms.at(1).size(); i++) { + SortedPair pair{(contact_geoms.at(1)).at(i)}; multibody::GeomGeomCollider collider(plant, pair); auto [phi_i, J_i] = collider.EvalPolytope( - *context, sampling_c3_options.num_friction_directions); + *context, sampling_c3_options.num_friction_directions); distances.push_back(phi_i); } @@ -439,15 +436,8 @@ bool IsSampleWithinDistanceOfSurface( min_distance_index = std::distance(distances.begin(), min_distance_it); double min_distance = *min_distance_it; - // Factor the EE radius into the clearance distance if requested. - double ee_radius_contribution = 0.0; - if (factor_in_ee_radius) { - ee_radius_contribution = GetEERadiusFromPlant( - plant, *context, contact_geoms); - } - // Require that min_distance be at least 1 mm within the clearance distance. - return min_distance <= clearance_distance + ee_radius_contribution - 1e-3; + return min_distance <= clearance_distance - 1e-3; } Eigen::VectorXd ProjectSampleOutsideObject( @@ -462,20 +452,18 @@ Eigen::VectorXd ProjectSampleOutsideObject( // Compute the witness points between the penetrating sample and the object // surface. multibody::GeomGeomCollider collider( - plant, contact_geoms.at(0).at(min_distance_index)); + plant, contact_geoms.at(1).at(min_distance_index)); auto [p_world_contact_a, p_world_contact_b] = collider.CalcWitnessPoints( context); - // Get the EE radius to factor into the projection. - double ee_radius = GetEERadiusFromPlant(plant, context, contact_geoms); - // Find vector in direction from sample to contact point on object. Eigen::Vector3d a_to_b = p_world_contact_b - p_world_contact_a; + double penetration_depth = a_to_b.norm(); Eigen::Vector3d a_to_b_normalized = a_to_b.normalized(); // Add clearance to point b in the same direction. Eigen::Vector3d p_world_contact_b_clearance = p_world_contact_b + - (ee_radius + sampling_params.sample_projection_clearance) * + (penetration_depth + sampling_params.sample_projection_clearance) * a_to_b_normalized; candidate_state.head(3) = p_world_contact_b_clearance; return candidate_state; diff --git a/examples/sampling_c3/generate_samples.h b/examples/sampling_c3/generate_samples.h index 75c2015ff..fae008631 100644 --- a/examples/sampling_c3/generate_samples.h +++ b/examples/sampling_c3/generate_samples.h @@ -129,7 +129,6 @@ bool IsSampleWithinDistanceOfSurface( const int& n_v, const int& n_u, const double& clearance_distance, - const bool& factor_in_ee_radius, const Eigen::VectorXd& candidate_state, drake::multibody::MultibodyPlant& plant, drake::systems::Context* context, diff --git a/examples/sampling_c3/goal_generator.cc b/examples/sampling_c3/goal_generator.cc index 3bd1b5129..3a1ac790a 100644 --- a/examples/sampling_c3/goal_generator.cc +++ b/examples/sampling_c3/goal_generator.cc @@ -164,10 +164,10 @@ void SamplingC3GoalGenerator::SetRandomizedTargetFinalObjectPosition() const { double x, y = 0; while ((sqrt(x * x + y * y) > goal_params_.random_goal_radius_limits[1]) || (sqrt(x * x + y * y) < goal_params_.random_goal_radius_limits[0])) { - double x = RandomUniform(goal_params_.random_goal_x_limits[0], - goal_params_.random_goal_x_limits[1]); - double y = RandomUniform(goal_params_.random_goal_y_limits[0], - goal_params_.random_goal_y_limits[1]); + x = RandomUniform(goal_params_.random_goal_x_limits[0], + goal_params_.random_goal_x_limits[1]); + y = RandomUniform(goal_params_.random_goal_y_limits[0], + goal_params_.random_goal_y_limits[1]); } target_final_object_position_ << x, y, goal_params_.resting_object_height; diff --git a/examples/sampling_c3/goal_generator.h b/examples/sampling_c3/goal_generator.h index ab14511a5..e70f0f4d9 100644 --- a/examples/sampling_c3/goal_generator.h +++ b/examples/sampling_c3/goal_generator.h @@ -38,6 +38,11 @@ inline const std::vector kNominalOrientationsJack{ kQuatAllUp, kQuatRedDown, kQuatBlueUp, kQuatAllDown, kQuatGreenUp, kQuatBlueDown, kQuatRedUp, kQuatGreenDown}; +// Define the nominal orientations for any planar demo. +inline const Eigen::Quaterniond kQUAT_FLAT{1.0, 0.0, 0.0, 0.0}; +inline const std::vector kNominalOrientationsPlanar{ + kQUAT_FLAT}; + namespace dairlib { namespace systems { @@ -137,5 +142,17 @@ class SamplingC3GoalGeneratorJacktoy : public SamplingC3GoalGenerator { object_plant, goal_params, kNominalOrientationsJack) {} }; + +// class TargetGeneratorPushT : public TargetGenerator { +class SamplingC3GoalGeneratorPushT : public SamplingC3GoalGenerator { + public: + SamplingC3GoalGeneratorPushT( + const drake::multibody::MultibodyPlant& object_plant, + const SamplingC3GoalParams& goal_params) : + SamplingC3GoalGenerator( + object_plant, goal_params, kNominalOrientationsPlanar) {} +}; + + } // namespace systems } // namespace dairlib diff --git a/examples/sampling_c3/jacktoy/franka_hardware.pmd b/examples/sampling_c3/jacktoy/franka_hardware_jack.pmd similarity index 100% rename from examples/sampling_c3/jacktoy/franka_hardware.pmd rename to examples/sampling_c3/jacktoy/franka_hardware_jack.pmd diff --git a/examples/sampling_c3/jacktoy/franka_sim_jacktoy.pmd b/examples/sampling_c3/jacktoy/franka_sim_jack.pmd similarity index 100% rename from examples/sampling_c3/jacktoy/franka_sim_jacktoy.pmd rename to examples/sampling_c3/jacktoy/franka_sim_jack.pmd diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml index 38ad1b11d..da0a74987 100644 --- a/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml +++ b/examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml @@ -15,7 +15,7 @@ lcm_channels_simulation_file: examples/sampling_c3/shared_parameters/lcm_channel # Note: Use capsule_1 body name as representative of the pose of the entire # jack model. This works because capsule_1's origin is the same as the jack's. -object_model: examples/sampling_c3/urdf/jack_ground.sdf +object_model: examples/sampling_c3/urdf/jack_control.sdf object_body_name: capsule_1 include_end_effector_orientation: false diff --git a/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml b/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml index 3e84da937..4c0c606a5 100644 --- a/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml +++ b/examples/sampling_c3/jacktoy/parameters/sampling_params.yaml @@ -19,7 +19,7 @@ num_additional_samples_repos: 1 num_additional_samples_c3: 2 # Sample buffer parameters. -consider_best_buffer_sample_when_leaving_c3: false +consider_best_buffer_sample_when_leaving_c3: true N_sample_buffer: 100 pos_error_sample_retention: 0.005 ang_error_sample_retention: 0.087 # 0.087 rad = 5 deg diff --git a/examples/sampling_c3/jacktoy/parameters/sim_params.yaml b/examples/sampling_c3/jacktoy/parameters/sim_params.yaml index cac1c35d9..2f3831551 100644 --- a/examples/sampling_c3/jacktoy/parameters/sim_params.yaml +++ b/examples/sampling_c3/jacktoy/parameters/sim_params.yaml @@ -10,6 +10,5 @@ publish_efforts: true dt: 0.0001 realtime_rate: 1 -# Set the initial locations to something more vertical: q_init_franka: [2.191, 1.1, -1.33, -2.22, 1.30, 2.02, 0.08] q_init_object: [-0.3347435, -0.026718954, 0.8878204, 0.31518626, 0.4, 0.30, 0.03] diff --git a/examples/sampling_c3/push_t/BUILD.bazel b/examples/sampling_c3/push_t/BUILD.bazel new file mode 100644 index 000000000..04584da75 --- /dev/null +++ b/examples/sampling_c3/push_t/BUILD.bazel @@ -0,0 +1,88 @@ +# -*- mode: python -*- +# vi: set ft=python : + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "urdfs", + data = glob([ + "urdf/**", + ]), +) + +cc_library( + name = "parameters", + data = glob([ + "*yaml", + ]), +) + +cc_library( + name = "franka_hardware", + deps = [], +) + +cc_library( + name = "lcm_channels_push_t", + hdrs = [], + data = [ + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:lcm_channels", + "//systems/framework:vector", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_sim_params_push_t", + hdrs = [], + data = [ + "parameters/sim_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:franka_sim_params", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "sampling_c3_controller_params_push_t", + hdrs = [], + data = [ + "parameters/sampling_c3_controller_params.yaml", + "parameters/sampling_params.yaml", + "//examples/sampling_c3:shared_params_yamls", + ], + deps = [ + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "goal_params_push_t", + hdrs = [], + data = [ + "parameters/goal_params.yaml", + ], + deps = [ + "//examples/sampling_c3/parameter_headers:goal_params", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) + +cc_library( + name = "franka_drake_lcm_driver_channels_push_t", + hdrs = [], + data = [ + "//common/parameters:franka_drake_lcm_driver_channels", + ], + deps = [ + "//common/parameters:franka_drake_lcm_driver_channels", + "//solvers:c3", + "@drake//:drake_shared_library", + ], +) diff --git a/examples/sampling_c3/push_t/franka_hardware_t.pmd b/examples/sampling_c3/push_t/franka_hardware_t.pmd new file mode 100644 index 000000000..9c953335d --- /dev/null +++ b/examples/sampling_c3/push_t/franka_hardware_t.pmd @@ -0,0 +1,103 @@ +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=false --demo_name=push_t"; + host = "sampling_c3_localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "sampling_c3_localhost"; + } + cmd "logger" { + exec = "python3 examples/sampling_c3/start_logging.py hw push_t"; + host = "sampling_c3_localhost"; + } + cmd "record_video" { + exec = "python3 record_video.py"; + host = "franka_control"; + } +} + +group "controllers (hardware)" { + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=false --demo_name=push_t; + host = "sampling_c3_localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=false --demo_name=push_t --lcm_url=udpm://239.255.76.67:7667?ttl=1; + host = "franka_control"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "sampling_c3_localhost"; + } +} + +group "drivers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=false --demo_name=push_t"; + host = "franka_control"; + } + cmd "franka_driver_out" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_out --demo_name=push_t"; + host = "franka_control"; + } + cmd "franka_driver_in" { + exec = "bazel-bin/examples/sampling_c3/franka_bridge_driver_in --demo_name=push_t"; + host = "franka_control"; + } + cmd "torque_driver" { + exec = "bazel-bin/franka-driver/franka_driver_v4 --robot_ip_address=172.16.0.2 --control_mode=torque"; + host = "drake_franka_driver"; + } +} + + +script "init_experiment" { + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 10000; + stop group "drivers"; +} + +script "start_experiment" { + restart cmd "xbox"; + stop cmd "franka_driver_out"; + stop cmd "franka_driver_in"; + stop cmd "torque_driver"; + stop cmd "franka_osc"; + start cmd "record_video"; + start cmd "logger"; + wait ms 1000; + start cmd "franka_driver_out"; + start cmd "franka_driver_in"; + start cmd "torque_driver"; + start cmd "move_to_init"; + wait ms 5000; + stop cmd "move_to_init"; + start cmd "franka_osc"; + start cmd "sampling_c3"; +} + +script "start_operator_commands" { + restart cmd "visualizer"; + restart cmd "xbox"; + restart cmd "lcm-spy"; +} + +script "stop_experiment" { + stop group "drivers"; + stop group "controllers (hardware)"; + stop cmd "record_video"; + stop cmd "logger"; +} diff --git a/examples/sampling_c3/push_t/franka_sim_t.pmd b/examples/sampling_c3/push_t/franka_sim_t.pmd new file mode 100644 index 000000000..420b50d58 --- /dev/null +++ b/examples/sampling_c3/push_t/franka_sim_t.pmd @@ -0,0 +1,71 @@ +group "simulations" { + cmd "franka_sim" { + exec = "bazel-bin/examples/sampling_c3/franka_sim --demo_name=push_t"; + host = "localhost"; + } +} + +group "operator" { + cmd "visualizer" { + exec = "bazel-bin/examples/sampling_c3/franka_visualizer --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "drake-director" { + exec = "bazel-bin/director/drake-director"; + host = "localhost"; + } + cmd "xbox" { + exec = "bazel-bin/examples/sampling_c3/xbox_script"; + host = "localhost"; + } + cmd "start_logging" { + exec = "python3 examples/sampling_c3/start_logging.py sim push_t"; + host = "localhost"; + } +} + +group "controllers" { + cmd "move_to_init" { + exec = "bazel-bin/examples/sampling_c3/franka_joint_osc_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "franka_osc" { + exec = "bazel-bin/examples/sampling_c3/franka_osc_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } + cmd "sampling_c3" { + exec = "bazel-bin/examples/sampling_c3/franka_sampling_c3_controller --is_simulation=true --demo_name=push_t"; + host = "localhost"; + } +} + +group "debug" { + cmd "lcm-spy" { + exec = "bazel-bin/lcmtypes/dair-lcm-spy"; + host = "localhost"; + } +} + +script "start_experiment_no_logs"{ + stop cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "start_experiment_with_logs"{ + restart cmd "start_logging"; + restart cmd "franka_sim"; + restart cmd "franka_osc"; + wait ms 50; + restart cmd "sampling_c3"; +} + +script "end_experiment"{ + stop cmd "sampling_c3"; + stop cmd "franka_osc"; + stop cmd "franka_sim"; + wait ms 10; + stop cmd "start_logging"; +} diff --git a/examples/sampling_c3/push_t/parameters/goal_params.yaml b/examples/sampling_c3/push_t/parameters/goal_params.yaml new file mode 100644 index 000000000..63c0dead7 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/goal_params.yaml @@ -0,0 +1,36 @@ +# Goal mode options (defined as GoalMode enum in goal_params.h): +# 0. kRandom: randomly generate a new goal. +# 1. kOrientationSequence: keep position goal the same, and cycle through a +# sequence of orientations. +# 2. kFixedGoal: keep the same goal. +goal_mode: 0 + +### Parameters used for multiple goal modes. +# Success thresholds for position and orientation. +position_success_threshold: 0.02 +orientation_success_threshold: 0.1 # 0.1 rad = 5.7 degrees + +resting_object_height: -0.009 # in world frame +ee_target_z_offset_above_object: 0.06 # defines EE goal w.r.t. object position + +# Lookahead parameters to define a sub-goal for C3. +lookahead_step_size: 0.1 # meters +lookahead_angle: 2 # rad + +# Apply hysteresis on the lookahead angle so the sub-goal orientation doesn't +# flip back and forth near the 180 degree error singularity. A lower number +# means the sub-goal orientation can switch more often; a higher number means +# the sub-goal orientation is more stable but possibly less optimal. +angle_hysteresis: 0.4 + +# Factor to convert an angular error to angular velocity command (0=disabled). +angle_err_to_vel_factor: 0 + +# Initial goal (and only goal for fixed goal mode). +fixed_target_position: [0.48199167, 0.18745379, -0.009] +fixed_target_orientation: [-0.9327733, 0, 0, 0.36046353] + +# Random-specific parameters. +random_goal_x_limits: [0.44, 0.52] +random_goal_y_limits: [0.02, 0.25] +random_goal_radius_limits: [0.5, 0.52] diff --git a/examples/sampling_c3/push_t/parameters/progress_params.yaml b/examples/sampling_c3/push_t/parameters/progress_params.yaml new file mode 100644 index 000000000..48609fc7d --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/progress_params.yaml @@ -0,0 +1,64 @@ +# Ways of computing C3 costs after solving the MPC problem (defined as +# C3CostComputationType enum in c3_options.h): +# 0. kSimLCS: Simulate the LCS dynamics from the planned +# inputs. +# 1. kUseC3Plan: Use the C3 planned trajectory and inputs. +# 2. kSimLCSReplaceC3EEPlan: Simulate the LCS dynamics from the planned +# inputs only for the object; use the planned +# EE trajectory. +# 3. kSimImpedance: Try to emulate the real cost of the system +# associated not only applying the planned +# inputs, but also tracking the planned EE +# trajectory with an impedance controller. +# 4. kSimImpedanceReplaceC3EEPlan: The same as kSimImpedance except the EE +# states are replaced with the plan from C3 +# at the end. +# 5. kSimImpedanceObjectCostOnly: The same as kSimImpedance except only the +# object terms contribute to the final cost. +cost_type: 5 +cost_type_position: 5 + +# Progress metric to use to determine if C3 is making progress, all phrased as +# improvement requirements over a number of control loops (defined as +# ProgressMetric enum in progress_params.h) +# 0. kC3Cost: C3 cost. +# 1. kConfigCost: Current object configuration cost. +# 2. kPosOrRotCost: Current position or rotation error. +# 3. kConfigCostDrop: Drop in object configuration cost (this is the same as +# kConfigCost if the required drop is 0; a more +# aggressive drop cuts C3 off earlier). +track_c3_progress_via: 3 + +# Ignore orientation errors when object is beyond this threshold from the goal. +cost_switching_threshold_distance: 0.50 + +# Penalize repositioning distance. +travel_cost_per_meter: 0 + +# Number of control loops to wait before cutting off C3 due to unproductivity. +# Used for kC3Cost, kConfigCost, kPosOrRotCost. +num_control_loops_to_wait: 5 +num_control_loops_to_wait_position: 5 + +# kConfigCostDrop parameters. +progress_enforced_cost_drop: 0.01 +progress_enforced_over_n_loops: 28 + +### Hysteresis parameters for switching between C3 and repositioning modes. +finished_reposition_cost: 1000000000 + +hyst_c3_to_repos: 300000 +hyst_c3_to_repos_position: 300000 +hyst_repos_to_c3: 200000 +hyst_repos_to_c3_position: 200000 +hyst_repos_to_repos: 60000000000000 +hyst_repos_to_repos_position: 60000000000000 + +# Relative hysteresis is in terms of a fraction of the current cost. +use_relative_hysteresis: true +hyst_c3_to_repos_frac: 0.4 +hyst_c3_to_repos_frac_position: 0.6 +hyst_repos_to_c3_frac: 0.9 +hyst_repos_to_c3_frac_position: 0.5 +hyst_repos_to_repos_frac: 0.3 +hyst_repos_to_repos_frac_position: 0.1 diff --git a/examples/sampling_c3/push_t/parameters/reposition_params.yaml b/examples/sampling_c3/push_t/parameters/reposition_params.yaml new file mode 100644 index 000000000..faaf08b02 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/reposition_params.yaml @@ -0,0 +1,32 @@ +# Repositioning trajectory type options (defined as RepositioningTrajectoryType +# enum in reposition_params.h): +# 0. kSpline: move from current to target with spline that distorts +# around the object. +# 1. kSpherical: move away from object to a radius, along the spherical +# surface to outside target, then in to target. +# 2. kCircular: (planar) move away from object to a planar radius, +# along the circle to outside target, then in to target. +# 3. kPiecewiseLinear: move up to waypoint height, over to above target, then +# down. +traj_type: 3 + +### Parameters used for multiple repositioning trajectory types. +speed: 0.18 # m/s + +# Thresholds for switching to straight line trajectories. +use_straight_line_traj_under_spline: 0.12 # xyz meters for spline +use_straight_line_traj_within_angle: 0.3 # rad for circle/sphere +use_straight_line_traj_under_piecewise_linear: 0.008 # xy meters for PWL + +# Spline-specific parameters. +spline_width: 0.17 + +# Spherical-specific parameters. +sphere_radius: 0.18 + +# Circular-specific parameters. +circle_radius: 0.20 +circle_height: 0.00 + +# Piecewise-linear-specific parameters. +pwl_waypoint_height: 0.06 diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml new file mode 100644 index 000000000..7977849b5 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_c3_controller_params.yaml @@ -0,0 +1,23 @@ +sampling_c3_options_file: examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml +reposition_params_file: examples/sampling_c3/push_t/parameters/reposition_params.yaml +progress_params_file: examples/sampling_c3/push_t/parameters/progress_params.yaml +sampling_params_file: examples/sampling_c3/push_t/parameters/sampling_params.yaml +goal_params_file: examples/sampling_c3/push_t/parameters/goal_params.yaml + +sim_params_file: examples/sampling_c3/push_t/parameters/sim_params.yaml +vis_params_file: examples/sampling_c3/push_t/parameters/vis_params.yaml +osc_params_file: examples/sampling_c3/shared_parameters/osc_params.yaml +osqp_settings_file: examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml +osc_qp_settings_file: examples/sampling_c3/shared_parameters/osc_qp_settings.yaml +franka_driver_channels_file: common/parameters/franka_drake_lcm_driver_channels.yaml +lcm_channels_hardware_file: examples/sampling_c3/shared_parameters/lcm_channels_hardware.yaml +lcm_channels_simulation_file: examples/sampling_c3/shared_parameters/lcm_channels_simulation.yaml + +# Note: Use vertical_link body name as representative of the pose of the entire +# T model. This works because vertical_link's origin is the same as the T's. +object_model: examples/sampling_c3/urdf/push_t_control.sdf #TODO T_vertical.sdf +object_body_name: vertical_link + +include_end_effector_orientation: false + +control_loop_delay_ms: 0 diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml new file mode 100644 index 000000000..73eba51e6 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml @@ -0,0 +1,163 @@ +### C3 options +admm_iter: 3 +rho: 0 #This isn't used anywhere! +rho_scale: 3 +num_threads: 5 +num_outer_threads: 4 +delta_option: 1 +projection_type: 'MIQP' # 'MIQP' or 'QP' +contact_model: 'anitescu' # 'stewart_and_trinkle' or 'anitescu'. +warm_start: false +end_on_qp_step: false +use_robust_formulation: false +solve_time_filter_alpha: 0.95 +publish_frequency: 0 +penalize_changes_in_u_across_solves: false # Penalize (u-u_prev) instead of u. +num_friction_directions: 2 + +N: 5 +gamma: 1.0 # discount factor on MPC costs + +# As alpha -> 0, any complimentarity constraint error in QP projection -> 0. +qp_projection_alpha: 0.01 +qp_projection_scaling: 1 + +### Limits +# Workspace limits specified as linear constraints [x, y, z, lb, ub] +workspace_limits: [[1, 0, 0, 0.15, 0.75], + [0, 1, 0, -0.6, 0.6], + [0, 0, 1, -0.01, 0.3]] +robot_radius_limits: [0.25, 0.75] +workspace_margins: 0.02 +u_horizontal_limits: [-50, 50] +u_vertical_limits: [-50, 50] + +### Whether to use a predicted EE location for each mode. +use_predicted_x0_c3: true +use_predicted_x0_repos: true +use_predicted_x0_reset_mechanism: true # Resets if prediction is too far. + +### Push-T demo specific parameters: +# Contact pairs include a number of (ee-ground, ee-T, T-ground). +mu_per_pair_type: [0.4165, 1, 0.4615] # match URDFs with (2mu1*mu2)/(mu1+mu2) +resolve_contacts_to_lists: [[0, 1, 3], + [0, 2, 3]] + +# Index into this list to choose number of contacts for C3 solve and cost. +num_contacts_index: 0 +num_contacts_index_for_cost: 1 + +planning_dt_position: 0.1 +planning_dt_pose: 0.05 + +### Cost parameters +### If list of lists, then index into them with num_contact_index(_for_cost). +# Set 4x4 portion of Q for quaternion based on current and desired quaternion. +use_quaternion_dependent_cost: true +q_quaternion_dependent_weight: 1000 +q_quaternion_dependent_regularizer_fraction: 0 + +Kp_for_ee_pd_rollout: 100 +Kd_for_ee_pd_rollout: 0.5 + +### Pose tracking cost parameters +# Matrix scaling +w_Q: 45 +w_R: 1 +w_G: 0.25 +w_U: 0.26 + +q_vector: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 200, 200, 120, # object position + 5, 5, 5, # EE linear velocity + 0.05, 0.05, 0.05, # object angular velocity + 0.05, 0.05, 0.05] # object linear velocity +r_vector: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x: [900, 900, 900, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_list: [[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, 1, 1, 1, 1]] +g_lambda_list: [[0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] #, +g_u: [30, 30, 30] + +# Penalty on matching the QP variables +u_x: [10, 10, 10, 100, 100, 100, 100, 10, 10, 10, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_list: [[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, 1, 1, 1, 1]] +# u_lambda_list is not used for the QP projection; overwritten by alpha*F. +u_lambda_list: [[10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u: [0.01, 0.01, 0.01] + +### Position tracking cost parameters +# Matrix scaling +w_Q_position: 45 +w_R_position: 1 +w_G_position: 0.25 +w_U_position: 0.26 + +q_vector_position: [0.01, 0.01, 0.01, # EE position + 0.1, 0.1, 0.1, 0.1, # object orientation + 250, 250, 250, # object position + 5, 5, 5, # EE linear velocity + 0.05, 0.05, 0.05, # object angular velocity + 0.05, 0.05, 0.05] # object linear velocity +r_vector_position: [0.01, 0.01, 0.01] + +# Penalty on matching projected variables +g_x_position: [900, 900, 900, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +g_lambda_t_position_list: [[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, 1, 1, 1, 1]] +g_lambda_position_list: [ + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005], + [0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005, 0.005]] +g_u_position: [30, 30, 30] + +# Penalty on matching the QP variables +u_x_position: [10, 10, 10, 100, 100, 100, 100, 10, 10, 10, 8, 8, 8, 1, 1, 1, 1, 1, 1] +u_gamma_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_n_position_list: [[1, 1, 1, 1], + [1, 1, 1, 1, 1]] +u_lambda_t_position_list: [[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, 1, 1, 1, 1]] +# u_lambda is not used for the QP projection; overwritten by alpha*F. +u_lambda_position_list: [ + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10], + [10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10]] +u_u_position: [0.01, 0.01, 0.01] + + +### NO NEED TO CHANGE THE BELOW. Parameters needed for the C3Options struct but +### are overwritten by other sampling C3 parameters. +use_predicted_x0: false # instead: use_predicted_x0_c3, + # use_predicted_x0_repos, + # use_predicted_x0_reset_mechanism +dt: 0 # instead: planning_dt_pose, planning_dt_position +solve_dt: 0 # unused +mu: [] # instead based on indexing into mu_per_pair_type +num_contacts: 0 # instead based on summing index of resolve_contacts_to_lists +# Instead for the below, index into their _list versions. +g_gamma: [] +g_lambda_n: [] +g_lambda_t: [] +g_lambda: [] +u_gamma: [] +u_lambda_n: [] +u_lambda_t: [] +u_lambda: [] diff --git a/examples/sampling_c3/push_t/parameters/sampling_params.yaml b/examples/sampling_c3/push_t/parameters/sampling_params.yaml new file mode 100644 index 000000000..ca66bb199 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sampling_params.yaml @@ -0,0 +1,44 @@ +# Sampling strategies (defined as SamplingStrategy enum in sampling_params.h): +# 0. kRadiallySymmetric: radially distributed samples on a planar circle. +# 1. kRandomOnCircle: random samples on a planar circle. +# 2. kRandomOnSphere: random samples on a spherical surface. +# 3. kFixed: fixed set of samples. +# 4. kRandomOnPerimeter: random samples on a perimeter offset past the object +# surface (roughly planar). +# 5. kRandomOnShell: random samples on a shell offset past the object +# surface. +sampling_strategy: 4 + +### Parameters relevant for all sampling strategies. +filter_samples_for_safety: true + +# Number of samples. Add 2 for repositioning (current location, previous +# repositioning target) and 1 for C3 (current location) to get total sample +# number solved in parallel for each control loop. +num_additional_samples_repos: 1 +num_additional_samples_c3: 2 + +# Sample buffer parameters. +consider_best_buffer_sample_when_leaving_c3: true +N_sample_buffer: 100 +pos_error_sample_retention: 0.003 +ang_error_sample_retention: 0.0349 # 0.0349 rad = 2 deg + +# Shared across multiple sampling strategies. +sampling_radius: 0.16 # kRadiallySymmetric, kRandomOnCircle, kRandomOnSphere +sampling_height: 0.005 # kRadiallySymmetric, kRandomOnCircle, kRandomOnPerimeter +sample_projection_clearance: 0.02 # kRandomOnPerimeter, kRandomOnShell +min_angle_from_vertical: 1.49 # kRandomOnSphere, kRandomOnShell +max_angle_from_vertical: 1.5 # kRandomOnSphere, kRandomOnShell + +# kFixed parameters. +fixed_sample_locations: [[0.45, 0.270, 0.270], # sample 1 + [0.34, 0.455, 0.225]] # sample 2 + +# kRandomOnPerimeter parameters. +grid_x_limits: [-0.12, 0.08] +grid_y_limits: [-0.08, 0.08] + +# kRandomOnShell parameters. +min_sampling_radius: 0.07 +max_sampling_radius: 0.10 diff --git a/examples/sampling_c3/push_t/parameters/sim_params.yaml b/examples/sampling_c3/push_t/parameters/sim_params.yaml new file mode 100644 index 000000000..8ed1c5225 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/sim_params.yaml @@ -0,0 +1,14 @@ +object_model: examples/sampling_c3/urdf/push_t.sdf + +franka_publish_rate: 1000 +object_publish_rate: 10 +actuator_delay: 0.000 + +visualize_drake_sim: false +publish_efforts: true + +dt: 0.0001 +realtime_rate: 1 + +q_init_franka: [2.191, 1.1, -1.33, -2.22, 1.30, 2.02, 0.08] +q_init_object: [1.0, 0, 0, 0, 0.5, 0, -0.009] diff --git a/examples/sampling_c3/push_t/parameters/vis_params.yaml b/examples/sampling_c3/push_t/parameters/vis_params.yaml new file mode 100644 index 000000000..128af60e8 --- /dev/null +++ b/examples/sampling_c3/push_t/parameters/vis_params.yaml @@ -0,0 +1,37 @@ +ee_vis_model: examples/sampling_c3/urdf/ee_visualization_model.urdf +object_vis_model: examples/sampling_c3/urdf/push_t.sdf +visualizer_publish_rate: 30 + +camera_pose: [1.5, 0, 0.6] +camera_target: [0.5, 0, 0.5] + +visualize_c3_workspace: false +visualize_c3_state: true + +visualize_center_of_mass_plan_curr: false +visualize_c3_forces_curr: false +visualize_center_of_mass_plan_best: false +visualize_c3_forces_best: false + +visualize_is_c3_mode: true +visualize_sample_locations: true +visualize_sample_buffer: true + +# Note: Colors can either be empty ([]) or RGB values between 0 and 1. If left +# empty, the colors defined in the URDF/SDF files will be used. +is_c3_mode_color: [1.0, 0.43, 1.0] +sample_color: [1.0, 1.0, 0.0] + +visualize_execution_plan: false + +visualize_c3_plan_curr: false +c3_curr_object_color: [] +c3_curr_ee_color: [1.0, 1.0, 1.0] +df_curr_object_color: [] +df_curr_ee_color: [0.5, 1.0, 0.5] + +visualize_c3_plan_best: false +c3_best_object_color: [1.0, 0.64, 0.0] +c3_best_ee_color: [1.0, 0.64, 0.0] +df_best_object_color: [1.0, 0.64, 0.0] +df_best_ee_color: [1.0, 0.64, 0.0] diff --git a/examples/sampling_c3/urdf/jack_ground.sdf b/examples/sampling_c3/urdf/jack_control.sdf similarity index 100% rename from examples/sampling_c3/urdf/jack_ground.sdf rename to examples/sampling_c3/urdf/jack_control.sdf diff --git a/examples/sampling_c3/urdf/push_t.sdf b/examples/sampling_c3/urdf/push_t.sdf new file mode 100644 index 000000000..bc3104bcf --- /dev/null +++ b/examples/sampling_c3/urdf/push_t.sdf @@ -0,0 +1,93 @@ + + + + + + 0 0 0 0 0 0 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + + -0.10 0 0 0 0 1.5708 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + vertical_link + horizontal_link + 0 0 0 0 0 0 + + + + \ No newline at end of file diff --git a/examples/sampling_c3/urdf/push_t_control.sdf b/examples/sampling_c3/urdf/push_t_control.sdf new file mode 100644 index 000000000..2c017fd56 --- /dev/null +++ b/examples/sampling_c3/urdf/push_t_control.sdf @@ -0,0 +1,117 @@ + + + + + + 0 0 0 0 0 0 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + 0 0 0 0 0 0 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + -0.12 0.08 -0.02 0 0 0 + + + 0.001 + + + + + -0.12 -0.08 -0.02 0 0 0 + + + 0.001 + + + + + 0.08 0.0 -0.02 0 0 0 + + + 0.001 + + + + + + + + -0.10 0 0 0 0 1.5708 + 0.5 + + 0.0001333 + 0.0011333 + 0.0011333 + 0.0 + 0.0 + 0.0 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + 0 0.8 0.8 1 + + + + -0.10 0 0 0 0 1.5708 + + + 0.16 0.04 0.04 + + + + + 3.0e7 + 0.18 + 10 + 0.3 + + + + + + vertical_link + horizontal_link + 0 0 0 0 0 0 + + + + \ No newline at end of file From f0c33d5d8b49777d300a1cce4c5bafc7fab42e07 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 2 Jul 2025 16:18:41 -0400 Subject: [PATCH 3/9] Bazel build file fixes --- examples/sampling_c3/jacktoy/BUILD.bazel | 1 - examples/sampling_c3/push_t/BUILD.bazel | 14 +------------- 2 files changed, 1 insertion(+), 14 deletions(-) diff --git a/examples/sampling_c3/jacktoy/BUILD.bazel b/examples/sampling_c3/jacktoy/BUILD.bazel index aea0f689a..faec88ea6 100644 --- a/examples/sampling_c3/jacktoy/BUILD.bazel +++ b/examples/sampling_c3/jacktoy/BUILD.bazel @@ -10,7 +10,6 @@ cc_library( ]), ) - cc_library( name = "lcm_channels_jacktoy", hdrs = [], diff --git a/examples/sampling_c3/push_t/BUILD.bazel b/examples/sampling_c3/push_t/BUILD.bazel index 04584da75..bd0951674 100644 --- a/examples/sampling_c3/push_t/BUILD.bazel +++ b/examples/sampling_c3/push_t/BUILD.bazel @@ -3,25 +3,13 @@ package(default_visibility = ["//visibility:public"]) -cc_library( - name = "urdfs", - data = glob([ - "urdf/**", - ]), -) - cc_library( name = "parameters", data = glob([ - "*yaml", + "parameters/*.yaml", ]), ) -cc_library( - name = "franka_hardware", - deps = [], -) - cc_library( name = "lcm_channels_push_t", hdrs = [], From 37addbdbe51e591fe789fc221e29e25dedbab271 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 2 Jul 2025 16:21:36 -0400 Subject: [PATCH 4/9] WIP unsuccessful test to set linsys_solver for OSQP after upgrade --- .../solver_settings/osqp_options_walking.yaml | 6 ++++-- .../osc_run/osc_running_qp_settings.yaml | 5 +++-- .../parameters/franka_c3_qp_settings.yaml | 5 +++-- .../parameters/franka_osc_qp_settings.yaml | 5 +++-- .../shared_parameters/osc_qp_settings.yaml | 5 +++-- .../sampling_c3_qp_settings.yaml | 5 +++-- solvers/BUILD.bazel | 1 + solvers/c3_qp.cc | 2 +- solvers/fast_osqp_solver.cc | 2 +- solvers/osqp_options_default.yaml | 6 ++++-- solvers/osqp_settings_yaml.h | 2 +- solvers/solver_options_io.h | 20 +++++++++++++++++++ 12 files changed, 47 insertions(+), 17 deletions(-) diff --git a/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml b/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml index f82b9ba31..4faeddc06 100644 --- a/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml +++ b/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml @@ -2,7 +2,6 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 500 - linsys_solver: 0 verbose: 0 warm_start: 1 polish: 1 @@ -11,6 +10,7 @@ int_options: polish_refine_iter: 3 scaling: 10 adaptive_rho: 1 + adaptive_rho_interval: 0 double_options: rho: 0.0001 @@ -21,9 +21,11 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0.1 +enum_options: + linsys_solver: 0 + string_options: {} \ No newline at end of file diff --git a/examples/Cassie/osc_run/osc_running_qp_settings.yaml b/examples/Cassie/osc_run/osc_running_qp_settings.yaml index 5cf72f59b..287e9a642 100644 --- a/examples/Cassie/osc_run/osc_running_qp_settings.yaml +++ b/examples/Cassie/osc_run/osc_running_qp_settings.yaml @@ -2,11 +2,11 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 250 - linsys_solver: 0 verbose: 0 warm_start: 1 scaling: 1 adaptive_rho: 1 + adaptive_rho_interval: 0 polish: 1 polish_refine_iter: 1 scaled_termination: 1 @@ -20,8 +20,9 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 +enum_options: + linsys_solver: 0 string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_qp_settings.yaml b/examples/franka/parameters/franka_c3_qp_settings.yaml index b136c938f..f0c837e40 100644 --- a/examples/franka/parameters/franka_c3_qp_settings.yaml +++ b/examples/franka/parameters/franka_c3_qp_settings.yaml @@ -2,11 +2,11 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 1000 - linsys_solver: 0 verbose: 0 warm_start: 1 scaling: 1 adaptive_rho: 1 + adaptive_rho_interval: 0 polish: 1 polish_refine_iter: 1 scaled_termination: 1 @@ -20,8 +20,9 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 +enum_options: + linsys_solver: 0 string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_osc_qp_settings.yaml b/examples/franka/parameters/franka_osc_qp_settings.yaml index 639eca37a..aa40a883d 100644 --- a/examples/franka/parameters/franka_osc_qp_settings.yaml +++ b/examples/franka/parameters/franka_osc_qp_settings.yaml @@ -2,11 +2,11 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 1000 - linsys_solver: 0 verbose: 0 warm_start: 1 scaling: 1 adaptive_rho: 1 + adaptive_rho_interval: 0 polish: 1 polish_refine_iter: 1 scaled_termination: 1 @@ -20,8 +20,9 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 +enum_options: + linsys_solver: 0 string_options: {} \ No newline at end of file diff --git a/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml b/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml index 39407d322..e6e27267f 100644 --- a/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml +++ b/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml @@ -2,11 +2,11 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 1000 - linsys_solver: 0 verbose: 0 warm_start: 1 scaling: 1 adaptive_rho: 1 + adaptive_rho_interval: 0 polish: 1 polish_refine_iter: 1 scaled_termination: 1 @@ -20,8 +20,9 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 +enum_options: + linsys_solver: 0 string_options: {} diff --git a/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml index 741647456..99e1fb935 100644 --- a/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml +++ b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml @@ -2,11 +2,11 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 1000 - linsys_solver: 0 verbose: 0 warm_start: 1 scaling: 1 adaptive_rho: 1 + adaptive_rho_interval: 0 # TODO @bibit move to int polish: 1 polish_refine_iter: 1 scaled_termination: 1 @@ -20,8 +20,9 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 +enum_options: + linsys_solver: 0 string_options: {} diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 21bfb3fcf..d7af9865d 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -15,6 +15,7 @@ cc_library( name = "solver_options_io", hdrs = ["solver_options_io.h"], deps = [ + "//solvers:fast_osqp_solver", "@drake//:drake_shared_library", ], ) diff --git a/solvers/c3_qp.cc b/solvers/c3_qp.cc index dca591db8..de98d5c63 100644 --- a/solvers/c3_qp.cc +++ b/solvers/c3_qp.cc @@ -78,7 +78,7 @@ VectorXd C3QP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, solver_options.SetOption(OsqpSolver::id(), "polish_refine_iter", 1); solver_options.SetOption(OsqpSolver::id(), "rho", 1e-4); solver_options.SetOption(OsqpSolver::id(), "scaled_termination", 1); - solver_options.SetOption(OsqpSolver::id(), "linsys_solver", 0); + solver_options.SetOption(OsqpSolver::id(), "osqp_linsys_solver_type", QDLDL_SOLVER); //0); // TODO @bibit might need to be osqp_linsys_solver_type prog.SetSolverOptions(solver_options); MathematicalProgramResult result = osqp_.Solve(prog); diff --git a/solvers/fast_osqp_solver.cc b/solvers/fast_osqp_solver.cc index 475dce35f..d19402531 100644 --- a/solvers/fast_osqp_solver.cc +++ b/solvers/fast_osqp_solver.cc @@ -319,7 +319,7 @@ void SetFastOsqpSolverSettings(const SolverOptions& solver_options, SetFastOsqpSolverSetting(options_int, "scaling", &(settings->scaling)); SetFastOsqpSolverSetting(options_int, "adaptive_rho", &(settings->adaptive_rho)); - SetFastOsqpSolverSetting(options_double, "adaptive_rho_interval", + SetFastOsqpSolverSetting(options_int, "adaptive_rho_interval", //TODO @bibit change to int &(settings->adaptive_rho_interval)); SetFastOsqpSolverSetting(options_double, "adaptive_rho_tolerance", &(settings->adaptive_rho_tolerance)); diff --git a/solvers/osqp_options_default.yaml b/solvers/osqp_options_default.yaml index 105d21597..b0a71dc63 100644 --- a/solvers/osqp_options_default.yaml +++ b/solvers/osqp_options_default.yaml @@ -2,7 +2,6 @@ print_to_console: 0 log_file_name: "" int_options: max_iter: 200 - linsys_solver: 0 verbose: 0 warm_start: 1 polish: 1 @@ -11,6 +10,7 @@ int_options: polish_refine_iter: 3 scaling: 10 adaptive_rho: 1 + adaptive_rho_interval: 0 double_options: rho: 0.0001 @@ -21,9 +21,11 @@ double_options: eps_dual_inf: 1e-5 alpha: 1.6 delta: 1e-6 - adaptive_rho_interval: 0 adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 1.0 +enum_options: + linsys_solver: 0 + string_options: {} \ No newline at end of file diff --git a/solvers/osqp_settings_yaml.h b/solvers/osqp_settings_yaml.h index fcd4cc6a0..c89231899 100644 --- a/solvers/osqp_settings_yaml.h +++ b/solvers/osqp_settings_yaml.h @@ -15,7 +15,7 @@ struct OSQPSettingsYaml { double eps_prim_inf; double eps_dual_inf; double alpha; - int linsys_solver; + int linsys_solver; // TODO @bibit might need to be linsys_solver everywhere double delta; int polish; int polish_refine_iter; diff --git a/solvers/solver_options_io.h b/solvers/solver_options_io.h index 5f7201281..e1742d7e8 100644 --- a/solvers/solver_options_io.h +++ b/solvers/solver_options_io.h @@ -1,6 +1,9 @@ #pragma once #include "drake/solvers/solver_options.h" +#include "drake/solvers/osqp_solver.h" // TODO @bibit may not need +#include "solvers/fast_osqp_solver.h" +#include namespace dairlib::solvers { @@ -34,6 +37,7 @@ struct SolverOptionsFromYaml { std::map int_options; std::map double_options; std::map string_options; + std::map enum_options; template void Serialize(Archive* a) { @@ -42,6 +46,7 @@ struct SolverOptionsFromYaml { a->Visit(DRAKE_NVP(int_options)); a->Visit(DRAKE_NVP(double_options)); a->Visit(DRAKE_NVP(string_options)); + a->Visit(DRAKE_NVP(enum_options)); } SolverOptions GetAsSolverOptions(const drake::solvers::SolverId& id) { @@ -57,6 +62,21 @@ struct SolverOptionsFromYaml { for (const auto& [key, val] : string_options) { options.SetOption(id, key, val); } + for (const auto& [key, val] : enum_options) { + if (key == "linsys_solver") { + std::cout << "Trying to set linsys solver..." << std::endl; + if (val == 0) { + options.SetOption(id, "linsys_solver", 0); //QDLDL_SOLVER); + } + else if (val == 1) { + options.SetOption(id, "linsys_solver", 1); //MKL_PARDISO_SOLVER); + } else { + std::cerr << ("Unknown osqp_linsys_solver_type: " + val) << std::endl; + } + // std::cout << "Skipping setting osqp solver" << std::endl; + } + else {std::cerr << ("Unknown OSQP enum: " + key) << std::endl;} + } return options; } }; From 94b3806f6ee9a46522af96577fd22dd8f9dc016a Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 2 Jul 2025 16:30:28 -0400 Subject: [PATCH 5/9] Remove linsys_solver OSQP setting since no longer exposed --- .../pydairlib/analysis/osqp_settings_sandbox.py | 1 - .../solver_settings/osqp_options_walking.yaml | 3 --- .../Cassie/osc_run/osc_running_qp_settings.yaml | 2 -- .../parameters/franka_c3_qp_settings.yaml | 2 -- .../parameters/franka_osc_qp_settings.yaml | 2 -- .../shared_parameters/osc_qp_settings.yaml | 2 -- .../sampling_c3_qp_settings.yaml | 2 -- solvers/c3_qp.cc | 1 - solvers/default_osc_osqp_settings.yaml | 1 - solvers/osqp_options_default.yaml | 3 --- solvers/osqp_settings_yaml.h | 2 -- solvers/solver_options_io.h | 17 ----------------- 12 files changed, 38 deletions(-) diff --git a/bindings/pydairlib/analysis/osqp_settings_sandbox.py b/bindings/pydairlib/analysis/osqp_settings_sandbox.py index a22860747..d42000cb5 100644 --- a/bindings/pydairlib/analysis/osqp_settings_sandbox.py +++ b/bindings/pydairlib/analysis/osqp_settings_sandbox.py @@ -165,7 +165,6 @@ def main(): 'eps_prim_inf': 1e-6, 'eps_dual_inf': 1e-6, 'alpha': 1.6, - 'linsys_solver': 0, 'delta': 1e-6, 'polish': 1, 'polish_refine_iter': 3, diff --git a/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml b/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml index 4faeddc06..d8a4e1eab 100644 --- a/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml +++ b/examples/Cassie/osc/solver_settings/osqp_options_walking.yaml @@ -25,7 +25,4 @@ double_options: adaptive_rho_fraction: 0.4 time_limit: 0.1 -enum_options: - linsys_solver: 0 - string_options: {} \ No newline at end of file diff --git a/examples/Cassie/osc_run/osc_running_qp_settings.yaml b/examples/Cassie/osc_run/osc_running_qp_settings.yaml index 287e9a642..f9939ffc7 100644 --- a/examples/Cassie/osc_run/osc_running_qp_settings.yaml +++ b/examples/Cassie/osc_run/osc_running_qp_settings.yaml @@ -23,6 +23,4 @@ double_options: adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 -enum_options: - linsys_solver: 0 string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_c3_qp_settings.yaml b/examples/franka/parameters/franka_c3_qp_settings.yaml index f0c837e40..8a12cbe60 100644 --- a/examples/franka/parameters/franka_c3_qp_settings.yaml +++ b/examples/franka/parameters/franka_c3_qp_settings.yaml @@ -23,6 +23,4 @@ double_options: adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 -enum_options: - linsys_solver: 0 string_options: {} \ No newline at end of file diff --git a/examples/franka/parameters/franka_osc_qp_settings.yaml b/examples/franka/parameters/franka_osc_qp_settings.yaml index aa40a883d..e14875b7a 100644 --- a/examples/franka/parameters/franka_osc_qp_settings.yaml +++ b/examples/franka/parameters/franka_osc_qp_settings.yaml @@ -23,6 +23,4 @@ double_options: adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 -enum_options: - linsys_solver: 0 string_options: {} \ No newline at end of file diff --git a/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml b/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml index e6e27267f..c34da5417 100644 --- a/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml +++ b/examples/sampling_c3/shared_parameters/osc_qp_settings.yaml @@ -23,6 +23,4 @@ double_options: adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 -enum_options: - linsys_solver: 0 string_options: {} diff --git a/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml index 99e1fb935..dfcf0db49 100644 --- a/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml +++ b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml @@ -23,6 +23,4 @@ double_options: adaptive_rho_tolerance: 5 adaptive_rho_fraction: 0.4 time_limit: 0 -enum_options: - linsys_solver: 0 string_options: {} diff --git a/solvers/c3_qp.cc b/solvers/c3_qp.cc index de98d5c63..686ec1e6d 100644 --- a/solvers/c3_qp.cc +++ b/solvers/c3_qp.cc @@ -78,7 +78,6 @@ VectorXd C3QP::SolveSingleProjection(const MatrixXd& U, const VectorXd& delta_c, solver_options.SetOption(OsqpSolver::id(), "polish_refine_iter", 1); solver_options.SetOption(OsqpSolver::id(), "rho", 1e-4); solver_options.SetOption(OsqpSolver::id(), "scaled_termination", 1); - solver_options.SetOption(OsqpSolver::id(), "osqp_linsys_solver_type", QDLDL_SOLVER); //0); // TODO @bibit might need to be osqp_linsys_solver_type prog.SetSolverOptions(solver_options); MathematicalProgramResult result = osqp_.Solve(prog); diff --git a/solvers/default_osc_osqp_settings.yaml b/solvers/default_osc_osqp_settings.yaml index 132e5afa0..8cd6e9af8 100644 --- a/solvers/default_osc_osqp_settings.yaml +++ b/solvers/default_osc_osqp_settings.yaml @@ -6,7 +6,6 @@ eps_rel: 1e-7 eps_prim_inf: 1e-5 eps_dual_inf: 1e-5 alpha: 1.6 -linsys_solver: 0 delta: 1e-6 polish: 1 polish_refine_iter: 3 diff --git a/solvers/osqp_options_default.yaml b/solvers/osqp_options_default.yaml index b0a71dc63..40d3e70c9 100644 --- a/solvers/osqp_options_default.yaml +++ b/solvers/osqp_options_default.yaml @@ -25,7 +25,4 @@ double_options: adaptive_rho_fraction: 0.4 time_limit: 1.0 -enum_options: - linsys_solver: 0 - string_options: {} \ No newline at end of file diff --git a/solvers/osqp_settings_yaml.h b/solvers/osqp_settings_yaml.h index c89231899..a4074222a 100644 --- a/solvers/osqp_settings_yaml.h +++ b/solvers/osqp_settings_yaml.h @@ -15,7 +15,6 @@ struct OSQPSettingsYaml { double eps_prim_inf; double eps_dual_inf; double alpha; - int linsys_solver; // TODO @bibit might need to be linsys_solver everywhere double delta; int polish; int polish_refine_iter; @@ -39,7 +38,6 @@ struct OSQPSettingsYaml { a->Visit(DRAKE_NVP(eps_prim_inf)); a->Visit(DRAKE_NVP(eps_dual_inf)); a->Visit(DRAKE_NVP(alpha)); - a->Visit(DRAKE_NVP(linsys_solver)); a->Visit(DRAKE_NVP(delta)); a->Visit(DRAKE_NVP(polish)); a->Visit(DRAKE_NVP(polish_refine_iter)); diff --git a/solvers/solver_options_io.h b/solvers/solver_options_io.h index e1742d7e8..170ab30c9 100644 --- a/solvers/solver_options_io.h +++ b/solvers/solver_options_io.h @@ -37,7 +37,6 @@ struct SolverOptionsFromYaml { std::map int_options; std::map double_options; std::map string_options; - std::map enum_options; template void Serialize(Archive* a) { @@ -46,7 +45,6 @@ struct SolverOptionsFromYaml { a->Visit(DRAKE_NVP(int_options)); a->Visit(DRAKE_NVP(double_options)); a->Visit(DRAKE_NVP(string_options)); - a->Visit(DRAKE_NVP(enum_options)); } SolverOptions GetAsSolverOptions(const drake::solvers::SolverId& id) { @@ -62,21 +60,6 @@ struct SolverOptionsFromYaml { for (const auto& [key, val] : string_options) { options.SetOption(id, key, val); } - for (const auto& [key, val] : enum_options) { - if (key == "linsys_solver") { - std::cout << "Trying to set linsys solver..." << std::endl; - if (val == 0) { - options.SetOption(id, "linsys_solver", 0); //QDLDL_SOLVER); - } - else if (val == 1) { - options.SetOption(id, "linsys_solver", 1); //MKL_PARDISO_SOLVER); - } else { - std::cerr << ("Unknown osqp_linsys_solver_type: " + val) << std::endl; - } - // std::cout << "Skipping setting osqp solver" << std::endl; - } - else {std::cerr << ("Unknown OSQP enum: " + key) << std::endl;} - } return options; } }; From 658f74c862a1b540188e06368cf44938f768c926 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 2 Jul 2025 16:40:10 -0400 Subject: [PATCH 6/9] Remove unnecessary changes --- .../sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml | 2 +- solvers/BUILD.bazel | 1 - solvers/fast_osqp_solver.cc | 2 +- solvers/solver_options_io.h | 3 --- 4 files changed, 2 insertions(+), 6 deletions(-) diff --git a/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml index dfcf0db49..d4f5187df 100644 --- a/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml +++ b/examples/sampling_c3/shared_parameters/sampling_c3_qp_settings.yaml @@ -6,7 +6,7 @@ int_options: warm_start: 1 scaling: 1 adaptive_rho: 1 - adaptive_rho_interval: 0 # TODO @bibit move to int + adaptive_rho_interval: 0 polish: 1 polish_refine_iter: 1 scaled_termination: 1 diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index d7af9865d..21bfb3fcf 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -15,7 +15,6 @@ cc_library( name = "solver_options_io", hdrs = ["solver_options_io.h"], deps = [ - "//solvers:fast_osqp_solver", "@drake//:drake_shared_library", ], ) diff --git a/solvers/fast_osqp_solver.cc b/solvers/fast_osqp_solver.cc index d19402531..a755752ca 100644 --- a/solvers/fast_osqp_solver.cc +++ b/solvers/fast_osqp_solver.cc @@ -319,7 +319,7 @@ void SetFastOsqpSolverSettings(const SolverOptions& solver_options, SetFastOsqpSolverSetting(options_int, "scaling", &(settings->scaling)); SetFastOsqpSolverSetting(options_int, "adaptive_rho", &(settings->adaptive_rho)); - SetFastOsqpSolverSetting(options_int, "adaptive_rho_interval", //TODO @bibit change to int + SetFastOsqpSolverSetting(options_int, "adaptive_rho_interval", &(settings->adaptive_rho_interval)); SetFastOsqpSolverSetting(options_double, "adaptive_rho_tolerance", &(settings->adaptive_rho_tolerance)); diff --git a/solvers/solver_options_io.h b/solvers/solver_options_io.h index 170ab30c9..5f7201281 100644 --- a/solvers/solver_options_io.h +++ b/solvers/solver_options_io.h @@ -1,9 +1,6 @@ #pragma once #include "drake/solvers/solver_options.h" -#include "drake/solvers/osqp_solver.h" // TODO @bibit may not need -#include "solvers/fast_osqp_solver.h" -#include namespace dairlib::solvers { From cbaefc6d7a830428eeaea35ad7dd14a9b60741e8 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 2 Jul 2025 17:26:11 -0400 Subject: [PATCH 7/9] Fix sample perimeter projection to proper clearance --- examples/sampling_c3/generate_samples.cc | 24 ++++++++++--------- .../push_t/parameters/sampling_params.yaml | 2 +- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/examples/sampling_c3/generate_samples.cc b/examples/sampling_c3/generate_samples.cc index 104bbb09f..62af91711 100644 --- a/examples/sampling_c3/generate_samples.cc +++ b/examples/sampling_c3/generate_samples.cc @@ -453,19 +453,21 @@ Eigen::VectorXd ProjectSampleOutsideObject( // surface. multibody::GeomGeomCollider collider( plant, contact_geoms.at(1).at(min_distance_index)); - auto [p_world_contact_a, p_world_contact_b] = collider.CalcWitnessPoints( + auto [p_world_contact_ee, p_world_contact_obj] = collider.CalcWitnessPoints( context); - // Find vector in direction from sample to contact point on object. - Eigen::Vector3d a_to_b = p_world_contact_b - p_world_contact_a; - double penetration_depth = a_to_b.norm(); - Eigen::Vector3d a_to_b_normalized = a_to_b.normalized(); - // Add clearance to point b in the same direction. - Eigen::Vector3d p_world_contact_b_clearance = - p_world_contact_b + - (penetration_depth + sampling_params.sample_projection_clearance) * - a_to_b_normalized; - candidate_state.head(3) = p_world_contact_b_clearance; + // Get the EE radius to factor into the projection. + double ee_radius = GetEERadiusFromPlant(plant, context, contact_geoms); + + // Find vector in direction from EE to object witness points. + Eigen::Vector3d ee_to_obj = p_world_contact_obj - p_world_contact_ee; + Eigen::Vector3d ee_to_obj_normalized = ee_to_obj.normalized(); + // Add clearance to the object in the same direction. + Eigen::Vector3d p_world_contact_obj_clearance = + p_world_contact_obj + + (ee_radius + sampling_params.sample_projection_clearance) * + ee_to_obj_normalized; + candidate_state.head(3) = p_world_contact_obj_clearance; return candidate_state; } diff --git a/examples/sampling_c3/push_t/parameters/sampling_params.yaml b/examples/sampling_c3/push_t/parameters/sampling_params.yaml index ca66bb199..6c5d4e7c8 100644 --- a/examples/sampling_c3/push_t/parameters/sampling_params.yaml +++ b/examples/sampling_c3/push_t/parameters/sampling_params.yaml @@ -27,7 +27,7 @@ ang_error_sample_retention: 0.0349 # 0.0349 rad = 2 deg # Shared across multiple sampling strategies. sampling_radius: 0.16 # kRadiallySymmetric, kRandomOnCircle, kRandomOnSphere sampling_height: 0.005 # kRadiallySymmetric, kRandomOnCircle, kRandomOnPerimeter -sample_projection_clearance: 0.02 # kRandomOnPerimeter, kRandomOnShell +sample_projection_clearance: 0.02 # kRandomOnPerimeter, kRandomOnShell min_angle_from_vertical: 1.49 # kRandomOnSphere, kRandomOnShell max_angle_from_vertical: 1.5 # kRandomOnSphere, kRandomOnShell From 31092b2c1bbcd0207cad43b12096784059d0e2d1 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 2 Jul 2025 17:39:15 -0400 Subject: [PATCH 8/9] Consolidate duplicated lines for building contact pairs --- .../franka_sampling_c3_controller.cc | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/examples/sampling_c3/franka_sampling_c3_controller.cc b/examples/sampling_c3/franka_sampling_c3_controller.cc index 0226e390e..0165d890a 100644 --- a/examples/sampling_c3/franka_sampling_c3_controller.cc +++ b/examples/sampling_c3/franka_sampling_c3_controller.cc @@ -103,6 +103,8 @@ int DoMain(int argc, char* argv[]) { // Build the contact pairs based on the demo. std::vector>> contact_pairs; + std::vector> ee_contact_pairs; + std::vector> ground_object_contact_pairs; std::unordered_map contact_geoms; // All demos include the end effector and ground. @@ -116,7 +118,6 @@ int DoMain(int argc, char* argv[]) { contact_geoms["GROUND"] = ground_geoms; std::vector> ee_ground_contact{ SortedPair(contact_geoms["EE"], contact_geoms["GROUND"])}; - contact_pairs.push_back(ee_ground_contact); if (FLAGS_demo_name == "jacktoy") { drake::geometry::GeometryId capsule1_geoms = @@ -158,18 +159,13 @@ int DoMain(int argc, char* argv[]) { contact_geoms["CAPSULE_3_SPHERE_1"] = capsule3_sphere1_geoms; contact_geoms["CAPSULE_3_SPHERE_2"] = capsule3_sphere2_geoms; - // EE-object contact pairs second. - std::vector> ee_contact_pairs; ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_1"])); ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_2"])); ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["CAPSULE_3"])); - contact_pairs.push_back(ee_contact_pairs); - // Object-ground contact pairs third. - std::vector> ground_object_contact_pairs; ground_object_contact_pairs.push_back(SortedPair( contact_geoms["CAPSULE_1_SPHERE_1"], contact_geoms["GROUND"])); ground_object_contact_pairs.push_back(SortedPair( @@ -182,7 +178,6 @@ int DoMain(int argc, char* argv[]) { contact_geoms["CAPSULE_3_SPHERE_1"], contact_geoms["GROUND"])); ground_object_contact_pairs.push_back(SortedPair( contact_geoms["CAPSULE_3_SPHERE_2"], contact_geoms["GROUND"])); - contact_pairs.push_back(ground_object_contact_pairs); } else if (FLAGS_demo_name == "push_t") { drake::geometry::GeometryId vertical_geoms = @@ -208,27 +203,25 @@ int DoMain(int argc, char* argv[]) { contact_geoms["TOP_RIGHT_SPHERE"] = top_right_sphere_geoms; contact_geoms["BOTTOM_SPHERE"] = bottom_sphere_geoms; - // EE-object contact pairs second. - std::vector> ee_contact_pairs; ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["HORIZONTAL_LINK"])); ee_contact_pairs.push_back( SortedPair(contact_geoms["EE"], contact_geoms["VERTICAL_LINK"])); - contact_pairs.push_back(ee_contact_pairs); - // Object-ground contact pairs third. - std::vector> ground_object_contact_pairs; ground_object_contact_pairs.push_back(SortedPair( contact_geoms["TOP_LEFT_SPHERE"], contact_geoms["GROUND"])); ground_object_contact_pairs.push_back(SortedPair( contact_geoms["TOP_RIGHT_SPHERE"], contact_geoms["GROUND"])); ground_object_contact_pairs.push_back(SortedPair( contact_geoms["BOTTOM_SPHERE"], contact_geoms["GROUND"])); - contact_pairs.push_back(ground_object_contact_pairs); } else { throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name); } + // Order: EE-ground, EE-object, object-ground. + contact_pairs.push_back(ee_ground_contact); + contact_pairs.push_back(ee_contact_pairs); + contact_pairs.push_back(ground_object_contact_pairs); // Piece together the diagram. DiagramBuilder builder; From 7a35e74e0d4861ed99e5a2e89831ad9ebc41ca8a Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 3 Jul 2025 11:15:16 -0400 Subject: [PATCH 9/9] Minor tuning for push T --- examples/sampling_c3/push_t/parameters/goal_params.yaml | 2 +- .../sampling_c3/push_t/parameters/sampling_c3_options.yaml | 4 ++-- examples/sampling_c3/push_t/parameters/vis_params.yaml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/sampling_c3/push_t/parameters/goal_params.yaml b/examples/sampling_c3/push_t/parameters/goal_params.yaml index 63c0dead7..f4c7785eb 100644 --- a/examples/sampling_c3/push_t/parameters/goal_params.yaml +++ b/examples/sampling_c3/push_t/parameters/goal_params.yaml @@ -14,7 +14,7 @@ resting_object_height: -0.009 # in world frame ee_target_z_offset_above_object: 0.06 # defines EE goal w.r.t. object position # Lookahead parameters to define a sub-goal for C3. -lookahead_step_size: 0.1 # meters +lookahead_step_size: 0.15 # meters lookahead_angle: 2 # rad # Apply hysteresis on the lookahead angle so the sub-goal orientation doesn't diff --git a/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml index 73eba51e6..c60e0c76b 100644 --- a/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml +++ b/examples/sampling_c3/push_t/parameters/sampling_c3_options.yaml @@ -35,7 +35,7 @@ u_vertical_limits: [-50, 50] ### Whether to use a predicted EE location for each mode. use_predicted_x0_c3: true use_predicted_x0_repos: true -use_predicted_x0_reset_mechanism: true # Resets if prediction is too far. +use_predicted_x0_reset_mechanism: false # Resets if prediction is too far. ### Push-T demo specific parameters: # Contact pairs include a number of (ee-ground, ee-T, T-ground). @@ -76,7 +76,7 @@ q_vector: [0.01, 0.01, 0.01, # EE position r_vector: [0.01, 0.01, 0.01] # Penalty on matching projected variables -g_x: [900, 900, 900, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] +g_x: [950, 950, 950, 1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] g_gamma_list: [[1, 1, 1, 1], [1, 1, 1, 1, 1]] g_lambda_n_list: [[1, 1, 1, 1], diff --git a/examples/sampling_c3/push_t/parameters/vis_params.yaml b/examples/sampling_c3/push_t/parameters/vis_params.yaml index 128af60e8..87c9195fd 100644 --- a/examples/sampling_c3/push_t/parameters/vis_params.yaml +++ b/examples/sampling_c3/push_t/parameters/vis_params.yaml @@ -26,7 +26,7 @@ visualize_execution_plan: false visualize_c3_plan_curr: false c3_curr_object_color: [] -c3_curr_ee_color: [1.0, 1.0, 1.0] +c3_curr_ee_color: [0.8, 0.8, 0.8] df_curr_object_color: [] df_curr_ee_color: [0.5, 1.0, 0.5]