Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
pinocchio
Commits
6ea9b78b
Verified
Commit
6ea9b78b
authored
Oct 06, 2019
by
Justin Carpentier
Browse files
mode: add the notion of subtrees
Will be useful for certain algorithms dealing with second order derivatives
parent
06995d18
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/multibody/model.hpp
View file @
6ea9b78b
...
...
@@ -128,6 +128,11 @@ namespace pinocchio
/// \brief Vector of operational frames registered on the model.
FrameVector
frames
;
/// \brief Vector of supports.
/// supports[j] corresponds to the collection of all joints located on the path between body *j* and the world.
/// The last element of supports[j] is the index of the joint *j* itself.
std
::
vector
<
IndexVector
>
supports
;
/// \brief Vector of subtrees.
/// subtree[j] corresponds to the subtree supported by the joint *j*.
/// The first element of subtree[j] is the index of the joint *j* itself.
...
...
@@ -154,6 +159,7 @@ namespace pinocchio
,
joints
(
1
)
,
parents
(
1
,
0
)
,
names
(
1
)
,
supports
(
1
,
IndexVector
(
1
,
0
))
,
subtrees
(
1
)
,
gravity
(
gravity981
,
Vector3
::
Zero
())
{
...
...
src/multibody/model.hxx
View file @
6ea9b78b
...
...
@@ -122,6 +122,11 @@ namespace pinocchio
subtrees
.
push_back
(
IndexVector
(
1
));
subtrees
[
idx
][
0
]
=
idx
;
addJointIndexToParentSubtrees
(
idx
);
// Init and add joint index to the supports
supports
.
push_back
(
supports
[
parent
]);
supports
[
idx
].
push_back
(
idx
);
return
idx
;
}
...
...
unittest/model.cpp
View file @
6ea9b78b
...
...
@@ -43,6 +43,30 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
}
}
BOOST_AUTO_TEST_CASE
(
test_model_support
)
{
Model
model
;
buildModels
::
humanoidRandom
(
model
);
const
Model
::
IndexVector
support0_ref
(
1
,
0
);
BOOST_CHECK
(
model
.
supports
[
0
]
==
support0_ref
);
// Check that i ends supports[i]
for
(
JointIndex
joint_id
=
1
;
joint_id
<
(
JointIndex
)
model
.
njoints
;
++
joint_id
)
{
BOOST_CHECK
(
model
.
supports
[
joint_id
].
back
()
==
joint_id
);
Model
::
IndexVector
&
support
=
model
.
supports
[
joint_id
];
size_t
current_id
=
support
.
size
()
-
2
;
for
(
JointIndex
parent_id
=
model
.
parents
[
joint_id
];
parent_id
>
0
;
parent_id
=
model
.
parents
[
parent_id
],
current_id
--
)
{
BOOST_CHECK
(
parent_id
==
support
[
current_id
]);
}
}
}
BOOST_AUTO_TEST_CASE
(
comparison
)
{
Model
model
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment