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)
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)