Commit 738ec148 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Tutorial rrt and manipulation planning.

parents
SOURCES = \
exercise-1.asciidoc \
exercise-2.asciidoc
HTML_FILES=$(SOURCES:%.asciidoc=%.html)
all: $(HTML_FILES)
%.html: %.asciidoc
asciidoc --filter latex -a toc -a icons -o $@ $<
clean:
rm -f $(HTML_FILES)
Programming RRT algorithm
=========================
Objective
---------
Program RRT algorithm in python.
Instructions
------------
Open a terminal cd into be-isae directory and open 3 tab by typing CTRL+SHIFT+T twice.
In the first terminal, type
[source,sh]
----
hppcorbaserver
----
In the second terminal, type
[source,sh]
----
hpp-gui
----
In the third terminal, type
[source,sh]
----
cd script
python -i rrt.py
----
image::hpp-gui-ur5.png[width="40%",alt="hpp-gui graphical interface"]
You should see the above window displaying a manipulator robot surrounded by obstacles.
In the third terminal, you should see the message "Method solveBiRRT is not implemented yet". Open file +script/motion_planner.py+ in a text editor. The message is produced by method +solveBiRRT+ of class +MotionPlanner+.
Exercise
--------
In file +script/motion_planner.py+, remove instruction
[source,python]
----
print ("Method solveBiRRT is not implemented yet")
----
and implement RRT algorithm between markers
[source,python]
----
#### RRT begin
#### RRT end
----
Displaying a path
-----------------
While running, your RRT algorithm will produce paths to store in the roadmap
edges. Those paths are stored in a vector and can be displayed by the path
player +pp+. To display the result of your algorithm, type
[source,python]
----
pid = ps.numberPaths () - 1
if pid < 0: raise RuntimeError ("No path in vector")
pp (pid)
----
Hint
----
You can use methods of objects +robot+ and +ps+. To get the list of these
methods with documentation, type in the python terminal
[source,python]
----
help (robot)
help (ps)
----
In +class MotionPlanner+, you can access these object by
[source,python]
----
self.robot
self.ps
----
Some useful methods
~~~~~~~~~~~~~~~~~~~
[source,python]
----
robot.shootRandomConfig
ps.getNearestConfig
ps.directPath
ps.addConfigToRoadmap
ps.addEdgeToRoadmap
ps.pathLength
ps.configAtParam
ps.numberConnectedComponents
----
\ No newline at end of file
This diff is collapsed.
Manipulation planning
=====================
Objective
---------
Define a constraint graph and plan a manipulation path.
Introduction
------------
Open a terminal, cd into be-isae directory and open 3 tab by typing CTRL+SHIFT+T
twice. In the first terminal, type
[source,sh]
----
hpp-manipulation-server
----
In the second terminal, type
[source,sh]
----
hpp-gui
----
In the third terminal, type
[source,sh]
----
cd script
python -i manipulation.py
----
image::manipulation.png[width="40%",alt="hpp-gui graphical interface"]
You should see the above manipulator on a horizontal plane and a ball.
You can display the initial and goal configurations of the problem defined in
the script by typing respectively
[source,python]
----
r (q_init)
r (q_goal)
----
Displaying the constraint graph
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
in the python terminal. You can display the constraint graph by typing
[source,python]
----
graph.display (format='svg')
----
The graph should be displayed in a brower. By moving the mouse on
nodes and edges, you can see the constraints associated to each graph
element.
Solving the problem
~~~~~~~~~~~~~~~~~~~
Typing
[source,python]
----
ps.solve ()
----
should solve the problem in a minute or so.
Displaying the path
~~~~~~~~~~~~~~~~~~~
As in exercise 1, the path can be displayed using the path player
[source,python]
----
pp (0)
----
A more difficult problem
------------------------
script +manipulation_box.py+ defines the same problem as
+manipulation.py+, except that in the initial configuration, the ball
is in a box. The resolution takes a lot more time since RRT algorithm
needs to generate a lot of nodes before the gripper reaches the
initial configuration of the box.
Instructions
------------
In order to help the manipulation planner, define a constraint graph with
intermediate states like for instance:
- a state where the gripper is empty above the ball
- a state where the gripper holds the ball above the ground.
The graph below provides an example.
image::constraintgraph.png[width="40%",alt="Constraint graph"]
Hints
-----
Displaying the constraint graph
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Type in a terminal
[source,sh]
----
hpp-plot-manipulation-graph
----
The following window should pop up.
image::hpp-plot-manipulation-graph.png[width="40%",alt="hpp-plot-manipulation-graph"]
Click on buttons "Refresh" and "Statistics" to display the current constraint graph.
By clicking on edges, you can see some statistics about the roadmap extension.
image::hpp-plot-manipulation-graph-statistics.png[width="40%",alt="hpp-plot-manipulation-graph"]
This diff is collapsed.
from hpp import Transform
from manipulation import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
vf.loadEnvironmentModel (Ground, 'ground')
vf.loadObjectModel (Pokeball, 'pokeball')
robot.setJointBounds ('pokeball/base_joint_xyz', [-.4,.4,-.4,.4,-.1,1.])
r = vf.createViewer ()
q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 1, 0, 0, 0]
r (q1)
## Create constraint graph
graph = ConstraintGraph (robot, 'graph')
## Create constraint of relative position of the ball in the gripper when ball
## is grasped
graph.createGrasp ('grasp', 'ur5/gripper', 'pokeball/handle')
## Create nodes and edges
graph.createNode (['grasp', 'placement'])
graph.createEdge ('placement', 'placement', 'transit', 1, 'placement')
graph.createEdge ('grasp', 'grasp', 'transfer', 1, 'grasp')
graph.createEdge ('placement', 'grasp', 'grasp-ball', 1, 'placement')
graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'placement')
## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps.createTransformationConstraint ('placement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [False, False, True, True, True, False,])
# Create complement constraint
ps.createTransformationConstraint ('placement/complement', 'pokeball/base_joint_SO3', '', [0,0,0.025,1,0,0,0], [True, True, False, False, False, True,])
ps.setConstantRightHandSide ('placement', True)
ps.setConstantRightHandSide ('placement/complement', False)
## Set constraints of nodes and edges
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])
graph.setConstraints (edge='transit',
numConstraints = ['placement', 'placement/complement'])
graph.setConstraints (edge='transfer', numConstraints = ['grasp'])
graph.setConstraints (edge='grasp-ball',
numConstraints = ['placement', 'placement/complement'])
graph.setConstraints (edge='release-ball',
numConstraints = ['placement', 'placement/complement'])
## Project initial configuration on state 'placement'
res, q_init, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q1)
q2 = q1 [::]
q2 [7] = .2
## Project goal configuration on state 'placement'
res, q_goal, error = ps.client.manipulation.problem.applyConstraints \
(graph.nodes ['placement'], q2)
## Define manipulation planning problem
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
pp = PathPlayer (ps.client.basic, r)
## Build relative position of the ball with respect to the gripper
for i in range (100):
q = robot.shootRandomConfig ()
res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q)
if res and robot.isConfigValid (q3): break;
if res:
robot.setCurrentConfig (q3)
gripperPose = Transform (robot.getLinkPosition (gripperName))
ballPose = Transform (robot.getLinkPosition (ballName))
gripperAboveBall = gripperPose.inverse () * ballPose
gripperAboveBall.translation [2] += .1
from common import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
from hpp.corbaserver.manipulation.ur5 import Robot
from hpp.corbaserver.manipulation import ConstraintGraph as Parent
from hpp.corbaserver.manipulation import ProblemSolver
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import PathPlayer
Robot.urdfName = "ur5_gripper"
Robot.urdfSuffix = ""
Robot.srdfSuffix = ""
class Pokeball (object):
rootJointType = 'freeflyer'
packageName = 'hpp_environments'
meshPackageName = 'hpp_environments'
urdfName = 'ur_benchmark/pokeball'
urdfSuffix = ""
srdfSuffix = ""
class Ground (object):
rootJointType = 'anchor'
packageName = 'hpp_environments'
urdfName = 'ur_benchmark/ground'
meshPackageName = 'hpp_environments'
urdfSuffix = ""
srdfSuffix = ""
class Box (object):
rootJointType = 'anchor'
packageName = 'hpp_environments'
urdfName = 'ur_benchmark/box'
meshPackageName = 'hpp_environments'
urdfSuffix = ""
srdfSuffix = ""
class ConstraintGraph (Parent) :
def __init__ (self, robot, graphName, makeGraph = True) :
Parent.__init__ (self, robot, graphName, makeGraph)
## Apply constaints to a configuration
#
# \param node name of the node the constraints of which to apply
# \param input input configuration,
# \retval output output configuration,
# \retval error norm of the residual error.
def applyNodeConstraints (self, node, input) :
return self.client.problem.applyConstraints (self.nodes [node],
input)
## Apply edge constaints to a configuration
#
# \param edge name of the edge
# \param qfrom configuration defining the right hand side of the edge
# constraint,
# \param input input configuration,
# \retval output output configuration,
# \retval error norm of the residual error.
#
# Compute a configuration in the destination node of the edge,
# reachable from qFrom.
def generateTargetConfig (self, edge, qfrom, input) :
return self.client.problem.applyConstraintsWithOffset \
(self.edges [edge], qfrom, input)
## Build a path from qb to qe using the Edge::build.
# \param edge name of the edge to use.
# \param qb configuration at the beginning of the path
# \param qe configuration at the end of the path
# \retval return true if the path is built and fully projected.
# \retval indexNotProj -1 is the path could not be built. The index
# of the built path (before projection) in the
# in the ProblemSolver path vector.
# \retval indexProj -1 is the path could not be fully projected. The
# index of the built path (before projection) in the
# in the ProblemSolver path vector.
# No path validation is made. The paths can be retrieved using
# corbaserver::Problem::configAtParam
def buildAndProjectPath (self, edge, qb, qe) :
return self.client.problem.buildAndProjectPath \
(self.edges [edge], qb, qe)
## Get error of a config with respect to a node constraint
#
# \param config Configuration,
# \param node name of the node.
# \retval error the error of the node constraint for the
# configuration
# \return whether the configuration belongs to the node.
# Call method core::ConstraintSet::isSatisfied for the node
# constraints.
def getConfigErrorForNode (self, config, nodeId) :
return self.client.graph.getConfigErrorForNode \
(config, self.nodes [nodeId])
## Add a configuration to the roadmap.
# \param config to be added to the roadmap.
def addConfigToRoadmap (self, config):
return self.clientBasic.problem.addConfigToRoadmap(config)
## Add an edge to roadmap. If
# \param config1, config2 the ends of the path,
# \param pathId the index if the path in the vector of path,
# \param bothEdges if FALSE, only add config1 to config2, otherwise, If TRUE. add edges config1->config2 AND config2->config1.
def addEdgeToRoadmap (self, config1, config2, pathId, bothEdges):
return self.clientBasic.problem.addEdgeToRoadmap \
(config1, config2, pathId, bothEdges)
## Set that an edge is short
# \param edge name of the edge
# \param True or False
#
# When an edge is tagged as short, extension along this edge is
# done differently in RRT-like algorithms. Instead of projecting
# a random configuration in the destination node, the
# configuration to extend itself is projected in the destination
# node. This makes the rate of success higher.
def setShort (self, edge, isShort) :
return self.client.graph.setShort (self.edges [edge], isShort)
robot = Robot ('ur5-pokeball', 'ur5')
robot.client.basic.problem.setErrorThreshold (1e-4)
robot.client.basic.problem.setMaxIterations (40)
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
gripperName = 'ur5/wrist_3_link-tool0_fixed_joint'
ballName = 'pokeball/base_joint_SO3'
from math import sqrt
from hpp import Transform
from manipulation import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName
vf.loadEnvironmentModel (Ground, 'ground')
vf.loadEnvironmentModel (Box, 'box')
vf.moveObstacle ('box/base_link_0', [0.3+0.04, 0, 0.04, 1, 0, 0, 0])
vf.moveObstacle ('box/base_link_1', [0.3-0.04, 0, 0.04, 1, 0, 0, 0])
vf.moveObstacle ('box/base_link_2', [0.3, 0.04, 0.04, 1, 0, 0, 0])
vf.moveObstacle ('box/base_link_3', [0.3, -0.04, 0.04, 1, 0, 0, 0])
vf.loadObjectModel (Pokeball, 'pokeball')
robot.setJointBounds ('pokeball/base_joint_xyz', [-.4,.4,-.4,.4,-.1,1.])
r = vf.createViewer ()
q1 = [0, -1.57, 1.57, 0, 0, 0, .3, 0, 0.025, 1, 0, 0, 0]
r (q1)
## Create graph
graph = ConstraintGraph (robot, 'graph')
res, q_init, error = graph.applyNodeConstraints ('placement', q1)
q2 = q1 [::]
q2 [7] = .2
res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectPathValidation ("Dichotomy", 0)
pp = PathPlayer (ps.client.basic, r)
class MotionPlanner:
def __init__ (self, robot, ps):
self.robot = robot
self.ps = ps
def solveBiRRT (self, maxIter = float("inf")):
print ("Method solveBiRRT is not implemented yet")
self.ps.prepareSolveStepByStep ()
finished = False
# In the framework of the course, we restrict ourselves to 2 connected components.
nbCC = self.ps.numberConnectedComponents ()
if nbCC != 2:
raise Exception ("There should be 2 connected components.")
iter = 0
while True:
#### RRT begin
newConfigs = list ()
## Try connecting the new nodes together
for i in range (len(newConfigs)):
pass
#### RRT end
## Check if the problem is solved.
nbCC = self.ps.numberConnectedComponents ()
if nbCC == 1:
# Problem solved
finished = True
break
iter = iter + 1
if iter > maxIter:
break
if finished:
self.ps.finishSolveStepByStep ()
return self.ps.numberPaths () - 1
def solvePRM (self):
self.ps.prepareSolveStepByStep ()
#### PRM begin
#### PRM end
self.ps.finishSolveStepByStep ()
from hpp.corbaserver.ur5 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import ViewerFactory, PathPlayer
robot = Robot ('ur5')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
vf.loadObstacleModel ("hpp_environments","ur_benchmark/obstacles","obstacles")
vf.loadObstacleModel ("hpp_environments","ur_benchmark/table","table")
vf.loadObstacleModel ("hpp_environments","ur_benchmark/wall","wall")
r = vf.createViewer ()
q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [1.57, -1.57, -1.8, 0, 0.8, 0]
r (q2)
r (q3)
ps.setInitialConfig (q2)
ps.addGoalConfig (q3)
from motion_planner import MotionPlanner
m = MotionPlanner (robot, ps)
pathId = m.solveBiRRT (maxIter = 1000)
pp = PathPlayer (robot.client, r)
#pp (pathId)
% ALGORITHM STYLE -- Released 8 April 1996
% for LaTeX-2e
% Copyright -- 1994 Peter Williams
% E-mail Peter.Williams@dsto.defence.gov.au
\NeedsTeXFormat{LaTeX2e}
\ProvidesPackage{algorithm}
\typeout{Document Style `algorithm' - floating environment}
\RequirePackage{float}
\RequirePackage{ifthen}
\newcommand{\ALG@within}{nothing}
\newboolean{ALG@within}
\setboolean{ALG@within}{false}
\newcommand{\ALG@floatstyle}{ruled}
\newcommand{\ALG@name}{Algorithm}
\newcommand{\listalgorithmname}{List of \ALG@name s}
% Declare Options
% first appearance
\DeclareOption{plain}{
\renewcommand{\ALG@floatstyle}{plain}
}
\DeclareOption{ruled}{
\renewcommand{\ALG@floatstyle}{ruled}
}
\DeclareOption{boxed}{
\renewcommand{\ALG@floatstyle}{boxed}
}
% then numbering convention
\DeclareOption{part}{
\renewcommand{\ALG@within}{part}
\setboolean{ALG@within}{true}
}
\DeclareOption{chapter}{
\renewcommand{\ALG@within}{chapter}
\setboolean{ALG@within}{true}
}
\DeclareOption{section}{
\renewcommand{\ALG@within}{section}
\setboolean{ALG@within}{true}
}
\DeclareOption{subsection}{
\renewcommand{\ALG@within}{subsection}
\setboolean{ALG@within}{true}
}
\DeclareOption{subsubsection}{
\renewcommand{\ALG@within}{subsubsection}
\setboolean{ALG@within}{true}
}
\DeclareOption{nothing}{
\renewcommand{\ALG@within}{nothing}