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 makeSupervisorWithFactory(robot):
from agimus_sot import Supervisor
from agimus_sot.factory import Factory, Affordance
from agimus_sot.srdf_parser import parse_srdf
grippers = ["talos/left_gripper", "talos/right_gripper"]
objects = []
handlesPerObjects = []
contactPerObjects = []
srdf = {}
srdfTalos = parse_srdf("srdf/talos.srdf",
packageName="talos_data",
prefix="talos")
srdfBox = parse_srdf("srdf/plank_of_wood1.srdf",
packageName="gerard_bauzil",
prefix="box")
srdfTable = parse_srdf("srdf/table_140_70_73.srdf",
packageName="gerard_bauzil",
prefix="table")
for w in ["grippers", "handles", "contacts"]:
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"] = False
# factory.parameters["addTimerToSotControl"] = True
factory.setGrippers(grippers)
factory.setObjects(objects, handlesPerObjects, contactPerObjects)
factory.environmentContacts(["table/top"])
# factory.setRules (rules)
factory.setupFrames(srdf["grippers"], srdf["handles"], robot)
factory.setupContactFrames(srdf["contacts"])
# disabledGrippers=grippers)
# disabledGrippers=["table/pose",])
factory.gripperFrames["talos/left_gripper"].hasVisualTag = True
factory.gripperFrames["talos/right_gripper"].hasVisualTag = True
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
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
def makeSupervisorWithFactory(robot):
from agimus_sot import Supervisor
from agimus_sot.factory import Factory, Affordance
from agimus_sot.srdf_parser import parse_srdf
if not hasattr(robot, "camera_frame"):
robot.camera_frame = "rgbd_rgb_optical_frame"
grippers = ["talos/left_gripper", "talos/right_gripper"]
objects = ["box"]
handlesPerObjects = [["box/handle" + str(i) for i in range(1, 5)]]
contactPerObjects = [["box/front_surface", "box/rear_surface"]]
srdf = {}
srdfTalos = parse_srdf("srdf/talos.srdf",
packageName="talos_data",
prefix="talos")
srdfBox = parse_srdf("srdf/plank_of_wood1.srdf",
packageName="gerard_bauzil",
prefix="box")
srdfTable = parse_srdf("srdf/rolling_table.srdf",
packageName="gerard_bauzil",
prefix="table")
for w in ["grippers", "handles", "contacts"]:
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"] = False
factory.parameters["addTracerToVisualServoing"] = True
# factory.parameters["addTimerToSotControl"] = True
factory.setGrippers(grippers)
factory.setObjects(objects, handlesPerObjects, contactPerObjects)
factory.environmentContacts(["table/top"])
# factory.setRules (rules)
factory.setupFrames(srdf["grippers"], srdf["handles"], robot)
factory.setupContactFrames(srdf["contacts"])
# disabledGrippers=grippers)
# disabledGrippers=["table/pose",])
factory.gripperFrames["talos/left_gripper"].hasVisualTag = True
factory.gripperFrames["talos/right_gripper"].hasVisualTag = True
for i in range(1, 5):
factory.handleFrames["box/handle" + str(i)].hasVisualTag = True
factory.contactFrames["table/top"].hasVisualTag = True
factory.contactFrames["box/front_surface"].hasVisualTag = True
for gripper in grippers:
for handle in handlesPerObjects[0]:
factory.addAffordance(
Affordance(
gripper,
handle,
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.addAffordance(
Affordance(
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.
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
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
from agimus_sot.srdf_parser import parse_srdf
from agimus_sot.control.gripper import AdmittanceControl
from agimus_sot.tools import OpFrame, EndEffector, Posture
from agimus_sot.factory import Affordance
from dynamic_graph.sot.core import SOT
dt = 0.001
desired_torque = (-5, )
threshold_up = tuple([ x / 10. for x in desired_torque ])
threshold_down = tuple([ x / 100. for x in desired_torque ])
srdfTalos = parse_srdf ("srdf/talos.srdf", packageName = "talos_data", prefix="talos")
srdfBox = parse_srdf ("srdf/cobblestone.srdf", packageName = "gerard_bauzil", prefix="box")
gripperName = "talos/left_gripper"
handleName = "box/handle1"
gripperFrame = OpFrame(srdfTalos["grippers"][gripperName], robot.dynamic.model, True)
handleFrame = OpFrame(srdfBox ["handles" ][handleName ])
affordance = Affordance ("talos/left_gripper", "box/handle1",
openControlType="position_torque", closeControlType="position_torque",
# openControlType="position", closeControlType="position",
refs = { "angle_open": (0,), "angle_close": (-0.5,), "torque": (-100.,) },
simuParams = { "refPos": (-0.2,) }))
ee_task = EndEffector (robot, gripperFrame, "test")
ee_task.makeAdmittanceControl (affordance, "close", dt, simulateTorqueFeedback = True)
keep_posture = Posture ("posture_keep", robot)
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, 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
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