Second sampling C3 example: push T

examples/sampling_c3/franka_sampling_c3_controller.cc

@@ -104,6 +104,8 @@ int DoMain(int argc, char* argv[]) {
 
   // Build the contact pairs based on the demo.
   std::vector<std::vector<SortedPair<GeometryId>>> contact_pairs;
+  std::vector<SortedPair<GeometryId>> ee_contact_pairs;
+  std::vector<SortedPair<GeometryId>> ground_object_contact_pairs;
   std::unordered_map<std::string, drake::geometry::GeometryId> contact_geoms;
 
   // All demos include the end effector and ground.
@@ -117,7 +119,6 @@ int DoMain(int argc, char* argv[]) {
   contact_geoms["GROUND"] = ground_geoms;
   std::vector<SortedPair<GeometryId>> 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 =
@@ -159,18 +160,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<SortedPair<GeometryId>> 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<SortedPair<GeometryId>> 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(
@@ -183,11 +179,50 @@ 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 =
+        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_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"]));
+
+    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"]));
   }
   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<double> builder;
@@ -215,6 +250,10 @@ int DoMain(int argc, char* argv[]) {
     target_generator =
         std::make_unique<systems::SamplingC3GoalGeneratorJacktoy>(
             plant_object, controller_params.goal_params);
+  } else if (FLAGS_demo_name == "push_t") {
+    target_generator =
+        std::make_unique<systems::SamplingC3GoalGeneratorPushT>(
+            plant_object, controller_params.goal_params);
   } else {
     throw std::runtime_error("Unknown --demo_name value: " + FLAGS_demo_name);
   }

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<drake::geometry::QueryObject<double>>(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<const drake::geometry::Sphere*>(&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<double>& plant,
     drake::systems::Context<double>* context,
     drake::multibody::MultibodyPlant<drake::AutoDiffXd>& plant_ad,
     drake::systems::Context<drake::AutoDiffXd>* context_ad,
     const std::vector<
-        std::vector<drake::SortedPair<drake::geometry::GeometryId>>>&
-        contact_geoms,
+      std::vector<drake::SortedPair<drake::geometry::GeometryId>>>&
+      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<double> distances;
-
-  for (int i = 0; i < contact_geoms.at(0).size(); i++) {
-    // Evaluate the distance for each pair
-    SortedPair<GeometryId> pair{(contact_geoms.at(0)).at(i)};
+  for (int i = 0; i < contact_geoms.at(1).size(); i++) {
+    SortedPair<GeometryId> 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,22 +452,22 @@ 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));
-  auto [p_world_contact_a, p_world_contact_b] = collider.CalcWitnessPoints(
+    plant, contact_geoms.at(1).at(min_distance_index));
+  auto [p_world_contact_ee, p_world_contact_obj] = 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;
-  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 +
+  // 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) *
-      a_to_b_normalized;
-  candidate_state.head(3) = p_world_contact_b_clearance;
+      ee_to_obj_normalized;
+  candidate_state.head(3) = p_world_contact_obj_clearance;
   return candidate_state;
 }
 

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<double>& plant,
     drake::systems::Context<double>* context,

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;

examples/sampling_c3/goal_generator.h

@@ -38,6 +38,11 @@ inline const std::vector<Eigen::Quaterniond> 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<Eigen::Quaterniond> 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<double>& object_plant,
+    const SamplingC3GoalParams& goal_params) :
+      SamplingC3GoalGenerator(
+        object_plant, goal_params, kNominalOrientationsPlanar) {}
+};
+
+
 }  // namespace systems
 }  // namespace dairlib

examples/sampling_c3/jacktoy/BUILD.bazel

@@ -10,7 +10,6 @@ cc_library(
     ]),
 )
 
-
 cc_library(
     name = "lcm_channels_jacktoy",
     hdrs = [],

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

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

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]