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
Guilhem Saurel
test-hpp
Commits
d8b94049
Commit
d8b94049
authored
Apr 25, 2015
by
Florent Lamiraux
Browse files
Make test projection compile
Test does not pass yet. Need to be fixed.
parent
889db853
Changes
1
Hide whitespace changes
Inline
Side-by-side
tests/projection.cc
View file @
d8b94049
...
...
@@ -53,7 +53,9 @@ using hpp::model::DevicePtr_t;
using
hpp
::
model
::
JointPtr_t
;
using
hpp
::
core
::
DifferentiableFunctionPtr_t
;
using
hpp
::
core
::
NumericalConstraint
;
using
hpp
::
core
::
NumericalConstraintPtr_t
;
using
hpp
::
core
::
Constraint
;
using
hpp
::
core
::
ConstraintPtr_t
;
using
hpp
::
core
::
ConfigProjector
;
using
hpp
::
core
::
ConfigProjectorPtr_t
;
using
hpp
::
core
::
BasicConfigurationShooter
;
...
...
@@ -66,7 +68,7 @@ using hpp::debug::Timer;
namespace
hpp_test
{
DevicePtr_t
robot
;
std
::
vector
<
std
::
vector
<
DifferentiableFunction
Ptr_t
>
>
functionSets
;
std
::
vector
<
std
::
vector
<
NumericalConstraint
Ptr_t
>
>
functionSets
;
template
<
typename
T
>
void
recursiveSubPartOf
(
const
std
::
vector
<
T
>&
set
,
std
::
vector
<
std
::
vector
<
T
>
>&
subparts
)
{
...
...
@@ -113,11 +115,12 @@ namespace hpp_test {
JointPtr_t
leftAnkle
=
robot
->
getJointByName
(
"LLEG_JOINT5"
);
JointPtr_t
rightAnkle
=
robot
->
getJointByName
(
"RLEG_JOINT5"
);
Configuration_t
nc
=
robot
->
neutralConfiguration
();
std
::
vector
<
DifferentiableFunctionPtr_t
>
funcs
=
hpp
::
wholebodyStep
::
createSlidingStabilityConstraint
(
robot
,
leftAnkle
,
rightAnkle
,
nc
);
std
::
vector
<
NumericalConstraintPtr_t
>
funcs
=
hpp
::
wholebodyStep
::
createSlidingStabilityConstraint
(
robot
,
leftAnkle
,
rightAnkle
,
nc
);
functionSets
.
clear
();
if
(
partOf
)
recursiveSubPartOf
<
DifferentiableFunction
Ptr_t
>
(
funcs
,
functionSets
);
recursiveSubPartOf
<
NumericalConstraint
Ptr_t
>
(
funcs
,
functionSets
);
else
functionSets
.
push_back
(
funcs
);
}
...
...
@@ -143,12 +146,14 @@ namespace hpp_test {
JointPtr_t
leftWrist
=
robot
->
getJointByBodyName
(
"l_gripper_tool_frame"
);
JointPtr_t
rightWrist
=
robot
->
getJointByBodyName
(
"r_gripper_tool_frame"
);
Configuration_t
nc
=
robot
->
neutralConfiguration
();
std
::
vector
<
DifferentiableFunction
Ptr_t
>
funcs
;
std
::
vector
<
NumericalConstraint
Ptr_t
>
funcs
;
fcl
::
Transform3f
transform
;
transform
.
setIdentity
();
funcs
.
push_back
(
hpp
::
constraints
::
RelativeTransformation
::
create
(
robot
,
leftWrist
,
rightWrist
,
transform
));
funcs
.
push_back
(
NumericalConstraint
::
create
(
hpp
::
constraints
::
RelativeTransformation
::
create
(
robot
,
leftWrist
,
rightWrist
,
transform
)));
functionSets
.
clear
();
if
(
partOf
)
recursiveSubPartOf
<
DifferentiableFunction
Ptr_t
>
(
funcs
,
functionSets
);
recursiveSubPartOf
<
NumericalConstraint
Ptr_t
>
(
funcs
,
functionSets
);
else
functionSets
.
push_back
(
funcs
);
}
...
...
@@ -201,7 +206,7 @@ namespace projection {
for
(
size_t
i
=
0
;
i
<
functionSets
.
size
();
i
++
)
{
cp
.
push_back
(
ConfigProjector
::
create
(
robot
,
""
,
ERROR_THRESHOLD
,
MAX_ITERATIONS
));
for
(
size_t
j
=
0
;
j
<
functionSets
[
i
].
size
();
j
++
)
cp
.
back
()
->
add
(
NumericalConstraint
::
create
(
functionSets
[
i
][
j
])
)
;
cp
.
back
()
->
add
(
functionSets
[
i
][
j
]);
}
MESSAGE_INF
(
"There are "
<<
cp
.
size
()
<<
" projectors to be tested."
);
...
...
@@ -278,16 +283,22 @@ namespace svd {
bool
isSatisfied
(
ConfigurationIn_t
config
,
vector_t
&
error
)
{
computeValueAndJacobian
(
config
);
return
value_
.
squaredNorm
()
-
squareErrorThreshold_
;
error
=
value_
;
return
value_
.
squaredNorm
()
<
squareErrorThreshold_
;
}
void
add
Constraint
(
const
DifferentiableFunctionPtr_t
&
constraint
)
Constraint
Ptr_t
copy
()
const
{
size_
+=
constraint
->
outputSize
();
vector_t
value
(
constraint
->
outputSize
());
matrix_t
jacobian
(
constraint
->
outputSize
(),
abort
();
}
void
addConstraint
(
const
NumericalConstraintPtr_t
&
constraint
)
{
DifferentiableFunctionPtr_t
function
=
constraint
->
functionPtr
();
size_
+=
function
->
outputSize
();
vector_t
value
(
function
->
outputSize
());
matrix_t
jacobian
(
function
->
outputSize
(),
robot_
->
numberDof
());
constraints_
.
push_back
(
FunctionValueAndJacobian_t
(
constraint
,
value
,
constraints_
.
push_back
(
FunctionValueAndJacobian_t
(
function
,
value
,
jacobian
));
value_
.
resize
(
size_
);
Jacobian_
.
resize
(
size_
,
robot_
->
numberDof
());
...
...
@@ -607,11 +618,11 @@ namespace shuffle {
using
hpp_test
::
functionSets
;
std
::
vector
<
ConfigProjectorPtr_t
>
cp
;
std
::
vector
<
DifferentiableFunction
Ptr_t
>
dfs
=
functionSets
.
front
();
std
::
vector
<
NumericalConstraint
Ptr_t
>
dfs
=
functionSets
.
front
();
for
(
size_t
i
=
0
;
i
<
10
;
i
++
)
{
cp
.
push_back
(
ConfigProjector
::
create
(
robot
,
""
,
ERROR_THRESHOLD
,
MAX_ITERATIONS
));
for
(
size_t
j
=
0
;
j
<
dfs
.
size
();
j
++
)
cp
.
back
()
->
add
(
NumericalConstraint
::
create
(
dfs
[
j
])
)
;
cp
.
back
()
->
add
(
dfs
[
j
]);
std
::
random_shuffle
(
dfs
.
begin
(),
dfs
.
end
());
}
MESSAGE_INF
(
"There are "
<<
cp
.
size
()
<<
" ConfigProjector to be tested."
);
...
...
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