graph.setConstraints (edge='move-gripper-away', numConstraints = ['placement/complement'])
graph.setConstraints (edge='grasp-ball', numConstraints = ['Box'])
graph.setConstraints (edge='move-gripper-up', numConstraints = ['placement/complement'])
graph.setConstraints (edge='transfer', numConstraints = [])
graph.setConstraints (edge='approach-ground', numConstraints = [])
graph.setConstraints (edge='take-ball-away', numConstraints = [])
graph.setConstraints (edge='take-ball-up', numConstraints = ['Box', 'grasp'])
graph.setConstraints (edge='put-ball-down', numConstraints = ['Box','grasp'])
#Define Nodes constraints
graph.setConstraints (node='placement', numConstraints = ['placement'])
graph.setConstraints (node='grasp', numConstraints = ['grasp'])
graph.setConstraints (node='ball-above-ground', numConstraints = ['ball-above-ground', 'grasp'])
graph.setConstraints (node='grasp-placement', numConstraints = ['grasp', 'placement'])
graph.setConstraints (node='gripper-above-ball', numConstraints = ['gripper-above-ball', 'placement'])
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 ("Discretized", 0.01)
ps.selectPathProjector ("Progressive", 0.1)
pp = PathPlayer (ps.client.basic, r)
# Get the built graph
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (envSurfaces)
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = locklhand))
cg.setWeight ('Loop | f', 1)
cg.setWeight ('Loop | 0-0', 1)
cg.initialize ()
res, q_init, err = cg.applyNodeConstraints ('free', q_init)
if not res:
raise RuntimeError ("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints ('free', q_goal)
if not res:
raise RuntimeError ("Failed to project initial configuration")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20; success = 0
for i in range (N):
ps.clearRoadmap ()
def readRoadmap(self, filename):
self.roadmap = ps.client.manipulation.problem.readRoadmap\
(filename, self.crobot, self.cgraph)
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 ()
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))
handlesPerObject = [['box/handle']]
# envSurfaces = ['kitchen_area/pancake_table_table_top',
# 'kitchen_area/white_counter_top_sink']
# contactPerObject = [['box/box_surface']]
envSurfaces = []
contactPerObject = [[]]
ps.client.manipulation.graph.autoBuild('graph', grippers, boxes,
handlesPerObject, contactPerObject,
envSurfaces, rules)
cg = ConstraintGraph(robot, 'graph', makeGraph=False)
cg.setConstraints(graph=True, lockDof=locklhand)
cg.setWeight('pr2/l_gripper < box/handle | 0-0_ls', 0)
res, q_init, err = cg.applyNodeConstraints('free', q_init)
if not res:
raise RuntimeError("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints('free', q_goal)
if not res:
raise RuntimeError("Failed to project initial configuration")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.setMaxIterPathPlanning(5000)
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
N = 20
success = 0
for i in range(N):
graph.removeCollisionPairFromEdge(edge, "talos/arm_left_7_joint", joint)
removeTouchpointsCollision("right", "talos/right_gripper > desk/touchpoint_right_top | f_12", "universe", True)
removeTouchpointsCollision("right", "talos/right_gripper < desk/touchpoint_right_top | 1-1_21" ,"universe", True)
removeTouchpointsCollision("right", "talos/right_gripper > desk/touchpoint_right_front | f_12", "universe")
removeTouchpointsCollision("right", "talos/right_gripper < desk/touchpoint_right_front | 1-2_21", "universe")
removeTouchpointsCollision("left", "talos/left_gripper > desk/touchpoint_left_front | f_12", "universe")
removeTouchpointsCollision("left", "talos/left_gripper < desk/touchpoint_left_front | 0-3_21", "universe")
removeTouchpointsCollision("left", "talos/left_gripper > desk/touchpoint_left_drawer | f_12", "universe")
removeTouchpointsCollision("left", "talos/left_gripper < desk/touchpoint_left_drawer | 0-4_21", "universe")
removeTouchpointsCollision("right", "talos/right_gripper > desk/touchpoint_right_front | f_12", "desk/upper_case_bottom_upper_drawer_bottom_joint")
removeTouchpointsCollision("right", "talos/right_gripper < desk/touchpoint_right_front | 1-2_21", "desk/upper_case_bottom_upper_drawer_bottom_joint")
# create q_init and q_goal respecting the defined constraints
_, q_init, _ = graph.applyNodeConstraints("free", q)
q_goal = q_init[:] ; q_goal[len(q_goal)-1] -= 0.25
_, q_goal, _ = graph.applyNodeConstraints("free", q_goal)
v(q_init)
# create intermediate touchpoints configuration
success3, q_touch_1, residual_error3 = graph.applyNodeConstraints("talos/right_gripper grasps desk/touchpoint_right_top", q_init) # Collision between legs
success4, q_touch_2, residual_error4 = graph.applyNodeConstraints("talos/right_gripper grasps desk/touchpoint_right_front", q_init) # Collision between legs
success1, q_touch_3, residual_error1 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_front", q_init) # Collision between legs
success2, q_touch_4, residual_error2 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_drawer", q_init) # fail
# q_touch_1 redefinition
q_touch_1 = coda.q_touch_1
# q_touch_2 redefinition
q_touch_2 = coda.q_touch_2
for e in cg.edges.keys():
if cg.getWeight(e) == -1:
continue
if e[:4] == "Loop":
cg.setWeight(e, 1)
else:
cg.setWeight(e, 5)
cg.initialize()
n_init = 'table/gripper1 grasps box/handle1'
n_goal = 'table/gripper1 grasps box/handle2'
q_goal = q_init[::]
q_goal[-4:] = [0.5, -0.5, 0.5, 0.5]
# Project initial configuration on initial node
res, q_init, err = cg.applyNodeConstraints(n_init, q_init)
if not res: raise RuntimeError('Failed to project initial configuration')
res, q_goal, err = cg.applyNodeConstraints(n_goal, q_goal)
if not res: raise RuntimeError('Failed to project goal configuration')
# Set Gaussian configuration shooter.
robot.setCurrentConfig(q_init)
sigma = robot.getNumberDof() * [1.]
rank = robot.rankInVelocity[robot.displayName + '/root_joint']
sigma[rank:rank + 6] = 6 * [0.]
rank = robot.rankInVelocity[objects[0].name + '/root_joint']
sigma[rank:rank + 6] = 6 * [0.05]
robot.setCurrentVelocity(sigma)
ps.setParameter('ConfigurationShooter/Gaussian/useRobotVelocity', True)
ps.selectConfigurationShooter('Gaussian')
ps.setInitialConfig(q_init)
rules = [ Rule([".*"], [".*"], True), ]
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (envContactSurfaces)
factory.setObjects (objects, handlesPerObject, contactSurfacesPerObject)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints = Constraints \
(lockedJoints = locklhand))
cg.initialize ()
# 2}}}
res, q_init_proj, err = cg.applyNodeConstraints("free", q_init)
res, q_goal_proj, err = cg.applyNodeConstraints("free", q_goal)
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
print ps.solve()
ps.setTargetState (cg.nodes["pr2/l_gripper grasps box/handle2"])
print ps.solve()
# 1}}}
v = vf.createViewer ()
v (q_init_proj)
pp = PathPlayer (v, robot.client.basic)
]
cg = ConstraintGraph(robot, 'graph')
factory = ConstraintGraphFactory(cg)
factory.setGrippers(grippers)
factory.environmentContacts(envContactSurfaces)
factory.setObjects(objects, handlesPerObject, contactSurfacesPerObject)
factory.setRules(rules)
factory.generate()
cg.addConstraints (graph = True, constraints = Constraints \
(lockedJoints = locklhand))
cg.initialize()
# 2}}}
res, q_init_proj, err = cg.applyNodeConstraints("free", q_init)
res, q_goal_proj, err = cg.applyNodeConstraints("free", q_goal)
ps.setInitialConfig(q_init_proj)
ps.addGoalConfig(q_goal_proj)
# print ps.solve()
# ps.setTargetState (cg.nodes["pr2/l_gripper grasps box/handle2"])
# print ps.solve()
# 1}}}
# v = vf.createViewer ()
# v (q_init_proj)
], True),
])
factory.generate()
graph.addConstraints(graph=True, constraints=Constraints(numConstraints=ljs))
for n in [
'driller/drill_tip > skin/hole | 0-0_pregrasp',
'tiago/gripper grasps driller/handle : driller/drill_tip grasps skin/hole'
]:
graph.addConstraints(node=n,
constraints=Constraints(numConstraints=["gaze"]))
graph.initialize()
# Constraint in this state are explicit so ps.setMaxIterProjection(1) should not
# make it fail.
res, q1, err = graph.applyNodeConstraints(
'tiago/gripper grasps driller/handle', q0)
q1valid, msg = robot.isConfigValid(q1)
if not q1valid:
print(msg)
assert res
ps.setInitialConfig(q1)
if not isSimulation:
qrand = q1
for i in range(100):
q2valid, q2, err = graph.generateTargetConfig(
'driller/drill_tip > skin/hole | 0-0', q1, qrand)
if q2valid:
q2valid, msg = robot.isConfigValid(q2)
if q2valid:
