Tutorial rrt and manipulation planning.

instructions/Makefile

+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)

instructions/exercise-1.asciidoc

+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

instructions/exercise-2.asciidoc

+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"]

script/manipulation.py

+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

script/manipulation/__init__.py

+from common import robot, vf, ConstraintGraph, ps, Ground, Box, Pokeball, PathPlayer, gripperName, ballName

script/manipulation/common.py

+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'

script/manipulation_box.py

+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)
+

script/motion_planner.py

+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 ()

script/rrt.py

+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)

slides/algorithm.sty

+% 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}