Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Extend Support for Mimic Joint #2441

Open
wants to merge 276 commits into
base: devel
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
276 commits
Select commit Hold shift + click to select a range
4c1973c
[EtienneAr feedback] Split jointCols jointRows and jointBLock for ful…
Sep 29, 2024
9c0dda9
[URDF] Modified the way mimic joint are added (still broken for romeo…
Sep 29, 2024
4215449
[urdf] Fix parsing of mimic joints
Sep 29, 2024
5148321
[Example] Fix to include mimic
Sep 29, 2024
d197948
[robot_wrapper] Fix to include mimic
Sep 29, 2024
fe5dd30
Merge branch
Sep 29, 2024
c258e46
[RNEA] Modify to take mimic into account
Sep 29, 2024
a728fa3
[Mimic] Modify to be able to mimic from base to Tpl
Sep 29, 2024
4df32c8
[SampleModel] Test to compare a mimic and non mimic model
Sep 29, 2024
ea434f8
[Data] Check on mimic is disable for now
Sep 29, 2024
abe3cbf
Working test
Sep 29, 2024
cc8ef1f
[EtienneAr feedback] Fix RNEA: initialize tau to zero before summing
Sep 29, 2024
0e203ba
[EtienneAr feedback] Fix crba for mimic joints
Sep 29, 2024
a708990
[EtienneAr feedback] Test crba before rnea for mimic joints
Sep 29, 2024
bf62387
[EtienneAr feedback] Port crba fix to algorithm with other convention
Sep 29, 2024
afd4e1f
[EtienneAr feedback] Port crba fix to rnea, compute-all-terms and con…
Sep 29, 2024
425fc0f
[EtienneAr feedback] Revert useless change on Fcrb in crna.hxx
Sep 29, 2024
d0282a4
[EtienneAr feedback] Make M matrix symetric in test
Sep 29, 2024
3190655
[EtienneAr feedback] Properly use data.nle instead of rnea
Sep 29, 2024
c3a09d9
[EtienneAr feedback] Fix crba for mimic joints
Sep 29, 2024
5682ddd
[EtienneAr feedback] Fix RNEA after chage for mimic joints
Sep 29, 2024
99e9953
[example] Change buildModelUrdf - added mimic bool
Sep 29, 2024
ecb907e
[mimic-joint] Added support for FreeFlyer
Sep 29, 2024
85b7dd4
[unittest] Added bool mimic to build urdf
Sep 29, 2024
4f12d96
[Joint] Split Joint ConfigSelector in two functions
Sep 29, 2024
6bce20d
Apply pre-commit
Sep 29, 2024
e8ef6b5
Split JointVelocitySelector in two functions
Sep 29, 2024
0926cb1
Apply pre-commit
Sep 29, 2024
b99cf2a
[EtienneAr feedback] Add more test for joint mimic in non trivial cases
Sep 29, 2024
8c7c1c0
[EtienneAr feedback] Fix scaling and offset in mimic joint calc
Sep 29, 2024
dc589ee
[EtienneAr feedback] WiP: Fix ConfigVectorAffineTransform template sp…
Sep 29, 2024
1c95b2c
[EtienneAr feedback] Make visitor for config vector affine transform
Sep 29, 2024
82b3059
[EtienneAr feedback] Move configVectorAffineTransform code to joint-b…
Sep 29, 2024
9848688
[EtienneAr feedback] Add more test cases for config vector affine tra…
Sep 29, 2024
7887a53
[EtienneAr feedback] Test affine transform for identity and non ident…
Sep 29, 2024
f95b045
[EtienneAr feedback] Cleaner floating point comparison for NoAffineTr…
Sep 29, 2024
5362823
[EtienneAr feedback] keep original q and v in JointDataMimic
Sep 29, 2024
602bbae
[EtienneAr feedback] [Mimic] Set indexes of ref joint to 0
Sep 29, 2024
ceb115f
[EtienneAr feedback] Fix mimic test for slightly new convention of re…
Sep 29, 2024
094eff8
[EtienneAr feedback] Fix serialization for mimic joint with setIndexes
Sep 29, 2024
4f61e82
[bindings] Added mimic to context
Sep 29, 2024
f176fc3
[parser] remove std::cout
Sep 29, 2024
7ae06c9
[bindings] BROKEN - Added specialization for mimic joint in bindings
Sep 29, 2024
3148c10
[python] Added jointModelMimicable to context
Sep 29, 2024
e4f1e22
[bindings] Fix mimic specialization for constructor
Sep 29, 2024
f4a7816
[context] Added mimicable joint
Sep 29, 2024
c9d0bb4
[bindings] Finished specialization for mimic
Sep 29, 2024
be97cd0
{Joints@ Added mimicable trait
Sep 29, 2024
b7563bc
[Visitor] Change TransferVariant visitor into checkMimic visitor
Sep 29, 2024
46886aa
[Joint] Remove mimicable joint collection
Sep 29, 2024
30d8c0d
[Joint] Use check mimic for jointMimic constructor
Sep 29, 2024
3679c50
[bindings] Make bindings conform to new jointMimic
Sep 29, 2024
f4f0a3e
[EtienneAr feedback] Make random humanoid with mimic joint
Sep 29, 2024
5671c7f
[EtienneAr feedback] Fix size in jointBlock
Sep 29, 2024
5a939bb
[EtienneAr feedback] Fix mimic serialization for joint_q joint_v
Sep 29, 2024
dd5a9d8
[EtienneAr feedback] Remove problematic testcase for mimic in python
Sep 29, 2024
8b5aa77
[EtienneAr feedback] Add slightly more test on sample models for mimic
Sep 29, 2024
f2144c7
[EtienneAr feedback] Fix rebase conflict
Sep 29, 2024
5fcaf9b
[EtienneAr feedback] Activate mimic for joint-jacobian test
Sep 29, 2024
2f8e76f
[EtienneAr feedback] Fix some size for mimic col/row selection
Sep 29, 2024
3e76128
[EtienneAr feedback] Fix joint jacobians computation for mimic
Sep 29, 2024
b16c5cc
[EtienneAr feedback] WiP fix crba malloc
Sep 29, 2024
06e64ca
[EtienneAr feedback] Remove temporary variables before return for joi…
Sep 29, 2024
ac93681
[EtienneAr feedback] Add stack allocation feature for JointMotionSubs…
Sep 29, 2024
adbb5d9
[CRBA] Update computation of Ag
Sep 29, 2024
104d118
[EtienneAr feedback] Small serialization fix after changes in JoitMot…
Sep 29, 2024
ec994a3
[EtienneAr feedback] Fix on Jcom for mimic
Sep 29, 2024
f293d7d
[EtienneAr feedback] Fix corriolis computation for mimic leading
Sep 29, 2024
ff7361f
[EtienneAr feedback] Enable mimic in rnea tests
Sep 29, 2024
01eedd7
[EtienneAr feedback] Define MaxDim in ScaledJointMotionSubspace and f…
Sep 29, 2024
e94a114
[EtienneAr feedback] Add dedicated mimic test for crba
Sep 29, 2024
6cfc3da
[EtienneAr feedback] Template ScaledMotionSubspace on MaxDim
Sep 29, 2024
5ef85cc
[EtienneAr feedback] Add getRelativePlacement method for kinematics
Sep 29, 2024
170f847
[EtienneAr feedback] add patch to crba for mimic joints
Sep 29, 2024
392b96c
[EtienneAr feedback] Update comment and remove hardcoded max size for…
Sep 29, 2024
384ea6a
[EtienneAr feedback] Make random humanoid with mimic joint
Sep 29, 2024
0cff961
[algorithm/model] Added a function to transform a joint into a mimic
Sep 29, 2024
2507ddb
[unittest/model] Added test for transformIntoJointMimic
Sep 29, 2024
703c9a0
Apply pre-commit
Sep 29, 2024
80aeb4b
Fix rebase
Sep 29, 2024
2df67e3
[Algorithm/model] Remove useless if
Sep 29, 2024
26be96e
[EtienneAr feedback] Fix crba in local convention to prevent heap all…
Sep 29, 2024
94dddcf
[EtienneAr feedback] Add assert for crba in world convention with mim…
Sep 29, 2024
759c0e6
[EtienneAr feedback] More exhaustive testing of crba for mimic
Sep 29, 2024
b4df55f
[EtienneAr feedback] Remove useless override of joint{Config,Velocity…
Sep 29, 2024
d3ba802
[EtienneAr feedback] Fix selectConfigFromDof for JointModelTpl in the…
Sep 29, 2024
5462657
[EtienneAr feedback] Fix ConfigSelectorFromDof visitor
Sep 29, 2024
09e0537
[EtienneAr feedback] Fix CRBA test for mimic, and remove mimic in wor…
Sep 29, 2024
ed7a8f1
[EtienneAr feedback] Fix G matric for mimic crba test
Sep 29, 2024
fe768ab
[EtienneAr feedback] Patch warnings in mimic crba test
Sep 29, 2024
5523548
[Algorithm/crba] Fix world convention for mimic joints
Sep 29, 2024
36406a9
fix not working for all cases (spaced and parallel)
Sep 29, 2024
be02d76
[algo/crba] Fix for crba world (works for spaced and parallel)
Sep 29, 2024
efeab67
Fix crba world (small issue with parallel joint at the root where the…
Sep 29, 2024
59d7dc7
Fix for world convention
Sep 29, 2024
3baef4c
Make centroidal algorithm compatible with mimic and add tests
Sep 29, 2024
42e4f65
Add mimic tests to rnea
Sep 29, 2024
1169ae9
Add security
Sep 29, 2024
4de03e5
[EtienneAr feedback] Fix getRelativePlacement and add unit test
Sep 29, 2024
f46e8d5
Add security
Sep 29, 2024
f710c40
[EtienneAr feedback] Fix ForceOperator for generic motion subspace an…
Sep 29, 2024
8a63fe6
[EtienneAr feedback] Patch previous fix on ForceOperator for motion s…
Sep 29, 2024
daa1fe4
Fix some errrors for com computation
Sep 29, 2024
ed1fbc9
[Rnea] Patch Algorithms for mimic
Sep 29, 2024
f363dc0
remove mimic joints from test
Sep 29, 2024
83dc188
[unittest/rnea] Add more test
Sep 29, 2024
9f8adbc
Fix tests and remove modification for Coriolis Matrix
Sep 29, 2024
f9d1adb
[rnea] Remove useless setZero
Sep 29, 2024
8b5e681
[Urdf] Rebase on new pinocchio release
Sep 29, 2024
37ae077
Add assert to non supported algorithms
Sep 29, 2024
4279a40
[algo/crba] Fix local convention for crba
Sep 29, 2024
baa0e76
[EtienneAr feedback] Comment local fix for crba and don't use getRela…
Sep 29, 2024
71d8168
[EtienneAr feedback] Add comments to world fix for crba and factorize…
Sep 29, 2024
f5860b2
Update examples
Sep 30, 2024
6b0e0af
[EtienneAr Feedback] Fix LOCAL enum ambiguity in getRelativePlacement
Oct 1, 2024
d15925b
[EtienneAr Feedback]Fix Scalar / int confusion for model index
Oct 1, 2024
47e8662
[EtienneAr Feedback] Fix explicit ConfigVectorAffineTransformVisitor …
Oct 1, 2024
875ca2d
[EtienneAr Feedback] Fix Scalar / int confusion for joint mimic index
Oct 1, 2024
43abc66
[bindings/urdf] Rebasing
Oct 3, 2024
f9b3e57
Make code compile with CppAd
Oct 3, 2024
e2fa463
[algo/crba] remove double function
Oct 4, 2024
8e7374b
[unittest/kinematics] Cast to avoid errors
Oct 4, 2024
b268f93
[multibody/geometry] Force scalar to be double
Oct 4, 2024
2d72a4f
[multibody/op] Only do comparison when scalar=double
Oct 4, 2024
331261b
Remove compilation warnings
Oct 4, 2024
3d1f010
Fix crba and some tests for casadi
Oct 4, 2024
5a89c28
[EtienneAr Feedback] Fix Scalar forced to double for GeometryObjects
Oct 6, 2024
4dc28f2
[EtienneAr Feedback] Fix casadi test for joint mimic
Oct 7, 2024
ec881ff
[serialization] Fix template errors
Oct 7, 2024
c224e77
Fix examples
Oct 7, 2024
947e2e5
Fix Serialization
Oct 7, 2024
18e1f45
Fix examples
Oct 7, 2024
78de13b
[test/rnea] Try fix for windows
Oct 8, 2024
87a8077
[EtienneAr Feedback] Swap import order
Oct 11, 2024
0271d46
[EtienneAr Feedback] Fix PINOCCHIO_EXPLICIT_INSTANTIATION DECLARATION…
Oct 11, 2024
828ebf0
[EtienneAr Feedback] Update changelog
Oct 11, 2024
d9ad2a4
[EtienneAr Feedback] Add explicit to single argument constructors
Oct 11, 2024
aa23cc9
Applied pre-commit
Oct 11, 2024
5b4d282
Fix init problem
Oct 11, 2024
c8ae316
[algo/jacobian] Fix rebasing
Nov 13, 2024
d04a675
python: Fix linting
jorisv Jan 15, 2025
ae3098d
[kinematics] Add documentation to getRelativePlacement
Jan 16, 2025
d7a0da5
[algo/model] Change error message to be more precise
Jan 16, 2025
cfbcb35
[algo/model] Access directly to variables
Jan 16, 2025
a7f18ce
[algo/model] Remove casting
Jan 16, 2025
c29d601
[algo/model] Simplify function
Jan 16, 2025
995dfee
[algo/jacobian] Remove useless line
Jan 16, 2025
103499a
[algo/jacobian] Remove useless cast
Jan 16, 2025
e117132
[algo/kinematics] Access directly variables
Jan 16, 2025
5aa1149
[algo/kinematics] Remove inline
Jan 16, 2025
d2e887a
[algo/crba] access variables directly
Jan 16, 2025
1864a26
[algo/crba] Remove inline
Jan 16, 2025
064c2a5
Change naming convention
Jan 16, 2025
3fa5eaa
Added assert for non supported algorithms and apply precommit
Jan 16, 2025
0abc4f6
[EtienneAr feedback] Fix -Wreorder for urdf parsing of mimic joints /…
Jan 20, 2025
8fdf846
[EtienneAr feedback] Simplify getRelativePlacement by doing two backw…
Jan 20, 2025
96bd68f
[EtienneAr feedback] Remove unused scaling in JointDataMimic
Jan 20, 2025
f6a28ee
[algorithms] Use throw instead of assert
Jan 20, 2025
6258e8b
[parser/urdf] Correct urdf parser
Jan 23, 2025
9f8fa73
Apply pre-commit
Jan 23, 2025
09735a7
Renaming nj and idx_j into nvExtended and idx_vExtended to be conform…
Jan 24, 2025
05925c1
[EtienneAr feedback] Revert all changes in unsupported algorithms
Jan 24, 2025
d58f010
[Check] Define MimicChecker to check if model has mimic joint
Jan 27, 2025
0ecb5d1
[Check] Add model checker to all non supported algorithms for model w…
Jan 27, 2025
5b9fd7b
[EtienneAr feedback] topic/mimic: nvSubtree_fromRow/parents_fromRow i…
Jan 27, 2025
53f3ea5
Remove mimicable collection variant and associated function
Jan 27, 2025
72c1159
[unittest] Revert changes to not pollute pr
Jan 27, 2025
f722b30
Apply pre-commit
Jan 27, 2025
95019e4
[Check] Add Mimic Checker to checker list
Jan 27, 2025
9329cc0
cppad: Deactivate algorithm geometry and model explicit template inst…
jorisv Jan 27, 2025
10ce844
algorithm: Split check into two headers to avoid include dependency h…
jorisv Jan 28, 2025
5b066c8
[Cmake] Update sources file
Jan 28, 2025
f951f46
cmake: Add check.hpp
jorisv Jan 28, 2025
5c40259
[Unittest] Check consistency of cast models with a mimic joint.
abussy-aldebaran Dec 12, 2024
dbd490b
[Unittest] Check specific indices when casting mimic joints.
abussy-aldebaran Dec 13, 2024
a7e70b0
[Unittest] Add test of buildReducedModel with mimic joints.
abussy-aldebaran Dec 26, 2024
67967fe
[urdf] Simplify mimic parsing
Jan 30, 2025
f91de04
[Check] Remove mimic exception
Jan 30, 2025
d9ca536
[Joint] Mimic is not allowed before its parent (throw)
Jan 30, 2025
4dcac6e
[test/check] Add test for mimic data
Jan 30, 2025
139f921
[test] Modify to be conform to the new throw in mimic joint
Jan 30, 2025
eb51453
[joint/translation] Go back to old api
Jan 30, 2025
74df625
[Check] Remove MimicChecker from default list
Jan 31, 2025
bcdd7d8
[Check] Remove mimic from default checker
Jan 31, 2025
8efd5b1
Remove dead code lines
Jan 31, 2025
93c2303
[serialize] Fix serialize for mimic
Jan 31, 2025
5677d73
[joint_mimic] Fix cast of joint
Jan 31, 2025
1180f08
[EtienneAr feedback] topic/mimic: pretty print for mimic joints
Feb 2, 2025
c923af5
[EtienneAr feedback] topic/mimic: Store nq/nv and remove mimicking jo…
Feb 2, 2025
5dcc533
[Joint] Re-introduce old api for setIndexes
Feb 3, 2025
801b224
Fix doc
Feb 3, 2025
1b2a0db
Remove useless include
Feb 3, 2025
b7abbcd
apply pre-commit
Feb 3, 2025
835f677
[EtienneAr feedback] topic/mimic: Minor visitor call remove
Feb 3, 2025
b96b832
[parser/urdf] Simplify mimic parsing
Feb 3, 2025
113f067
[parser/urdf] Mimic default to false
Feb 3, 2025
ca07e42
[bindings/urdf] Parser default argument to bindings to not break api
Feb 3, 2025
72db2e7
Revert mimic argument
Feb 3, 2025
4e768fe
Revert setIndexes to old api
Feb 3, 2025
86b1b0a
[SampleModel] Simplify bindings
Feb 3, 2025
33e18d0
Revert setIndexes uses back to old api
Feb 3, 2025
42d4c4c
[EtienneAr feedback] topic/mimic: rename m_jmodel_ref > m_jmodel_mimi…
Feb 4, 2025
810f750
[EtienneAr feedback] topic/mimic: add size check for new data in chec…
Feb 4, 2025
01706ce
[EtienneAr feedback] topic/mimic: Minor comment update for mimic setI…
Feb 4, 2025
9e31871
[EtienneAr feedback] topic/mimic: Fix mimic serialization
Feb 4, 2025
cce159e
[bindings/sampleModel] Add missing arguments
Feb 4, 2025
9e374dd
[bindings/model] Expose transformJointIntoMimic
Feb 4, 2025
9e21813
[bindings/joint] Expose new aatributes idx_vExtended and nvExtended
Feb 4, 2025
1a013e0
[bindings/sampleModel] Coherent with cpp implementation
Feb 4, 2025
1b4ca4c
[joint/Mimic] Add velocitySelector and fix typo
Feb 4, 2025
bc59825
[bindings/model] Fix scalar of transformJointIntoMimic
Feb 5, 2025
2f2e5ee
[algo/model] Fix reduced model for mimic
Feb 5, 2025
65adbb1
[algo/model] Remove useless typedef that causes compilation warning
Feb 5, 2025
5046018
[data] Use nvExtended instead of nv in nvSubtree
Feb 6, 2025
e7d89a2
[unittest] Reactivate mimic in reachable workspace
Feb 7, 2025
735b807
[unittest] Build mimic model generic
Feb 7, 2025
628a599
[unittest] Use generic buildMimic function and add test for double mi…
Feb 7, 2025
ccd0241
[Common-operation] Assert that it's not possible
Feb 7, 2025
76c6ce8
[joint/helical] Add affine transform to mimic
Feb 7, 2025
11998b4
[unittest] Fix acceleration
Feb 7, 2025
bca6914
[EtienneAr Feedback] joint-mimic: Remove mutable from joint data
Feb 7, 2025
8695187
[serialization] Fix mimic data serialization
Feb 7, 2025
0d14165
[algo/centroidal] Add comments to explain mimic
Feb 10, 2025
6c711ed
[algo/rnea] Add comment to explain mimic
Feb 10, 2025
8b6e9f4
[unittest/mimic] Remove test for no transform (since it throws now)
Feb 10, 2025
1a30833
Apply pre-commit
Feb 10, 2025
95688f7
[Serialization/Data] Fix mimic data serialization
Feb 10, 2025
4df852a
[parser/urdf] Don't break api
Feb 10, 2025
9eebd7e
[bindings/model] Use proxy
Feb 11, 2025
a004312
[model] Iintroduce mimic_joints and mimic_pairs value (missing serial…
Feb 11, 2025
017368d
[algo/model] Add new model attributes to transformMimicJoint
Feb 11, 2025
22f87ea
[data] Fix nvSubtree
Feb 11, 2025
bb5464f
[parser/urdf] Add false to verbose argument and fix argument order is…
Feb 12, 2025
98daec1
[unittest] Factorize fonction
Feb 12, 2025
17cfcd9
[unittest] Test mimic manipulator
Feb 12, 2025
42cc6bf
[unittest] add mimic kinematics test
Feb 12, 2025
cb60868
[unittest] Remove dead code
Feb 12, 2025
c2da672
[data & check-data] change nvSubtree convention for mimic
Feb 12, 2025
a5c6f91
[unittest] Remove test for sample-model
Feb 12, 2025
696663f
[serialization] Remove dead code
Feb 13, 2025
749dc76
[algo/center-of-mass] Add assert for jacobian subtree
Feb 13, 2025
2648805
[unittest] Fix model generator
Feb 13, 2025
901998e
[unittest] Fix G matrix
Feb 13, 2025
ea4fd58
[unittest] Remove cout
Feb 13, 2025
ac09de1
wip for crba patch
Feb 11, 2025
b22fbb6
[algo/crba] Use new patch for crba with local mimic
Feb 14, 2025
eb0dc20
[EtienneAr feedback] topic/mimic: restore jacobian perfomrances (remo…
Feb 13, 2025
9b4e237
[EtienneAr feedback] topic/mimic: Rename Joint[Velocity/Config]Extend…
Feb 13, 2025
e1c3bb7
[EtienneAr feedback] topic/mimic: Add more jacobian tests for mimic j…
Feb 14, 2025
b4f0d8a
[algo/crba] Add patch for crba world with mimic joints
Feb 14, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
[EtienneAr feedback] Simplify getRelativePlacement by doing two backw…
…ard passes
Megane Millan committed Jan 20, 2025
commit 8fdf846e3d6040f6c1943aec47b4708c83d5633c
35 changes: 14 additions & 21 deletions include/pinocchio/algorithm/kinematics.hxx
Original file line number Diff line number Diff line change
@@ -292,34 +292,27 @@ namespace pinocchio
switch (convention)
{
case Convention::LOCAL: {
SE3Tpl<Scalar, Options> result;
const JointIndex child_id = std::max(jointIdRef, jointIdTarget);
const JointIndex parent_id = std::min(jointIdRef, jointIdTarget);
result.setIdentity();
SE3Tpl<Scalar, Options> ancestorMref(1); // Initialize to Identity
SE3Tpl<Scalar, Options> ancestorMtarget(1); // Initialize to Identity

// Traverse the kinematic chain from "tip" to "root"
size_t common_ancestor_parent, common_ancestor_child;
findCommonAncestor(model, parent_id, child_id, common_ancestor_parent, common_ancestor_child);
size_t ancestor_ref, ancestor_target;
findCommonAncestor(model, jointIdRef, jointIdTarget, ancestor_ref, ancestor_target);

// Traverse the kinematic chain backward to the common ancestor
for (size_t i = model.supports[child_id].size() - 1; i > common_ancestor_child; i--)
// Traverse the kinematic chain backward from Ref to the common ancestor
for (size_t i = model.supports[jointIdRef].size() - 1; i > ancestor_ref; i--)
{
const JointIndex j = model.supports[child_id][(size_t)i];
result = data.liMi[j].act(result);
}
// Then forward to the "parent"
for (size_t i = common_ancestor_parent + 1; i <= model.supports[parent_id].size() - 1; i++)
{
const JointIndex j = model.supports[parent_id][i];
result = data.liMi[j].actInv(result);
const JointIndex j = model.supports[jointIdRef][(size_t)i];
ancestorMref = data.liMi[j].act(ancestorMref);
}

// Inverse the result if necessary
if (child_id == jointIdRef && jointIdRef != jointIdTarget)
// Traverse the kinematic chain backward from Target to the common ancestor
for (size_t i = model.supports[jointIdTarget].size() - 1; i > ancestor_target; i--)
{
result = result.inverse();
const JointIndex j = model.supports[jointIdTarget][(size_t)i];
ancestorMtarget = data.liMi[j].act(ancestorMtarget);
}
return result;

return ancestorMref.actInv(ancestorMtarget);
}
case Convention::WORLD:
return data.oMi[jointIdRef].actInv(data.oMi[jointIdTarget]);