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)
