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)