Adding only c3 specific files - part 1 of 3 of c3_merge

lcmtypes/lcmt_c3_forces.lcm

@@ -0,0 +1,11 @@
+package dairlib;
+
+/*  lcmtype containing information to visualize forces in meshcat
+*/
+struct lcmt_c3_forces
+{
+  int64_t utime;
+
+  int32_t num_forces;
+  lcmt_force forces[num_forces];
+}

lcmtypes/lcmt_c3_intermediates.lcm

@@ -0,0 +1,12 @@
+package dairlib;
+
+struct lcmt_c3_intermediates
+{
+  int32_t num_points;
+  int32_t num_total_variables;
+
+  float time_vec[num_points];
+  float z_sol[num_total_variables][num_points];
+  float delta_sol[num_total_variables][num_points];
+  float w_sol[num_total_variables][num_points];
+}

lcmtypes/lcmt_c3_output.lcm

@@ -0,0 +1,9 @@
+package dairlib;
+
+struct lcmt_c3_output
+{
+  int64_t utime;
+
+  lcmt_c3_solution c3_solution;
+  lcmt_c3_intermediates c3_intermediates;
+}

lcmtypes/lcmt_c3_solution.lcm

@@ -0,0 +1,14 @@
+package dairlib;
+
+struct lcmt_c3_solution
+{
+  int32_t num_points;
+  int32_t num_state_variables;
+  int32_t num_contact_variables;
+  int32_t num_input_variables;
+
+  float time_vec[num_points];
+  float x_sol[num_state_variables][num_points];
+  float lambda_sol[num_contact_variables][num_points];
+  float u_sol[num_input_variables][num_points];
+}

lcmtypes/lcmt_c3_state.lcm

@@ -0,0 +1,10 @@
+package dairlib;
+
+struct lcmt_c3_state
+{
+  int64_t utime;
+  int32_t num_states;
+
+  float state [num_states];
+  string state_names [num_states];
+}

lcmtypes/lcmt_force.lcm

@@ -0,0 +1,12 @@
+package dairlib;
+
+/*  lcmtype containing information to visualize forces in meshcat
+*/
+struct lcmt_force
+{
+  int64_t utime;
+
+  // These are all expressed in the world frame.
+  double contact_point[3];
+  double contact_force[3];
+}

lcmtypes/lcmt_object_state.lcm

@@ -0,0 +1,16 @@
+package dairlib;
+
+struct lcmt_object_state
+{
+  int64_t utime;
+  string object_name;
+
+  int32_t num_positions;
+  int32_t num_velocities;
+
+  string position_names [num_positions];
+  double position [num_positions];
+
+  string velocity_names [num_velocities];
+  double velocity [num_velocities];
+}

lcmtypes/lcmt_timestamped_saved_traj.lcm

@@ -0,0 +1,10 @@
+package dairlib;
+
+/*  lcmtype analog for LcmTrajectory WITH TIMESTAMP to enable using a
+   LcmSerializer to save/load trajectories
+*/
+
+struct lcmt_timestamped_saved_traj {
+  int64_t utime;
+  lcmt_saved_traj saved_traj;
+}

multibody/geom_geom_collider.cc

@@ -1,17 +1,17 @@
 #include "multibody/geom_geom_collider.h"
-#include "multibody/geom_geom_collider.h"
 
 #include "drake/math/rotation_matrix.h"
 
-using Eigen::Vector3d;
-using Eigen::Matrix;
 using drake::EigenPtr;
 using drake::MatrixX;
+using drake::VectorX;
 using drake::geometry::GeometryId;
 using drake::geometry::SignedDistancePair;
 using drake::multibody::JacobianWrtVariable;
 using drake::multibody::MultibodyPlant;
 using drake::systems::Context;
