def makeSupervisorWithFactory (robot): from sot_hpp import Supervisor from sot_hpp.factory import Factory from sot_hpp.tools import Manifold from hpp.corbaserver.manipulation import Rule hppclient = Client() grippers = [ "talos/left_gripper", "talos/right_gripper" ] objects = [ "box" ] handlesPerObjects = [ [ "box/top", "box/bottom" ], ] rules = [ Rule([ "talos/left_gripper", ], [ "box/top", ], True), Rule([ "talos/right_gripper", ], [ "box/bottom", ], True), ] supervisor = Supervisor (robot, hpTasks = hpTasks(robot)) # supervisor = Supervisor (robot, hpTasks = hpTasks(robot), lpTasks = Manifold()) factory = Factory(supervisor) factory.setGrippers (grippers) factory.setObjects (objects, handlesPerObjects, [ [] for e in objects ]) factory.setRules (rules) factory.setupFrames (hppclient, robot) factory.generate () factory.finalize (hppclient) supervisor.makeInitialSot () return supervisor
def createInitializationGraph(name): graph = ConstraintGraph.buildGenericGraph( robot, name, ["talos/left_gripper", "talos/right_gripper"], ["box"], [Object.handles], [[]], [], [ Rule(["talos/left_gripper"], [Object.handles[0]], True), Rule(["talos/right_gripper"], [Object.handles[1]], True), ], ) return graph
def makeSupervisorWithFactory(robot): from agimus_sot import Supervisor from agimus_sot.factory import Factory, Affordance from hpp.corbaserver.manipulation import Rule holes = range(12) # holes = [ 11, ] hppclient = Client() grippers = ["tiago/center"] objects = ["workspace"] handlesPerObjects = [["workspace/hole2_" + str(i) for i in holes]] rules = [Rule(["tiago/center"], [".*"], True)] supervisor = Supervisor(robot) factory = Factory(supervisor) factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, [[] for e in objects]) factory.setRules(rules) factory.setupFrames(hppclient, robot) # This for loop could be omitted and are here for educational purpose. for i in holes: factory.addAffordance( Affordance( "tiago/center", "workspace/hole2_" + str(i), refOpen=(), refClose=() ) ) factory.generate() factory.finalize(hppclient) supervisor.makeInitialSot() return supervisor
def makeGraph(robot): from hpp.corbaserver.manipulation.constraint_graph_factory import ( ConstraintGraphFactory, ) graph = ConstraintGraph(robot, "graph") factory = ConstraintGraphFactory(graph) factory.setGrippers(["talos/left_gripper"]) factory.setObjects(["box"], [Object.handles], [Object.contacts]) factory.environmentContacts(Table.contacts) factory.setRules( [ Rule(["talos/left_gripper"], [Object.handles[1]], False), # Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), Rule(["talos/left_gripper"], [".*"], True), # Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ] ) factory.generate() return graph
def makeGraph(robot): graph = ConstraintGraph.buildGenericGraph( robot, 'graph', # [ "talos/left_gripper", "talos/right_gripper", "table/pose", ], [ "talos/left_gripper", "table/pose", ], [ "box", ], [ Object.handles + Object.poses, ], # [ Object.contacts, ], [ [], ], Table.contacts, [ Rule([ "table/pose", ], [ "box/handle[12]", ], False), Rule([ "talos/left_gripper", ], [ "box/pose[12]", ], False), Rule([ "table/pose", ], [ "box/pose1", ], False), Rule([ "talos/left_gripper", ], [ Object.handles[1], ], False), Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), # Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), Rule([ "table/pose", ], [ "box/pose2", ], True), ]) return graph
def makeRule(grasps): ''' Build a rule that will generate a state where each specified gripper will grasp the specify handle. This rule is used in the construction of the constraint graph to generate the corresponding state ''' _grippers = list() _handles = list() for (g, h) in grasps: _grippers.append(g) _handles.append(h) for g in grippers: if not g in _grippers: _grippers.append(g) _handles += (len(_grippers) - len(_handles)) * ['^$'] return Rule(grippers=_grippers, handles=_handles, link=True)
def makeSupervisorWithFactory (robot): from agimus_sot import Supervisor from agimus_sot.factory import Factory, Affordance from agimus_sot.tools import Manifold from agimus_sot.srdf_parser import parse_srdf from hpp.corbaserver.manipulation import Rule grippers = [ "talos/left_gripper", "table/pose" ] objects = [ "box" ] handlesPerObjects = [ [ "box/handle1", "box/handle2", "box/pose1", "box/pose2" ], ] rules = [ Rule([ "table/pose", ], [ "box/handle[12]", ], False), Rule([ "talos/left_gripper", ], [ "box/pose[12]", ], False), Rule([ "table/pose", ], [ "box/pose1", ], False), Rule([ "talos/left_gripper", ], [ "box/handle2", ], False), Rule([ "talos/left_gripper", ], [ "box/handle1", ], True), # Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), Rule([ "table/pose", ], [ "box/pose2", ], True), ] srdf = {} srdfTalos = parse_srdf ("srdf/talos.srdf", packageName = "talos_data", prefix="talos") srdfBox = parse_srdf ("srdf/cobblestone.srdf", packageName = "gerard_bauzil", prefix="box") srdfTable = parse_srdf ("srdf/pedestal_table.srdf", packageName = "gerard_bauzil", prefix="table") for w in [ "grippers", "handles" ]: srdf[w] = dict() for d in [ srdfTalos, srdfBox, srdfTable ]: srdf[w].update (d[w]) supervisor = Supervisor (robot, hpTasks = hpTasks(robot)) factory = Factory(supervisor) factory.parameters["period"] = rospy.get_param("/sot_controller/dt") factory.parameters["simulateTorqueFeedback"] = simulateTorqueFeedbackForEndEffector factory.parameters["addTracerToAdmittanceController"] = True factory.parameters["useMeasurementOfObjectsPose"] = True # factory.parameters["addTimerToSotControl"] = True factory.setGrippers (grippers) factory.setObjects (objects, handlesPerObjects, [ [] for e in objects ]) factory.setRules (rules) factory.setupFrames (srdf["grippers"], srdf["handles"], robot, disabledGrippers = ["table/pose"]) factory.addAffordance ( Affordance ("talos/left_gripper", "box/handle1", openControlType="torque", closeControlType="torque", refs = { "angle_open": (0,), "angle_close": (-0.5,), "torque": (-0.05,) }, simuParams = { "refPos": (-0.2,) })) factory.addAffordance ( Affordance ("talos/left_gripper", None, openControlType="position", closeControlType="position", refs = { "angle_open": (0,), "angle_close": (-0.5,), "torque": (-0.05,) }, simuParams = { "refPos": (-0.2,) })) factory.generate () supervisor.makeInitialSot () return supervisor
def makeRules (robot, grippers): handles = ['box/handle1', 'box/handle2', 'box/handle3', 'box/handle4'] res = list () res.append (Rule (grippers = grippers, handles = ['^$', '^$',], link = True)) for h in handles: res.append (Rule (grippers = grippers, handles = [h, '^$',], link = True)) res.append (Rule (grippers = grippers, handles = ['^$', h,], link = True)) res.append (Rule (grippers = grippers, handles = [handles [0], handles [1]], link = True)) res.append (Rule (grippers = grippers, handles = [handles [1], handles [0]], link = True)) res.append (Rule (grippers = grippers, handles = [handles [0], handles [3]], link = True)) res.append (Rule (grippers = grippers, handles = [handles [3], handles [0]], link = True)) res.append (Rule (grippers = grippers, handles = [handles [2], handles [1]], link = True)) res.append (Rule (grippers = grippers, handles = [handles [1], handles [2]], link = True)) res.append (Rule (grippers = grippers, handles = [handles [2], handles [3]], link = True)) res.append (Rule (grippers = grippers, handles = [handles [3], handles [2]], link = True)) return res
lockAll = lockFingers + lockHead # 4}}} # 3}}} handlesPerObject = list () handles = list () shapes = list () for i in xrange(K): handlesPerObject.append ([boxes[i] + "/handle2"]) handles.append (boxes[i] + "/handle2") shapes .append ([boxes[i] + "/box_surface"]) # Build rules rules = [Rule ([".*"], [".*"], True)] ps.client.manipulation.graph.autoBuild ("graph", grippers, boxes, handlesPerObject, shapes, ['table/pancake_table_table_top'], rules) # Get the built graph cg = ConstraintGraph (robot, 'graph', makeGraph = False) cg.setConstraints (graph = True, lockDof = lockAll) res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init) if not res[0]: raise Exception ('Init configuration could not be projected.') q_init_proj = res [1]
'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
# Create the graph. {{{3 grippers = ['pr2/l_gripper', 'pr2/r_gripper'] boxes = ['box'] handlesPerObject = [ ['box/handle', 'box/handle2'], ] objContactSurfaces = [ ['box/box_surface'], ] envSurfaces = [ 'kitchen_area/pancake_table_table_top', ] # Build rules rules = [Rule([""], [""], True)] 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 = lockAll)) cg.initialize() res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q_init) if not res[0]:
def makeSupervisorWithFactory(robot): from agimus_sot import Supervisor from agimus_sot.factory import Factory, Affordance from agimus_sot.srdf_parser import parse_srdf, attach_all_to_link import pinocchio from rospkg import RosPack rospack = RosPack() if not hasattr(robot, "camera_frame"): robot.camera_frame = "xtion_optical_frame" drillerModel = pinocchio.buildModelFromUrdf( rospack.get_path("gerard_bauzil") + "/urdf/driller_with_qr_drill.urdf") partModel = pinocchio.buildModelFromUrdf( rospack.get_path("agimus_demos") + "/urdf/P72-with-table.urdf") srdf = {} srdfTiago = parse_srdf("srdf/pal_hey5_gripper.srdf", packageName="tiago_data", prefix="tiago") srdfDriller = parse_srdf("srdf/driller.srdf", packageName="gerard_bauzil", prefix="driller") srdfQRDrill = parse_srdf("srdf/qr_drill.srdf", packageName="gerard_bauzil", prefix="driller") srdfPart = parse_srdf("srdf/P72.srdf", packageName="agimus_demos", prefix="part") attach_all_to_link(drillerModel, "base_link", srdfDriller) attach_all_to_link(drillerModel, "base_link", srdfQRDrill) attach_all_to_link(partModel, "base_link", srdfPart) grippers = "tiago/gripper", "driller/drill_tip" objects = "driller", "part", handlesPerObjects = ["driller/handle"], sorted(srdfPart["handles"].keys()), contactPerObjects = [], [] for w in ["grippers", "handles", "contacts"]: srdf[w] = dict() for d in [srdfTiago, srdfDriller, srdfQRDrill, srdfPart]: srdf[w].update(d[w]) supervisor = Supervisor(robot, hpTasks=hpTasks(robot)) factory = Factory(supervisor) factory.parameters["period"] = 0.01 # TODO soon: robot.getTimeStep() factory.parameters[ "simulateTorqueFeedback"] = simulateTorqueFeedbackForEndEffector factory.parameters["addTracerToAdmittanceController"] = False # factory.parameters["addTimerToSotControl"] = True factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, contactPerObjects) from hpp.corbaserver.manipulation import Rule 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.setupFrames(srdf["grippers"], srdf["handles"], robot) factory.gripperFrames["driller/drill_tip"].hasVisualTag = True for k in handlesPerObjects[1]: factory.handleFrames[k].hasVisualTag = True factory.addAffordance( Affordance( "tiago/gripper", "driller/handle", openControlType="position", closeControlType="position", refs={ "angle_open": (0., 0., 0.), "angle_close": (5.3, 5.72, 8.0), #"angle_close": (6.2,6.7,9.1), }, )) factory.generate() supervisor.makeInitialSot() return supervisor
], ] # Define the set of contact surfaces used for each object contactSurfacesPerObject = [ [ "box/box_surface", ], ] # Define the set of contact surfaces of the environment used to put objects envContactSurfaces = [ "kitchen_area/pancake_table_table_top", ] # Define rules for associating grippers and handles (here all associations are # allowed) 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(numConstraints=locklhand)) cg.initialize() ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal)
]) lockAll = lockFingers + lockHead # 4}}} # 3}}} handles = list() shapes = list() for i in xrange(K): handles.append([boxes[i] + "/handle2"]) shapes.append([boxes[i] + "/box_surface"]) if K is 2: rules = [ Rule(grippers[0], handles[1][0], False), Rule(grippers[1], handles[0][0], False), ] else: rules = [] # Build the constraint graph cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, boxes, handles, shapes, ['table/pancake_table_table_top'], rules) cg.setConstraints(graph=True, lockDof=lockAll) res = ps.client.manipulation.problem.applyConstraints(cg.nodes['free'], q_init) if not res[0]: raise Exception('Init configuration could not be projected.')
def makeSupervisorWithFactory(robot): from agimus_sot import Supervisor from agimus_sot.factory import Factory, Affordance from agimus_sot.srdf_parser import parse_srdf, attach_all_to_link import pinocchio from rospkg import RosPack rospack = RosPack() if not hasattr(robot, "camera_frame"): robot.camera_frame = "xtion_optical_frame" grippers = "tiago/gripper", "driller/drill_tip", objects = "driller", "skin", handlesPerObjects = [ "driller/handle", ], [ "skin/hole", ], contactPerObjects = [], [], drillerModel = pinocchio.buildModelFromUrdf( rospack.get_path("gerard_bauzil") + "/urdf/driller_with_qr_drill.urdf") srdf = {} srdfTiago = parse_srdf("srdf/pal_hey5_gripper.srdf", packageName="tiago_data", prefix="tiago") srdfDriller = parse_srdf("srdf/driller.srdf", packageName="gerard_bauzil", prefix="driller") srdfQRDrill = parse_srdf("srdf/qr_drill.srdf", packageName="gerard_bauzil", prefix="driller") srdfSkin = parse_srdf("srdf/aircraft_skin_with_marker.srdf", packageName="agimus_demos", prefix="skin") attach_all_to_link(drillerModel, "base_link", srdfDriller) attach_all_to_link(drillerModel, "base_link", srdfQRDrill) for w in ["grippers", "handles", "contacts"]: srdf[w] = dict() for d in [srdfTiago, srdfDriller, srdfQRDrill, srdfSkin]: srdf[w].update(d[w]) supervisor = Supervisor(robot, hpTasks=hpTasks(robot)) factory = Factory(supervisor) factory.parameters["period"] = 0.01 # TODO soon: robot.getTimeStep() factory.parameters[ "simulateTorqueFeedback"] = simulateTorqueFeedbackForEndEffector factory.parameters["addTracerToAdmittanceController"] = False # factory.parameters["addTimerToSotControl"] = True factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, contactPerObjects) from hpp.corbaserver.manipulation import Rule factory.setRules([ # Tiago always hold the gripper. Rule([ "tiago/gripper", ], [ "driller/handle", ], True), Rule([ "tiago/gripper", ], [ ".*", ], False), # Allow to associate drill_tip with skin/hole only. Rule([ "driller/drill_tip", ], [ "driller/handle", ], False), Rule([ "driller/drill_tip", ], [ ".*", ], True), ]) factory.setupFrames(srdf["grippers"], srdf["handles"], robot) factory.gripperFrames["driller/drill_tip"].hasVisualTag = True factory.handleFrames["skin/hole"].hasVisualTag = True factory.addAffordance( Affordance( "tiago/gripper", "driller/handle", openControlType="position", closeControlType="position", refs={ "angle_open": (0., 0., 0.), "angle_close": (5.3, 5.72, 8.0), #"angle_close": (6.2,6.7,9.1), }, )) # factory.addAffordance( # Affordance("driller/drill_tip", "skin/hole", # openControlType="torque", # closeControlType="position_torque", # refs={ # "angle_open": (0,), # "angle_close": (-0.5,), # "torque": (-0.07,), # }, # controlParams={ # "torque_num": (1.,), # "torque_denom": (10.,1.), # }, # simuParams={ # "M": 0., # "d": 5., # "k": 100., # "refPos": (-0.4,), # }, # ) factory.generate() supervisor.makeInitialSot() # starting_motion: From half_sitting to position where gaze and COM constraints are satisfied. # sot_loop = supervisor.sots["Loop | f"] # supervisor.addSolver("starting_motion", sot_loop) # supervisor.addSolver("loop_ss", sot_loop) # supervisor.addSolver("go_to_starting_state", sot_loop) return supervisor
s = robot.getJointConfigSize(n) r = robot.rankInConfiguration[n] if n.startswith("talos/gripper_right"): ps.createLockedJoint(n, n, q_init[r:r + s]) right_gripper_lock.append(n) elif n.startswith("talos/gripper_left"): ps.createLockedJoint(n, n, q_init[r:r + s]) left_gripper_lock.append(n) elif n in other_lock: ps.createLockedJoint(n, n, q_init[r:r + s]) cg = ConstraintGraph(robot, 'graph') # generate rules grippers = ["table/gripper1", "talos/left_gripper", "talos/right_gripper"] rules = [ Rule(grippers=grippers, handles=['box/handle1', '', ''], link=True), Rule(grippers=grippers, handles=['box/handle1', 'box/handle2', ''], link=True), Rule(grippers=grippers, handles=['', 'box/handle2', ''], link=True), Rule(grippers=grippers, handles=['', 'box/handle2', 'box/handle1'], link=True), Rule(grippers=grippers, handles=['', '', 'box/handle1'], link=True), Rule(grippers=grippers, handles=['box/handle2', '', 'box/handle1'], link=True), Rule(grippers=grippers, handles=['box/handle2', '', ''], link=True) ] factory = ConstraintGraphFactory(cg) factory.setGrippers(grippers)
elif n in other_lock: ps.createLockedJoint(n, n, half_sitting[r:r + s]) graph = ConstraintGraph.buildGenericGraph(robot, 'graph', [ "talos/left_gripper", "talos/right_gripper", ], [ "box", ], [ Object.handles, ], [ [], ], [], [ Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ]) graph.setConstraints(graph=True, lockDof=left_gripper_lock + right_gripper_lock + other_lock, numConstraints=["com_talos_box", "gaze"] + foot_placement) # graph.createNode (['free'], False, [1, ]) graph.initialize()
rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] q_goal[rank] = -0.5 rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] q_goal[rank] = 0.5 rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] q_goal[rank] = -0.5 rank = robot.rankInConfiguration['box/root_joint'] q_goal[rank:rank + 7] = [-4.8, -4.8, 0.9, 0, 0, 1, 0] #rank = robot.rankInConfiguration ['box/base_joint_SO3'] #q_goal [rank:rank+4] = [0, 0, 0, 1] # 2}}} # Build rules rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), Rule([""], [""], True)] locklhand = ['l_l_finger', 'l_r_finger'] ps.createLockedJoint('l_l_finger', 'pr2/l_gripper_l_finger_joint', [ 0.5, ]) ps.createLockedJoint('l_r_finger', 'pr2/l_gripper_r_finger_joint', [ 0.5, ]) grippers = ['pr2/l_gripper'] boxes = ['box'] handlesPerObject = [['box/handle']] # envSurfaces = ['kitchen_area/pancake_table_table_top', # 'kitchen_area/white_counter_top_sink'] # contactPerObject = [['box/box_surface']]
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle ps.addPartialCom('romeo', ['romeo/root_joint']) q = robot.getInitialConfig() r = placard.rank q[r:r + 3] = [.4, 0, 1.2] ps.createStaticStabilityConstraints('balance-romeo', q, 'romeo', type=ProblemSolver.FIXED_ON_THE_GROUND) commonConstraints = Constraints(numConstraints=ps.balanceConstraints(), lockedJoints=lockHands) # build graph rules = [ Rule([ "romeo/l_hand", "romeo/r_hand", ], ["placard/low", ""], True), Rule([ "romeo/l_hand", "romeo/r_hand", ], ["", "placard/high"], True), Rule([ "romeo/l_hand", "romeo/r_hand", ], ["placard/low", "placard/high"], True), ] grippers = ['romeo/r_hand', 'romeo/l_hand'] handlesPerObjects = [placard.handles.values()] cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, [
if n.startswith ("talos/gripper_right"): ps.createLockedJoint(n, n, half_sitting[r:r+s]) right_gripper_lock.append(n) elif n.startswith ("talos/gripper_left"): ps.createLockedJoint(n, n, half_sitting[r:r+s]) left_gripper_lock.append(n) elif n in other_lock: ps.createLockedJoint(n, n, half_sitting[r:r+s]) graph = ConstraintGraph.buildGenericGraph(robot, 'graph', [ "talos/left_gripper", "talos/right_gripper", ], [ "box", ], [ Object.handles, ], [ [ ], ], [ ], [ Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ] ) graph.setConstraints (graph=True, lockDof = left_gripper_lock + right_gripper_lock + other_lock, numConstraints = [ "com_talos_box", "gaze"] + foot_placement) graph.initialize() res, q_init, err = graph.applyNodeConstraints("talos/left_gripper grasps box/top", half_sitting) res, q_goal, err = graph.applyNodeConstraints("talos/right_gripper grasps box/bottom", half_sitting) print ps.directPath(q_init, q_init, True) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.setParameter("SimpleTimeParameterization/safety", CORBA.Any(CORBA.TC_double, 0.5)) ps.setParameter("SimpleTimeParameterization/velocity", CORBA.Any(CORBA.TC_boolean, True))
def makeSupervisorWithFactory(robot): from sot_hpp import Supervisor from sot_hpp.factory import Factory, Affordance from sot_hpp.tools import Manifold from sot_hpp.srdf_parser import parse_srdf from hpp.corbaserver.manipulation import Rule grippers = ["talos/left_gripper", "talos/right_gripper"] objects = ["box"] handlesPerObjects = [ ["box/top", "box/bottom"], ] rules = [ Rule([ "talos/left_gripper", ], [ "box/top", ], True), Rule([ "talos/right_gripper", ], [ "box/bottom", ], True), ] srdf = {} srdfTalos = parse_srdf("srdf/talos.srdf", packageName="talos_data", prefix="talos") # Full path can be provided with # srdfBox = parse_srdf ("cup.srdf") # srdfBox = parse_srdf ("srdf/cup.srdf", packageName = "hpp_tutorial", prefix="box") srdfBox = parse_srdf("talos/pass_box_between_hands/cup.srdf", packageName="sot_hpp_demo", prefix="box") for w in ["grippers", "handles"]: srdf[w] = dict() for d in [srdfTalos, srdfBox]: srdf[w].update(d[w]) supervisor = Supervisor(robot, hpTasks=hpTasks(robot)) factory = Factory(supervisor) # factory.addTimerToSotControl = True factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, [[] for e in objects]) factory.setRules(rules) factory.setupFrames(srdf["grippers"], srdf["handles"], robot) factory.addAffordance( Affordance("talos/left_gripper", "box/top", refOpen=(0, ), refClose=(-0.2, ))) factory.addAffordance( Affordance("talos/right_gripper", "box/bottom", refOpen=(0, ), refClose=(-0.2, ))) factory.generate() supervisor.makeInitialSot() return supervisor
def makeSupervisorWithFactory(robot): from agimus_sot import Supervisor from agimus_sot.factory import Factory, Affordance from agimus_sot.srdf_parser import parse_srdf from hpp.corbaserver.manipulation import Rule grippers = ["talos/left_gripper", "talos/right_gripper"] objects = ["mire"] handlesPerObjects = [["mire/left", "mire/right"]] contactPerObjects = [[]] rules = [ Rule(["talos/left_gripper"], ["mire/left"], True), # Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), Rule(["talos/right_gripper"], ["mire/right"], True), # Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ] srdf = {} srdfTalos = parse_srdf("srdf/talos.srdf", packageName="talos_data", prefix="talos") srdfMire = parse_srdf("srdf/calibration_mire.srdf", packageName="agimus_demos", prefix="mire") for w in ["grippers", "handles"]: srdf[w] = dict() for d in [srdfTalos, srdfMire]: srdf[w].update(d[w]) supervisor = Supervisor(robot, hpTasks=hpTasks(robot)) factory = Factory(supervisor) factory.parameters["period"] = 0.001 # TODO soon: robot.getTimeStep() factory.parameters[ "simulateTorqueFeedback"] = simulateTorqueFeedbackForEndEffector factory.parameters["addTracerToAdmittanceController"] = False factory.parameters["useMeasurementOfObjectsPose"] = False # factory.parameters["addTimerToSotControl"] = True factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, contactPerObjects) factory.setRules(rules) factory.setupFrames(srdf["grippers"], srdf["handles"], robot) factory.addAffordance( Affordance( "talos/left_gripper", "mire/left", openControlType="torque", closeControlType="torque", refs={ "angle_open": (0, ), "angle_close": (-0.5, ), "torque": (-0.05, ) }, controlParams={ "torque_num": (5000.0, 1000.0), "torque_denom": (0.01, ) }, simuParams={"refPos": (-0.2, )}, )) factory.addAffordance( Affordance( "talos/left_gripper", None, openControlType="position", closeControlType="position", refs={ "angle_open": (0, ), "angle_close": (-0.5, ), "torque": (-0.05, ) }, simuParams={"refPos": (-0.2, )}, )) factory.generate() supervisor.makeInitialSot() return supervisor
def makeSupervisorWithFactory(robot): from agimus_sot import Supervisor from agimus_sot.factory import Factory, Affordance from agimus_sot.srdf_parser import parse_srdf from hpp.corbaserver.manipulation import Rule grippers = ["talos/left_gripper"] objects = ["box"] handlesPerObjects = [["box/handle1", "box/handle2"]] contactPerObjects = [["box/bottom_surface"]] rules = [ Rule(["talos/left_gripper"], ["box/handle2"], False), # Rule([ "talos/left_gripper", ], [ Object.handles[0], ], True), Rule(["talos/left_gripper"], [".*"], True), # Rule([ "talos/right_gripper", ], [ Object.handles[1], ], True), ] srdf = {} srdfTalos = parse_srdf("srdf/talos.srdf", packageName="talos_data", prefix="talos") srdfBox = parse_srdf( "srdf/cobblestone.srdf", packageName="gerard_bauzil", prefix="box" ) srdfTable = parse_srdf( "srdf/pedestal_table.srdf", packageName="gerard_bauzil", prefix="table" ) for w in ["grippers", "handles"]: srdf[w] = dict() for d in [srdfTalos, srdfBox, srdfTable]: srdf[w].update(d[w]) supervisor = Supervisor(robot, hpTasks=hpTasks(robot)) factory = Factory(supervisor) factory.parameters["period"] = 0.001 # TODO soon: robot.getTimeStep() factory.parameters["simulateTorqueFeedback"] = simulateTorqueFeedbackForEndEffector factory.parameters["addTracerToAdmittanceController"] = True factory.parameters["useMeasurementOfObjectsPose"] = False # factory.parameters["addTimerToSotControl"] = True factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, contactPerObjects) factory.environmentContacts(["table/support"]) factory.setRules(rules) factory.setupFrames( srdf["grippers"], srdf["handles"], robot, disabledGrippers=["table/pose"] ) factory.addAffordance( Affordance( "talos/left_gripper", "box/handle1", openControlType="torque", closeControlType="torque", refs={"angle_open": (0,), "angle_close": (-0.5,), "torque": (-0.05,)}, controlParams={"torque_num": (5000.0, 1000.0), "torque_denom": (0.01,)}, simuParams={"refPos": (-0.2,)}, ) ) factory.addAffordance( Affordance( "talos/left_gripper", None, openControlType="position", closeControlType="position", refs={"angle_open": (0,), "angle_close": (-0.5,), "torque": (-0.05,)}, simuParams={"refPos": (-0.2,)}, ) ) factory.generate() supervisor.makeInitialSot() # starting_motion: From half_sitting to position where gaze and COM constraints are satisfied. supervisor.duplicateSolver("Loop | f", "starting_motion") return supervisor
def makeSupervisorWithFactory(robot): from agimus_sot import Supervisor from agimus_sot.factory import Factory, Affordance from agimus_sot.srdf_parser import parse_srdf from hpp.corbaserver.manipulation import Rule grippers = ["talos/left_gripper", "talos/right_gripper"] objects = ["box"] handlesPerObjects = [["box/top", "box/bottom"]] rules = [ Rule(["talos/left_gripper"], ["box/top"], True), Rule(["talos/right_gripper"], ["box/bottom"], True), ] srdf = {} srdfTalos = parse_srdf("srdf/talos.srdf", packageName="talos_data", prefix="talos") # Full path can be provided with srdfBox = parse_srdf("srdf/cup.srdf", packageName="agimus_demos", prefix="box") for w in ["grippers", "handles"]: srdf[w] = dict() for d in [srdfTalos, srdfBox]: srdf[w].update(d[w]) supervisor = Supervisor(robot, hpTasks=hpTasks(robot)) factory = Factory(supervisor) factory.parameters["period"] = rospy.get_param("/sot_controller/dt") factory.parameters[ "simulateTorqueFeedback"] = simulateTorqueFeedbackForEndEffector factory.parameters["addTracerToAdmittanceController"] = True # factory.parameters["addTimerToSotControl"] = True factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, [[] for e in objects]) factory.setRules(rules) factory.setupFrames(srdf["grippers"], srdf["handles"], robot) # Left gripper # Use admittance control when closing factory.addAffordance( Affordance( "talos/left_gripper", "box/top", openControlType="position_torque", closeControlType="position_torque", refs={ "angle_open": (0, ), "angle_close": (-0.5, ), "torque": (-5.0, ) }, simuParams={"refPos": (-0.2, )}, )) # Use position control for opening factory.addAffordance( Affordance( "talos/left_gripper", None, openControlType="position", closeControlType="position", refs={ "angle_open": (0, ), "angle_close": (-0.5, ) }, )) # Right gripper # Use position control factory.addAffordance( Affordance( "talos/right_gripper", "box/bottom", openControlType="position", closeControlType="position", refs={ "angle_open": (0, ), "angle_close": (-0.2, ) }, )) factory.generate() supervisor.makeInitialSot() return supervisor
# static stability constraint ps.addPartialCom("Talos_CoM", ["talos/root_joint"]) ps.createStaticStabilityConstraints("talos_static_stability", q, "Talos_CoM", ProblemSolver.FIXED_ON_THE_GROUND) tpc = ps.getPartialCom("Talos_CoM") rla = robot.getJointPosition(robot.leftAnkle) com_wf = np.array(tpc) tf_la = Transform(rla) com_la = tf_la.inverse().transform(com_wf) ps.createRelativeComConstraint("talos_com_constraint_to_la", "Talos_CoM", robot.leftAnkle, com_la.tolist(), (True, True, False)) ps.setMaxIterProjection(40) # Set the maximum number of iterations of the projection algorithm. Must be called before the graph creation. # constraint graph rules = [ Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_top"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_front"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_front", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_drawer", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/upper_drawer_spherical_handle"], True) ] graph = ConstraintGraph(robot, "graph") factory = ConstraintGraphFactory(graph) factory.setGrippers(["talos/left_gripper", "talos/right_gripper"]) factory.setObjects(["desk"], [["desk/upper_drawer_spherical_handle", "desk/touchpoint_right_top", "desk/touchpoint_right_front", "desk/touchpoint_left_front", "desk/touchpoint_left_drawer"]], [[]]) factory.setRules(rules) factory.generate() graph.addConstraints(graph = True,
0, 1, 0.2, 0, 0.02, 0, 0, 0, 1, ] lang = 'py' if lang == 'cxx': rules = [ Rule(grippers, [''], True), Rule(grippers, ['sphere0/handle'], True), Rule(grippers, ['sphere1/handle'], True) ] cg = ConstraintGraph.buildGenericGraph(robot=robot, name="manipulation", grippers=grippers, objects=objects, handlesPerObjects=handlesPerObject, shapesPerObjects=contactsPerObject, envNames=[], rules=rules) if lang == 'py': cg = ConstraintGraph(robot, "manipulation") factory = ConstraintGraphFactory(cg)
elif n.startswith("talos/head"): ps.createLockedJoint(n, n, half_sitting[r:r + s]) head_lock.append(n) elif n in other_lock: ps.createLockedJoint(n, n, half_sitting[r:r + s]) graph = ConstraintGraph.buildGenericGraph( robot, "graph", ["talos/left_gripper", "talos/right_gripper"], ["mire"], [Mire.handles], [[]], [], [ Rule(["talos/left_gripper"], [Mire.handles[0]], True), Rule(["talos/right_gripper"], [Mire.handles[1]], True), ], ) graph.setConstraints( graph=True, lockDof=left_gripper_lock + right_gripper_lock + other_lock, numConstraints=["com_talos_mire"] + foot_placement, ) graph.initialize() def setGuassianShooter(mean, stddev=[0.01] * robot.getNumberDof()): robot.setCurrentConfig(mean)