afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05]) # Load obstacle models and visualise affordances. When loading an obstacle, # the affordance analysis is done automatically. afftool.loadObstacleModel("hpp-affordance-corba", "darpa", "planning", r) afftool.loadObstacleModel("hpp-affordance-corba", "box", "box1", r) afftool.visualiseAffordances('Support', r, SupportColour) # If an object is translated or rotated, the affordance analysis must # be relaunched. # First, delete the already created affordance objects (this function also # deletes them from viewer) afftool.deleteAffordances(r, 'box1/base_link_0') # Next, move the obstacle and recompute its position (updates viewer) ps.moveObstacle('box1/base_link_0', [0, 1, 0.25, 1, 0, 0, 0]) r.computeObjectPosition() # Now, reanalyse the object that was moved afftool.analyseObject('box1/base_link_0') # And visualise. Note that the below function may be used to visualise one # object or all objects, depending on its parameters afftool.visualiseAffordances('Support', r, SupportColour, 'box1/base_link_0') afftool.visualiseAffordances('Lean', r, LeanColour, 'box1/base_link_0') # Create more obstacles to further demonstrate the analysis process: # Some Lean affordances are no more valid due to the orientation of # the obstacle afftool.loadObstacleModel("hpp-affordance-corba", "box", "box2", r) afftool.deleteAffordances(r, 'box2/base_link_0') ps.moveObstacle('box2/base_link_0', [0, 1.5, 0.5, 0.985, 0.174, 0, 0]) r.computeObjectPosition()
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = q_init [::] q_goal [0:3] = [3, 0, 0.63]; r (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.loadObstacleModel (packageName, "darpa", "planning", r) afftool.loadObstacleModel ("hpp-ompl-benchmark", "cubicles_robot", "robo", r) afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) afftool.deleteAffordances(r,'robo/base_link_0') ps.moveObstacle('robo/base_link_0', [0,-0.75,0, 0.5,0.5,0.5,0.5]) r.computeObjectPosition() afftool.analyseObject('robo/base_link_0') afftool.visualiseAffordances('Support', r, [0.75, 0.75, 0.1], 'robo/base_link_0') # Choosing RBPRM shooter and path validation methods. # Note that the standard RRT algorithm is used. ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) # Solve the problem t = ps.solve () # Playing the computed path from hpp.gepetto import PathPlayer pp = PathPlayer (rbprmBuilder.client.basic, r) pp (0)