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
82a13aba
Commit
82a13aba
authored
Jun 19, 2017
by
jcarpent
Browse files
[Unit Test] Correct unit test on frame jacobians
parent
b1908a1b
Changes
1
Hide whitespace changes
Inline
Side-by-side
unittest/frames.cpp
View file @
82a13aba
...
...
@@ -64,46 +64,53 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
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
=
VectorXd
::
Ones
(
model
.
nq
);
q
.
middleRows
<
4
>
(
3
).
normalize
();
VectorXd
q_dot
=
VectorXd
::
Ones
(
model
.
nv
);
framesForwardKinematics
(
model
,
data
,
q
);
computeJacobians
(
model
,
data
,
q
);
/// In global frame
Data
::
Matrix6x
Joj
(
6
,
model
.
nv
);
Joj
.
fill
(
0
);
Data
::
Matrix6x
Jof
(
6
,
model
.
nv
);
Jof
.
fill
(
0
);
Model
::
Index
idx
=
model
.
getFrameId
(
frame_name
);
const
Frame
&
frame
=
model
.
frames
[
idx
];
BOOST_CHECK
(
frame
.
placement
.
isApprox_impl
(
framePlacement
));
computeJacobians
(
model
,
data
,
q
);
computeJacobians
(
model
,
data_ref
,
q
);
framesForwardKinematics
(
model
,
data
);
getFrameJacobian
<
false
>
(
model
,
data
,
idx
,
Jof
);
getJacobian
<
false
>
(
model
,
data
,
parent_idx
,
Joj
);
getJacobian
<
false
>
(
model
,
data
_ref
,
parent_idx
,
Joj
);
Motion
nu_frame
(
Jof
*
q_dot
);
Motion
nu_joint
(
Joj
*
q_dot
);
SE3
translation
(
SE3
::
Identity
());
translation
.
translation
(
data
.
oMi
[
parent_idx
].
rotation
()
*
frame
.
placement
.
translation
());
Motion
nu_frame_from_nu_joint
(
translation
.
actInv
(
nu_joint
));
Motion
nu_frame_from_nu_joint
(
nu_joint
);
nu_frame_from_nu_joint
.
linear
()
-=
(
data
.
oMi
[
parent_idx
].
rotation
()
*
framePlacement
.
translation
()).
cross
(
nu_joint
.
angular
());
BOOST_CHECK
(
nu_frame
.
toVector
().
isApprox
(
nu_frame_from_nu_joint
.
toVector
(),
1e-12
));
BOOST_CHECK
(
nu_frame
.
isApprox
(
nu_frame_from_nu_joint
,
1e-12
));
/// In local frame
Data
::
Matrix6x
Jjj
(
6
,
model
.
nv
);
Jjj
.
fill
(
0
);
Data
::
Matrix6x
Jff
(
6
,
model
.
nv
);
Jff
.
fill
(
0
);
getFrameJacobian
<
true
>
(
model
,
data
,
idx
,
Jff
);
getJacobian
<
true
>
(
model
,
data
,
parent_idx
,
Jjj
);
nu_frame
=
Jff
*
q_dot
;
nu_joint
=
Jjj
*
q_dot
;
BOOST_CHECK
(
nu_frame
.
toVector
().
isApprox
(
framePlacement
.
actInv
(
nu_joint
).
toVector
(),
1e-12
));
getJacobian
<
true
>
(
model
,
data_ref
,
parent_idx
,
Jjj
);
nu_frame
=
Motion
(
Jff
*
q_dot
);
nu_joint
=
Motion
(
Jjj
*
q_dot
);
const
SE3
::
ActionMatrix_t
jXf
=
frame
.
placement
.
toActionMatrix
();
Data
::
Matrix6x
Jjj_from_frame
(
jXf
*
Jff
);
BOOST_CHECK
(
Jjj_from_frame
.
isApprox
(
Jjj
));
nu_frame_from_nu_joint
=
frame
.
placement
.
act
(
nu_frame
);
BOOST_CHECK
(
nu_frame
.
isApprox
(
frame
.
placement
.
actInv
(
nu_joint
),
1e-12
));
}
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