Sigma inverse edits#430
Merged
Merged
Conversation
dellaert
reviewed
Jun 30, 2026
Contributor
There was a problem hiding this comment.
Pull request overview
This PR adds support for overriding the joint-angle prior noise model on a per-joint basis (by joint name) in kinematics inverse-kinematics solves, enabling users to “favor” certain joints by tightening/loosening their prior sigmas. It also adds C++ and Python regression tests demonstrating that looser gantry priors allow the gantry joints to absorb more motion in a redundant IK setup.
Changes:
- Add
KinematicsParameters::joint_prior_overridesplussetJointPriorSigma(joint_name, sigma)to override the defaultprior_q_cost_modelfor specific joints. - Update
Kinematics::jointAngleObjectives<Slice>to use the per-joint override noise model when present. - Add Bar-Lab gantry sigma behavior tests in both C++ and Python.
Reviewed changes
Copilot reviewed 5 out of 5 changed files in this pull request and generated 3 comments.
Show a summary per file
| File | Description |
|---|---|
| python/tests/test_forward_inv_kinematics.py | Adds a Python test validating looser gantry priors increase gantry joint motion (but currently uses randomized initialization). |
| gtdynamics/kinematics/tests/testKinematicsSlice.cpp | Adds a C++ test validating looser gantry priors increase gantry joint motion (but currently uses randomized initialization). |
| gtdynamics/kinematics/KinematicsSlice.cpp | Uses per-joint prior override noise models when building joint-angle prior factors. |
| gtdynamics/kinematics/KinematicsParameters.h | Introduces per-joint prior overrides and a helper setter. |
| gtdynamics.i | Exposes setJointPriorSigma to the wrapper interface. |
Comment on lines
+172
to
+182
| 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) |
Comment on lines
+445
to
+453
| // 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); | ||
| }; |
| 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); |
Member
|
Several co-pilot comments are valid, Please resolve them as you fix them, or deem them irrelevant - be honest though :-). Except for very simple types like float, int etc. always use constant references (const &) in c++. |
Member
|
Also, CI fails :-/ |
dellaert
reviewed
Jul 1, 2026
dellaert
approved these changes
Jul 2, 2026
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Notes: