Commit 7383979
Add joint-to-joint position error function to support relative position constraint (#900)
Summary:
Pull Request resolved: #900
This diff adds a new `JointToJointPositionErrorFunction` that constrains the relative position between two joints. Unlike the existing `PositionErrorFunction` which targets world-space positions, this error function expresses constraints in a reference joint's local coordinate frame.
**Motivation:**
This is useful for constraints where you want to control the relative position between two body parts, such as:
- Keeping a hand at a specific position relative to the torso
- Maintaining specific spatial relationships between joints that should move together
- Expressing constraints that are naturally defined in a local reference frame rather than world space
**Implementation:**
The error is computed as:
```
error = w * ||R_ref^T * (p_src - p_ref) - target||²
```
where:
- `p_src = T_src * sourceOffset` is the source point in world coordinates
- `p_ref = T_ref * referenceOffset` is the reference point in world coordinates
- `R_ref` is the rotation of the reference joint
- `target` is the desired relative position in the reference frame
The gradient computation handles three contributions:
1. Source joint chain: positive contribution from `d(srcWorldPos)/dq`
2. Reference joint chain: negative contribution from `d(refWorldPos)/dq`
3. Reference rotation: contribution from `d(R_ref^T)/dq` affecting the frame transformation
Reviewed By: cdtwigg
Differential Revision: D89738249
fbshipit-source-id: d11b64a95db4c149ae39484f0bf0a614fecd95c21 parent b025d55 commit 7383979
File tree
4 files changed
+857
-0
lines changed- cmake
- momentum/character_solver
- pymomentum/solver2
4 files changed
+857
-0
lines changed| Original file line number | Diff line number | Diff line change | |
|---|---|---|---|
| |||
227 | 227 | | |
228 | 228 | | |
229 | 229 | | |
| 230 | + | |
230 | 231 | | |
231 | 232 | | |
232 | 233 | | |
| |||
258 | 259 | | |
259 | 260 | | |
260 | 261 | | |
| 262 | + | |
261 | 263 | | |
262 | 264 | | |
263 | 265 | | |
| |||
0 commit comments