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
gepetto-gui
In the third terminal, type
cd script
python -i grasp_ball.py
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 the python terminal. You can display the constraint graph by typing
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
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 (0)
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 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.
|
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
Displaying the constraint graph
Type in a terminal
hpp-plot-manipulation-graph
The following window should pop up.
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.
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. 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 # constraints: list of 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.setConstraints (node = nodeName, edge = edgeName, numConstraints = constraints) # 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. graph.generateTargetConfig (edgeName, q1, q2)