Add an Approximate Joint Centering Cost to GlobalInerseKinematics

bindings/generated_docstrings/multibody_inverse_kinematics.h

@@ -2215,6 +2215,64 @@ kinematics constraints, with some error. The approach is described in
 Global Inverse Kinematics via Mixed-integer Convex Optimization by
 Hongkai Dai, Gregory Izatt and Russ Tedrake, International Journal of
 Robotics Research, 2019.)""";
+        // Symbol: drake::multibody::GlobalInverseKinematics::AddJointCenteringCost
+        struct /* AddJointCenteringCost */ {
+          // Source: drake/multibody/inverse_kinematics/global_inverse_kinematics.h
+          const char* doc =
+R"""(Adds a cost that penalizes deviation of a revolute joint from a
+specified nominal angle. For the joint connecting the specified body
+to its parent, this method adds a cost that encourages the joint’s
+orientation to align with the nominal rotation nominal_value about its
+revolute axis. The cost is evaluated by comparing the rotation of a
+small set of unit vectors orthogonal to the joint axis, as expressed
+in both the parent and child frames.
+
+Specifically, let R_WB be the rotation matrix of the child body frame
+B in world frame W. R_WP be the rotation matrix of the parent body
+frame P in world frame W. X_CJc and X_PJp be the fixed poses of the
+joint frames in the child and parent body frames, respectively. R(k,
+nominal_value) be the rotation about the joint’s revolute axis by the
+nominal angle. For a small set of unit vectors vᵢ orthogonal to the
+joint axis, the cost penalizes ∑ᵢ ‖ R_WB X_CJc vᵢ − R_WP X_PJp R(k,
+nominal_value) vᵢ ‖ₙ where ‖·‖ₙ denotes either the L1 or L2 norm
+(depending on norm), and the result is scaled by weight. If squared is
+true and norm = 2, the squared 2-norm is used instead.
+
+This cost can be interpreted as a joint centering term that drives the
+joint angle toward its nominal value, independently of the global
+posture. This contrasts with AddPostureCost(), which penalizes
+deviations in body poses from those achieved at a target
+configuration. For approximating a true quadratic joint centering
+cost, L2-squared is the most accurate (and slowest), followed by L2,
+L1, and then the posture cost is the least accurate (but fastest).
+
+Parameter ``body_index``:
+    The index of the child body whose inboard joint will have this
+    centering cost applied.
+
+Parameter ``nominal_value``:
+    The nominal joint angle (in radians). The cost is minimized when
+    the joint’s rotation equals this value.
+
+Parameter ``weight``:
+    The scalar weight applied to this cost.
+
+Parameter ``norm``:
+    Specifies which norm to use in the cost: 1: use an L1 norm on the
+    unit-vector differences. 2: use an L2 norm (optionally squared).
+
+Parameter ``squared``:
+    If true and norm = 2, applies the squared L2 norm cost; otherwise,
+    applies the unsquared version.
+
+Raises:
+    RuntimeError if the body has a floating base, if its joint is not
+    a revolute joint, or if norm is not 1 or 2.
+
+Note:
+    Only revolute joints are currently supported. For floating bodies,
+    use AddPostureCost() instead.)""";
+        } AddJointCenteringCost;
         // Symbol: drake::multibody::GlobalInverseKinematics::AddJointLimitConstraint
         struct /* AddJointLimitConstraint */ {
           // Source: drake/multibody/inverse_kinematics/global_inverse_kinematics.h

bindings/pydrake/multibody/inverse_kinematics_py.cc

@@ -1086,6 +1086,10 @@ PYBIND11_MODULE(inverse_kinematics, m) {
             py::arg("joint_upper_bound"),
             py::arg("linear_constraint_approximation") = false,
             cls_doc.AddJointLimitConstraint.doc)
+        .def("AddJointCenteringCost", &Class::AddJointCenteringCost,
+            py::arg("body_index"), py::arg("desired_theta"),
+            py::arg("weight") = 1.0, py::arg("norm") = 1,
+            py::arg("squared") = false)
         .def("SetInitialGuess", &Class::SetInitialGuess, py::arg("q"),
             cls_doc.SetInitialGuess.doc);
     // TODO(cohnt): Convert methods that use Polytope3D to use ConvexSets.

bindings/pydrake/multibody/test/inverse_kinematics_test.py

@@ -964,8 +964,8 @@ def test_api(self):
             global_ik.ReconstructGeneralizedPositionSolution(result=result)
 
     def test_joint_limits(self):
-        # We need a separate test since the AddJointLimitsConstraint method
-        # is not usable with floating bodies.
+        # We need a separate test since the AddJointLimitsConstraint and
+        # AddJointCenteringCost methods are not usable with floating bodies.
         plant = MultibodyPlant(time_step=0.01)
         model_instance, = Parser(plant).AddModels(FindResourceOrThrow(
             "drake/bindings/pydrake/multibody/test/double_pendulum.sdf"))
@@ -979,3 +979,9 @@ def test_joint_limits(self):
             joint_lower_bound=-10.0,
             joint_upper_bound=10.0,
             linear_constraint_approximation=False)
+        global_ik.AddJointCenteringCost(
+            body_index=body_index,
+            desired_theta=0.0,
+            weight=1.0,
+            norm=1,
+            squared=False)

multibody/inverse_kinematics/global_inverse_kinematics.cc

@@ -7,6 +7,7 @@
 
 #include "drake/common/eigen_types.h"
 #include "drake/common/scope_exit.h"
+#include "drake/common/symbolic/decompose.h"
 #include "drake/math/roll_pitch_yaw.h"
 #include "drake/math/rotation_matrix.h"
 #include "drake/multibody/tree/revolute_joint.h"
@@ -19,6 +20,7 @@ using Eigen::Vector3d;
 using std::string;
 
 using drake::math::RigidTransformd;
+using drake::math::RotationMatrixd;
 using drake::solvers::VectorDecisionVariable;
 using drake::symbolic::Expression;
 
@@ -610,6 +612,35 @@ void ApproximateBoundedNormByLinearConstraints(
   prog->AddLinearConstraint(x(0) - x(1) + x(2), -sqrt3_c, sqrt3_c);
   prog->AddLinearConstraint(x(0) - x(1) - x(2), -sqrt3_c, sqrt3_c);
 }
+}  // namespace
+namespace {
+
+// Helper struct containing precomputed quantities shared by
+// AddJointLimitConstraint() and AddJointCenteringCost().
+struct RevoluteSamples {
+  std::array<Eigen::Vector3d, 4> v_samples;
+  Eigen::Matrix3d rotmat_joint_offset;
+};
+
+// Helper to compute v_samples and rotmat_joint_offset for a revolute joint.
+RevoluteSamples ComputeRevoluteSamples(const Eigen::Vector3d& axis_F,
+                                       double angle_offset) {
+  const auto R_F =
+      RotationMatrixd::MakeFromOneUnitVector(axis_F, /* axis_index */ 2);
+
+  std::array<Eigen::Vector3d, 4> v_samples;
+  v_samples[0] = R_F.col(0);
+  v_samples[1] = R_F.col(1);
+  v_samples[2] = (R_F.col(0) + R_F.col(1)).normalized();
+  v_samples[3] = (R_F.col(0) - R_F.col(1)).normalized();
+
+  // rotmat_joint_offset is R(k, angle_offset) explained above.
+  const Matrix3d rotmat_joint_offset =
+      Eigen::AngleAxisd(angle_offset, axis_F).toRotationMatrix();
+
+  return {v_samples, rotmat_joint_offset};
+}
+
 }  // namespace
 
 void GlobalInverseKinematics::AddJointLimitConstraint(
@@ -638,10 +669,10 @@ void GlobalInverseKinematics::AddJointLimitConstraint(
   }
   const RigidBody<double>& parent = joint->parent_body();
   const int parent_idx = parent.index();
-  const RigidTransformd X_PJp =
-      joint->frame_on_parent().GetFixedPoseInBodyFrame();
-  const RigidTransformd X_CJc =
-      joint->frame_on_child().GetFixedPoseInBodyFrame();
+  const RotationMatrixd R_PJp =
+      joint->frame_on_parent().GetFixedPoseInBodyFrame().rotation();
+  const RotationMatrixd R_CJc =
+      joint->frame_on_child().GetFixedPoseInBodyFrame().rotation();
   switch (joint->num_velocities()) {
     case 0: {
       // Fixed to the parent body.
@@ -710,62 +741,31 @@ void GlobalInverseKinematics::AddJointLimitConstraint(
             // |R_WC*R_CJc*v - R_WP * R_PJp * R(k,(a+b)/2)*v | <= 2*sin (α / 2)
             // as we explained above.
 
-            // First generate a vector v_C that is perpendicular to rotation
-            // axis, in child frame.
-            Vector3d v_C = axis_F.cross(Vector3d(1, 0, 0));
-            double v_C_norm = v_C.norm();
-            if (v_C_norm < sqrt(2) / 2) {
-              // axis_F is almost parallel to [1; 0; 0]. Try another axis
-              // [0, 1, 0]
-              v_C = axis_F.cross(Vector3d(0, 1, 0));
-              v_C_norm = v_C.norm();
-            }
-            // Normalizes the revolute vector.
-            v_C /= v_C_norm;
-
-            // The constraint would be tighter, if we choose many unit
-            // length vector `v`, perpendicular to the joint axis, in the
-            // joint frame. Here to balance between the size of the
-            // optimization problem, and the tightness of the convex
-            // relaxation, we just use four vectors in `v`. Notice that
-            // v_basis contains the orthonormal basis of the null space
-            // null(axis_F).
-            std::array<Eigen::Vector3d, 2> v_basis = {{v_C, axis_F.cross(v_C)}};
-            v_basis[1] /= v_basis[1].norm();
-
-            std::array<Eigen::Vector3d, 4> v_samples;
-            v_samples[0] = v_basis[0];
-            v_samples[1] = v_basis[1];
-            v_samples[2] = v_basis[0] + v_basis[1];
-            v_samples[2] /= v_samples[2].norm();
-            v_samples[3] = v_basis[0] - v_basis[1];
-            v_samples[3] /= v_samples[3].norm();
-
-            // rotmat_joint_offset is R(k, (a+b)/2) explained above.
-            const Matrix3d rotmat_joint_offset =
-                Eigen::AngleAxisd((joint_lower_bounds_[position_idx] +
-                                   joint_upper_bounds_[position_idx]) /
-                                      2,
-                                  axis_F)
-                    .toRotationMatrix();
-
-            // joint_limit_expr is going to be within the Lorentz cone.
+            const double offset_angle = (joint_lower_bounds_[position_idx] +
+                                         joint_upper_bounds_[position_idx]) /
+                                        2;
+
+            const RevoluteSamples joint_samples =
+                ComputeRevoluteSamples(axis_F, offset_angle);
+            const Eigen::Matrix3d rotmat_joint_offset =
+                joint_samples.rotmat_joint_offset;
+            const std::array<Eigen::Vector3d, 4>& v_samples =
+                joint_samples.v_samples;
+
             Eigen::Matrix<Expression, 4, 1> joint_limit_expr;
             const double joint_limit_lorentz_rhs = 2 * sin(joint_bound / 2);
             joint_limit_expr(0) = joint_limit_lorentz_rhs;
-            for (const auto& v : v_samples) {
+            for (const Vector3d& v : v_samples) {
               // joint_limit_expr.tail<3> is
               // R_WC * R_CJc * v - R_WP * R_PJp * R(k,(a+b)/2) * v mentioned
               // above.
               joint_limit_expr.tail<3>() =
-                  R_WB_[body_index] * X_CJc.rotation().matrix() * v -
-                  R_WB_[parent_idx] * X_PJp.rotation().matrix() *
-                      rotmat_joint_offset * v;
+                  R_WB_[body_index] * R_CJc.matrix() * v -
+                  R_WB_[parent_idx] * R_PJp.matrix() * rotmat_joint_offset * v;
               if (linear_constraint_approximation) {
                 ApproximateBoundedNormByLinearConstraints(
                     joint_limit_expr.tail<3>(), joint_limit_lorentz_rhs,
                     &prog_);
-
               } else {
                 prog_.AddLorentzConeConstraint(joint_limit_expr);
               }
@@ -808,12 +808,14 @@ void GlobalInverseKinematics::AddJointLimitConstraint(
               // [M(0, 0), M(1, 1), M(1, 0)] is in the rotated Lorentz cone.
               // R_joint_beta is R(k, β) in the documentation.
               Eigen::Matrix<symbolic::Expression, 3, 3> R_joint_beta =
-                  (X_WP.rotation().matrix() * X_PJp.rotation().matrix() *
+                  (X_WP.rotation().matrix() * R_PJp.matrix() *
                    rotmat_joint_offset)
                       .transpose() *
-                  R_WB_[body_index] * X_CJc.rotation().matrix();
+                  R_WB_[body_index] * R_CJc.matrix();
               const double joint_bound_cos{std::cos(joint_bound)};
               if (!linear_constraint_approximation) {
+                std::array<Eigen::Vector3d, 2> v_basis = {
+                    {v_samples[0], (v_samples[0].cross(axis_F)).normalized()}};
                 Eigen::Matrix<double, 3, 2> V;
                 V << v_basis[0], v_basis[1];
                 const Eigen::Matrix<symbolic::Expression, 2, 2> M =
@@ -823,7 +825,6 @@ void GlobalInverseKinematics::AddJointLimitConstraint(
                 prog_.AddRotatedLorentzConeConstraint(
                     Vector3<symbolic::Expression>(M(0, 0), M(1, 1), M(1, 0)));
               }
-
               // From Rodriguez formula, we know that -α <= β <= α implies
               // trace(R(k, β)) = 1 + 2 * cos(β) >= 1 + 2*cos(α)
               // So we can impose the constraint
@@ -849,6 +850,100 @@ void GlobalInverseKinematics::AddJointLimitConstraint(
   }
 }
 
+void GlobalInverseKinematics::AddJointCenteringCost(BodyIndex body_index,
+                                                    double desired_theta,
+                                                    double weight, int norm,
+                                                    bool squared) {
+  // Validate arguments: norm parameters and body index.
+  DRAKE_THROW_UNLESS(norm == 1 || norm == 2);
+  DRAKE_THROW_UNLESS(norm == 2 || squared == false);
+  DRAKE_THROW_UNLESS(body_index >= 0 && body_index < plant_.num_bodies() &&
+                     body_index != plant_.world_body().index());
+
+  // Identify joint to parent.
+  const Joint<double>* joint_ptr{nullptr};
+  for (JointIndex joint_index : plant_.GetJointIndices()) {
+    if (plant_.get_joint(joint_index).child_body().index() == body_index) {
+      joint_ptr = &(plant_.get_joint(joint_index));
+      break;
+    }
+  }
+
+  // Confirm the joint is of supported type (right now, only Revolute).
+  if (joint_ptr->type_name() != "revolute") {
+    throw std::runtime_error(fmt::format(
+        "AddJointCenteringCost() can only be invoked on a body that is "
+        "connected to its parent by a revolute joint. The body '{}' ({}) "
+        "connects via a {} joint.",
+        joint_ptr->child_body().name(), body_index, joint_ptr->type_name()));
+  }
+
+  const auto& joint = *static_cast<const RevoluteJoint<double>*>(joint_ptr);
+
+  const RigidBody<double>& parent = joint.parent_body();
+  const int parent_idx = parent.index();
+
+  const RotationMatrixd R_PJp =
+      joint.frame_on_parent().GetFixedPoseInBodyFrame().rotation();
+  const RotationMatrixd R_CJc =
+      joint.frame_on_child().GetFixedPoseInBodyFrame().rotation();
+
+  // The axis is expressed in Jp or Jc (same measures either way).
+  const Vector3d axis_Jc = joint.revolute_axis();
+
+  // Compute shared geometric quantities.
+  const RevoluteSamples joint_samples =
+      ComputeRevoluteSamples(axis_Jc, desired_theta);
+  const Eigen::Matrix3d& R_JpJc_theta = joint_samples.rotmat_joint_offset;
+
+  // These aren't literally RotationMatrix, but they behave like them.
+  const auto& R_WP = R_WB_[parent_idx];
+  const auto& R_WC = R_WB_[body_index];
+  const auto R_WJc = R_WC * R_CJc.matrix();
+  const auto R_WJp_theta = R_WP * (R_PJp.matrix() * R_JpJc_theta);
+
+  for (const Vector3d& v : joint_samples.v_samples) {
+    // v has the same measures in both Jp and Jc frames.
+    const auto v_Jc = v;
+    const auto v_Jc_theta = v;
+    Eigen::Matrix<Expression, 3, 1> unit_vector_difference =
+        weight * (R_WJc * v_Jc - R_WJp_theta * v_Jc_theta);
+
+    if (norm == 1) {
+      for (int i = 0; i < 3; ++i) {
+        auto s = prog_.NewContinuousVariables<1>("s")[0];
+        prog_.AddLinearConstraint(s >= unit_vector_difference[i]);
+        prog_.AddLinearConstraint(s >= -unit_vector_difference[i]);
+        prog_.AddLinearCost(s);
+      }
+    } else if (norm == 2) {
+      // Get the union of all variables in all expressions.
+      symbolic::Variables var_set;
+      for (int i = 0; i < unit_vector_difference.size(); ++i) {
+        var_set += unit_vector_difference[i].GetVariables();
+      }
+      solvers::VectorXDecisionVariable vars(var_set.size());
+      int count = 0;
+      for (const auto& var : var_set) {
+        vars[count++] = var;
+      }
+
+      // Decompose unit_vector_difference as an affine expression Ax+b.
+      Eigen::MatrixXd A(3, vars.size());
+      Eigen::VectorXd b(3);
+      symbolic::DecomposeAffineExpressions(unit_vector_difference, vars, &A,
+                                           &b);
+
+      if (squared) {
+        // Sign flip is necessary since this method adds a cost on Ax-b.
+        prog_.Add2NormSquaredCost(A, -b, vars);
+      } else {
+        prog_.AddL2NormCostUsingConicConstraint(A, b, vars);
+      }
+    }
+  }
+}
+
 // TODO(russt): This method is currently calling Solve in order to compute the
 // integer variables (given the poses). This is an easy, but relatively
 // expensive way; we could instead compute them in closed form based on the