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
4142668c
Commit
4142668c
authored
Sep 07, 2018
by
Rohan Budhiraja
Browse files
[unittest/frames] add unittest for framejacobiantimederivative
parent
f8c5cee4
Changes
1
Hide whitespace changes
Inline
Side-by-side
unittest/frames.cpp
View file @
4142668c
...
...
@@ -23,12 +23,20 @@
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/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_kinematics
)
...
...
@@ -96,5 +104,111 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
BOOST_CHECK
(
Jff
.
isApprox
(
Jjj
));
}
BOOST_AUTO_TEST_CASE
(
test_frame_jacobian_time_variation
)
{
using
namespace
Eigen
;
using
namespace
se3
;
se3
::
Model
model
;
se3
::
buildModels
::
humanoidSimple
(
model
);
Model
::
Index
parent_idx
=
model
.
existJointName
(
"rarm2_joint"
)
?
model
.
getJointId
(
"rarm2_joint"
)
:
(
Model
::
Index
)(
model
.
njoints
-
1
);
const
std
::
string
&
frame_name
=
std
::
string
(
model
.
names
[
parent_idx
]
+
"_frame"
);
const
SE3
&
framePlacement
=
SE3
::
Random
();
model
.
addFrame
(
Frame
(
frame_name
,
parent_idx
,
0
,
framePlacement
,
OP_FRAME
));
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
);
computeJointJacobiansTimeVariation
(
model
,
data
,
q
,
v
);
forwardKinematics
(
model
,
data_ref
,
q
,
v
,
a
);
framesForwardKinematics
(
model
,
data_ref
);
framesForwardKinematics
(
model
,
data
);
BOOST_CHECK
(
isFinite
(
data
.
dJ
));
Model
::
Index
idx
=
model
.
getFrameId
(
frame_name
);
const
Frame
&
frame
=
model
.
frames
[
idx
];
BOOST_CHECK
(
frame
.
placement
.
isApprox_impl
(
framePlacement
));
Data
::
Matrix6x
J
(
6
,
model
.
nv
);
J
.
fill
(
0.
);
Data
::
Matrix6x
dJ
(
6
,
model
.
nv
);
dJ
.
fill
(
0.
);
// Regarding to the world origin
getFrameJacobian
<
WORLD
>
(
model
,
data
,
idx
,
J
);
getFrameJacobianTimeVariation
<
WORLD
>
(
model
,
data
,
idx
,
dJ
);
Motion
v_idx
(
J
*
v
);
const
Motion
&
v_ref_local
=
frame
.
placement
.
actInv
(
data_ref
.
v
[
parent_idx
]);
const
Motion
&
v_ref
=
data_ref
.
oMf
[
idx
].
act
(
v_ref_local
);
BOOST_CHECK
(
v_idx
.
isApprox
(
v_ref
));
Motion
a_idx
(
J
*
a
+
dJ
*
v
);
const
Motion
&
a_ref_local
=
frame
.
placement
.
actInv
(
data_ref
.
a
[
parent_idx
]);
const
Motion
&
a_ref
=
data_ref
.
oMf
[
idx
].
act
(
a_ref_local
);
BOOST_CHECK
(
a_idx
.
isApprox
(
a_ref
));
J
.
fill
(
0.
);
dJ
.
fill
(
0.
);
// Regarding to the local frame
getFrameJacobian
<
LOCAL
>
(
model
,
data
,
idx
,
J
);
getFrameJacobianTimeVariation
<
LOCAL
>
(
model
,
data
,
idx
,
dJ
);
v_idx
=
(
Motion
::
Vector6
)(
J
*
v
);
BOOST_CHECK
(
v_idx
.
isApprox
(
v_ref_local
));
a_idx
=
(
Motion
::
Vector6
)(
J
*
a
+
dJ
*
v
);
BOOST_CHECK
(
a_idx
.
isApprox
(
a_ref_local
));
// compare to finite differencies
{
Data
data_ref
(
model
),
data_ref_plus
(
model
);
const
double
alpha
=
1e-8
;
Eigen
::
VectorXd
q_plus
(
model
.
nq
);
q_plus
=
integrate
(
model
,
q
,
alpha
*
v
);
//data_ref
Data
::
Matrix6x
J_ref_world
(
6
,
model
.
nv
),
J_ref_local
(
6
,
model
.
nv
);
J_ref_world
.
fill
(
0.
);
J_ref_local
.
fill
(
0.
);
computeJointJacobians
(
model
,
data_ref
,
q
);
framesForwardKinematics
(
model
,
data_ref
);
const
SE3
&
oMf_q
=
data_ref
.
oMf
[
idx
];
getFrameJacobian
<
WORLD
>
(
model
,
data_ref
,
idx
,
J_ref_world
);
getFrameJacobian
<
LOCAL
>
(
model
,
data_ref
,
idx
,
J_ref_local
);
//data_ref_plus
Data
::
Matrix6x
J_ref_plus_world
(
6
,
model
.
nv
),
J_ref_plus_local
(
6
,
model
.
nv
);
J_ref_plus_world
.
fill
(
0.
);
J_ref_plus_local
.
fill
(
0.
);
computeJointJacobians
(
model
,
data_ref_plus
,
q_plus
);
framesForwardKinematics
(
model
,
data_ref_plus
);
const
SE3
&
oMf_qplus
=
data_ref_plus
.
oMf
[
idx
];
getFrameJacobian
<
WORLD
>
(
model
,
data_ref_plus
,
idx
,
J_ref_plus_world
);
getFrameJacobian
<
LOCAL
>
(
model
,
data_ref_plus
,
idx
,
J_ref_plus_local
);
//Move J_ref_plus_local to reference frame
J_ref_plus_local
=
(
oMf_q
.
inverse
()
*
oMf_qplus
).
toActionMatrix
()
*
(
J_ref_plus_local
);
Data
::
Matrix6x
dJ_ref_world
(
6
,
model
.
nv
),
dJ_ref_local
(
6
,
model
.
nv
);
dJ_ref_world
.
fill
(
0.
);
dJ_ref_local
.
fill
(
0.
);
dJ_ref_world
=
(
J_ref_plus_world
-
J_ref_world
)
/
alpha
;
dJ_ref_local
=
(
J_ref_plus_local
-
J_ref_local
)
/
alpha
;
//data
computeJointJacobiansTimeVariation
(
model
,
data
,
q
,
v
);
forwardKinematics
(
model
,
data
,
q
,
v
);
framesForwardKinematics
(
model
,
data
);
Data
::
Matrix6x
dJ_world
(
6
,
model
.
nv
),
dJ_local
(
6
,
model
.
nv
);
dJ_world
.
fill
(
0.
);
dJ_local
.
fill
(
0.
);
getFrameJacobianTimeVariation
<
WORLD
>
(
model
,
data
,
idx
,
dJ_world
);
getFrameJacobianTimeVariation
<
LOCAL
>
(
model
,
data
,
idx
,
dJ_local
);
BOOST_CHECK
(
dJ_world
.
isApprox
(
dJ_ref_world
,
sqrt
(
alpha
)));
BOOST_CHECK
(
dJ_local
.
isApprox
(
dJ_ref_local
,
sqrt
(
alpha
)));
}
}
BOOST_AUTO_TEST_SUITE_END
()
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