Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
H
hpp-manipulation
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Guilhem Saurel
hpp-manipulation
Commits
1ad7f333
Commit
1ad7f333
authored
10 years ago
by
Joseph Mirabel
Committed by
Joseph Mirabel
10 years ago
Browse files
Options
Downloads
Patches
Plain Diff
Integrate GraphSteeringMethod
parent
d25d4a9e
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
include/hpp/manipulation/problem.hh
+6
-1
6 additions, 1 deletion
include/hpp/manipulation/problem.hh
src/graph-path-validation.cc
+7
-4
7 additions, 4 deletions
src/graph-path-validation.cc
src/manipulation-planner.cc
+2
-3
2 additions, 3 deletions
src/manipulation-planner.cc
with
15 additions
and
8 deletions
include/hpp/manipulation/problem.hh
+
6
−
1
View file @
1ad7f333
...
@@ -19,6 +19,7 @@
...
@@ -19,6 +19,7 @@
#include
"hpp/manipulation/robot.hh"
#include
"hpp/manipulation/robot.hh"
#include
"hpp/manipulation/graph/graph.hh"
#include
"hpp/manipulation/graph/graph.hh"
#include
"hpp/manipulation/graph-path-validation.hh"
#include
"hpp/manipulation/graph-path-validation.hh"
#include
"hpp/manipulation/graph-steering-method.hh"
#include
"hpp/manipulation/fwd.hh"
#include
"hpp/manipulation/fwd.hh"
namespace
hpp
{
namespace
hpp
{
...
@@ -30,9 +31,10 @@ namespace hpp {
...
@@ -30,9 +31,10 @@ namespace hpp {
/// Constructor
/// Constructor
Problem
(
RobotPtr_t
robot
)
:
core
::
Problem
(
robot
),
Problem
(
RobotPtr_t
robot
)
:
core
::
Problem
(
robot
),
graph_
()
graph_
()
,
steeringMethod_
(
new
GraphSteeringMethod
(
robot
))
{
{
Parent
::
pathValidation
(
GraphPathValidation
::
create
(
Parent
::
pathValidation
(),
graph_
));
Parent
::
pathValidation
(
GraphPathValidation
::
create
(
Parent
::
pathValidation
(),
graph_
));
steeringMethod
(
steeringMethod_
);
}
}
/// Set the graph of constraints
/// Set the graph of constraints
...
@@ -42,6 +44,7 @@ namespace hpp {
...
@@ -42,6 +44,7 @@ namespace hpp {
GraphPathValidationPtr_t
graphValidation
=
pathValidation
();
GraphPathValidationPtr_t
graphValidation
=
pathValidation
();
if
(
graphValidation
)
if
(
graphValidation
)
graphValidation
->
constraintGraph
(
graph
);
graphValidation
->
constraintGraph
(
graph
);
steeringMethod_
->
constraintGraph
(
graph
);
}
}
/// Get the graph of constraints
/// Get the graph of constraints
...
@@ -66,6 +69,8 @@ namespace hpp {
...
@@ -66,6 +69,8 @@ namespace hpp {
private
:
private
:
/// The graph of constraints
/// The graph of constraints
graph
::
GraphPtr_t
graph_
;
graph
::
GraphPtr_t
graph_
;
/// Steering method
GraphSteeringMethodPtr_t
steeringMethod_
;
};
};
}
// namespace manipulation
}
// namespace manipulation
}
// namespace hpp
}
// namespace hpp
This diff is collapsed.
Click to expand it.
src/graph-path-validation.cc
+
7
−
4
View file @
1ad7f333
...
@@ -33,9 +33,10 @@ namespace hpp {
...
@@ -33,9 +33,10 @@ namespace hpp {
bool
GraphPathValidation
::
validate
(
bool
GraphPathValidation
::
validate
(
const
PathPtr_t
&
path
,
bool
reverse
,
PathPtr_t
&
validPart
)
const
PathPtr_t
&
path
,
bool
reverse
,
PathPtr_t
&
validPart
)
{
{
assert
(
path
);
PathPtr_t
pathGraphValid
;
PathPtr_t
pathGraphValid
;
bool
graphValid
=
impl_validate
(
path
,
reverse
,
pathGraphValid
);
bool
graphValid
=
impl_validate
(
path
,
reverse
,
pathGraphValid
);
bool
collisionValid
=
pathValidation_
->
validate
(
pathGraphValid
,
reverse
,
validPart
);
bool
collisionValid
=
pathValidation_
->
validate
(
pathGraphValid
,
reverse
,
validPart
);
return
graphValid
&&
collisionValid
;
return
graphValid
&&
collisionValid
;
}
}
...
@@ -45,18 +46,20 @@ namespace hpp {
...
@@ -45,18 +46,20 @@ namespace hpp {
size_t
start
=
0
,
size_t
start
=
0
,
end
=
path
->
numberPaths
();
end
=
path
->
numberPaths
();
int
inc
=
1
;
int
inc
=
1
;
value_type
timeOffset
=
path
->
timeRange
().
first
;
if
(
reverse
)
{
if
(
reverse
)
{
std
::
swap
(
start
,
end
);
std
::
swap
(
start
,
end
);
start
--
;
end
--
;
inc
=
-
1
;
inc
=
-
1
;
timeOffset
=
path
->
timeRange
().
second
;
}
}
PathPtr_t
validSubPart
;
PathPtr_t
validSubPart
;
value_type
timeOffset
=
path
->
timeRange
().
first
;
for
(
size_t
index
=
start
;
index
!=
end
;
index
+=
inc
)
{
for
(
size_t
index
=
start
;
index
!=
end
;
index
+=
inc
)
{
// We should stop at the first non valid subpath.
// We should stop at the first non valid subpath.
if
(
!
impl_validate
(
path
->
pathAtRank
(
index
),
reverse
,
validSubPart
))
{
if
(
!
impl_validate
(
path
->
pathAtRank
(
index
),
reverse
,
validSubPart
))
{
if
(
reverse
)
if
(
reverse
)
validPart
=
path
->
extract
(
validPart
=
path
->
extract
(
std
::
make_pair
(
timeOffset
+
validSubPart
->
timeRange
().
first
,
std
::
make_pair
(
timeOffset
-
validSubPart
->
timeRange
().
second
,
path
->
timeRange
().
second
));
path
->
timeRange
().
second
));
else
else
validPart
=
path
->
extract
(
validPart
=
path
->
extract
(
...
@@ -64,7 +67,7 @@ namespace hpp {
...
@@ -64,7 +67,7 @@ namespace hpp {
timeOffset
+
validSubPart
->
timeRange
().
second
));
timeOffset
+
validSubPart
->
timeRange
().
second
));
return
false
;
return
false
;
}
}
timeOffset
+=
path
->
pathAtRank
(
index
)
->
length
();
timeOffset
+=
inc
*
path
->
pathAtRank
(
index
)
->
length
();
}
}
// Here, every subpath is valid.
// Here, every subpath is valid.
validPart
=
path
;
validPart
=
path
;
...
...
This diff is collapsed.
Click to expand it.
src/manipulation-planner.cc
+
2
−
3
View file @
1ad7f333
...
@@ -107,9 +107,9 @@ namespace hpp {
...
@@ -107,9 +107,9 @@ namespace hpp {
return
false
;
return
false
;
core
::
SteeringMethodPtr_t
sm
(
problem
().
steeringMethod
());
core
::
SteeringMethodPtr_t
sm
(
problem
().
steeringMethod
());
core
::
PathPtr_t
path
=
(
*
sm
)
(
*
q_near
,
qProj_
);
core
::
PathPtr_t
path
=
(
*
sm
)
(
*
q_near
,
qProj_
);
path
->
constraints
(
graph
->
pathConstraint
(
edges
,
*
q_near
));
if
(
!
path
)
if
(
!
path
)
return
false
;
return
false
;
path
->
constraints
(
graph
->
pathConstraint
(
edges
,
*
q_near
));
core
::
PathValidationPtr_t
pathValidation
(
problem
().
pathValidation
());
core
::
PathValidationPtr_t
pathValidation
(
problem
().
pathValidation
());
pathValidation
->
validate
(
path
,
false
,
validPath
);
pathValidation
->
validate
(
path
,
false
,
validPath
);
return
true
;
return
true
;
...
@@ -138,8 +138,7 @@ namespace hpp {
...
@@ -138,8 +138,7 @@ namespace hpp {
if
(
possibleEdges
.
empty
())
if
(
possibleEdges
.
empty
())
continue
;
continue
;
path
=
(
*
sm
)
(
*
q1
,
*
q2
);
path
=
(
*
sm
)
(
*
q1
,
*
q2
);
path
->
constraints
(
graph
->
pathConstraint
(
possibleEdges
[
0
],
*
q1
));
if
(
path
&&
pathValidation
->
validate
(
path
,
false
,
validPath
))
{
if
(
pathValidation
->
validate
(
path
,
false
,
validPath
))
{
roadmap
()
->
addEdge
(
*
itn1
,
*
itn2
,
path
);
roadmap
()
->
addEdge
(
*
itn1
,
*
itn2
,
path
);
core
::
interval_t
timeRange
=
path
->
timeRange
();
core
::
interval_t
timeRange
=
path
->
timeRange
();
roadmap
()
->
addEdge
(
*
itn2
,
*
itn1
,
path
->
extract
roadmap
()
->
addEdge
(
*
itn2
,
*
itn1
,
path
->
extract
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment