signal = '{0}.{1}'.format(entityName, signalName)
filename = '{0}-{1}'.format(entityName, signalName)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
# Create task for the waist
robot_pose = (
(1., 0, 0, 0),
(0, 1., 0, 0),
(0, 0, 1., 0.089159),
(0, 0, 0, 1.),
)
feature_waist = FeaturePosition('position_waist',
robot.dynamic.shoulder_pan_joint,
robot.dynamic.Jshoulder_pan_joint, robot_pose)
task_waist = Task('waist_task')
task_waist.controlGain.value = 100.
task_waist.add(feature_waist.name)
# Create task for the lift
I4 = (
(1., 0, 0, 0.0),
(0, 1., 0, 0.136),
(0, 0, 1., 0.089),
(0, 0, 0, 1.),
)
feature_lift = FeaturePosition('position_lift',
robot.dynamic.shoulder_lift_joint,
robot.dynamic.Jshoulder_lift_joint, I4)
)
dimension = model.getDimension()
device.resize(dimension)
plug(device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0., )
model.acceleration.value = dimension * (0., )
# Create taks for the base
model.createOpPoint("base", "waist")
# Create task for the wrist
model.createOpPoint("wrist", "wrist_3_joint")
feature_wrist = FeaturePosition('position_wrist', model.wrist, model.Jwrist,
I4)
task_wrist = Task('wrist_task')
task_wrist.controlGain.value = 1.
task_wrist.add(feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint("ee", "ee_fixed_joint")
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_wrist.name)
plug(solver.control, device.control)
device.increment(0.01)
#Create tracer
tracer = TracerRealTime('trace')
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)
"""
Add a signal to a tracer and recompute it automatically if necessary.
"""
signal = '{0}.{1}'.format(entityName, signalName)
filename = '{0}-{1}'.format(entityName, signalName)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
# Create task for the waist
robot_pose = ((1.,0,0,0),(0,1.,0,0),(0,0,1.,0),(0,0,0,1.),)
feature_waist = FeaturePosition ('position_waist', robot.dynamic.base_joint, robot.dynamic.Jbase_joint, robot_pose)
feature_waist.selec.value = '001110'
task_waist = Task ('waist_task')
task_waist.controlGain.value = 5
task_waist.add (feature_waist.name)
'''
feature_waist = MetaTaskKine6d('contact',robot.dynamic,'contact','base_joint')
feature_waist.feature.frame('desired')
feature_waist.feature.selec.value = '011100'
feature_waist.gain.setConstant(10)
#locals()['contact'] = task
return task
'''
# Create task for the wrist
'''
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)
