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
Gabriele Buondonno
pinocchio
Commits
8cf0b9e0
Commit
8cf0b9e0
authored
Mar 16, 2016
by
jcarpent
Browse files
[C++][Unittest] Add unit tests related to forward dynamics
parent
f334f932
Changes
2
Hide whitespace changes
Inline
Side-by-side
unittest/CMakeLists.txt
View file @
8cf0b9e0
...
...
@@ -55,6 +55,7 @@ ADD_UNIT_TEST(crba eigen3)
ADD_UNIT_TEST
(
com eigen3
)
ADD_UNIT_TEST
(
jacobian eigen3
)
ADD_UNIT_TEST
(
cholesky eigen3
)
ADD_UNIT_TEST
(
dynamics eigen3
)
IF
(
URDFDOM_FOUND
)
ADD_UNIT_TEST
(
urdf
"eigen3;urdfdom"
)
...
...
@@ -88,4 +89,4 @@ ENDIF(LUA5_1_FOUND)
ADD_UNIT_TEST
(
joints eigen3
)
ADD_UNIT_TEST
(
compute-all-terms eigen3
)
ADD_UNIT_TEST
(
energy eigen3
)
ADD_UNIT_TEST
(
operational-frames eigen3
)
\ No newline at end of file
ADD_UNIT_TEST
(
operational-frames eigen3
)
unittest/dynamics.cpp
0 → 100644
View file @
8cf0b9e0
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include
"pinocchio/spatial/se3.hpp"
#include
"pinocchio/multibody/model.hpp"
#include
"pinocchio/algorithm/jacobian.hpp"
#include
"pinocchio/algorithm/dynamics.hpp"
#include
"pinocchio/multibody/parser/sample-models.hpp"
#include
"pinocchio/tools/timer.hpp"
#include
<iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE DynamicTest
#include
<boost/test/unit_test.hpp>
#include
<boost/utility/binary.hpp>
BOOST_AUTO_TEST_SUITE
(
DynamicsTest
)
BOOST_AUTO_TEST_CASE
(
test_FD
)
{
using
namespace
Eigen
;
using
namespace
se3
;
se3
::
Model
model
;
se3
::
buildModels
::
humanoidSimple
(
model
,
true
);
se3
::
Data
data
(
model
);
VectorXd
q
=
VectorXd
::
Ones
(
model
.
nq
);
q
.
segment
<
4
>
(
3
).
normalize
();
se3
::
computeJacobians
(
model
,
data
,
q
);
VectorXd
v
=
VectorXd
::
Ones
(
model
.
nv
);
VectorXd
tau
=
VectorXd
::
Zero
(
model
.
nv
);
const
std
::
string
RF
=
"rleg6_body"
;
const
std
::
string
LF
=
"lleg6_body"
;
Data
::
Matrix6x
J_RF
(
6
,
model
.
nv
);
getJacobian
<
true
>
(
model
,
data
,
model
.
getBodyId
(
RF
),
J_RF
);
Data
::
Matrix6x
J_LF
(
6
,
model
.
nv
);
getJacobian
<
true
>
(
model
,
data
,
model
.
getBodyId
(
LF
),
J_LF
);
Eigen
::
MatrixXd
J
(
12
,
model
.
nv
);
J
.
topRows
<
6
>
()
=
J_RF
;
J
.
bottomRows
<
6
>
()
=
J_LF
;
Eigen
::
VectorXd
gamma
(
VectorXd
::
Ones
(
12
));
Eigen
::
MatrixXd
H
(
J
.
transpose
());
se3
::
forwardDynamics
(
model
,
data
,
q
,
v
,
tau
,
J
,
gamma
,
true
);
data
.
M
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
data
.
M
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
MatrixXd
Minv
(
data
.
M
.
inverse
());
MatrixXd
JMinvJt
(
J
*
Minv
*
J
.
transpose
());
Eigen
::
MatrixXd
G_ref
(
J
.
transpose
());
cholesky
::
Uiv
(
model
,
data
,
G_ref
);
for
(
int
k
=
0
;
k
<
model
.
nv
;
++
k
)
G_ref
.
row
(
k
)
/=
sqrt
(
data
.
D
[
k
]);
Eigen
::
MatrixXd
H_ref
(
G_ref
.
transpose
()
*
G_ref
);
BOOST_CHECK
(
H_ref
.
isApprox
(
JMinvJt
,
1
-
12
));
VectorXd
lambda_ref
=
-
JMinvJt
.
inverse
()
*
(
J
*
Minv
*
(
tau
-
data
.
nle
)
+
gamma
);
BOOST_CHECK
(
data
.
lambda
.
isApprox
(
lambda_ref
,
1e-12
));
VectorXd
a_ref
=
Minv
*
(
tau
-
data
.
nle
+
J
.
transpose
()
*
lambda_ref
);
Eigen
::
VectorXd
dynamics_residual_ref
(
data
.
M
*
a_ref
+
data
.
nle
-
tau
-
J
.
transpose
()
*
lambda_ref
);
BOOST_CHECK
(
dynamics_residual_ref
.
norm
()
<=
1e-12
);
Eigen
::
VectorXd
constraint_residual
(
J
*
data
.
ddq
+
gamma
);
BOOST_CHECK
(
constraint_residual
.
norm
()
<=
1e-12
);
Eigen
::
VectorXd
dynamics_residual
(
data
.
M
*
data
.
ddq
+
data
.
nle
-
tau
-
J
.
transpose
()
*
data
.
lambda
);
BOOST_CHECK
(
dynamics_residual
.
norm
()
<=
1e-12
);
}
BOOST_AUTO_TEST_CASE
(
timings_fd_llt
)
{
using
namespace
Eigen
;
using
namespace
se3
;
se3
::
Model
model
;
se3
::
buildModels
::
humanoidSimple
(
model
,
true
);
se3
::
Data
data
(
model
);
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
const
size_t
NBT
=
1000
*
1000
;
#else
const
size_t
NBT
=
100
;
#endif
#else
const
size_t
NBT
=
1
;
std
::
cout
<<
"(the time score in debug mode is not relevant) "
;
#endif // ifndef NDEBUG
VectorXd
q
=
VectorXd
::
Ones
(
model
.
nq
);
q
.
segment
<
4
>
(
3
).
normalize
();
se3
::
computeJacobians
(
model
,
data
,
q
);
VectorXd
v
=
VectorXd
::
Ones
(
model
.
nv
);
VectorXd
tau
=
VectorXd
::
Zero
(
model
.
nv
);
const
std
::
string
RF
=
"rleg6_body"
;
const
std
::
string
LF
=
"lleg6_body"
;
Data
::
Matrix6x
J_RF
(
6
,
model
.
nv
);
getJacobian
<
true
>
(
model
,
data
,
model
.
getBodyId
(
RF
),
J_RF
);
Data
::
Matrix6x
J_LF
(
6
,
model
.
nv
);
getJacobian
<
true
>
(
model
,
data
,
model
.
getBodyId
(
LF
),
J_LF
);
Eigen
::
MatrixXd
J
(
12
,
model
.
nv
);
J
.
topRows
<
6
>
()
=
J_RF
;
J
.
bottomRows
<
6
>
()
=
J_LF
;
Eigen
::
VectorXd
gamma
(
VectorXd
::
Ones
(
12
));
q
=
Eigen
::
VectorXd
::
Zero
(
model
.
nq
);
StackTicToc
timer
(
StackTicToc
::
US
);
timer
.
tic
();
SMOOTH
(
NBT
)
{
se3
::
forwardDynamics
(
model
,
data
,
q
,
v
,
tau
,
J
,
gamma
,
true
);
}
timer
.
toc
(
std
::
cout
,
NBT
);
}
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