agimus_sot.factory.Factory Class Reference

Create a set of controllers for a set of tasks. More...

Inheritance diagram for agimus_sot.factory.Factory:
[legend]
Collaboration diagram for agimus_sot.factory.Factory:
[legend]

Classes

class  State
 

Public Member Functions

def __init__ (self, supervisor)
 
def addAffordance (self, aff)
 Add an Affordance or ObjectAffordance. More...
 
def generate (self)
 
def setupFrames (self, srdfGrippers, srdfHandles, sotrobot, disabledGrippers=())
 
def setupContactFrames (self, srdfContacts)
 
def makeState (self, grasps, priority)
 
def makeLoopTransition (self, state)
 
def makeTransition (self, stateFrom, stateTo, ig)
 

Public Attributes

 tasks
 
 hpTasks
 
 lpTasks
 
 affordances
 
 objectAffordances
 
 sots
 
 postActions
 A dictionnary. More...
 
 preActions
 A dictionnary. More...
 
 tracers
 
 controllers
 
 supervisor
 
 parameters
 Accepted parameters: More...
 
 SoTtracer
 
 sotrobot
 
 grippersIdx
 
 handlesIdx
 
 gripperFrames
 
 handleFrames
 
 contactFrames
 

Detailed Description

Create a set of controllers for a set of tasks.

A controller is created for each transition of the graph of constraints.

See the following example for usage.

from agimus_sot import Supervisor
from agimus_sot.factory import Factory, Affordance
from agimus_sot.task import Task
from agimus_sot.srdf_parser import parse_srdf
from hpp.corbaserver.manipulation import Rule
# Constraint graph definition. Should be the same as the one used for planning
# in HPP.
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),
]
# Parse SRDF files to extract gripper and handle information.
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)
# Define parameters
factory.parameters["period"] = robot.getTimeStep() # This must be made available for your robot
factory.parameters["simulateTorqueFeedback"] = simulateTorqueFeedbackForEndEffector
factory.parameters["addTracerToAdmittanceController"] = 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",])
# At the moment, one must manually enable visual feedback
factory.gripperFrames["talos/left_gripper"].hasVisualTag = True
factory.handleFrames["box/handle1"].hasVisualTag = True
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., 1000.),
"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 ()
See also
manipulation.constraint_graph_factory.GraphFactoryAbstract, TaskFactory, Affordance

Constructor & Destructor Documentation

◆ __init__()

def agimus_sot.factory.Factory.__init__ (   self,
  supervisor 
)

Member Function Documentation

◆ addAffordance()

def agimus_sot.factory.Factory.addAffordance (   self,
  aff 
)

◆ generate()

◆ makeLoopTransition()

def agimus_sot.factory.Factory.makeLoopTransition (   self,
  state 
)

◆ makeState()

def agimus_sot.factory.Factory.makeState (   self,
  grasps,
  priority 
)

◆ makeTransition()

◆ setupContactFrames()

def agimus_sot.factory.Factory.setupContactFrames (   self,
  srdfContacts 
)

◆ setupFrames()

def agimus_sot.factory.Factory.setupFrames (   self,
  srdfGrippers,
  srdfHandles,
  sotrobot,
  disabledGrippers = () 
)

Member Data Documentation

◆ affordances

agimus_sot.factory.Factory.affordances

◆ contactFrames

agimus_sot.factory.Factory.contactFrames

◆ controllers

agimus_sot.factory.Factory.controllers

◆ gripperFrames

agimus_sot.factory.Factory.gripperFrames

◆ grippersIdx

agimus_sot.factory.Factory.grippersIdx

◆ handleFrames

agimus_sot.factory.Factory.handleFrames

◆ handlesIdx

agimus_sot.factory.Factory.handlesIdx

◆ hpTasks

◆ lpTasks

◆ objectAffordances

agimus_sot.factory.Factory.objectAffordances

◆ parameters

agimus_sot.factory.Factory.parameters

Accepted parameters:

  • period: [double, no default] Time interval between two iterations of the graph.
  • simulateTorqueFeedback: [boolean, False] do not use torque feedback from the robot but simulate it instead.

Referenced by agimus_sot.factory.Factory.generate().

◆ postActions

agimus_sot.factory.Factory.postActions

A dictionnary.

  • key: name of the transition after which an action must be done
  • value: dictionnary:
    • key: the reached state (at most two values, depending on whether the dst state was reached)
    • value: sot representing the post-action

Referenced by agimus_sot.supervisor.Supervisor.addPostActions(), agimus_sot.factory.Factory.makeTransition(), and agimus_sot.supervisor.Supervisor.runPostAction().

◆ preActions

agimus_sot.factory.Factory.preActions

A dictionnary.

  • key: name of the transition before which an action must be done
  • value: sot representing the pre-action

Referenced by agimus_sot.supervisor.Supervisor.addPreAction(), agimus_sot.factory.Factory.makeTransition(), and agimus_sot.supervisor.Supervisor.runPreAction().

◆ sotrobot

◆ sots

◆ SoTtracer

agimus_sot.factory.Factory.SoTtracer

◆ supervisor

◆ tasks

◆ tracers

agimus_sot.factory.Factory.tracers