travelling_salesman.py

# {{{2 Imports and argument parsing
from __future__ import print_function
from CORBA import Any, TC_long, TC_float
from hpp.corbaserver import shrinkJointRange
from hpp.corbaserver.manipulation import Robot, loadServerPlugin, createContext, newProblem, ProblemSolver, ConstraintGraph, Rule, Constraints, CorbaClient
from hpp.gepetto.manipulation import ViewerFactory
from hpp_idl.hpp import Error as HppError
from agimus_demos import InStatePlanner
import sys, argparse, numpy as np, time, rospy
try:
    import tqdm
    def progressbar_iterable(iterable, *args, **kwargs):
        return tqdm.tqdm(iterable, *args, **kwargs)
    def progressbar_object(*args, **kwargs):
        return tqdm.tqdm(*args, **kwargs)
except ImportError:
    def progressbar_iterable(iterable, *args, **kwargs):
        return iterable
    def progressbar_object(*args, **kwargs):
        class faketqdm:
            def set_description(self, *args, **kwargs):
                pass
            def update(self, *args, **kwargs):
                pass
            def write(self, s):
                print(s)
            def close(self):
                pass
        return faketqdm()

# parse arguments
defaultContext = "corbaserver"
p = argparse.ArgumentParser (description=
                             'Initialize demo of Pyrene manipulating a box')
p.add_argument ('--context', type=str, metavar='context',
                default=defaultContext,
                help="identifier of ProblemSolver instance")
p.add_argument ('--n-random-handles', type=int, default=None,
                help="Generate a random model with N handles.")
args = p.parse_args ()

if args.context != defaultContext:
    createContext (args.context)
isSimulation = args.context == "simulation"
# 2}}}

# {{{2 Robot and problem definition

#Robot.urdfFilename = "package://tiago_data/robots/tiago_steel_without_wheels.urdf"
#Robot.srdfFilename = "package://tiago_data/srdf/tiago.srdf"
try:
    import rospy
    Robot.urdfString = rospy.get_param('robot_description')
    print("reading URDF from ROS param")
except:
    print("reading generic URDF")
    from hpp.rostools import process_xacro, retrieve_resource
    Robot.urdfString = process_xacro("package://tiago_description/robots/tiago.urdf.xacro", "robot:=steel", "end_effector:=pal-hey5", "ft_sensor:=schunk-ft")
Robot.srdfString = ""

if args.n_random_handles is None:
    srdf_cylinder = "package://agimus_demos/srdf/cylinder.srdf"
else:
    from generate_obstacle_model import generate_srdf
    bvh_file = "/home/jmirabel/devel/hpp/src/agimus-demos/meshes/cylinder.stl"
    srdf_cylinder = "/tmp/cylinder.srdf"
    with open(srdf_cylinder, 'w') as output:
        generate_srdf(bvh_file, args.n_random_handles, output)

class Cylinder:
    urdfFilename = "package://agimus_demos/urdf/cylinder.urdf"
    srdfFilename = srdf_cylinder
    rootJointType = "freeflyer"

class Driller:
    urdfFilename = "package://gerard_bauzil/urdf/driller_with_qr_drill.urdf"
    srdfFilename = "package://gerard_bauzil/srdf/driller.srdf"
    rootJointType = "freeflyer"

class PartP72:
    urdfFilename = "package://agimus_demos/urdf/P72-with-table.urdf"
    srdfFilename = "package://agimus_demos/srdf/P72.srdf"
    rootJointType = "freeflyer"


print("context=" + args.context)
loadServerPlugin (args.context, "manipulation-corba.so")
newProblem()
client = CorbaClient(context=args.context)
client.basic._tools.deleteAllServants()
def wd(o):
    from hpp.corbaserver import wrap_delete
    return wrap_delete(o, client.basic._tools)
client.manipulation.problem.selectProblem (args.context)

robot = Robot("robot", "tiago", rootJointType="planar", client=client)
crobot = wd(wd(robot.hppcorba.problem.getProblem()).robot())

from tiago_fov import TiagoFOV, TiagoFOVGuiCallback
from hpp import Transform
tiago_fov = TiagoFOV(urdfString = Robot.urdfString,
        # Real field of view angles are (49.5, 60),
        fov = np.radians((44.5, 55)),
        geoms = [ "arm_3_link_0" ])

# Compute Tiago joints that are bounded to shrink their range when necessary
tiagoBoundedJoints = list()
for j in robot.jointNames:
    if j[:6] != "tiago/": continue
    tj = j[6:]
    if tj.startswith("torso") or tj.startswith("arm") or tj.startswith("head"):
        tiagoBoundedJoints.append(j)

class Tag:
    def __init__(self, n, s):
        self.name = n
        self.size = s
