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
a4454e84
Commit
a4454e84
authored
Jun 21, 2017
by
Justin Carpentier
Committed by
GitHub
Jun 21, 2017
Browse files
Merge pull request #392 from jcarpent/topic/devel
Correct frameJacobians
parents
8c95e5c7
82a13aba
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/algorithm/frames.hpp
View file @
a4454e84
...
...
@@ -129,20 +129,19 @@ namespace se3
const
int
colRef
=
nv
(
model
.
joints
[
parent
])
+
idx_v
(
model
.
joints
[
parent
])
-
1
;
// Lever between the joint center and the frame center expressed in the global frame
const
SE3
::
Vector3
lever
(
data
.
oMi
[
parent
].
rotation
()
*
frame
.
placement
.
translation
());
getJacobian
<
local_frame
>
(
model
,
data
,
parent
,
J
);
if
(
!
local_frame
)
getJacobian
<
local_frame
>
(
model
,
data
,
parent
,
J
);
if
(
!
frame
.
placement
.
isIdentity
())
// Lever between the joint center and the frame center expressed in the global frame
const
SE3
::
Vector3
lever
(
data
.
oMi
[
parent
].
rotation
()
*
frame
.
placement
.
translation
());
for
(
int
j
=
colRef
;
j
>=
0
;
j
=
data
.
parents_fromRow
[(
size_t
)
j
])
{
for
(
int
j
=
colRef
;
j
>=
0
;
j
=
data
.
parents_fromRow
[(
size_t
)
j
])
{
if
(
!
local_frame
)
J
.
col
(
j
).
topRows
<
3
>
()
-=
lever
.
cross
(
J
.
col
(
j
).
bottomRows
<
3
>
());
else
J
.
col
(
j
)
=
oMframe
.
actInv
(
Motion
(
data
.
J
.
col
(
j
))).
toVector
();
}
if
(
!
local_frame
)
J
.
col
(
j
).
topRows
<
3
>
()
-=
lever
.
cross
(
J
.
col
(
j
).
bottomRows
<
3
>
());
else
J
.
col
(
j
)
=
oMframe
.
actInv
(
Motion
(
data
.
J
.
col
(
j
))).
toVector
();
}
}
...
...
unittest/frames.cpp
View file @
a4454e84
...
...
@@ -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