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
59385e63
Commit
59385e63
authored
Sep 24, 2017
by
jcarpent
Browse files
[Unit test] Add unit test of the time derivative of J
parent
38cec6d5
Changes
1
Hide whitespace changes
Inline
Side-by-side
unittest/jacobian.cpp
View file @
59385e63
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015
-2017
CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
...
...
@@ -17,16 +17,24 @@
#include
"pinocchio/multibody/model.hpp"
#include
"pinocchio/algorithm/jacobian.hpp"
#include
"pinocchio/algorithm/kinematics.hpp"
#include
"pinocchio/algorithm/rnea.hpp"
#include
"pinocchio/spatial/act-on-set.hpp"
#include
"pinocchio/parsers/sample-models.hpp"
#include
"pinocchio/tools/timer.hpp"
#include
"pinocchio/algorithm/joint-configuration.hpp"
#include
<iostream>
#include
<boost/test/unit_test.hpp>
#include
<boost/utility/binary.hpp>
template
<
typename
Derived
>
inline
bool
isFinite
(
const
Eigen
::
MatrixBase
<
Derived
>
&
x
)
{
return
((
x
-
x
).
array
()
==
(
x
-
x
).
array
()).
all
();
}
BOOST_AUTO_TEST_SUITE
(
BOOST_TEST_MODULE
)
BOOST_AUTO_TEST_CASE
(
test_jacobian
)
...
...
@@ -67,6 +75,54 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
}
BOOST_AUTO_TEST_CASE
(
test_jacobian_time_variation
)
{
using
namespace
Eigen
;
using
namespace
se3
;
se3
::
Model
model
;
se3
::
buildModels
::
humanoidSimple
(
model
);
se3
::
Data
data
(
model
);
se3
::
Data
data_ref
(
model
);
VectorXd
q
=
randomConfiguration
(
model
,
-
1
*
Eigen
::
VectorXd
::
Ones
(
model
.
nq
),
Eigen
::
VectorXd
::
Ones
(
model
.
nq
)
);
VectorXd
v
=
VectorXd
::
Random
(
model
.
nv
);
VectorXd
a
=
VectorXd
::
Random
(
model
.
nv
);
computeJacobiansTimeVariation
(
model
,
data
,
q
,
v
);
BOOST_CHECK
(
isFinite
(
data
.
dJ
));
forwardKinematics
(
model
,
data_ref
,
q
,
v
,
a
);
Model
::
Index
idx
=
model
.
existJointName
(
"rarm2"
)
?
model
.
getJointId
(
"rarm2"
)
:
(
Model
::
Index
)(
model
.
njoints
-
1
);
Data
::
Matrix6x
J
(
6
,
model
.
nv
);
J
.
fill
(
0.
);
Data
::
Matrix6x
dJ
(
6
,
model
.
nv
);
dJ
.
fill
(
0.
);
// Regarding to the world origin
getJacobian
<
false
>
(
model
,
data
,
idx
,
J
);
getJacobianTimeVariation
<
false
>
(
model
,
data
,
idx
,
dJ
);
Motion
v_idx
(
J
*
v
);
BOOST_CHECK
(
v_idx
.
isApprox
(
data_ref
.
oMi
[
idx
].
act
(
data_ref
.
v
[
idx
])));
Motion
a_idx
(
J
*
a
+
dJ
*
v
);
const
Motion
&
a_ref
=
data_ref
.
oMi
[
idx
].
act
(
data_ref
.
a
[
idx
]);
BOOST_CHECK
(
a_idx
.
isApprox
(
a_ref
));
// Regarding to the local frame
getJacobian
<
true
>
(
model
,
data
,
idx
,
J
);
getJacobianTimeVariation
<
true
>
(
model
,
data
,
idx
,
dJ
);
v_idx
=
(
Motion
::
Vector6
)(
J
*
v
);
BOOST_CHECK
(
v_idx
.
isApprox
(
data_ref
.
v
[
idx
]));
a_idx
=
(
Motion
::
Vector6
)(
J
*
a
+
dJ
*
v
);
BOOST_CHECK
(
a_idx
.
isApprox
(
data_ref
.
a
[
idx
]));
}
BOOST_AUTO_TEST_CASE
(
test_timings
)
{
...
...
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