class Tags:
    """
    Set of tags with visibility specifications

      - n_visibility_thr: minimal number of tags visible in the set to be
        able to perform an accurate localization,
      - depth_margin: increasing ratio of the pyramidal fields of view in the
        depth direction. If C is the top of the pyramid and P a vertex, P is
        moved away from C in such a way that the distance PC is multiplied by
        1 + depth_margin.
      - size_margin increasing ratio of the pyramid base.
    """
    def __init__(self, tags, n_visibility_thr, depth_margin, size_margin):
        self.tags = tags
        self.n_visibility_thr = n_visibility_thr
        self.depth_margin = depth_margin
        self.size_margin = size_margin
    @property
    def names(self):
        return [ t.name for t in self.tags ]
    @property
    def sizes(self):
        return [ t.size for t in self.tags ]
tagss = [
        Tags([ Tag('driller/tag36_11_00230', 0.0400+0.01),
               Tag('driller/tag36_11_00023', 0.0400+0.01)
               ], 
               1, 0.005, 0.1),
        Tags([ Tag('part/tag36_11_00100', 0.0600+0.01),
               Tag('part/tag36_11_00102', 0.0600+0.01),
               Tag('part/tag36_11_00001', 0.0845+0.01),
               Tag('part/tag36_11_00006', 0.0845+0.01),
               Tag('part/tag36_11_00005', 0.0845+0.01),
               Tag('part/tag36_11_00015', 0.0845+0.01),
               Tag('part/tag36_11_00014', 0.0845+0.01),
               Tag('part/tag36_11_00013', 0.0845+0.01) ],
               2, 0.01, 0.1),
               ]
tiago_fov_gui = TiagoFOVGuiCallback(robot, tiago_fov, tagss)

qneutral = crobot.neutralConfiguration()
qneutral[robot.rankInConfiguration['tiago/hand_thumb_abd_joint']] = 1.5707
qneutral[robot.rankInConfiguration['tiago/hand_index_abd_joint']]  = 0.35
qneutral[robot.rankInConfiguration['tiago/hand_middle_abd_joint']] = -0.1
qneutral[robot.rankInConfiguration['tiago/hand_ring_abd_joint']]   = -0.2
qneutral[robot.rankInConfiguration['tiago/hand_little_abd_joint']] = -0.35
removedJoints = [
            'tiago/caster_back_left_1_joint',
            'tiago/caster_back_left_2_joint',
            'tiago/caster_back_right_1_joint',
            'tiago/caster_back_right_2_joint',
            'tiago/caster_front_left_1_joint',
            'tiago/caster_front_left_2_joint',
            'tiago/caster_front_right_1_joint',
            'tiago/caster_front_right_2_joint',
            'tiago/suspension_left_joint',
            'tiago/wheel_left_joint',
            'tiago/suspension_right_joint',
            'tiago/wheel_right_joint',

            # Comment this 3 joints otherwise sot is not happy
            #'tiago/hand_index_joint',
            #'tiago/hand_mrl_joint',
            #'tiago/hand_thumb_joint',

            'tiago/hand_index_abd_joint',
            'tiago/hand_index_virtual_1_joint',
            'tiago/hand_index_flex_1_joint',
            'tiago/hand_index_virtual_2_joint',
            'tiago/hand_index_flex_2_joint',
            'tiago/hand_index_virtual_3_joint',
            'tiago/hand_index_flex_3_joint',
            'tiago/hand_little_abd_joint',
            'tiago/hand_little_virtual_1_joint',
            'tiago/hand_little_flex_1_joint',
            'tiago/hand_little_virtual_2_joint',
            'tiago/hand_little_flex_2_joint',
            'tiago/hand_little_virtual_3_joint',
            'tiago/hand_little_flex_3_joint',
            'tiago/hand_middle_abd_joint',
            'tiago/hand_middle_virtual_1_joint',
            'tiago/hand_middle_flex_1_joint',
            'tiago/hand_middle_virtual_2_joint',
            'tiago/hand_middle_flex_2_joint',
            'tiago/hand_middle_virtual_3_joint',
            'tiago/hand_middle_flex_3_joint',
            'tiago/hand_ring_abd_joint',
            'tiago/hand_ring_virtual_1_joint',
            'tiago/hand_ring_flex_1_joint',
            'tiago/hand_ring_virtual_2_joint',
            'tiago/hand_ring_flex_2_joint',
            'tiago/hand_ring_virtual_3_joint',
            'tiago/hand_ring_flex_3_joint',
            'tiago/hand_thumb_abd_joint',
            'tiago/hand_thumb_virtual_1_joint',
            'tiago/hand_thumb_flex_1_joint',
            'tiago/hand_thumb_virtual_2_joint',
            'tiago/hand_thumb_flex_2_joint',
            ]
crobot.removeJoints(removedJoints, qneutral)
tiago_fov.reduceModel(removedJoints, qneutral, len_prefix=len("tiago/"))
del crobot
robot.insertRobotSRDFModel("tiago", "package://tiago_data/srdf/tiago.srdf")
robot.insertRobotSRDFModel("tiago", "package://tiago_data/srdf/pal_hey5_gripper.srdf")
robot.setJointBounds('tiago/root_joint', [-10, 10, -10, 10])
ps = ProblemSolver(robot)
ps.loadPlugin("manipulation-spline-gradient-based.so")
vf = ViewerFactory(ps)

vf.loadRobotModel (Driller, "driller")
robot.insertRobotSRDFModel("driller", "package://gerard_bauzil/srdf/qr_drill.srdf")
robot.setJointBounds('driller/root_joint', [-10, 10, -10, 10, 0, 2])
vf.loadRobotModel (PartP72, "part")

#Modify from [-2,2] to [-5,5] to allow testing near the tables
robot.setJointBounds('part/root_joint', [-5, 5, -5, 5, -2, 2])

srdf_disable_collisions_fmt = """  <disable_collisions link1="{}" link2="{}" reason=""/>\n"""
# Disable collision between tiago/hand_safety_box_0 and driller
srdf_disable_collisions = """<robot>"""
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/base_link")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_top")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_back")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_left")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_top_horizontal")
linka, linkb, enabled = robot.hppcorba.robot.autocollisionPairs()
for la, lb, en in zip(linka, linkb, enabled):
    if not en: continue
    disable = False
    # Disable collision between caster wheels and anything else
    if not disable:
        disable = ('caster' in la or 'caster' in lb)
    # Disable collision between tiago/hand (except hand_safety_box_0) and all other tiago links
    if not disable:
        hand_vs_other = False
        for l in [la, lb]:
            if l.startswith("tiago/hand_") and l != "tiago/hand_safety_box_0":
                disable = True
                break
    # Disable collision between driller/* and tiago/arm_[1234567]_link
    if not disable:
        for l0, l1 in [ (la, lb), (lb, la) ]:
            if l0.startswith("driller/") and l1.startswith("tiago/arm_") and l1.endswith("_link_0") and l1[10] in "1234567":
                disable = True
                break
    if disable:
        srdf_disable_collisions += srdf_disable_collisions_fmt.format(la[:la.rfind('_')], lb[:lb.rfind('_')])
# TODO
srdf_disable_collisions += "</robot>"
robot.client.manipulation.robot.insertRobotSRDFModelFromString("", srdf_disable_collisions)

vf.loadObstacleModel ("package://gerard_bauzil/urdf/gerard_bauzil.urdf", "room")
#vf.loadObstacleModel ("package://agimus_demos/urdf/P72-table.urdf", "table")
# Display Tiago Field of view.
#vf.guiRequest.append( (tiago_fov.loadInGui, {'self':None}))
# Display visibility cones.
vf.addCallback(tiago_fov_gui)

try:
    v = vf.createViewer()
except:
    print("Did not find viewer")

joint_bounds = {}
def setRobotJointBounds(which):
    for jn, bound in joint_bounds[which]:
        robot.setJointBounds(jn, bound)

joint_bounds["default"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]
shrinkJointRange(robot, tiagoBoundedJoints, 0.6)
joint_bounds["grasp-generation"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]
setRobotJointBounds("default")
shrinkJointRange(robot, tiagoBoundedJoints, 0.95)
joint_bounds["planning"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]

ps.selectPathValidation("Graph-Discretized", 0.05)
#ps.selectPathValidation("Graph-Dichotomy", 0)
#ps.selectPathValidation("Graph-Progressive", 0.02)
ps.selectPathProjector("Progressive", 0.2)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
if isSimulation:
    ps.setMaxIterProjection (1)

ps.setParameter("SimpleTimeParameterization/safety", 0.25)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
ps.setParameter("SteeringMethod/Carlike/turningRadius", 0.05)

q0 = robot.getCurrentConfig()
q0[:4] = [-3., 2., 0, -1]
#q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.15
q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.34
q0[robot.rankInConfiguration['tiago/arm_1_joint']] = 0.10
q0[robot.rankInConfiguration['tiago/arm_2_joint']] = -1.47
q0[robot.rankInConfiguration['tiago/arm_3_joint']] = -0.16
q0[robot.rankInConfiguration['tiago/arm_4_joint']] = 1.87
q0[robot.rankInConfiguration['tiago/arm_5_joint']] = -1.57
q0[robot.rankInConfiguration['tiago/arm_6_joint']] = 1.3
q0[robot.rankInConfiguration['tiago/arm_7_joint']] = 0.00

#q0[robot.rankInConfiguration['part/root_joint']:] = [0.926, -0.024, 0.000,0,0,0,1]
q0[robot.rankInConfiguration['part/root_joint']:] = [0,0.3,0.8,0,0,0,1]
# 2}}}

# {{{2 Constraint graph initialization

# {{{3 Constraint definition
def lockJoint(jname, q, cname=None, constantRhs=True):
    if cname is None:
        cname = jname
    s = robot.rankInConfiguration[jname]
    e = s+robot.getJointConfigSize(jname)
    ps.createLockedJoint(cname, jname, q[s:e])
    ps.setConstantRightHandSide(cname, constantRhs)
    return cname