+using Eigen::Matrix;
+using Eigen::Vector3d;
 
 namespace dairlib {
 namespace multibody {
@@ -22,8 +22,7 @@ GeomGeomCollider<T>::GeomGeomCollider(
     const drake::SortedPair<drake::geometry::GeometryId> geometry_pair)
     : plant_(plant),
       geometry_id_A_(geometry_pair.first()),
-      geometry_id_B_(geometry_pair.second()) {
-}
+      geometry_id_B_(geometry_pair.second()) {}
 
 template <typename T>
 std::pair<T, MatrixX<T>> GeomGeomCollider<T>::Eval(const Context<T>& context,
@@ -35,22 +34,21 @@ template <typename T>
 std::pair<T, MatrixX<T>> GeomGeomCollider<T>::EvalPolytope(
     const Context<T>& context, int num_friction_directions,
     JacobianWrtVariable wrt) {
-
   if (num_friction_directions == 1) {
     throw std::runtime_error(
-      "GeomGeomCollider cannot specificy 1 friction direction unless "
-          "using EvalPlanar.");
+        "GeomGeomCollider cannot specify 1 friction direction unless "
+        "using EvalPlanar.");
   }
 
   // Build friction basis
-  Matrix<double, Eigen::Dynamic, 3> force_basis(
-      2 * num_friction_directions + 1, 3);
+  Matrix<double, Eigen::Dynamic, 3> force_basis(2 * num_friction_directions + 1,
+                                                3);
   force_basis.row(0) << 1, 0, 0;
 
   for (int i = 0; i < num_friction_directions; i++) {
     double theta = (M_PI * i) / num_friction_directions;
-    force_basis.row(2*i + 1) = Vector3d(0, cos(theta), sin(theta));
-    force_basis.row(2*i + 2) = -force_basis.row(2*i + 1);
+    force_basis.row(2 * i + 1) = Vector3d(0, cos(theta), sin(theta));
+    force_basis.row(2 * i + 2) = -force_basis.row(2 * i + 1);
   }
   return DoEval(context, force_basis, wrt);
 }
@@ -62,12 +60,41 @@ std::pair<T, MatrixX<T>> GeomGeomCollider<T>::EvalPlanar(
   return DoEval(context, planar_normal.transpose(), wrt, true);
 }
 
-  // }
+template <typename T>
+std::pair<VectorX<double>, VectorX<double>>
+GeomGeomCollider<T>::CalcWitnessPoints(const Context<double>& context) {
+  const auto& query_port = plant_.get_geometry_query_input_port();
+  const auto& query_object =
+      query_port.template Eval<drake::geometry::QueryObject<T>>(context);
+
+  const SignedDistancePair<T> signed_distance_pair =
+      query_object.ComputeSignedDistancePairClosestPoints(geometry_id_A_,
+                                                          geometry_id_B_);
+  const auto& inspector = query_object.inspector();
+  const auto frame_A_id = inspector.GetFrameId(geometry_id_A_);
+  const auto frame_B_id = inspector.GetFrameId(geometry_id_B_);
+  const auto& frameA = plant_.GetBodyFromFrameId(frame_A_id)->body_frame();
+  const auto& frameB = plant_.GetBodyFromFrameId(frame_B_id)->body_frame();
+
+  const Vector3d& p_ACa =
+      inspector.GetPoseInFrame(geometry_id_A_).template cast<T>() *
+      signed_distance_pair.p_ACa;
+  const Vector3d& p_BCb =
+      inspector.GetPoseInFrame(geometry_id_B_).template cast<T>() *
+      signed_distance_pair.p_BCb;
+  Vector3d p_WCa = Vector3d::Zero();
+  Vector3d p_WCb = Vector3d::Zero();
+  plant_.CalcPointsPositions(context, frameA, p_ACa, plant_.world_frame(),
+                             &p_WCa);
+  plant_.CalcPointsPositions(context, frameB, p_BCb, plant_.world_frame(),
+                             &p_WCb);
+  return std::pair<VectorX<double>, VectorX<double>>(p_WCa, p_WCb);
+}
+
 template <typename T>
 std::pair<T, MatrixX<T>> GeomGeomCollider<T>::DoEval(
     const Context<T>& context, Matrix<double, Eigen::Dynamic, 3> force_basis,
     JacobianWrtVariable wrt, bool planar) {
-
   const auto& query_port = plant_.get_geometry_query_input_port();
   const auto& query_object =
       query_port.template Eval<drake::geometry::QueryObject<T>>(context);
@@ -84,27 +111,25 @@ std::pair<T, MatrixX<T>> GeomGeomCollider<T>::DoEval(
 
   const Vector3d& p_ACa =
       inspector.GetPoseInFrame(geometry_id_A_).template cast<T>() *
-          signed_distance_pair.p_ACa;
+      signed_distance_pair.p_ACa;
   const Vector3d& p_BCb =
       inspector.GetPoseInFrame(geometry_id_B_).template cast<T>() *
-          signed_distance_pair.p_BCb;
-
+      signed_distance_pair.p_BCb;
 
-  int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities() :
-      plant_.num_positions();
+  int n_cols = (wrt == JacobianWrtVariable::kV) ? plant_.num_velocities()
+                                                : plant_.num_positions();
   Matrix<double, 3, Eigen::Dynamic> Jv_WCa(3, n_cols);
   Matrix<double, 3, Eigen::Dynamic> Jv_WCb(3, n_cols);
 
-  plant_.CalcJacobianTranslationalVelocity(context, wrt,
-                                            frameA, p_ACa, plant_.world_frame(),
-                                            plant_.world_frame(), &Jv_WCa);
-  plant_.CalcJacobianTranslationalVelocity(context, wrt,
-                                            frameB, p_BCb, plant_.world_frame(),
-                                            plant_.world_frame(), &Jv_WCb);
+  plant_.CalcJacobianTranslationalVelocity(context, wrt, frameA, p_ACa,
+                                           plant_.world_frame(),
+                                           plant_.world_frame(), &Jv_WCa);
+  plant_.CalcJacobianTranslationalVelocity(context, wrt, frameB, p_BCb,
+                                           plant_.world_frame(),
+                                           plant_.world_frame(), &Jv_WCb);
 
-  const auto& R_WC =
-      drake::math::RotationMatrix<T>::MakeFromOneVector(
-          signed_distance_pair.nhat_BA_W, 0);
+  const auto& R_WC = drake::math::RotationMatrix<T>::MakeFromOneVector(
+      signed_distance_pair.nhat_BA_W, 0);
 
   // if this is a planar problem, then the basis has one row and encodes
   // the planar normal direction.
@@ -116,7 +141,8 @@ std::pair<T, MatrixX<T>> GeomGeomCollider<T>::DoEval(
     force_basis.resize(3, 3);
 
     // First row is the contact normal, projected to the plane
-    force_basis.row(0) = signed_distance_pair.nhat_BA_W -
+    force_basis.row(0) =
+        signed_distance_pair.nhat_BA_W -
         planar_normal * planar_normal.dot(signed_distance_pair.nhat_BA_W);
     force_basis.row(0).normalize();
 
@@ -125,13 +151,12 @@ std::pair<T, MatrixX<T>> GeomGeomCollider<T>::DoEval(
     force_basis.row(1).normalize();
     force_basis.row(2) = -force_basis.row(1);
   }
-    // Standard case
+  // Standard case
   auto J = force_basis * R_WC.matrix().transpose() * (Jv_WCa - Jv_WCb);
   return std::pair<T, MatrixX<T>>(signed_distance_pair.distance, J);
 }
 
 }  // namespace multibody
 }  // namespace dairlib
 
-
 template class dairlib::multibody::GeomGeomCollider<double>;

multibody/geom_geom_collider.h

@@ -31,7 +31,6 @@ class GeomGeomCollider {
       drake::multibody::JacobianWrtVariable wrt =
           drake::multibody::JacobianWrtVariable::kV);
 
-
   /// Calculates the distance and contact frame Jacobian.
   /// Jacobian is ordered [J_n; J_t], and has shape
   ////   (2*num_friction_directions + 1) x (nq or nv), depending
@@ -48,12 +47,10 @@ class GeomGeomCollider {
   /// @param num_friction_directions
   /// @return A pair with <distance as a scalar, J>
   std::pair<T, drake::MatrixX<T>> EvalPolytope(
-      const drake::systems::Context<T>& context,
-      int num_friction_directions,
+      const drake::systems::Context<T>& context, int num_friction_directions,
       drake::multibody::JacobianWrtVariable wrt =
           drake::multibody::JacobianWrtVariable::kV);
 
-
   /// Calculates the distance and contact frame Jacobian for a 2D planar problem
   /// Jacobian is ordered [J_n; +J_t; -J_t], and has shape 3 x (nq).
   /// J_t refers to the (contact_normal x planar_normal) direction
@@ -66,6 +63,15 @@ class GeomGeomCollider {
       drake::multibody::JacobianWrtVariable wrt =
           drake::multibody::JacobianWrtVariable::kV);
 
+  /// Returns the position expressed in the World frame of the closest points on
+  /// each Geometry. The distance between the two points should match the
+  /// distance returned by Eval. Note the order of the points return depend on the
+  /// order that the geometries were added to the MultibodyPlant
+  /// @param context The context for the MultibodyPlant
+  /// @return A pair of positions in sorted order as determined by SortedPair
+  std::pair<drake::VectorX<double>, drake::VectorX<double>> CalcWitnessPoints(
+      const drake::systems::Context<double>& context);
+
  private:
   std::pair<T, drake::MatrixX<T>> DoEval(
       const drake::systems::Context<T>& context,