Skip to content

Commit 7383979

Browse files
clarkdykangmeta-codesync[bot]
authored andcommitted
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: d11b64a95db4c149ae39484f0bf0a614fecd95c2
1 parent b025d55 commit 7383979

File tree

4 files changed

+857
-0
lines changed

4 files changed

+857
-0
lines changed

cmake/build_variables.bzl

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -227,6 +227,7 @@ character_solver_public_headers = [
227227
"character_solver/gauss_newton_solver_qr.h",
228228
"character_solver/height_error_function.h",
229229
"character_solver/joint_to_joint_distance_error_function.h",
230+
"character_solver/joint_to_joint_position_error_function.h",
230231
"character_solver/limit_error_function.h",
231232
"character_solver/model_parameters_error_function.h",
232233
"character_solver/normal_error_function.h",
@@ -258,6 +259,7 @@ character_solver_sources = [
258259
"character_solver/gauss_newton_solver_qr.cpp",
259260
"character_solver/height_error_function.cpp",
260261
"character_solver/joint_to_joint_distance_error_function.cpp",
262+
"character_solver/joint_to_joint_position_error_function.cpp",
261263
"character_solver/limit_error_function.cpp",
262264
"character_solver/model_parameters_error_function.cpp",
263265
"character_solver/normal_error_function.cpp",

0 commit comments

Comments
 (0)