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
68896cea
Verified
Commit
68896cea
authored
Oct 06, 2019
by
Justin Carpentier
Browse files
data: add supports_fromRow field
parent
a138bd03
Changes
4
Hide whitespace changes
Inline
Side-by-side
src/multibody/data.hpp
View file @
68896cea
...
...
@@ -230,6 +230,7 @@ namespace pinocchio
/// \brief Index of the last child (for CRBA)
std
::
vector
<
int
>
lastChild
;
/// \brief Dimension of the subtree motion space (for CRBA)
std
::
vector
<
int
>
nvSubtree
;
...
...
@@ -248,6 +249,10 @@ namespace pinocchio
/// \brief First previous non-zero row in M (used in Cholesky Decomposition).
std
::
vector
<
int
>
parents_fromRow
;
/// \brief Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tree of the
/// given index at the row level. It may be helpful to retrieve the sparsity pattern through it.
std
::
vector
<
std
::
vector
<
int
>
>
supports_fromRow
;
/// \brief Subtree of the current row index (used in Cholesky Decomposition).
std
::
vector
<
int
>
nvSubtree_fromRow
;
...
...
@@ -347,6 +352,7 @@ namespace pinocchio
private:
void
computeLastChild
(
const
Model
&
model
);
void
computeParents_fromRow
(
const
Model
&
model
);
void
computeSupports_fromRow
(
const
Model
&
model
);
};
...
...
src/multibody/data.hxx
View file @
68896cea
...
...
@@ -68,6 +68,7 @@ namespace pinocchio
,
Dinv
(
model
.
nv
)
,
tmp
(
model
.
nv
)
,
parents_fromRow
((
std
::
size_t
)
model
.
nv
)
,
supports_fromRow
((
std
::
size_t
)
model
.
nv
)
,
nvSubtree_fromRow
((
std
::
size_t
)
model
.
nv
)
,
J
(
6
,
model
.
nv
)
,
dJ
(
6
,
model
.
nv
)
...
...
@@ -106,13 +107,14 @@ namespace pinocchio
{
Fcrb
[
i
].
resize
(
6
,
model
.
nv
);
}
computeLastChild
(
model
);
/* Init for Coriolis */
C
.
setZero
();
/* Init for Cholesky */
U
.
setIdentity
();
computeParents_fromRow
(
model
);
computeSupports_fromRow
(
model
);
/* Init Jacobian */
J
.
setZero
();
...
...
@@ -180,6 +182,36 @@ namespace pinocchio
}
}
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
inline
void
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
::
computeSupports_fromRow
(
const
Model
&
model
)
{
typedef
typename
Model
::
JointIndex
JointIndex
;
for
(
JointIndex
joint_id
=
1
;
joint_id
<
(
JointIndex
)(
model
.
njoints
);
joint_id
++
)
{
const
int
nvj
=
nv
(
model
.
joints
[
joint_id
]);
const
int
idx_vj
=
idx_v
(
model
.
joints
[
joint_id
]);
assert
(
idx_vj
>=
0
&&
idx_vj
<
model
.
nv
);
const
int
parent_fromRow
=
parents_fromRow
[(
size_t
)
idx_vj
];
if
(
parent_fromRow
>=
0
)
supports_fromRow
[(
size_t
)
idx_vj
]
=
supports_fromRow
[(
size_t
)
parent_fromRow
];
supports_fromRow
[(
size_t
)
idx_vj
].
push_back
(
idx_vj
);
for
(
int
row
=
1
;
row
<
nvj
;
++
row
)
{
supports_fromRow
[(
size_t
)(
idx_vj
+
row
)]
=
supports_fromRow
[(
size_t
)(
idx_vj
+
row
-
1
)];
supports_fromRow
[(
size_t
)(
idx_vj
+
row
)].
push_back
(
idx_vj
+
row
);
}
}
}
}
// namespace pinocchio
/// @endcond
...
...
unittest/CMakeLists.txt
View file @
68896cea
...
...
@@ -96,6 +96,7 @@ IF(BUILD_PYTHON_INTERFACE)
ENDIF
(
BUILD_PYTHON_INTERFACE
)
ADD_PINOCCHIO_UNIT_TEST
(
model
)
ADD_PINOCCHIO_UNIT_TEST
(
data
)
ADD_PINOCCHIO_UNIT_TEST
(
constraint
)
ADD_PINOCCHIO_UNIT_TEST
(
joints
)
ADD_PINOCCHIO_UNIT_TEST
(
compute-all-terms
)
...
...
unittest/data.cpp
0 → 100644
View file @
68896cea
//
// Copyright (c) 2019 INRIA
//
#include
"pinocchio/multibody/data.hpp"
#include
"pinocchio/multibody/model.hpp"
#include
"pinocchio/parsers/sample-models.hpp"
#include
<boost/test/unit_test.hpp>
#include
<boost/utility/binary.hpp>
using
namespace
pinocchio
;
BOOST_AUTO_TEST_SUITE
(
BOOST_TEST_MODULE
)
BOOST_AUTO_TEST_CASE
(
test_data_supports_fromRow
)
{
Model
model
;
buildModels
::
humanoidRandom
(
model
);
Data
data
(
model
);
for
(
size_t
k
=
0
;
k
<
(
size_t
)
model
.
nv
;
++
k
)
{
const
std
::
vector
<
int
>
&
support
=
data
.
supports_fromRow
[
k
];
const
int
parent_id
=
data
.
parents_fromRow
[
k
];
if
(
parent_id
>=
0
)
{
const
std
::
vector
<
int
>
&
support_parent
=
data
.
supports_fromRow
[(
size_t
)
parent_id
];
BOOST_CHECK
(
support
.
size
()
==
support_parent
.
size
()
+
1
);
for
(
size_t
j
=
0
;
j
<
support_parent
.
size
();
++
j
)
{
BOOST_CHECK
(
support
[
j
]
==
support_parent
[
j
]);
}
}
BOOST_CHECK
(
support
.
back
()
==
(
int
)
k
);
}
}
BOOST_AUTO_TEST_SUITE_END
()
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