def __init__(self, filename, robot, solver, defaultDirectories,
logger = None):
if not logger:
logger = initializeLogging()
self.defaultDirectories = defaultDirectories
self.robot = robot
self.solver = solver
self.logger = logger
self.logger.info('loading motion plan file \'{0}\''.format(filename))
self.plan = yaml.load(
open(searchFile(filename, defaultDirectories), "r"))
self.duration = float(self.plan['duration'])
# Middleware proxies.
self.corba = CorbaServer('corba_server')
self.ros = RosExport('rosExport')
# Supervisor.
self.supervisor = Supervisor('supervisor')
self.robot.device.after.addSignal(self.supervisor.name + '.trigger')
self.supervisor.setSolver(self.solver.sot.name)
# Load plan.
self.logger.debug('loading environment')
self.loadEnvironment()
self.logger.debug('loading motion elements')
self.loadMotion()
self.logger.debug('loading control elements')
self.loadControl()
# For now, only 1 feet follower is allowed (must start at t=0).
feetFollowerElement = find(lambda e: type(e) == MotionWalk, self.motion)
hasControl = len(self.control) > 0
self.supervisor.setPostureFeature(
feetFollowerElement.feetFollower.postureFeature.name)
# Plug motion signals which depend on control.
for m in self.motion:
if type(m) == MotionVisualPoint:
# FIXME: this is so wrong.
plug(self.ros.signal(m.objectName),
m.vispPointProjection.cMo)
plug(self.ros.signal(m.objectName + 'Timestamp'),
m.vispPointProjection.cMoTimestamp)
if hasControl and feetFollowerElement:
self.feetFollower = FeetFollowerGraphWithCorrection(
robot, solver, feetFollowerElement.feetFollower,
MotionPlanErrorEstimationStrategy,
maxX = self.maxX, maxY = self.maxY,
maxTheta = self.maxTheta)
self.feetFollower.errorEstimationStrategy.motionPlan = self
#FIXME: not enough generic
self.feetFollower.feetFollower.setFootsteps(
2., makeFootsteps(self.footsteps))
elif feetFollowerElement:
self.feetFollower = feetFollowerElement.feetFollower
else:
self.feetFollower = None
self.logger.debug('motion plan created with success')
class MotionPlan(object):
robot = None
solver = None
feetFollower = None
corba = None
ros = None
plan = None
duration = 0
motion = []
control = []
footsteps = []
environment = {}
trace = None
started = False
maxX = FeetFollowerGraphWithCorrection.maxX
maxY = FeetFollowerGraphWithCorrection.maxY
maxTheta = FeetFollowerGraphWithCorrection.maxTheta
def __init__(self, filename, robot, solver, defaultDirectories,
logger = None):
if not logger:
logger = initializeLogging()
self.defaultDirectories = defaultDirectories
self.robot = robot
self.solver = solver
self.logger = logger
self.logger.info('loading motion plan file \'{0}\''.format(filename))
self.plan = yaml.load(
open(searchFile(filename, defaultDirectories), "r"))
self.duration = float(self.plan['duration'])
# Middleware proxies.
self.corba = CorbaServer('corba_server')
self.ros = RosExport('rosExport')
# Supervisor.
self.supervisor = Supervisor('supervisor')
self.robot.device.after.addSignal(self.supervisor.name + '.trigger')
self.supervisor.setSolver(self.solver.sot.name)
# Load plan.
self.logger.debug('loading environment')
self.loadEnvironment()
self.logger.debug('loading motion elements')
self.loadMotion()
self.logger.debug('loading control elements')
self.loadControl()
# For now, only 1 feet follower is allowed (must start at t=0).
feetFollowerElement = find(lambda e: type(e) == MotionWalk, self.motion)
hasControl = len(self.control) > 0
self.supervisor.setPostureFeature(
feetFollowerElement.feetFollower.postureFeature.name)
# Plug motion signals which depend on control.
for m in self.motion:
if type(m) == MotionVisualPoint:
# FIXME: this is so wrong.
plug(self.ros.signal(m.objectName),
m.vispPointProjection.cMo)
plug(self.ros.signal(m.objectName + 'Timestamp'),
m.vispPointProjection.cMoTimestamp)
if hasControl and feetFollowerElement:
self.feetFollower = FeetFollowerGraphWithCorrection(
robot, solver, feetFollowerElement.feetFollower,
MotionPlanErrorEstimationStrategy,
maxX = self.maxX, maxY = self.maxY,
maxTheta = self.maxTheta)
self.feetFollower.errorEstimationStrategy.motionPlan = self
#FIXME: not enough generic
self.feetFollower.feetFollower.setFootsteps(
2., makeFootsteps(self.footsteps))
elif feetFollowerElement:
self.feetFollower = feetFollowerElement.feetFollower
else:
self.feetFollower = None
self.logger.debug('motion plan created with success')
def loadEnvironment(self):
if not 'environment' in self.plan:
return
for obj in self.plan['environment']:
checkDict('object', obj)
checkDict('name', obj['object'])
self.environment[obj['object']['name']] = \
EnvironmentObject(self, obj['object'])
self.logger.debug('adding object \'{0}\''.format(obj['object']['name']))
def loadMotion(self):
if not 'motion' in self.plan or not self.plan['motion']:
return
if 'maximum-correction-per-step' in self.plan:
self.maxX = self.plan['maximum-correction-per-step']['x']
self.maxY = self.plan['maximum-correction-per-step']['y']
self.maxTheta = self.plan['maximum-correction-per-step']['theta']
motionClasses = [MotionWalk, MotionJoint, MotionTask, MotionVisualPoint]
for motion in self.plan['motion']:
if len(motion.items()) != 1:
raise RuntimeError('each motion should have only one type')
(tag, data) = motion.items()[0]
cls = find(lambda c: c.yaml_tag == tag, motionClasses)
if not cls:
raise RuntimeError('invalid motion element')
self.motion.append(cls(self, data, self.defaultDirectories))
self.logger.debug('adding motion element \'{0}\''.format(tag))
if self.trace:
for motion in self.motion:
motion.setupTrace(self.trace)
def loadControl(self):
if not 'control' in self.plan or not self.plan['control']:
return
controlClasses = [ControlConstant, ControlMocap, ControlViSP,
ControlHueblob, ControlVirtualSensor]
for control in self.plan['control']:
if len(control.items()) != 1:
raise RuntimeError('each control should have only one type')
(tag, data) = control.items()[0]
cls = find(lambda c: c.yaml_tag == tag, controlClasses)
if not cls:
raise RuntimeError('invalid control element')
self.control.append(cls(self, data))
self.logger.debug('adding control element \'{0}\''.format(tag))
def __str__(self):
res = 'Motion:\n'
res += '-------\n'
for motion in self.motion:
res += '\t* {0}\n'.format(str(motion))
res += '\n'
res += 'Control:\n'
res += '--------\n'
for control in self.control:
res += '\t* {0}\n'.format(str(control))
res += '\n'
res += str(self.feetFollower)
res += '\n\n'
res += self.supervisor.display()
return res
def start(self):
if self.started:
self.logger.info('already started')
return
if not self.canStart():
self.logger.info('failed to start')
return
self.started = True
self.logger.info('execution starts')
if self.feetFollower:
self.feetFollower.start()
# Remove default tasks and let the supervisor take over the
# tasks management.
self.solver.sot.clear()
tOrigin = self.feetFollower.feetFollower.getStartTime()
self.supervisor.setOrigin(max(0., tOrigin))
def canStart(self):
canStart = reduce(lambda acc, c: c.canStart() and acc,
self.control, True)
if not canStart:
return False
if self.feetFollower:
return self.feetFollower.canStart()
else:
return True
