class MetaTaskVisualPoint(object):
name = ''
opPoint = ''
dyn = 0
task = 0
feature = 0
featureDes = 0
proj = 0
def opPointExist(self, opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J' + opPoint,
self.dyn.signals())
return len(sigsP) == 1 & len(sigsJ) == 1
def defineDynEntities(self, dyn):
self.dyn = dyn
def createOpPoint(self, opPoint, opPointRef='right-wrist'):
self.opPoint = opPoint
if self.opPointExist(opPoint):
return
self.dyn.createOpPoint(opPoint, opPointRef)
def createOpPointModif(self):
self.opPointModif = OpPointModifier('opmodif' + self.name)
plug(self.dyn.signal(self.opPoint),
self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint),
self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = False
def createFeatures(self):
self.feature = FeatureVisualPoint('feature' + self.name)
self.featureDes = FeatureVisualPoint('feature' + self.name + '_ref')
self.feature.selec.value = '11'
def createTask(self):
self.task = Task('task' + self.name)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
def createProj(self):
self.proj = VisualPointProjecter('proj' + self.name)
def plugEverything(self):
self.feature.setReference(self.featureDes.name)
plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
plug(self.proj.signal('point2D'), self.feature.signal('xy'))
plug(self.proj.signal('depth'), self.feature.signal('Z'))
plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
self.name = name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint, opPointRef)
self.createOpPointModif()
self.createFeatures()
self.createTask()
self.createGain()
self.createProj()
self.plugEverything()
@property
def ref(self):
return self.featureDes.xy.value
@ref.setter
def ref(self, m):
self.featureDes.xy.value = m
@property
def target(self):
return self.proj.point3D
@target.setter
def target(self, m):
self.proj.point3D.value = m
@property
def opmodif(self):
if not self.opPointModif.activ:
return False
else:
return self.opPointModif.getTransformation()
@opmodif.setter
def opmodif(self, m):
if isinstance(m, bool) and not m:
plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo'))
plug(self.dyn.signal('J' + self.opPoint),
self.feature.signal('Jq'))
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
plug(self.opPointModif.signal('position'),
self.proj.signal('transfo'))
plug(self.opPointModif.signal('jacobian'),
self.feature.signal('Jq'))
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
def goto3D(self, point3D, gain=None, ref2D=None, selec=None):
self.target = point3D
if ref2D is not None:
self.ref = ref2D
if selec is not None:
self.feature.selec.value = selec
setGain(self.gain, gain)
def __init__ (self, robot, solver):
self.samplingPeriod = .005
self.robot = robot
self.solver = solver
self.ros = Ros (robot)
self.ros.rosExport.add ('matrixHomoStamped', 'ballInCamera',
'/wide/blobs/rose/transform')
self.ros.rosExport.add ('matrixHomoStamped', 'ballInWaist',
'/wide/blobs/rose/transform_base_link')
self.rightGripperId = 28
# Right hand task
self.featureRightHand = \
FeaturePosition ('featureRightHand',
robot.frames ['rightHand'].position,
robot.frames ['rightHand'].jacobian)
self.featureRightHand.selec.value = '000111'
# interpolation
self.prodSE3 = Multiply_of_matrixHomo ("prod SE3")
self.interpolation = CylindricalCubicInterpolationSE3 ('interpolation')
self.interpolation.setSamplingPeriod (self.samplingPeriod)
plug (self.robot.waist.position, self.prodSE3.sin1)
plug (self.interpolation.reference,
self.featureRightHand.signal('reference'))
plug (self.ros.rosExport.ballInWaist, self.prodSE3.sin2)
plug (self.prodSE3.sout, self.interpolation.goal)
plug (self.featureRightHand.position, self.interpolation.init)
plug (self.robot.waist.position, self.interpolation.localFrame)
self.taskRightHand = TaskPD ('taskRightHand')
self.taskRightHand.add (self.featureRightHand.name)
#plug (interpolation.errorDot, taskRightHand.errorDot)
self.taskRightHand.controlGain.value = 180.0
self.solver.push (self.taskRightHand)
# Waist task
self.robot.waist.selec.value = '011000'
self.robot.waist.value = I4
self.solver.push (self.robot.tasks ['waist'])
# Ball tracking
refBallInCamera = FeatureVisualPoint ('featureBallInCameraRef')
refBallInCamera.xy.value = (0., 0.)
self.pinholeProjection = VispPointProjection('pinholeProjection')
plug (self.ros.rosExport.ballInCamera, self.pinholeProjection.cMo)
plug (self.ros.rosTime.time, self.pinholeProjection.time)
plug (self.ros.rosExport.ballInCameraTimestamp,
self.pinholeProjection.cMoTimestamp)
self.ballInCamera = FeatureVisualPoint ('featureBallInCamera')
plug (self.pinholeProjection.xy, self.ballInCamera.xy)
self.ballInCamera.Z.value = 1.
self.ballInCamera.setReference (refBallInCamera.name)
self.ballInCamera.selec.value = '11'
plug (self.robot.frames ['cameraTopRight'].jacobian,
self.ballInCamera.Jq)
self.taskBallTracking = TaskPD ('taskBallTracking')
self.taskBallTracking.add (self.ballInCamera.name)
self.taskBallTracking.controlGain.value = 1.0
# Posture task
self.posture = list (self.robot.halfSitting [::])
self.featurePosture = FeaturePosture ('featurePosture')
plug (self.robot.device.state, self.featurePosture.state)
self.featurePosture.setPosture (tuple (self.posture))
for dof in range (6, self.robot.dimension):
self.featurePosture.selectDof (dof, False)
self.postureTask = TaskPD ('postureTask')
self.postureTask.add (self.featurePosture.name)
self.postureTask.controlGain.value = 1.
self.solver.push (self.postureTask)
def createFeatures(self):
self.feature = FeatureVisualPoint('feature' + self.name)
self.featureDes = FeatureVisualPoint('feature' + self.name + '_ref')
self.feature.selec.value = '11'
class Motion (object):
reachTime = 3.
stillTime = .5
def __init__ (self, robot, solver):
self.samplingPeriod = .005
self.robot = robot
self.solver = solver
self.ros = Ros (robot)
self.ros.rosExport.add ('matrixHomoStamped', 'ballInCamera',
'/wide/blobs/rose/transform')
self.ros.rosExport.add ('matrixHomoStamped', 'ballInWaist',
'/wide/blobs/rose/transform_base_link')
self.rightGripperId = 28
# Right hand task
self.featureRightHand = \
FeaturePosition ('featureRightHand',
robot.frames ['rightHand'].position,
robot.frames ['rightHand'].jacobian)
self.featureRightHand.selec.value = '000111'
# interpolation
self.prodSE3 = Multiply_of_matrixHomo ("prod SE3")
self.interpolation = CylindricalCubicInterpolationSE3 ('interpolation')
self.interpolation.setSamplingPeriod (self.samplingPeriod)
plug (self.robot.waist.position, self.prodSE3.sin1)
plug (self.interpolation.reference,
self.featureRightHand.signal('reference'))
plug (self.ros.rosExport.ballInWaist, self.prodSE3.sin2)
plug (self.prodSE3.sout, self.interpolation.goal)
plug (self.featureRightHand.position, self.interpolation.init)
plug (self.robot.waist.position, self.interpolation.localFrame)
self.taskRightHand = TaskPD ('taskRightHand')
self.taskRightHand.add (self.featureRightHand.name)
#plug (interpolation.errorDot, taskRightHand.errorDot)
self.taskRightHand.controlGain.value = 180.0
self.solver.push (self.taskRightHand)
# Waist task
self.robot.waist.selec.value = '011000'
self.robot.waist.value = I4
self.solver.push (self.robot.tasks ['waist'])
# Ball tracking
refBallInCamera = FeatureVisualPoint ('featureBallInCameraRef')
refBallInCamera.xy.value = (0., 0.)
self.pinholeProjection = VispPointProjection('pinholeProjection')
plug (self.ros.rosExport.ballInCamera, self.pinholeProjection.cMo)
plug (self.ros.rosTime.time, self.pinholeProjection.time)
plug (self.ros.rosExport.ballInCameraTimestamp,
self.pinholeProjection.cMoTimestamp)
self.ballInCamera = FeatureVisualPoint ('featureBallInCamera')
plug (self.pinholeProjection.xy, self.ballInCamera.xy)
self.ballInCamera.Z.value = 1.
self.ballInCamera.setReference (refBallInCamera.name)
self.ballInCamera.selec.value = '11'
plug (self.robot.frames ['cameraTopRight'].jacobian,
self.ballInCamera.Jq)
self.taskBallTracking = TaskPD ('taskBallTracking')
self.taskBallTracking.add (self.ballInCamera.name)
self.taskBallTracking.controlGain.value = 1.0
# Posture task
self.posture = list (self.robot.halfSitting [::])
self.featurePosture = FeaturePosture ('featurePosture')
plug (self.robot.device.state, self.featurePosture.state)
self.featurePosture.setPosture (tuple (self.posture))
for dof in range (6, self.robot.dimension):
self.featurePosture.selectDof (dof, False)
self.postureTask = TaskPD ('postureTask')
self.postureTask.add (self.featurePosture.name)
self.postureTask.controlGain.value = 1.
self.solver.push (self.postureTask)
def trackBall (self):
self.solver.push (self.taskBallTracking)
def openHand (self):
# open hand
self.featurePosture.selectDof (self.rightGripperId, True)
self.posture [self.rightGripperId] = 0.7
self.featurePosture.setPosture (tuple (self.posture))
def reach (self):
# move hand
self.handInitPos = self.robot.frames ['rightHand'].position.value
self.interpolation.start (self.reachTime)
def stopTracking (self):
self.solver.remove (self.taskBallTracking)
def closeHand (self):
self.posture [self.rightGripperId] = 0.3
self.featurePosture.setPosture (tuple (self.posture))
self.postureTask.controlGain.value = 4.
def moveBack (self):
# move hand back
self.postureTask.controlGain.value = 0.
for dof in range (6, len (self.posture)):
self.featurePosture.selectDof (dof, True)
self.interpolation.goal.value = self.handInitPos
self.postureTask.controlGain.value = 3.
self.interpolation.start (2.)
def stopMotion (self):
self.solver.remove (self.taskBallTracking)
plug (self.ros.rosExport.ballInWaist, self.interpolation.goal)
def catchBall (self):
#self.trackBall ()
self.openHand ()
self.reach ()
time.sleep (self.reachTime)
self.closeHand ()
time.sleep (1.)
self.moveBack ()
self.stopMotion ()
def play(self, totalTime):
nbSteps = int(totalTime/timeStep) + 1
path = []
for i in xrange(nbSteps):
print (i)
if i == 100:
self.start ()
t = timeStep*i
self.robot.device.increment(timeStep)
config = self.robot.device.state.value
path.append(config)
if hasRobotViewer:
clt.updateElementConfig(rvName, toViewerConfig(config))
ballPos = []
for i in range (3):
ballPos.append(self.ros.rosExport.ballInWaist.value[i][3])
ballPos.extend (3*[0])
clt.updateElementConfig("rose_ball", ballPos)