# Add an alignment constraint between pregrasp and grasp for each
# part handle
# \param ps ProblemSolver instance,
# \param handle name of the handle: should be "part/handle_i" where i is an
#        integer,
# \param graph constraint graph
def addAlignmentConstrainttoEdge(ps, handle, graph):
    #recover id of handle
    handleId = all_handles.index(handle)
    J1, gripperPose = ps.robot.getGripperPositionInJoint(tool_gripper)
    J2, handlePose = ps.robot.getHandlePositionInJoint(handle)
    T1 = Transform(gripperPose)
    T2 = Transform(handlePose)
    constraintName = handle + '/alignment'
    ps.client.basic.problem.createTransformationConstraint2\
        (constraintName, J1, J2, T1.toTuple(), T2.toTuple(),
         [False, True, True, False, False, False])
    # Set constraint
    edgeName = tool_gripper + ' > ' + handle + ' | 0-0_12'
    graph.addConstraints(edge = edgeName, constraints = \
                         Constraints(numConstraints=[constraintName]))
    edgeName = tool_gripper + ' < ' + handle + ' | 0-0:1-{}_21'.format(handleId)
    graph.addConstraints(edge = edgeName, constraints = \
                         Constraints(numConstraints=[constraintName]))

ljs = list()
ps.createLockedJoint("tiago_base", "tiago/root_joint", [0,0,1,0])

lockJoint("part/root_joint", q0, "lock_part", constantRhs=False)
c_lock_part = ps.hppcorba.problem.getConstraint("lock_part")

for n in robot.jointNames:
    if n.startswith('tiago/gripper_') or n.startswith('tiago/hand_'):
        ljs.append(lockJoint(n, q0))

lock_arm = [ lockJoint(n, q0) for n in robot.jointNames
        if n.startswith("tiago/arm") or n.startswith('tiago/torso')]
lock_head = [ lockJoint(n, q0) for n in robot.jointNames
        if n.startswith("tiago/head")]

# Create "Look at gripper" constraints: for (X,Y,Z) the position of the gripper in the camera frame,
# (X, Y) = 0 and Z >= 0
tool_gripper = "driller/drill_tip"
ps.createPositionConstraint("look_at_gripper", "tiago/xtion_rgb_optical_frame", tool_gripper,
        (0,0,0), (0,0,0), (True,True,True))
look_at_gripper = ps.hppcorba.problem.getConstraint("look_at_gripper")
import hpp_idl
look_at_gripper.setComparisonType([hpp_idl.hpp.EqualToZero,hpp_idl.hpp.EqualToZero,hpp_idl.hpp.Superior])

# Create "Look at part" constraint
ps.createPositionConstraint("look_at_part", "tiago/xtion_rgb_optical_frame", "part/to_tag_00015",
        (0,0,0), (0,0,0), (True,True,False))
look_at_part = ps.hppcorba.problem.getConstraint("look_at_part")
# 3}}}

# {{{3 Constraint graph instanciation
from hpp.corbaserver.manipulation import ConstraintGraphFactory
graph = ConstraintGraph(robot, 'graph')
factory = ConstraintGraphFactory(graph)
factory.setGrippers([ "tiago/gripper", "driller/drill_tip", ])

all_handles = ps.getAvailable('handle')
part_handles = filter(lambda x: x.startswith("part/"), all_handles)
test_handles = [    'part/handle_10',
                    'part/handle_11',
                    'part/handle_12',
                    'part/handle_13',
                    'part/handle_14',
                    'part/handle_15',
                    'part/handle_16',
                    'part/handle_17',
                    'part/handle_18',
                    'part/handle_19',
                    'part/handle_20',
                    'part/handle_4',
                    'part/handle_5',
                    'part/handle_9']

factory.setObjects([ "driller", "part", ], [ [ "driller/handle", ], part_handles, ], [ [], [] ])

factory.setRules([
    # Forbid driller to grasp itself.
    Rule([ "driller/drill_tip", ], [ "driller/handle", ], False),
    # Tiago always hold the gripper.
    Rule([ "tiago/gripper", ], [ "", ], False), Rule([ "tiago/gripper", ], [ "part/.*", ], False),
    # Allow to associate drill_tip with part holes only.
    Rule([ "tiago/gripper", "driller/drill_tip", ], [ "driller/handle", ".*", ], True), ])
factory.generate()

graph.addConstraints(graph=True, constraints=Constraints(numConstraints=ljs))
free = "tiago/gripper grasps driller/handle"
loop_free = 'Loop | 0-0'
for n in graph.nodes.keys():
    if n == free: continue
    graph.addConstraints(node=n, constraints=Constraints(numConstraints=["look_at_gripper"]))
for e in graph.edges.keys():
    graph.addConstraints(edge=e, constraints=Constraints(numConstraints=["tiago_base"]))
#Add alignment constraints
for handle in part_handles:
    addAlignmentConstrainttoEdge(ps, handle, graph)

graph.createNode('home', priority=1000)
graph.createEdge('home', 'home', 'move_base')
graph.createEdge('home',  free , 'start_arm', isInNode="home")
graph.createEdge( free , 'home', 'end_arm', isInNode=free)

