Fix position limits oink constraint for models with continuous joints (#147)

Author: sea-bass

roboplan/src/core/scene.cpp

@@ -138,13 +138,39 @@ Scene::Scene(const std::string& name, const std::string& urdf, const std::string
     const auto& mimicked_joint_name = mimic_model.names[mimicked_idx];
 
     auto* mimic_joint = boost::get<pinocchio::JointModelMimic>(&mimicking_joint);
-    const auto mimicked_joint_type = joint_info_map_.at(mimicked_joint_name).type;
-    auto info = JointInfo(mimicked_joint_type);
+    const auto mimicked_joint_info = joint_info_map_.at(mimicked_joint_name);
+    auto info = JointInfo(mimicked_joint_info.type);
     info.mimic_info = JointMimicInfo{
         .mimicked_joint_name = mimicked_joint_name,
         .scaling = mimic_joint->scaling(),
         .offset = mimic_joint->offset(),
     };
+
+    // Compute derived joint limits.
+    // If the scaling is negative, the position limits have to be swapped.
+    if (mimic_joint->scaling() > 0.0) {
+      info.limits.min_position =
+          (mimicked_joint_info.limits.min_position.array() * mimic_joint->scaling() +
+           mimic_joint->offset())
+              .matrix();
+      info.limits.max_position =
+          (mimicked_joint_info.limits.max_position.array() * mimic_joint->scaling() +
+           mimic_joint->offset())
+              .matrix();
+    } else {
+      info.limits.min_position =
+          (mimicked_joint_info.limits.max_position.array() * mimic_joint->scaling() +
+           mimic_joint->offset())
+              .matrix();
+      info.limits.max_position =
+          (mimicked_joint_info.limits.min_position.array() * mimic_joint->scaling() +
+           mimic_joint->offset())
+              .matrix();
+    }
+    const auto scaling_abs = std::abs(mimic_joint->scaling());
+    info.limits.max_velocity = mimicked_joint_info.limits.max_velocity * scaling_abs;
+    info.limits.max_acceleration = mimicked_joint_info.limits.max_acceleration * scaling_abs;
+    info.limits.max_jerk = mimicked_joint_info.limits.max_jerk * scaling_abs;
     joint_info_map_.emplace(mimicking_joint_name, info);
   }
 

roboplan_oink/include/roboplan_oink/constraints/position_limit.hpp

@@ -34,6 +34,8 @@ struct PositionLimit : public Constraints {
 
   double config_limit_gain;             /// Gain parameter for steering away from limits
   int num_variables;                    /// Number of variables (cached from model.nv)
+  mutable Eigen::VectorXd q_max;        /// Pre-allocated maximum joint position limits.
+  mutable Eigen::VectorXd q_min;        /// Pre-allocated minimum joint position limits.
   mutable Eigen::VectorXd delta_q_max;  /// Pre-allocated workspace for max joint deltas
   mutable Eigen::VectorXd delta_q_min;  /// Pre-allocated workspace for min joint deltas
 };

roboplan_oink/src/constraints/position_limit.cpp

@@ -1,6 +1,7 @@
 #include <roboplan_oink/constraints/position_limit.hpp>
 
 #include <OsqpEigen/OsqpEigen.h>
+#include <roboplan/core/scene_utils.hpp>
 
 namespace roboplan {
 
@@ -16,6 +17,50 @@ tl::expected<void, std::string> PositionLimit::computeQpConstraints(
   const auto& model = scene.getModel();
   const auto& q = scene.getCurrentJointPositions();
 
+  auto maybe_q_collapsed = collapseContinuousJointPositions(scene, "", q);
+  if (!maybe_q_collapsed) {
+    return tl::make_unexpected("Failed to compute position constraint: " +
+                               maybe_q_collapsed.error());
+  }
+  const auto& q_collapsed = maybe_q_collapsed.value();
+
+  // Get joint limits from the model (only do this once)
+  if (q_min.size() == 0u) {
+    q_min.resize(num_variables);
+    q_max.resize(num_variables);
+    const auto joint_names = scene.getJointNames();
+    for (int idx = 0; idx < num_variables; ++idx) {
+      const auto& joint_name = joint_names.at(idx);
+      const auto maybe_joint_info = scene.getJointInfo(joint_name);
+      if (!maybe_joint_info) {
+        return tl::make_unexpected("Failed to get joint limits for position constraint: " +
+                                   maybe_joint_info.error());
+      }
+      const auto& joint_info = maybe_joint_info.value();
+
+      switch (joint_info.type) {
+      case JointType::FLOATING:
+      case JointType::PLANAR:
+        return tl::make_unexpected("Multi-DOF joints not yet supported by position constraints.");
+      case JointType::CONTINUOUS:
+        q_min(idx) = -std::numeric_limits<double>::infinity();
+        q_max(idx) = std::numeric_limits<double>::infinity();
+        break;
+      default:
+        if (joint_info.limits.min_position.size() == 0) {
+          q_min(idx) = -std::numeric_limits<double>::infinity();
+        } else {
+          q_min(idx) = joint_info.limits.min_position(0);
+        }
+        if (joint_info.limits.max_position.size() == 0) {
+          q_max(idx) = std::numeric_limits<double>::infinity();
+        } else {
+          q_max(idx) = joint_info.limits.max_position(0);
+        }
+      }
+    }
+  }
+
   // Validate that model dimensions match constructor
   if (model.nv != num_variables) {
     return tl::make_unexpected("PositionLimit: model.nv (" + std::to_string(model.nv) +
@@ -42,23 +87,19 @@ tl::expected<void, std::string> PositionLimit::computeQpConstraints(
                                std::to_string(upper_bounds.size()));
   }
 
-  // Get joint limits from the model
-  const Eigen::VectorXd& q_min = model.lowerPositionLimit;
-  const Eigen::VectorXd& q_max = model.upperPositionLimit;
-
   // Assuming single DOF joints (revolute/prismatic), nq == nv
   // Compute distances to limits and scale by gain, then write to bounds
   for (int i = 0; i < num_variables; ++i) {
     // Compute distance to upper limit
     if (std::isfinite(q_max(i))) {
-      delta_q_max(i) = q_max(i) - q(i);
+      delta_q_max(i) = q_max(i) - q_collapsed(i);
     } else {
       delta_q_max(i) = std::numeric_limits<double>::infinity();
     }
 
     // Compute distance to lower limit
     if (std::isfinite(q_min(i))) {
-      delta_q_min(i) = q(i) - q_min(i);
+      delta_q_min(i) = q_collapsed(i) - q_min(i);
     } else {
       delta_q_min(i) = std::numeric_limits<double>::infinity();
     }

roboplan_oink/test/constraints/test_position_limit.cpp

@@ -393,4 +393,65 @@ TEST_F(PositionLimitTest, DifferentConfigurationsDifferentBounds) {
   EXPECT_FALSE(lower_bounds1.isApprox(lower_bounds2));
 }
 
+class KinovaPositionLimitTest : public ::testing::Test {
+protected:
+  void SetUp() override {
+    // Use Kinova robot here, since it has continuous and mimic joints.
+    const auto model_prefix = example_models::get_package_models_dir();
+    urdf_path_ = model_prefix / "kinova_robot_model" / "kinova_robotiq.urdf";
+    srdf_path_ = model_prefix / "kinova_robot_model" / "kinova_robotiq.srdf";
+    package_paths_ = {example_models::get_package_share_dir()};
+    yaml_config_path_ = model_prefix / "kinova_robot_model" / "kinova_robotiq_config.yaml";
+
+    scene_ = std::make_shared<Scene>("test_scene", urdf_path_, srdf_path_, package_paths_,
+                                     yaml_config_path_);
+
+    const auto& model = scene_->getModel();
+    num_variables_ = model.nv;
+
+    // Set initial configuration at the neutral configuration
+    Eigen::VectorXd q = pinocchio::neutral(model);
+    scene_->setJointPositions(q);
+  }
+
+  std::filesystem::path urdf_path_;
+  std::filesystem::path srdf_path_;
+  std::vector<std::filesystem::path> package_paths_;
+  std::filesystem::path yaml_config_path_;
+  std::shared_ptr<Scene> scene_;
+  int num_variables_;
+};
+
+// Test bounds at neutral configuration
+TEST_F(KinovaPositionLimitTest, BoundsAtNeutral) {
+  const auto& model = scene_->getModel();
+
+  // Set configuration at the center of joint limits
+  Eigen::VectorXd q = pinocchio::neutral(model);
+  scene_->setJointPositions(q);
+
+  PositionLimit constraint(num_variables_, 1.0);
+
+  Eigen::MatrixXd constraint_matrix(num_variables_, num_variables_);
+  Eigen::VectorXd lower_bounds(num_variables_);
+  Eigen::VectorXd upper_bounds(num_variables_);
+
+  ASSERT_TRUE(
+      constraint.computeQpConstraints(*scene_, constraint_matrix, lower_bounds, upper_bounds)
+          .has_value());
+
+  for (int i = 0; i < num_variables_; ++i) {
+    if (i == 0 || i == 2 || i == 4 || i == 6) {
+      // Joints 1, 3, 5, and 7 are continuous so should have (virtually) infinite limits.
+      ASSERT_LE(lower_bounds[i], -1.0e30);
+      ASSERT_GE(upper_bounds[i], 1.0e30);
+    } else {
+      // Joints 2, 3, 4, and all the gripper joints are revolute,
+      // so they should have finite limits.
+      ASSERT_GE(lower_bounds[i], -3.0);
+      ASSERT_LE(upper_bounds[i], 3.0);
+    }
+  }
+}
+
 }  // namespace roboplan