Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
pinocchio
Commits
467702c7
Commit
467702c7
authored
Sep 20, 2016
by
jcarpent
Browse files
[C++] Update checker to new naming convention
parent
14b6d3be
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/algorithm/aba.hxx
View file @
467702c7
...
...
@@ -226,7 +226,7 @@ namespace se3
// The second test is overconstraining.
inline
bool
ABAChecker
::
checkModel_impl
(
const
Model
&
model
)
const
{
for
(
JointIndex
j
=
1
;
int
(
j
)
<
model
.
njoint
;
j
++
)
for
(
JointIndex
j
=
1
;
int
(
j
)
<
model
.
njoint
s
;
j
++
)
if
(
(
model
.
inertias
[
j
].
mass
()
<
1e-5
)
||
(
model
.
inertias
[
j
].
inertia
().
data
()[
0
]
<
1e-5
)
||
(
model
.
inertias
[
j
].
inertia
().
data
()[
3
]
<
1e-5
)
...
...
src/algorithm/check.hxx
View file @
467702c7
...
...
@@ -50,7 +50,7 @@ namespace se3
// Check the validity of the kinematic tree defined by parents.
inline
bool
ParentChecker
::
checkModel_impl
(
const
Model
&
model
)
const
{
for
(
JointIndex
j
=
1
;(
int
)
j
<
model
.
njoint
;
++
j
)
for
(
JointIndex
j
=
1
;(
int
)
j
<
model
.
njoint
s
;
++
j
)
if
(
model
.
parents
[
j
]
>=
j
)
return
false
;
return
true
;
...
...
@@ -64,23 +64,23 @@ namespace se3
// TODO impulse_c
// They are not check neither
CHECK_DATA
(
(
int
)
data
.
joints
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
a
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
a_gf
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
v
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
f
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
oMi
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
liMi
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
Ycrb
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
Yaba
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
Fcrb
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
joints
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
a
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
a_gf
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
v
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
f
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
oMi
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
liMi
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
Ycrb
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
Yaba
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
Fcrb
.
size
()
==
model
.
njoint
s
);
BOOST_FOREACH
(
const
Data
::
Matrix6x
&
F
,
data
.
Fcrb
)
CHECK_DATA
(
F
.
cols
()
==
model
.
nv
);
CHECK_DATA
(
(
int
)
data
.
iMf
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
iMf
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
com
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
vcom
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
acom
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
mass
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
iMf
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
iMf
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
com
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
vcom
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
acom
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
mass
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
data
.
tau
.
size
()
==
model
.
nv
);
CHECK_DATA
(
data
.
nle
.
size
()
==
model
.
nv
);
...
...
@@ -99,17 +99,17 @@ namespace se3
CHECK_DATA
(
data
.
dq_after
.
size
()
==
model
.
nv
);
//CHECK_DATA( data.impulse_c.size()== model.nv );
CHECK_DATA
(
(
int
)
data
.
oMf
.
size
()
==
model
.
n
F
rames
);
CHECK_DATA
(
(
int
)
data
.
oMf
.
size
()
==
model
.
n
f
rames
);
CHECK_DATA
(
(
int
)
data
.
lastChild
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
nvSubtree
.
size
()
==
model
.
njoint
);
CHECK_DATA
(
(
int
)
data
.
lastChild
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
nvSubtree
.
size
()
==
model
.
njoint
s
);
CHECK_DATA
(
(
int
)
data
.
parents_fromRow
.
size
()
==
model
.
nv
);
CHECK_DATA
(
(
int
)
data
.
nvSubtree_fromRow
.
size
()
==
model
.
nv
);
for
(
JointIndex
j
=
1
;
int
(
j
)
<
model
.
njoint
;
++
j
)
for
(
JointIndex
j
=
1
;
int
(
j
)
<
model
.
njoint
s
;
++
j
)
{
JointIndex
c
=
data
.
lastChild
[
j
];
CHECK_DATA
((
int
)
c
<
model
.
njoint
);
CHECK_DATA
((
int
)
c
<
model
.
njoint
s
);
int
nv
=
model
.
joints
[
j
].
nv
();
for
(
JointIndex
d
=
j
+
1
;
d
<=
c
;
++
d
)
// explore all descendant
{
...
...
@@ -118,7 +118,7 @@ namespace se3
}
CHECK_DATA
(
nv
==
data
.
nvSubtree
[
j
]);
for
(
JointIndex
d
=
c
+
1
;(
int
)
d
<
model
.
njoint
;
++
d
)
for
(
JointIndex
d
=
c
+
1
;(
int
)
d
<
model
.
njoint
s
;
++
d
)
CHECK_DATA
(
(
model
.
parents
[
d
]
<
j
)
||
(
model
.
parents
[
d
]
>
c
)
);
int
row
=
model
.
joints
[
j
].
idx_v
();
...
...
src/algorithm/crba.hxx
View file @
467702c7
...
...
@@ -240,7 +240,7 @@ namespace se3
{
inline
bool
isDescendant
(
const
Model
&
model
,
const
JointIndex
j
,
const
JointIndex
root
)
{
if
(
int
(
j
)
>=
model
.
njoint
)
return
false
;
if
(
int
(
j
)
>=
model
.
njoint
s
)
return
false
;
if
(
j
==
0
)
return
root
==
0
;
return
(
j
==
root
)
||
isDescendant
(
model
,
model
.
parents
[
j
],
root
);
}
...
...
@@ -252,11 +252,11 @@ namespace se3
// immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1
// can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant
// of i and none of [k+1..n-1] is a descendant of i.
for
(
JointIndex
i
=
1
;
int
(
i
)
<
model
.
njoint
-
1
;
++
i
)
// no need to check joints 0 and N-1
for
(
JointIndex
i
=
1
;
int
(
i
)
<
model
.
njoint
s
-
1
;
++
i
)
// no need to check joints 0 and N-1
{
JointIndex
k
=
i
+
1
;
while
(
internal
::
isDescendant
(
model
,
k
,
i
))
++
k
;
for
(
;
int
(
k
)
<
model
.
njoint
;
++
k
)
for
(
;
int
(
k
)
<
model
.
njoint
s
;
++
k
)
if
(
internal
::
isDescendant
(
model
,
k
,
i
)
)
return
false
;
}
return
true
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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