graph.addConstraints(node="home", constraints=Constraints(numConstraints=lock_arm+lock_head))
graph.addConstraints(edge="end_arm", constraints=Constraints(numConstraints=["tiago_base", "lock_part"]))
graph.addConstraints(edge="move_base", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"]))
graph.addConstraints(edge="start_arm", constraints=Constraints(numConstraints=['tiago/gripper grasps driller/handle', "lock_part"]))

cproblem = ps.hppcorba.problem.getProblem()

cgraph = cproblem.getConstraintGraph()

graph.initialize()
# 3}}}

res, q0, err = graph.generateTargetConfig('start_arm', q0, q0)
assert (res)

# {{{3 Constraint graph validation
graphValidation = ps.client.manipulation.problem.createGraphValidation()
graphValidation.validate(cgraph)
if graphValidation.hasErrors():
    print(graphValidation.str())
    print("Graph has infeasibilities")
    sys.exit(1)
elif graphValidation.hasWarnings():
    print(graphValidation.str())
    print("Graph has only warnings")
else:
    print("Graph *seems* valid.")
# 3}}}
# 2}}}

# {{{2 Functions for handling the TSP
def generate_valid_config_for_handle(handle, qinit, qguesses = [], NrandomConfig=10):
    edge = tool_gripper + " > " + handle
    ok = False
    from itertools import chain
    def project_and_validate(e, qrhs, q):
        res, qres, err = graph.generateTargetConfig (e, qrhs, q)
        return res and not tiago_fov.clogged(qres, robot, tagss) and robot.configIsValid(qres), qres
    qpg, qg = None, None
    for qrand in chain(qguesses, ( robot.shootRandomConfig() for _ in range(NrandomConfig) )):
        res, qpg = project_and_validate (edge+" | 0-0_01", qinit, qrand)
        if res:
            ok, qg = project_and_validate(edge+" | 0-0_12", qpg, qpg)
            if ok: break
    return ok, qpg, qg

def generate_valid_config(constraint, qguesses = [], NrandomConfig=10):
    from itertools import chain
    for qrand in chain(qguesses, ( robot.shootRandomConfig() for _ in range(NrandomConfig) )):
        res, qres = constraint.apply (qrand)
        if res and not tiago_fov.clogged(qres, robot, tagss) and robot.configIsValid(qres):
            return True, qres
    return False, None

class ClusterComputation:
    def __init__(self, cgraph, lock_part):
        self._cgraph = cgraph
        self._lock_part = lock_part

    ## Return constraints of node where the tool is in front of handle
    #  \param hi handle the tool should get in front of
    #  \return solver with constraints of the pregrasp node and with
    #          constraint of fixed part pose.
    #  \note the returned solver contains the following constraints
    #    \li driller in gripper,
    #    \li tool handle in pregrasp in front of hi,
    #    \li fixed position of object (parameterizable).
    #    \li camera looks at gripper
    def freeBaseConstraint(self, hi):
        ni = tool_gripper + " > " + hi + " | 0-0_pregrasp"
        cnode = wd(self._cgraph.get(graph.nodes[ni]))
        c = wd(cnode.configConstraint().getConfigProjector()).copy()
        c.add(self._lock_part, 0)
        return c

    ## Return target constraints of node where the tool is in front of handle
    #  \param hi handle the tool should get in front of
    #  \return solver with target constraints of the edge leading to the goal
    #          node.
    #  \note the returned solver contains the following constraints
    #    \li driller in gripper,
    #    \li tool handle in pregrasp in front of hi,
    #    \li fixed position of the base (parameterizable),
    #    \li fixed position of object (parameterizable).
    #    \li camera looks at gripper
    def fixedBaseConstraint(self, hi):
        ei = tool_gripper + " > " + hi + " | 0-0_01"
        cedge = wd(self._cgraph.get(graph.edges[ei]))
        return wd(cedge.targetConstraint().getConfigProjector())

    ## Return constraint of edge between pregrasp and grasp of tool gripper
    #  \param hi handle the tool should reach
    #  \return solver with target constraints of edge leading from pregrasp to
    #          grasp.
    #  \note the returned solver contains the following constraints
    #    \li driller in gripper,
    #    \li tool gripper at handle
    #    \li complement of tool gripper at handle (parameterizable)
    #    \li fixed position of the base (parameterizable),
    #    \li fixed position of object (parameterizable).
    #    \li camera looks at gripper
    def pregraspToGraspConstraint(self, hi):
        ei = tool_gripper + " > " + hi + " | 0-0_12"
        cedge = wd(self._cgraph.get(graph.edges[ei]))
        return wd(cedge.targetConstraint().getConfigProjector())

    ## Find a cluster of handles that contains one handle
    # \param hi handle contained in the cluster,
    # \param handles superset of the resulting cluster
    # \param qrhs configuration of the robot
    # \param fixed_based whether the base is fixed or can move in each cluster
    def find_cluster(self, hi, handles, qrhs, fixed_base=False, pbar=None,
            N_find_first=20,
            N_find_others=20):
        if pbar is None:
            write = print
        else:
            write = pbar.write
        #ni = part_gripper + " grasps " + hi
        #ni = part_gripper + " > " + hi + " | 0-0_pregrasp"
        qrand = q0
        best_cluster = []
        if fixed_base:
            cpi = self.fixedBaseConstraint(hi)
        else:
            cpi = self.freeBaseConstraint(hi)
        cpi.setRightHandSideFromConfig(qrhs)
        ci = self.pregraspToGraspConstraint(hi)

        step = [ False, ] * 4
        for k in range(N_find_first):
            # Compute config where Tiago is not in collision
            if k == 0:
                res, qphi = generate_valid_config(cpi, qguesses=[q0,], NrandomConfig = 0)
            else:
                res, qphi = generate_valid_config(cpi, qguesses=[], NrandomConfig = 1)
            if not res: continue
            step[0] = True
            ci.setRightHandSideFromConfig(qphi)
            res, qhi = generate_valid_config(ci, qguesses=[qphi,], NrandomConfig = 0)
            if not res: continue
            step[1] = True

            t_base = np.array(robot.hppcorba.robot.getJointsPosition(qhi, ['tiago/torso_fixed_joint'])[0][:3])
            cur_cluster = [ (hi, qphi, qhi) ]
            for hj in handles:
                if hj == hi: continue
                t_handle = np.array(robot.getHandlePositionInJoint(hi)[1][:3])
                if False and np.linalg.norm(t_handle-t_base) > 0.6:
                    #print("prune")
                    continue
                ok, qphj, qhj = generate_valid_config_for_handle(hj, qhi,
                        qguesses = [ qq for _, qq, qqq in cur_cluster ],
                        NrandomConfig=N_find_others)
                if ok:
                    cur_cluster.append((hj, qphj, qhj))
            if len(cur_cluster) > len(best_cluster):
                best_cluster = cur_cluster
                step[2] = True
        if   not step[0]: write("not able to generate pregrasp config")
        elif not step[1]: write("not able to generate grasp config")
        return best_cluster

    ## Compute clusters of handles reachable by separate base positions
    # \param handles list of handles to reach,
    # \param qrhs initial configuration of the robot.
    # \return list of clusters. Each cluster is a list of triples
    #         (handle_name, pregrasp config, config)
    def find_clusters(self, handles, qrhs,
            N_find_first=20,
            N_find_others=20):
        from random import choice
        start = time.time()
        clusters = []
        remaining_handles = handles[:]
        pbar = progressbar_object(desc="Looking for clusters", total=len(remaining_handles))
        n_consecutive_failure = 0
        while remaining_handles:
            cluster = self.find_cluster(choice(remaining_handles), remaining_handles, qrhs,
                    pbar=pbar, N_find_first=N_find_first, N_find_others=N_find_others)
            if len(cluster) == 0:
                n_consecutive_failure += 1
                if n_consecutive_failure <= 20: continue
                pbar.close()
                for h in remaining_handles:
                    print("Could not reach handle", h)
                break
            for hi, qpi, qi in cluster:
                remaining_handles.remove(hi)
            clusters.append(cluster)
            pbar.set_description(desc=str(len(clusters)) + " clusters found", refresh=False)
            pbar.update(len(cluster))
        return clusters

    ## Solve traveling salesman problem between configurations of a cluster
    # \param armPlanner motion planner for the arm only,
    # \param cluster the set of configurations to order,
    # \param qhome initial and final configuration. If not provided, the
    #        configuration with the same base pose as the first config in the
    #        cluster is computed.
    #
    # \li Build back and forth path of tool between pregrasp and grasp for each
    #     handle,
    # \li call InStatePlanner::solveTSP to compute the handle order, and
    # \li build the resulting path by concatenation.
    def solveTSP(self, armPlanner, cluster, qhome = None, pb_kwargs = {}):
        # Create home position
        if qhome is None:
            res, qhome, err = graph.generateTargetConfig('end_arm', cluster[0][1], cluster[0][1])

        configs = [ qhome ]
        # build back and forth path of tool between pregrasp and grasp for each
        # handle
        grasp_paths = []
        #print(permutation, arm_paths)
        for hi, qphi, qhi in cluster:
            # Compute grasp config
            ei = tool_gripper + " > " + hi + " | 0-0_12"
            cedge = wd(self._cgraph.get(graph.edges[ei]))
            p01 = cedge.getSteeringMethod().call(qphi, qhi)
            # from grasp to pregrasp
            eir = tool_gripper + " < " + hi + " | 0-0:1-{}_21".format(all_handles.index(hi))
            cedge = wd(self._cgraph.get(graph.edges[eir]))
            p10 = cedge.getSteeringMethod().call(qhi, qphi)

            grasp_paths.append((p01, p10))
            configs.append(qphi)

        if len(configs) == 1: return []
        permutation, arm_paths = armPlanner.solveTSP(configs, resetRoadmapEachTime=True,
                pb_kwargs = pb_kwargs)
        paths = []
        #print(permutation, arm_paths)
        for i, p in zip(permutation[1:], arm_paths):
            # p: path to go to
            # - pregrasp of cluster[i][0] if i > 0
            # - home if i == 0
            paths.append(p)
            if i > 0:
                paths.append(grasp_paths[i-1][0])
                paths.append(grasp_paths[i-1][1])
        return paths

def concatenate_paths(paths):
    if len(paths) == 0: return None
    p = paths[0].asVector()
    for q in paths[1:]:
        p.appendPath(q)
    return p
# 2}}}

# {{{2 Problem resolution

# {{{3 Finalize FOV filter
res, q, err = graph.applyNodeConstraints(free, q0)
assert res
robot.setCurrentConfig(q)
oMh, oMd = robot.hppcorba.robot.getJointsPosition(q, ["tiago/hand_tool_link", "driller/base_link"])
tiago_fov.appendUrdfModel(Driller.urdfFilename, "hand_tool_link",
        (Transform(oMh).inverse() * Transform(oMd)).toTuple(),
        prefix="driller/")
# 3}}}

# {{{3 Create InStatePlanner
armPlanner = InStatePlanner (ps, graph)
armPlanner.setEdge(loop_free)
#armPlanner.optimizerTypes = [ "SplineGradientBased_bezier3", ]
armPlanner.optimizerTypes = [ ]
armPlanner.cproblem.setParameter("SimpleTimeParameterization/safety", Any(TC_float, 0.25))
armPlanner.cproblem.setParameter("SimpleTimeParameterization/order", Any(TC_long, 2))
armPlanner.cproblem.setParameter("SimpleTimeParameterization/maxAcceleration", Any(TC_float, 1.0))
armPlanner.maxIterPathPlanning = 600
armPlanner.timeOutPathPlanning = 10.
# Set collision margin between mobile base and the rest because the collision model is not correct.
bodies = ("tiago/torso_fixed_link_0", "tiago/base_link_0")
cfgVal = wd(armPlanner.cproblem.getConfigValidations())
pathVal = wd(armPlanner.cproblem.getPathValidation())
for _, la, lb, _, _ in zip(*robot.distancesToCollision()):
    if la in bodies or lb in bodies:
        cfgVal.setSecurityMarginBetweenBodies(la, lb, 0.07)
        pathVal.setSecurityMarginBetweenBodies(la, lb, 0.07)
del cfgVal
del pathVal

basePlanner = InStatePlanner (ps, graph)
basePlanner.plannerType = "kPRM*"
basePlanner.optimizerTypes.append("RandomShortcut")
basePlanner.setEdge("move_base")
basePlanner.setReedsAndSheppSteeringMethod()

import security_margins
smBase = security_margins.SecurityMargins(robot.jointNames)
margin = 0.1
for i in [ 0, smBase.jid("part/root_joint") ]:
    smBase.margins[i,:] = margin
    smBase.margins[:,i] = margin
basePlanner.cproblem.setSecurityMargins(smBase.margins.tolist())

basePlanner.createEmptyRoadmap()
# 3}}}

# {{{3 Load (or create) mobile base roadmap
basePlannerUsePrecomputedRoadmap = False
if basePlannerUsePrecomputedRoadmap:
    from os import getcwd, path
    roadmap_file = getcwd() + "/roadmap-hpp.bin"
    if path.exists(roadmap_file):
        print("Reading mobile base roadmap", roadmap_file)
        basePlanner.readRoadmap(roadmap_file)
    else:
        print("Building mobile base roadmap")
        try:
            basePlanner.cproblem.setParameter('kPRM*/numberOfNodes', Any(TC_long,2000))
            basePlanner.buildRoadmap(q0)
            #sm.margins[:,:] = 0.
            #basePlanner.cproblem.setSecurityMargins(sm.margins.tolist())
        except HppError as e:
            print(e)
        print("Writing mobile base roadmap", roadmap_file)
        basePlanner.writeRoadmap(roadmap_file)

basePlanner.plannerType = "DiffusingPlanner"
basePlanner.maxIterPathPlanning = 1000
# 3}}}

# {{{3 Find handle clusters and solve TSP for each clusters.
clusters_comp = ClusterComputation(armPlanner.cgraph, c_lock_part)
setRobotJointBounds("grasp-generation")
clusters = clusters_comp.find_clusters(
        #part_handles[:4],
        part_handles,
        q0, N_find_first = 40, N_find_others = 40)
setRobotJointBounds("planning")
solve_tsp_problems = False
if solve_tsp_problems:
    clusters_path = []
    qhomes = [q0, ]
    for cluster in progressbar_iterable(clusters, "Find path for each cluster"):
        paths = clusters_comp.solveTSP(armPlanner, cluster, pb_kwargs={'position': 1})

        clusters_path.append(concatenate_paths(paths))
        if len(paths) > 0:
            qhomes.append(paths[0].initial())
# 3}}}

# {{{3 Solve TSP for the mobile base
if solve_tsp_problems:
    base_order, base_paths = basePlanner.solveTSP(qhomes, resetRoadmapEachTime=not basePlannerUsePrecomputedRoadmap)
# }}}

# 2}}}

# {{{2 Function for online reso 
def recompute_clusters(handles = None, qcurrent = None):
    if qcurrent is None:
        import estimation 
        qcurrent = estimation.get_current_config(robot, graph, q0)
        try:
            qcurrent = estimation.get_cylinder_pose(robot, qcurrent, timeout=0.5)
        except RuntimeError:
            pass
    if handles is None:
        handles = part_handles

    setRobotJointBounds("grasp-generation")
    clusters = clusters_comp.find_clusters(handles,
            qcurrent, N_find_first = 40, N_find_others = 40)
    setRobotJointBounds("planning")
    return clusters

def compute_base_path_to_cluster_init(i_cluster, qcurrent = None):
    if qcurrent is None:
        import estimation 
        qcurrent = estimation.get_current_config(robot, graph, q0)
        try:
            qcurrent = estimation.get_cylinder_pose(robot, qcurrent, timeout=0.5)
        except RuntimeError:
            pass

    setRobotJointBounds("default")
    valid, msg = robot.isConfigValid(qcurrent)
    outOfCollisionPath = None
    if not valid:
        if msg == 'Collision between object tiago/torso_fixed_column_link_0 and tiago/hand_safety_box_0' \
            or msg == 'Collision between object tiago/torso_fixed_column_link_0 and driller/base_link_0' \
            or msg == 'Collision between object tiago/base_link_0 and driller/tag_support_link_top_0' \
            or msg == 'Collision between object tiago/torso_fixed_column_link_0 and driller/tag_support_link_back_0':
            qcurrent2 = qcurrent[:]
            qcurrent2[robot.rankInConfiguration['tiago/arm_6_joint']] = 1.
            res, qcurrent2, err = graph.applyNodeConstraints("tiago/gripper grasps driller/handle", qcurrent2)
            if not res:
                print("could not get out of collision")
            else:
                csm = armPlanner.cproblem.getSteeringMethod()
                outOfCollisionPath = csm.call(qcurrent, qcurrent2)
                qcurrent = qcurrent2
        else:
            print("qcurrent is in collision")

    # Tuck arm.
    res, qtuck, err = graph.generateTargetConfig('end_arm', qcurrent, qcurrent)
    if not res: print("failed to tuck arm")
    if not robot.configIsValid(qtuck): print("qtuck invalid")
    tuckpath = armPlanner.computePath(qcurrent, qtuck, resetRoadmap=True)
    #tuckpath = armPlanner.timeParameterization(tuckpath)
    setRobotJointBounds("planning")
    # Move mobile base.
    qhome = clusters[i_cluster][0][1][:4] + qtuck[4:]
    res, qhome, err = graph.generateTargetConfig('move_base', qhome, qhome)
    if not res: print("failed to move base")
    basepath = basePlanner.computePath(qtuck, qhome, resetRoadmap=not basePlannerUsePrecomputedRoadmap)

    # Move the head.
    # Compute config where the robot looks at the part
    constraints = wd(armPlanner.cconstraints.getConfigProjector().copy())
    constraints.add(look_at_part, 0)
    constraints.setRightHandSideFromConfig(qhome)
    res, qend = constraints.apply(qhome)
    if not res: print("failed to look at the part. It may not be visible.")
    headpath = armPlanner.computePath(qhome, qend, resetRoadmap=True)
    #headpath = armPlanner.timeParameterization(headpath)

    if outOfCollisionPath is not None:
        path = outOfCollisionPath.asVector()
        path.concatenate(tuckpath)
    else:
        path = tuckpath
    path.concatenate(basepath)
    path.concatenate(headpath)
    return path

def compute_path_for_cluster(i_cluster, qcurrent = None):
    if qcurrent is None:
        import estimation 
        qcurrent = estimation.get_current_robot_and_cylinder_config(robot, graph, q0[:])

    cluster = clusters[i_cluster]
    print("Cluster:", i_cluster)
    print(cluster)
    # Recompute a configuration for each handle
    new_cluster = []
    setRobotJointBounds("grasp-generation")
    for hi, qphi, qhi in cluster:
        ok, qphi2, qhi2 = generate_valid_config_for_handle(hi, qcurrent,
                qguesses = [qphi,qcurrent,] + [ qpg for _, qpg, qg in new_cluster ],
                NrandomConfig=15)
        if ok:
            new_cluster.append( (hi, qphi2, qhi2) )
        else:
            print("Could not reach handle", hi)

    setRobotJointBounds("planning")
    if len(new_cluster) == 0: return None, None

    paths = clusters_comp.solveTSP(armPlanner, new_cluster, qhome=qcurrent)
    path = concatenate_paths(paths)
    #optpath = armPlanner.timeParameterization(path)
    return new_cluster, path
# 2}}}

def gotoHandle(handle, q0, robot):
    # generate_valid_config_for_handle(handle, qinit, qguesses = [], NrandomConfig=10):
    ok, qphi2, qhi2 = generate_valid_config_for_handle(handle, q0, [], NrandomConfig=15)
    new_cluster =[]
    if ok:
            
        new_cluster.append((handle, qphi2, qhi2))
    else:
        print("Could not reach handle", handle)
        return None
    
    setRobotJointBounds("default")
    
    paths = clusters_comp.solveTSP(armPlanner, new_cluster, qhome=q0)
    path = concatenate_paths(paths)
    return path
# vim: foldmethod=marker foldlevel=1