Merge push_anything_dev into push_anything

common/math_utils.cc

@@ -10,3 +10,17 @@ double blend_sigmoid(double t, double tau, double window) {
 double blend_exp(double t, double tau, double window) {
   return 1 - exp(-(t + window) / tau);
 }
+int FindBin(const double* bins, int n, double x) {
+  int low = 0;
+  int high = n - 1;
+
+  while (low < high - 1) {
+    int mid = (high + low) / 2;
+    if (bins[mid] <= x) {
+      low = mid;
+    } else {
+      high = mid;
+    }
+  }
+  return low;
+}

common/math_utils.h

@@ -14,3 +14,5 @@ inline double RandomUniform(double min, double max) {
     std::uniform_real_distribution<double> dist(min, max);
     return dist(rng);
 }
+
+int FindBin(const double* bins, int n, double x);

examples/sampling_c3/BUILD.bazel

@@ -251,6 +251,7 @@ cc_library(
         "//examples/sampling_c3/parameter_headers:sampling_c3_options",
         "//examples/sampling_c3/parameter_headers:sampling_params",
         "//multibody:utils",
+        "//systems/controllers:face",
         "//multibody:geom_geom_collider",
         "//common:math_utils",
         "//common:update_context",

examples/sampling_c3/anything/franka_sim_anything.pmd

@@ -22,6 +22,10 @@ group "operator" {
         exec = "python3 examples/sampling_c3/start_logging.py sim anything";
         host = "localhost";
     }
+    cmd "generate_files" {
+        exec = "python3 examples/sampling_c3/sampling_generation/full_script.py";
+        host = "localhost";
+    }
 }
 
 group "controllers" {    

examples/sampling_c3/anything/parameters/goal_params.yaml

@@ -3,14 +3,15 @@
 #   1. kOrientationSequence:  keep position goal the same, and cycle through a
 #                             sequence of orientations.
 #   2. kFixedGoal:            keep the same goal.
-goal_mode: 0  
+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
+resting_object_height: -0.009000000000000001 # in world frame
+# This ^ will be overwritten by generating new files.
 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.
@@ -27,7 +28,8 @@ angle_hysteresis: 0.4
 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_position: [0.48199167, 0.18745379, -0.009000000000000001]
+# This ^ will be overwritten by generating new files.
 fixed_target_orientation: [-0.9327733, 0, 0, 0.36046353]
 
 # Random-specific parameters.

examples/sampling_c3/anything/parameters/progress_params.yaml

@@ -37,12 +37,12 @@ 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
+num_control_loops_to_wait: 50
+num_control_loops_to_wait_position: 50
 
 # kConfigCostDrop parameters.
-progress_enforced_cost_drop: 0.01
-progress_enforced_over_n_loops: 28
+progress_enforced_cost_drop: 0.1
+progress_enforced_over_n_loops: 60
 
 ### Hysteresis parameters for switching between C3 and repositioning modes.
 finished_reposition_cost: 1000000000
@@ -56,8 +56,8 @@ 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_c3_to_repos_frac: 0.9
+hyst_c3_to_repos_frac_position: 0.75
 hyst_repos_to_c3_frac: 0.9
 hyst_repos_to_c3_frac_position: 0.5
 hyst_repos_to_repos_frac: 0.3

examples/sampling_c3/anything/parameters/reposition_params.yaml

@@ -29,4 +29,5 @@ circle_radius: 0.20
 circle_height: 0.00
 
 # Piecewise-linear-specific parameters.
-pwl_waypoint_height: 0.06
+pwl_waypoint_height: 0.041
+# This ^ will be overwritten by generating new files.

examples/sampling_c3/anything/parameters/sampling_c3_controller_params.yaml

@@ -13,8 +13,11 @@ franka_driver_channels_file: common/parameters/franka_drake_lcm_driver_channels.
 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
 
-object_model: examples/sampling_c3/urdf/anything_control.sdf
+object_model: examples/sampling_c3/urdf/frame/frame_controller.sdf
+# This ^ will be overwritten by generating new files.
 object_body_name: body
+base_name: frame
+# The only parameter that needs to be changed for different objects is the base_name (7/10/25).
 
 include_end_effector_orientation: false
 

examples/sampling_c3/anything/parameters/sampling_params.yaml

@@ -7,7 +7,9 @@
 #                           surface (roughly planar).
 #   5. kRandomOnShell:      random samples on a shell offset past the object
 #                           surface.
-sampling_strategy: 4
+#   6. kMeshNormal:         randomly projects normal vectors off of mesh faces
+#                           and keeps samples far enough from object
+sampling_strategy: 6
 
 ### Parameters relevant for all sampling strategies.
 filter_samples_for_safety: true
@@ -27,13 +29,14 @@ 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.025   # kRandomOnPerimeter, kRandomOnShell, kMeshNormal
 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
+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.11, 0.11]
@@ -42,3 +45,12 @@ grid_y_limits: [-0.11, 0.11]
 # kRandomOnShell parameters.
 min_sampling_radius: 0.07
 max_sampling_radius: 0.10
+
+# kMeshNormal parameters
+buffer_distance: 0.05
+max_attempts: 100
+barycentric_bias: 0.6 # Values from 0 to infinity, <1 biases towards the vertices, 
+# >1 biases towards centroid or edges, =1 is uniform.
+
+z_height: 0.0009999999999999992
+# This ^ will be overwritten by generating new files.

examples/sampling_c3/anything/parameters/sim_params.yaml

@@ -1,4 +1,5 @@
-object_model: examples/sampling_c3/urdf/anything.sdf  # TODO @bibit what do we want here?
+object_model: examples/sampling_c3/urdf/frame/frame.sdf
+#This ^ will be overwritten by generating new files.
 
 franka_publish_rate: 1000
 object_publish_rate: 10
@@ -11,4 +12,5 @@ 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]
+q_init_object: [1.0, 0, 0, 0, 0.5, 0, -0.009000000000000001]
+# This ^ will be overwritten by generating new files.

examples/sampling_c3/anything/parameters/vis_params.yaml

@@ -1,5 +1,6 @@
 ee_vis_model: examples/sampling_c3/urdf/ee_visualization_model.urdf
-object_vis_model: examples/sampling_c3/urdf/anything_control.sdf
+object_vis_model: examples/sampling_c3/urdf/frame/frame.sdf
+# This ^ will be overwritten by generating new files.
 visualizer_publish_rate: 30
 
 camera_pose: [1.5, 0, 0.6]

examples/sampling_c3/generate_samples.cc

@@ -32,7 +32,10 @@ std::vector<Eigen::VectorXd> GenerateSampleStates(
     drake::systems::Context<drake::AutoDiffXd>* context_ad,
     const std::vector<
         std::vector<drake::SortedPair<drake::geometry::GeometryId>>>&
-        contact_geoms) {
+        contact_geoms,
+    std::vector<Face> faces,
+    std::vector<double> face_bins
+      ) {
   // Determine number of samples based on mode.
   int num_samples;
   if (is_doing_c3) {
@@ -49,6 +52,9 @@ std::vector<Eigen::VectorXd> GenerateSampleStates(
   for (int i = 0; i < num_samples; i++) {
     candidate_states[i] = x_lcs;
   }
+  const auto& query_port = plant.get_geometry_query_input_port();
+  const auto& query_object =
+    query_port.template Eval<drake::geometry::QueryObject<double>>(*context);
 
   // Split function calls based on sampling strategy.
   SamplingStrategy strategy = sampling_params.sampling_strategy;
@@ -117,7 +123,18 @@ std::vector<Eigen::VectorXd> GenerateSampleStates(
       } while (sampling_params.filter_samples_for_safety &&
                !IsSampleInWorkspace(candidate_states[i], sampling_c3_options));
     }
-  } else {
+  } else if (strategy == SamplingStrategy::kMeshNormal) {
+    for (int i = 0; i < num_samples; i++){
+      do{
+        candidate_states[i] = MeshNormalSampling(
+          n_q, n_v, n_u, x_lcs, plant, context, plant_ad, context_ad,
+          sampling_params, query_object, faces, face_bins);
+      } while(sampling_params.filter_samples_for_safety &&
+               !IsSampleInWorkspace(candidate_states[i], sampling_c3_options));
+    }
+  }  
+
+  else {
     throw std::runtime_error("Error:  Sampling strategy not recognized.");
   }
   return candidate_states;
@@ -361,6 +378,104 @@ Eigen::Vector3d ShellSampling(
   }
 }
 
