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
Humanoid Path Planner
hpp-model
Commits
25f48b04
Commit
25f48b04
authored
Apr 26, 2016
by
Steve Tonneau
Browse files
BUG FIX: clone method for joint does not initialize initial transform
parent
8b735321
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/joint.cc
View file @
25f48b04
...
...
@@ -53,10 +53,12 @@ namespace hpp {
Joint
::
Joint
(
const
Joint
&
joint
)
:
configuration_
(
joint
.
configuration_
),
currentTransformation_
(
joint
.
initialPosition_
),
positionInParentFrame_
(
joint
.
positionInParentFrame_
),
linkInJointFrame_
(
joint
.
linkInJointFrame_
),
maximalDistanceToParent_
(
joint
.
maximalDistanceToParent_
),
configSize_
(
joint
.
configSize_
),
numberDof_
(
joint
.
numberDof_
),
initialPosition_
(
joint
.
initialPosition_
),
robot_
(),
body_
(
joint
.
body_
?
joint
.
body_
->
clone
(
this
)
:
0x0
),
name_
(
joint
.
name_
),
linkName_
(
joint
.
linkName_
),
children_
(),
parent_
(),
rankInConfiguration_
(
-
1
),
rankInVelocity_
(
-
1
),
...
...
tests/test-clone.cc
View file @
25f48b04
...
...
@@ -77,6 +77,19 @@ BOOST_AUTO_TEST_CASE(clone_device)
d1
->
computeForwardKinematics
();
d2
->
computeForwardKinematics
();
// now verifying that transforms are correctly updated as well
fcl
::
Vec3f
x
(
0
,
0
,
1
);
fcl
::
Transform3f
r1
,
r2
;
for
(
std
::
size_t
i
=
0
;
i
!=
jv1
.
size
();
++
i
)
{
r1
=
jv1
[
i
]
->
currentTransformation
();
r2
=
jv2
[
i
]
->
currentTransformation
();
BOOST_CHECK_MESSAGE
((
r1
.
getRotation
()
*
x
+
r1
.
getTranslation
()
-
r2
.
getRotation
()
*
x
+
r2
.
getTranslation
()).
norm
()
<
10e-2
,
"Transformation not matching for joint "
+
jv1
[
i
]
->
name
()
+
" and "
+
jv2
[
i
]
->
name
()
);
}
//collision checking with objects
fcl
::
CollisionObjectPtr_t
c1
=
d1
->
rootJoint
()
->
linkedBody
()
->
innerObjects
(
hpp
::
model
::
COLLISION
).
front
()
->
fcl
();
fcl
::
CollisionObjectPtr_t
c2
=
d2
->
rootJoint
()
->
linkedBody
()
->
innerObjects
(
hpp
::
model
::
COLLISION
).
front
()
->
fcl
();
...
...
@@ -96,4 +109,5 @@ BOOST_AUTO_TEST_CASE(clone_device)
fcl
::
collide
(
c1
.
get
(),
c2
.
get
(),
colReq2
,
colRes2
);
BOOST_CHECK_MESSAGE
(
!
colRes2
.
isCollision
(),
"Device robot and clone should not be colliding after collision"
);
}
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