Adding End Effector SE(3) Targets

[kshaji3]

This PR adds the ability to specify an end effector SE(3) position target (previously it was limited to only position, so this adds orientation) with the goal of being mainly applicable to manipulation platforms (such as that used in https://www.bar-lab.org/).

• Also added a unit test for this based on BAR Lab code in the Python wrapper.

[dellaert]

It's late here, so I won't do a full review tonight, but here is one comment:

com_T_goal = gts.Pose3()              # identity: constrain the link frame itself (set TCP offset here)

1. naming convention we use is to_T_from or, if transforms from frame B -> A, aTb
2. same for rotations to_R_from, aRb
3. always gtsam, never gts :-)

Maybe do a quick pass so I don't stumble on those guys tomorrow :-).

Also, why not convert that Python as a test? That would be a good test for the wrapper.

[kshaji3]

This should have addressed all three of those notes :)

[dellaert]

Still in meetings all day - asked copilot review which will give me over view and possibly actionable comments. Please see conventions in .github/copilot-instructions.md, which it uses to review new code

[Copilot]

Pull request overview

This PR extends GTDynamics’ kinematics goal interface to support full SE(3) end-effector pose targets (including orientation), with an option to enforce pose goals as hard equality constraints, and adds BAR Lab–based test coverage (C++ + Python) plus a new example URDF.

Changes:

• Add SE(3) pose-goal residuals (PoseGoalConstraint) and a corresponding soft cost factor (PoseGoalFactor), and wire pose goals into IK either as soft priors or hard constraints.
• Add BAR Lab manipulator/gantry URDF model and C++ tests exercising pose-goal IK with augmented Lagrangian hard constraints.
• Update Python wrapper + tests to exercise pose-goal IK from the Python API.

Reviewed changes

Copilot reviewed 9 out of 9 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
python/tests/test_forward_kinematics.py	Removed in favor of a consolidated forward/inverse kinematics test file.
python/tests/test_forward_inv_kinematics.py	Adds Python tests for FK factor + pose-goal IK round-trip (but currently has a keying bug in PoseGoals).
models/urdfs/bar_lab.urdf	Adds BAR Lab gantry + manipulator URDF used by new tests.
gtdynamics/kinematics/tests/testKinematicsSlice.cpp	Adds BAR Lab pose-goal IK tests (hard constraints) and a small TODO comment.
gtdynamics/kinematics/tests/testEndEffFactor.cpp	Adds unit tests for the new pose-goal factor/constraint residual and Jacobians.
gtdynamics/kinematics/KinematicsSlice.cpp	Implements pose-goal constraints path and adds pose_goals_as_constraints option for slice IK.
gtdynamics/kinematics/Kinematics.h	Introduces/clarifies PoseGoal semantics (CoM-to-goal offset + world goal) and adds API for pose-goal constraints + IK option flag.
gtdynamics/kinematics/endEffGoalFactor.h	New header defining PoseGoalConstraint expression and PoseGoalFactor.
gtdynamics.i	Exposes PoseGoal, Kinematics::inverse(... PoseGoals ...), and PoseGoalFactor to the Python wrapper.

[dellaert]

LGTM. CI fails, unrelated, but you could maybe fix it as well so board is green again

[dellaert]

Awesome! Please delete the branch after you merge