Objective

Define a constraint graph and plan a manipulation path.

Introduction

Open a terminal, cd into hpp-practicals directory and open 3 tab by typing CTRL+SHIFT+T twice. In the first terminal, type

hppcorbaserver

In the second terminal, type

cd script
python -i grasp_ball.py

In the third terminal, type

gepetto-gui

To display the robot and environment, create again a client to gepetto-gui in the python terminal:

>>> v = vf.createViewer ()
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

>>> v (q_init)
>>> v (q_goal)

Displaying the constraint graph

In gepetto-gui menu Windows, select item Constraint Graph. The following window should appear in gepetto-gui.

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.

hpp-plot-manipulation-graph

Alternatively, in the python terminal. You can display the constraint graph by typing

>>> graph.display (format='svg')

The graph should be displayed in a browser. By moving the mouse on nodes and edges, you can see the constraints associated to each graph element.

Solving the problem

Typing

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

>>> pp = PathPlayer (v)
>>> pp (0)

You can also display the path using the path player in gepetto-gui.

A more difficult problem

script grasp_ball_in_box.py defines the same problem as grasp_ball.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.

Exercise 2

Exercise 2.1

In order to help the manipulation planner, define in file grasp_ball_in_box.py 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.

Constraint graph
Warning In method graph.createNode the order of the nodes in the list given as input is important: when checking in which node a configuration lies, node constraints will be checked in the order of node creation.

Exercise 2.2

Using the above constraint graph, write a script that builds a sequence of paths from q_init to q_goal. An example of how to generate a path from q_init to a grasp configuration is provided by script solve_by_hand.py, for the manipulation problem without box.

The script, named solve_by_hand_with_box.py should define a list of indices named paths corresponding to indices in the vector of paths. Each path should be admissible with respect to the manipulation constraints, and the concatenation of the paths should start at q_init and end at q_goal.

Hints

To test a node named nodeName, you can use the following loop:

for i in range(100):
  q = robot.shootRandomConfig()
  res, q1, err = graph.applyNodeConstraints (nodeName, q)
  if res: break

If i==99 and res == False, the constraint is probably malformed. Otherwise configuration q1 is in node nodeName.

To test an edge name edgeName, you can use the following loop:

for i in range(100):
  q = robot.shootRandomConfig()
  res, q3, err = graph.generateTargetConfig (edgeName, q1, q2)
  if res: break

where q1 is a configuration in the starting node of edgeName. If i==99 and res == False, the constraint is probably malformed. Otherwise, configuration q3 is in the destination node of edgeName and accessible from q1 following this edge.

Some useful methods

# create a relative transformation constraint between two joints
#
#  name   :            name of the constraint,
#  joint1 :            name of the first joint,
#  joint2 :            name of the second joint,
#  relativeTransform : relative transformation of joint2 frame in joint1 frame,
#  mask :              list of 6 Boolean to select active coordinates of the constraint.
#  note if joint1 == "", joint1 is the world frame.
ps.createTransformationConstraint (name, joint1, joint2, relativeTransform, mask)

# Set whether right hand side of constraint is constant
#
#  name :     name of the constraint,
#  constant : Boolean value.
ps.setConstantRightHandSide (name, constant)

# Create nodes of the constraint graph
#
#  nodeList: list of names of the nodes to be created.
#
#  note:     Nodes are ordered according to the list passed to this method. When
#            determining to which node a configuration belongs, constraints of
#            the nodes are tested in the creation order. As a consequence, it
#            is important that if the subspace defined by "node1" is a subset of
#            the subspace defined by "node2", "node1" is placed before "node2"
#            in the input list.
graph.createNode (nodeList)

# Create an edge of the constraint graph
#
#  node1:     name of the node the edge starts from,
#  node2:     name of the node the edge reaches,
#  edgeName:  name of the edge,
#  belongsTo: name of the node in the subspace defined by which paths of the edge lie.
graph.createEdge (node1, node2, edgeName, weight, belongsTo)

# Set constraint relative to nodes and edges
#
#  nodeName :   name of the node
#  edgeName :   name of the edge
#  c1, c2   :  names of constraints to be passed to the node or edge
#
#  note:        one and only one argument between node and edge should be
#              provided.
graph.addConstraints (node = nodeName, edge = edgeName, constraints = \
                      Constraints (numConstraints = ['c1', 'c2']))

# Project a configuration on the supspace defined by a node
#
#  nodeName: name of the node,
#  q:        input configuration.
#
#  return:
#    whether projection succeeded (Boolean),
#    projected configuration,
#    numerical error
graph.applyNodeConstraints (nodeName, q)

# Project a configuration on a leaf of the foliation defined by an edge
#
#  edgeName: name of the edge,
#  q1:       configuration defining the leaf (right hand side of constraint),
#  q2:       configuration to project on the leaf.
#
#  return:
#    whether projection succeeded (Boolean),
#    projected configuration,
#    numerical error
graph.generateTargetConfig (edgeName, q1, q2)