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
Jason Chemin
hpp-rbprm-corba
Commits
e43e79d8
Commit
e43e79d8
authored
Apr 11, 2018
by
Pierre Fernbach
Browse files
in comRRT methods : use projectStateToCOm instead of project_or_throw
parent
d2d0f01a
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/rbprmbuilder.impl.cc
View file @
e43e79d8
...
...
@@ -2309,8 +2309,13 @@ assert(s2 == s1 +1);
State
s1Bis
(
state1
);
hppDout
(
notice
,
"state1 = "
<<
model
::
displayConfig
(
state1
.
configuration_
));
hppDout
(
notice
,
"proj to CoM position : "
<<
paths
[
cT1
]
->
end
().
head
<
3
>
());
s1Bis
.
configuration_
=
project_or_throw
(
fullBody
(),
s1Bis
,
paths
[
cT1
]
->
end
().
head
<
3
>
(),
true
);
hppDout
(
notice
,
"state1 after projection= "
<<
model
::
displayConfig
(
s1Bis
.
configuration_
));
bool
successProjS1
=
(
projectStateToCOMEigen
(
s1Bis
,
paths
[
cT1
]
->
end
().
head
<
3
>
(),
100
)
>
0.5
);
if
(
!
successProjS1
)
{
hppDout
(
notice
,
"could not project state S1Bis on COM constraint"
);
throw
std
::
runtime_error
(
"could not project state S1Bis on COM constraint"
);
}
hppDout
(
notice
,
"state1 bis after projection= "
<<
model
::
displayConfig
(
s1Bis
.
configuration_
));
for
(
std
::
map
<
std
::
string
,
bool
>::
const_iterator
cit
=
s1Bis
.
contacts_
.
begin
();
cit
!=
s1Bis
.
contacts_
.
end
();
++
cit
)
{
...
...
@@ -2320,56 +2325,26 @@ assert(s2 == s1 +1);
State
s2Bis
(
state2
);
hppDout
(
notice
,
"state2 = "
<<
model
::
displayConfig
(
state2
.
configuration_
));
hppDout
(
notice
,
"proj to CoM position : "
<<
paths
[
cT2
]
->
end
().
head
<
3
>
());
s2Bis
.
configuration_
=
project_or_throw
(
fullBody
(),
s2Bis
,
paths
[
cT2
]
->
end
().
head
<
3
>
(),
true
);
hppDout
(
notice
,
"state2 after projection= "
<<
model
::
displayConfig
(
s2Bis
.
configuration_
));
for
(
std
::
map
<
std
::
string
,
bool
>::
const_iterator
cit
=
s2Bis
.
contacts_
.
begin
();
cit
!=
s2Bis
.
contacts_
.
end
();
++
cit
)
bool
successProjS2
=
(
projectStateToCOMEigen
(
s2Bis
,
paths
[
cT2
]
->
end
().
head
<
3
>
(),
100
)
>
0.5
);
if
(
!
successProjS2
)
{
hppDout
(
notice
,
"contact : "
<<
cit
->
first
<<
" = "
<<
cit
->
second
);
hppDout
(
notice
,
"could not project state S2Bis on COM constraint"
);
throw
std
::
runtime_error
(
"could not project state S2Bis on COM constraint"
);
}
hppDout
(
notice
,
"state2 bis after projection= "
<<
model
::
displayConfig
(
s2Bis
.
configuration_
));
ValidationReportPtr_t
rport
(
ValidationReportPtr_t
(
new
CollisionValidationReport
));
BasicConfigurationShooterPtr_t
shooter
=
BasicConfigurationShooter
::
create
(
fullBody
()
->
device_
);
bool
found
=
false
;
for
(
int
i
=
0
;
i
<
100
&&
!
found
;
++
i
)
{
fullBody
()
->
device_
->
currentConfiguration
(
s1Bis
.
configuration_
);
found
=
problemSolver
()
->
problem
()
->
configValidations
()
->
validate
(
s1Bis
.
configuration_
,
rport
);
if
(
!
found
)
{
std
::
cout
<<
"could not project s1Bis without collision at state "
<<
s1
<<
std
::
endl
;
std
::
cout
<<
"collission "
<<
*
rport
<<
std
::
endl
;
s1Bis
.
configuration_
=
*
shooter
->
shoot
();
s1Bis
.
configuration_
=
project_or_throw
(
fullBody
(),
s1Bis
,
paths
[
cT1
]
->
end
().
head
<
3
>
());
std
::
cout
<<
"projection succeedded "
<<
paths
[
cT1
]
->
end
().
head
<
3
>
()
<<
std
::
endl
;
}
}
if
(
found
)
std
::
cout
<<
"got out ! "
<<
std
::
endl
;
bool
found2
=
false
;
for
(
int
i
=
0
;
i
<
100
&&
found
&&
!
found2
;
++
i
)
{
fullBody
()
->
device_
->
currentConfiguration
(
s2Bis
.
configuration_
);
found2
=
problemSolver
()
->
problem
()
->
configValidations
()
->
validate
(
s2Bis
.
configuration_
,
rport
);
if
(
!
found2
)
{
std
::
cout
<<
"could not project s2Bis without collision at state "
<<
s1
<<
std
::
endl
;
std
::
cout
<<
"collission "
<<
*
rport
<<
std
::
endl
;
s2Bis
.
configuration_
=
*
shooter
->
shoot
();
s2Bis
.
configuration_
=
project_or_throw
(
fullBody
(),
s2Bis
,
paths
[
cT2
]
->
end
().
head
<
3
>
());
std
::
cout
<<
"projection succeedded "
<<
paths
[
cT2
]
->
end
().
head
<
3
>
()
<<
std
::
endl
;
}
}
if
(
!
found
||
!
found2
)
for
(
std
::
map
<
std
::
string
,
bool
>::
const_iterator
cit
=
s2Bis
.
contacts_
.
begin
();
cit
!=
s2Bis
.
contacts_
.
end
();
++
cit
)
{
std
::
cout
<<
"could not project without collision at state "
<<
s1
<<
std
::
endl
;
throw
std
::
runtime_error
(
"could not project without collision at state "
+
s1
);
hppDout
(
notice
,
"contact : "
<<
cit
->
first
<<
" = "
<<
cit
->
second
);
}
core
::
PathVectorPtr_t
resPath
=
core
::
PathVector
::
create
(
fullBody
()
->
device_
->
configSize
(),
fullBody
()
->
device_
->
numberDof
());
hppDout
(
notice
,
"ComRRT : projections done. begin rrt"
);
try
{
hppDout
(
notice
,
"begin comRRT states 1 and 1bis"
);
hppDout
(
notice
,
"begin comRRT states 1 and 1bis"
);
hppDout
(
notice
,
"state1 = r(["
<<
model
::
displayConfig
(
state1
.
configuration_
)
<<
"])"
);
hppDout
(
notice
,
"state1 bis = r(["
<<
model
::
displayConfig
(
s1Bis
.
configuration_
)
<<
"])"
);
core
::
PathPtr_t
p1
=
interpolation
::
comRRT
(
fullBody
(),
problemSolver
()
->
problem
(),
paths
[
cT1
],
state1
,
s1Bis
,
numOptimizations
,
true
);
hppDout
(
notice
,
"end comRRT"
);
...
...
src/rbprmbuilder.impl.hh
View file @
e43e79d8
...
...
@@ -331,7 +331,9 @@ namespace hpp {
virtual
CORBA
::
Short
createState
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
Names_t
&
contactLimbs
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
getConfigAtState
(
unsigned
short
stateId
)
throw
(
hpp
::
Error
);
virtual
double
setConfigAtState
(
unsigned
short
stateId
,
const
hpp
::
floatSeq
&
config
)
throw
(
hpp
::
Error
);
double
projectStateToCOMEigen
(
State
&
s
,
const
model
::
Configuration_t
&
com_target
,
unsigned
short
maxNumeSamples
)
throw
(
hpp
::
Error
);
double
projectStateToCOMEigen
(
unsigned
short
stateId
,
const
model
::
Configuration_t
&
com_target
,
unsigned
short
maxNumeSamples
)
throw
(
hpp
::
Error
);
virtual
double
projectStateToCOM
(
unsigned
short
stateId
,
const
hpp
::
floatSeq
&
com
,
unsigned
short
max_num_sample
)
throw
(
hpp
::
Error
);
virtual
void
saveComputedStates
(
const
char
*
filepath
)
throw
(
hpp
::
Error
);
virtual
void
saveLimbDatabase
(
const
char
*
limbname
,
const
char
*
filepath
)
throw
(
hpp
::
Error
);
...
...
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