+Eigen::VectorXd MeshNormalSampling(
+    const int& n_q, const int& n_v, const int& n_u,
+    const Eigen::VectorXd& x_lcs,
+    drake::multibody::MultibodyPlant<double>& plant,
+    drake::systems::Context<double>* context,
+    drake::multibody::MultibodyPlant<drake::AutoDiffXd>& plant_ad,
+    drake::systems::Context<drake::AutoDiffXd>* context_ad,
+    const SamplingParams& sampling_params,
+    const drake::geometry::QueryObject<double>& query_object,
+    std::vector<Face> faces, std::vector<double> face_bins) {
+  const double buffer_distance = sampling_params.buffer_distance;
+  const double z_height = sampling_params.z_height;
+  const int max_attempts = sampling_params.max_attempts;
+  const double barycentric_bias = sampling_params.barycentric_bias;
+  int attempts = 0;
+  double distance = 0;
+
+  Eigen::VectorXd q_vec = x_lcs.head(n_q);
+  Eigen::Vector3d object_xyz = q_vec.tail(3);
+  double trans_x = object_xyz[0];
+  double trans_y = object_xyz[1];
+  double trans_z = object_xyz[2];
+  Eigen::Quaterniond quat_object(q_vec[n_q - 7], q_vec[n_q - 6], q_vec[n_q - 5],
+                                 q_vec[n_q - 4]);
+  Eigen::Matrix3d R = quat_object.toRotationMatrix();
+  Eigen::Vector3d t(trans_x, trans_y, trans_z);
+
+  double total_area = 0;
+  std::vector<Face> faces_world;  // face vector in world frame
+  faces_world.reserve(faces.size());
+
+  for (int i = 0; i < faces.size(); i++) {
+    Face transformed_face;
+    transformed_face.v[0] = R * faces[i].v[0] + t;
+    transformed_face.v[1] = R * faces[i].v[1] + t;
+    transformed_face.v[2] = R * faces[i].v[2] + t;
+    transformed_face.normal = R * faces[i].normal;
+    transformed_face.area = faces[i].area;
+
+    total_area += transformed_face.area;
+    faces_world.emplace_back(transformed_face);
+  }
+
+  do {
+    // Sample value from total selected area
+    std::mt19937 gen(std::random_device{}());
+    std::uniform_real_distribution<double> dis(0.0, total_area);
+    double target = dis(gen);
+    const Face* selected_face = nullptr;
+
+    // Binary search to find index of selected face (weighted by area)
+    int face_idx = FindBin(face_bins.data(), face_bins.size(), target);
+    selected_face = &faces_world[face_idx];
+
+    // Sample point on selected face
+    std::uniform_real_distribution<double> dis_u(0.0, 1.0);
+    double a = std::pow(dis_u(gen), barycentric_bias);
+    double b = std::pow(dis_u(gen), barycentric_bias);
+    if (a + b > 1.0) {
+      a = 1.0 - a;
+      b = 1.0 - b;
+    }
+
+    // Project normal from point
+    const auto& point_vector = selected_face->v;
+    Eigen::Vector3d sample_point = (1.0 - a - b) * point_vector[0] +
+                                   a * point_vector[1] + b * point_vector[2];
+    Eigen::Vector3d projected_sample_point =
+        sample_point + buffer_distance * selected_face->normal;
+    projected_sample_point[2] = z_height;
+
+    Eigen::VectorXd candidate_state = Eigen::VectorXd::Zero(n_q + n_v);
+    candidate_state.segment(0, 3) = projected_sample_point;  // ee position
+    candidate_state.segment(3, 7) =
+        x_lcs.segment(3, 7);  // object orientation/position
+
+    UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad,
+                  candidate_state);
+
+    // Check distance from mesh
+    const auto& results = query_object.ComputeSignedDistanceToPoint(
+        candidate_state.segment(0, 3));
+    distance = results[2].distance;  // index 2 = body_volume
+
+    bool in_collision =
+        (distance <= sampling_params.sample_projection_clearance);
+
+    if (!in_collision) {
+      UpdateContext(n_q, n_v, n_u, plant, context, plant_ad, context_ad,
+                    candidate_state);
+      return candidate_state;
+    }
+    ++attempts;
+  } while (attempts < max_attempts);
+  throw std::runtime_error("Failed to generate a valid sample after " +
+                           std::to_string(max_attempts) + " attempts.");
+}
+
 bool IsSampleInWorkspace(const Eigen::VectorXd& candidate_state,
                          const SamplingC3Options& sampling_c3_options) {
   double candidate_radius =

examples/sampling_c3/generate_samples.h

@@ -11,14 +11,16 @@
 #include "multibody/geom_geom_collider.h"
 #include "multibody/multibody_utils.h"
 #include "solvers/c3_options.h"
+#include <drake/geometry/query_object.h>
+#include "systems/controllers/face.h"
 
 using Eigen::Vector3d;
 using Eigen::VectorXd;
+using dairlib::systems::Face;
 
 namespace dairlib {
 namespace systems {
 
-
 /// Public function
 std::vector<Eigen::VectorXd> GenerateSampleStates(
     const int& n_q,
@@ -34,7 +36,9 @@ std::vector<Eigen::VectorXd> GenerateSampleStates(
     drake::systems::Context<drake::AutoDiffXd>* context_ad,
     const std::vector<
         std::vector<drake::SortedPair<drake::geometry::GeometryId>>>&
-        contact_geoms
+        contact_geoms,
+    std::vector<Face> faces,
+    std::vector<double> face_bins
 );
 
 /// Individual sampling strategies returning 3D EE position
@@ -101,8 +105,19 @@ Eigen::Vector3d ShellSampling(
     const SamplingC3Options sampling_c3_options
 );
 
-
-/// Helper functions
+Eigen::VectorXd MeshNormalSampling(
+    const int& n_q,
+    const int& n_v,
+    const int& n_u,
+    const Eigen::VectorXd& x_lcs,
+    drake::multibody::MultibodyPlant<double>& plant, 
+    drake::systems::Context<double>* context, 
+    drake::multibody::MultibodyPlant<drake::AutoDiffXd>& plant_ad,
+    drake::systems::Context<drake::AutoDiffXd>* context_ad,
+    const SamplingParams& sampling_params,
+    const drake::geometry::QueryObject<double>& query_object,
+    std::vector<Face> faces,
+    std::vector<double> face_bins);
 
 /// Whether the candidate state's EE location is within the robot workspace.
 bool IsSampleInWorkspace(

examples/sampling_c3/jacktoy/parameters/sampling_c3_controller_params.yaml

@@ -17,6 +17,7 @@ lcm_channels_simulation_file: examples/sampling_c3/shared_parameters/lcm_channel
 # jack model.  This works because capsule_1's origin is the same as the jack's.
 object_model: examples/sampling_c3/urdf/jack_control.sdf
 object_body_name: capsule_1
+base_name: dummy
 
 include_end_effector_orientation: false