Sigma inverse edits#430
Open
kshaji3 wants to merge 13 commits into
Open
Conversation
dellaert
reviewed
Jun 30, 2026
|
|
||
| /// 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; |
Member
There was a problem hiding this comment.
Sus: why string? Joint key not known?
| // 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()); |
Member
There was a problem hiding this comment.
You have the key: string is slow
Collaborator
Author
There was a problem hiding this comment.
Fair, I'll clean it up. I'll use the jid instead.
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); |
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: