Sigma inverse edits

gtdynamics.i

@@ -374,6 +374,8 @@ class KinematicsParameters : gtdynamics::OptimizationParameters {
                        double bv_cost_model_sigma = 1e-4,
                        double v_cost_model_sigma = 1e-4,
                        double cv_cost_model_sigma = 1e-2);
+  void setJointPriorSigma(string joint_name, double sigma);
+  void setJointLimit(string joint_name, double lower, double upper);
 };
 
 class Kinematics {

gtdynamics/kinematics/KinematicsParameters.h

@@ -15,20 +15,31 @@
 #include <gtdynamics/optimizer/Optimizer.h>
 #include <gtsam/linear/NoiseModel.h>
 
+#include <map>
+#include <string>
+#include <utility>
+
 namespace gtdynamics {
 
 /// Noise models etc specific to Kinematics class
 struct KinematicsParameters : public OptimizationParameters {
   using Isotropic = gtsam::noiseModel::Isotropic;
   gtsam::SharedNoiseModel p_cost_model,  // pose factor
       g_cost_model,                      // goal point
-      prior_q_cost_model,                // joint angle prior factor
+      prior_q_cost_model,                // joint angle prior factor (default)
       bp_cost_model,                     // fixed-link pose prior factor
       cp_cost_model,                     // contact-height factor
       bv_cost_model,                     // fixed-link twist prior factor
       v_cost_model,                      // twist factor
       cv_cost_model;                     // contact-twist factor
 
+  /// Per-joint joint-angle prior models keyed by joint name; overrides
+  /// prior_q_cost_model for listed joints (used by kinematics IK only).
+  std::map<std::string, gtsam::SharedNoiseModel> joint_prior_overrides;
+
+  /// Per-joint {lower, upper} limit overrides by joint name (IK only).
+  std::map<std::string, std::pair<double, double>> joint_limit_overrides;
+
   KinematicsParameters()
       : KinematicsParameters(1e-4, 1e-2, 0.5, 1e-4, 1e-2, 1e-4, 1e-4, 1e-2) {}
 
@@ -68,6 +79,16 @@ struct KinematicsParameters : public OptimizationParameters {
                              Isotropic::Sigma(6, bv_cost_model_sigma),
                              Isotropic::Sigma(6, v_cost_model_sigma),
                              Isotropic::Sigma(3, cv_cost_model_sigma)) {}
+
+  /// Override the joint-angle prior sigma for one joint by name.
+  void setJointPriorSigma(const std::string& joint_name, double sigma) {
+    joint_prior_overrides[joint_name] = Isotropic::Sigma(1, sigma);
+  }
+
+  /// Override the joint-angle limits for one joint by name.
+  void setJointLimit(const std::string& joint_name, double lower, double upper) {
+    joint_limit_overrides[joint_name] = {lower, upper};
+  }
 };
 
 }  // namespace gtdynamics

gtdynamics/kinematics/KinematicsSlice.cpp

@@ -302,11 +302,15 @@ NonlinearFactorGraph Kinematics::jointAngleObjectives<Slice>(
     const Slice& slice, const Robot& robot, const Values& mean) const {
   NonlinearFactorGraph graph;
 
-  // Minimize the joint angles.
+  // Per-joint override from p_.joint_prior_overrides, else prior_q_cost_model.
   for (auto&& joint : robot.joints()) {
     const gtsam::Key key = JointAngleKey(joint->id(), slice.k);
+    auto it = p_.joint_prior_overrides.find(joint->name());
+    const gtsam::SharedNoiseModel cost_model =
+        (it != p_.joint_prior_overrides.end()) ? it->second
+                                               : p_.prior_q_cost_model;
     graph.addPrior<double>(key, (mean.exists(key) ? mean.at<double>(key) : 0.0),
-                           p_.prior_q_cost_model);
+                           cost_model);
   }
 
   return graph;
@@ -317,12 +321,18 @@ NonlinearFactorGraph Kinematics::jointAngleLimits<Slice>(
     const Slice& slice, const Robot& robot) const {
   NonlinearFactorGraph graph;
   for (auto&& joint : robot.joints()) {
-    graph.add(JointLimitFactor(
-        JointAngleKey(joint->id(), slice.k),
-        gtsam::noiseModel::Isotropic::Sigma(1, 0.001),
-        joint->parameters().scalar_limits.value_lower_limit,
-        joint->parameters().scalar_limits.value_upper_limit,
-        0.04));  // joint->parameters().scalar_limits.value_limit_threshold));
+    // Override URDF limits for joints listed in p_.joint_limit_overrides.
+    auto it = p_.joint_limit_overrides.find(joint->name());
+    const auto& limits = joint->parameters().scalar_limits;
+    const double lower =
+        (it != p_.joint_limit_overrides.end()) ? it->second.first
+                                               : limits.value_lower_limit;
+    const double upper =
+        (it != p_.joint_limit_overrides.end()) ? it->second.second
+                                               : limits.value_upper_limit;
+    graph.add(JointLimitFactor(JointAngleKey(joint->id(), slice.k),
+                               gtsam::noiseModel::Isotropic::Sigma(1, 0.001),
+                               lower, upper, 0.04));
   }
   return graph;
 }

gtdynamics/kinematics/tests/testKinematicsSlice.cpp

@@ -422,6 +422,54 @@ TEST(Slice, BarLabPoseConstraintIK) {
                       result.at<Pose3>(PoseKey(base_link->id(), k)), tol));
 }
 
+// Higher gantry prior sigmas let the gantry absorb more of the motion to goal.
+TEST(Slice, BarLabGantryPriorSigma) {
+  using gtsam::Pose3;
+  using gtsam::Values;
+
+  const Robot robot =
+      CreateRobotFromFile(kUrdfPath + std::string("bar_lab.urdf"))
+          .fixLink("columns");
+  auto ee_link = robot.link("robot1_link_6");
+  const auto ee_id = ee_link->id();
+  const std::vector<std::string> gantry_joints = {
+      "bridge1_joint_EA_X", "robot1_joint_EA_Y", "robot1_joint_EA_Z"};
+
+  // Reachable goal from a config that uses the gantry, so there is redundancy.
+  Kinematics goal_kinematics;
+  Values fk = goal_kinematics.initialValues(kSlice, robot, 0.0,
+                                            barLabConfig(robot), true);
+  const Pose3 wTcom_goal = fk.at<Pose3>(PoseKey(ee_id, k));
+  PoseGoals pose_goals = {{k, PoseGoal(ee_link, Pose3(), wTcom_goal)}};
+
+  // Solve IK with the given gantry prior sigma; pose goal is a hard constraint.
+  auto solve = [&](double gantry_sigma) {
+    KinematicsParameters params;
+    params.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN;
+    for (const auto& name : gantry_joints)
+      params.setJointPriorSigma(name, gantry_sigma);
+    return Kinematics(params).inverse(kSlice, robot, pose_goals, gtsam::Values(),
+                                      /*pose_goals_as_constraints=*/true);
+  };
+
+  // Total absolute travel of the gantry joints from their zero prior mean.
+  auto gantry_travel = [&](const Values& result) {
+    double travel = 0.0;
+    for (const auto& name : gantry_joints)
+      travel += std::fabs(
+          result.at<double>(JointAngleKey(robot.joint(name)->id(), k)));
+    return travel;
+  };
+
+  const Values tight = solve(0.1);
+  const Values loose = solve(10.0);
+
+  // Both reach the goal pose; the looser gantry priors move the gantry more.
+  EXPECT(assert_equal(wTcom_goal, tight.at<Pose3>(PoseKey(ee_id, k)), 1e-3));
+  EXPECT(assert_equal(wTcom_goal, loose.at<Pose3>(PoseKey(ee_id, k)), 1e-3));
+  EXPECT(gantry_travel(tight) < gantry_travel(loose));
+}
+
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);

python/tests/test_forward_inv_kinematics.py

@@ -146,6 +146,44 @@ def test_forward_inv_kinematics(self):
                 got = gtd.JointAngle(result, j.id(), k)
                 self.assertAlmostEqual(got, self.angle_rad[j.name()], delta=1e-2)
 
+    def test_gantry_prior_sigma(self):
+        """Looser gantry prior sigmas let the gantry absorb more of the motion."""
+        k = self.k
+        # Fix the gantry base so the gantry joints lie in the chain to the EE.
+        robot = gtd.CreateRobotFromFile(
+            str(Path(gtd.URDF_PATH) / "bar_lab.urdf")).fixLink("columns")
+        link6 = robot.link("robot1_link_6")
+        gantry_joints = ["bridge1_joint_EA_X", "robot1_joint_EA_Y",
+                         "robot1_joint_EA_Z"]
+
+        # Reachable goal from a config that uses the gantry (so it is redundant).
+        config = {
+            "bridge1_joint_EA_X": 2.0, "robot1_joint_EA_Y": 1.5,
+            "robot1_joint_EA_Z": 0.5, "robot1_joint_1": 0.3,
+            "robot1_joint_2": 0.4, "robot1_joint_3": -0.5,
+            "robot1_joint_4": 0.2, "robot1_joint_5": 0.6, "robot1_joint_6": -0.3,
+        }
+        q = gtsam.Values()
+        for j in robot.joints():
+            gtd.InsertJointAngle(q, j.id(), k, config.get(j.name(), 0.0))
+        fk = robot.forwardKinematics(q, k, "columns")
+        pose_goals = {k: gtd.PoseGoal(link6, gtsam.Pose3(),
+                                      gtd.Pose(fk, link6.id(), k))}
+        slice0 = gtd.Slice(k)
+
+        # Total gantry travel from the zero prior mean for a given gantry sigma.
+        def gantry_travel(gantry_sigma):
+            params = gtd.KinematicsParameters()
+            for name in gantry_joints:
+                params.setJointPriorSigma(name, gantry_sigma)
+            result = gtd.Kinematics(params).inverse(
+                slice0, robot, pose_goals, gtsam.Values(), True)
+            return sum(abs(gtd.JointAngle(result, robot.joint(n).id(), k))
+                       for n in gantry_joints)
+
+        # Looser gantry priors -> the gantry moves more to reach the goal.
+        self.assertLess(gantry_travel(0.1), gantry_travel(10.0))
+