class MotionJoint(Motion): yaml_tag = u'joint' gain = None name = None reference = None # FIXME: not generic enough and joints are missing. nameToId = { 'left-claw': 35, 'right-claw': 28 } def __init__(self, motion, yamlData, defaultDirectories): checkDict('interval', yamlData) Motion.__init__(self, motion, yamlData) self.gain = yamlData.get('gain', 1.) self.name = yamlData['name'] self.reference = yamlData['reference'] self.task = Task('joint'+str(id(yamlData))) self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData))) plug(motion.robot.device.state, self.feature.state) jointId = self.nameToId[self.name] posture = np.array(motion.robot.halfSitting) posture[jointId] = self.reference self.feature.setPosture(tuple(posture.tolist())) for i in xrange(len(posture) - 6): if i + 6 == jointId: self.feature.selectDof(i + 6, True) else: self.feature.selectDof(i + 6, False) self.task.add(self.feature.name) self.task.controlGain.value = self.gain # Push the task into supervisor. motion.supervisor.addTask(self.task.name, self.interval[0], self.interval[1], self.priority, (jointId,)) def __str__(self): return "joint motion ({0})".format(self.name) def setupTrace(self, trace): pass
class LegsFollowerGraph(object): legsFollower = None postureTask = None postureFeature = None postureFeatureDes = None postureError = None legsTask = None legsFeature = None legsFeatureDes = None legsError = None waistTask = None waistFeature = None waistFeatureDes = None waistError = None trace = None def __init__(self, robot, solver, trace = None, postureTaskDofs = None): print("Constructor of LegsFollower Graph") self.robot = robot self.solver = solver self.legsFollower = LegsFollower('legs-follower') self.statelength = len(robot.device.state.value) # Initialize the posture task. print("Posture Task") self.postureTaskDofs = postureTaskDofs if not self.postureTaskDofs: self.postureTaskDofs = [] for i in xrange(len(robot.halfSitting) - 6): # Disable legs dofs. if i < 12: #FIXME: not generic enough self.postureTaskDofs.append((i + 6, False)) else: self.postureTaskDofs.append((i + 6, True)) # This part is taken from feet_follower_graph self.postureTask = Task(self.robot.name + '_posture') self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature') plug(self.robot.device.state, self.postureFeature.state) posture = list(self.robot.halfSitting) self.postureFeature.setPosture(tuple(posture)) for (dof, isEnabled) in self.postureTaskDofs: self.postureFeature.selectDof(dof, isEnabled) self.postureTask.add(self.postureFeature.name) self.postureTask.controlGain.value = 2. # Initialize the waist follower task. print("Waist Task") self.robot.features['waist'].selec.value = '111111' plug(self.legsFollower.waist, self.robot.features['waist'].reference) self.robot.tasks['waist'].controlGain.value = 1. # Initialize the legs follower task. print("Legs Task") self.legsTask = Task(self.robot.name + '_legs') self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature') legsFeatureDesName = self.robot.name + '_legsFeatureDes' self.legsFeatureDes = FeatureGeneric(legsFeatureDesName) self.legsError = LegsError('LegsError') plug(self.robot.device.state, self.legsError.state) # self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN) self.legsFeature.jacobianIN.value = self.legsJacobian() self.legsFeature.setReference(legsFeatureDesName) plug(self.legsError.error, self.legsFeature.errorIN) self.legsTask.add(self.legsFeature.name) self.legsTask.controlGain.value = 5. #CoM task print("Com Task") print (0., 0., self.robot.dynamic.com.value[2]) self.robot.comTask.controlGain.value = 50. self.robot.featureComDes.errorIN.value = (0., 0., self.robot.dynamic.com.value[2]) self.robot.featureCom.selec.value = '111' plug(self.legsFollower.com, self.robot.featureComDes.errorIN) # Plug the legs follower zmp output signals. plug(self.legsFollower.zmp, self.robot.device.zmp) solver.sot.remove(self.robot.comTask.name) print("Push in solver.") solver.sot.push(self.legsTask.name) solver.sot.push(self.postureTask.name) solver.sot.push(self.robot.tasks['waist'].name) solver.sot.push(self.robot.comTask.name) solver.sot.remove(self.robot.tasks['left-ankle'].name) solver.sot.remove(self.robot.tasks['right-ankle'].name) print solver.sot.display() print("Tasks added in solver.\n") print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n" " - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n") def legsJacobian(self): j = [] for i in xrange(12): j.append(oneVector(6+i,self.statelength)) return tuple(j) def waistJacobian(self): j = [] for i in xrange(6): j.append(oneVector(i,self.statelength)) return tuple(j) def postureJacobian(self): j = [] for i in xrange(self.statelength): if i >= 6 + 2 * 6: j.append(oneVector(i)) if i == 3 or i == 4: j.append(zeroVector()) return tuple(j) def computeDesiredValue(self): e = self.robot.halfSitting #e = halfSitting e_ = [e[3], e[4]] offset = 6 + 2 * 6 for i in xrange(len(e) - offset): e_.append(e[offset + i]) return tuple(e_) def canStart(self): securityThreshold = 1e-3 return (self.postureTask.error.value <= (securityThreshold,) * len(self.postureTask.error.value)) def setupTrace(self): self.trace = TracerRealTime('trace') self.trace.setBufferSize(2**20) self.trace.open('/tmp/','legs_follower_','.dat') self.trace.add('legs-follower.com', 'com') self.trace.add('legs-follower.zmp', 'zmp') self.trace.add('legs-follower.ldof', 'ldof') self.trace.add('legs-follower.waist', 'waist') self.trace.add(self.robot.device.name + '.state', 'state') self.trace.add(self.legsTask.name + '.error', 'errorLegs') self.trace.add(self.robot.comTask.name + '.error', 'errorCom') #self.trace.add('legs-follower.outputStart','start') #self.trace.add('legs-follower.outputYaw','yaw') self.trace.add('corba.planner_steps','steps') self.trace.add('corba.planner_outputGoal','goal') self.trace.add('corba.waist','waistMocap') self.trace.add('corba.left-foot','footMocap') self.trace.add('corba.table','tableMocap') self.trace.add('corba.bar','barMocap') self.trace.add('corba.chair','chairMocap') self.trace.add('corba.helmet','helmetMocap') self.trace.add('corba.planner_outputObs','obstacles') self.trace.add(self.robot.dynamic.name + '.left-ankle', self.robot.dynamic.name + '-left-ankle') self.trace.add(self.robot.dynamic.name + '.right-ankle', self.robot.dynamic.name + '-right-ankle') # Recompute trace.triger at each iteration to enable tracing. self.robot.device.after.addSignal('legs-follower.zmp') self.robot.device.after.addSignal('legs-follower.outputStart') self.robot.device.after.addSignal('legs-follower.outputYaw') self.robot.device.after.addSignal('corba.planner_steps') self.robot.device.after.addSignal('corba.planner_outputGoal') self.robot.device.after.addSignal('corba.waist') self.robot.device.after.addSignal('corba.left-foot') self.robot.device.after.addSignal('corba.table') self.robot.device.after.addSignal('corba.bar') self.robot.device.after.addSignal('corba.chair') self.robot.device.after.addSignal('corba.helmet') self.robot.device.after.addSignal('corba.planner_outputObs') self.robot.device.after.addSignal(self.robot.dynamic.name + '.left-ankle') self.robot.device.after.addSignal(self.robot.dynamic.name + '.right-ankle') self.robot.device.after.addSignal('trace.triger') return def plugPlanner(self): print("Plug planner.") plug(corba.planner_radQ, self.legsFollower.inputRef) plug(self.legsFollower.outputStart, corba.planner_inputStart) plug(corba.waistTimestamp, corba.planner_timestamp) plug(corba.table, corba.planner_table) plug(corba.chair, corba.planner_chair) plug(corba.bar, corba.planner_bar) plug(corba.waist, corba.planner_waist) plug(corba.helmet, corba.planner_inputGoal) plug(corba.torus1, corba.planner_torus1) plug(corba.torus2, corba.planner_torus2) plug(corba.signal('left-foot'), corba.planner_foot) plug(corba.signal('left-footTimestamp'), corba.planner_footTimestamp) return def plugPlannerWithoutMocap(self): print("Plug planner without mocap.") plug(corba.planner_radQ, self.legsFollower.inputRef) plug(self.legsFollower.outputStart, corba.planner_inputStart) return def plugViewer(self): print("Plug viewer.") plug(self.legsFollower.ldof, corba.viewer_Ldof) plug(self.legsFollower.outputStart, corba.viewer_Start) plug(self.legsFollower.com, corba.viewer_Com) plug(self.legsFollower.outputYaw, corba.viewer_Yaw) plug(corba.planner_steps, corba.viewer_Steps) plug(corba.planner_outputGoal, corba.viewer_Goal) plug(corba.table, corba.viewer_Table) plug(corba.chair, corba.viewer_Chair) plug(corba.bar, corba.viewer_Bar) plug(corba.torus1, corba.viewer_Torus1) plug(corba.torus2, corba.viewer_Torus2) plug(corba.waist, corba.viewer_Waist) plug(corba.planner_outputLabyrinth, corba.viewer_Labyrinth) plug(corba.planner_outputObs, corba.viewer_Obs) return def plug(self): self.plugPlanner() self.plugViewer() return def readMocap(self): print "Table : " print corba.table.value print "Waist : " if len(corba.waist.value)<3: corba.waist.value = (0,0,0) print corba.waist.value print "Helmet : " print corba.helmet.value return; def start(self): if not self.canStart(): print("Robot has not yet converged to the initial position," " please wait and try again.") return print("Start.") self.postureTask.controlGain.value = 180. #self.waistTask.controlGain.value = 90. self.legsTask.controlGain.value = 180. self.robot.comTask.controlGain.value = 180. self.robot.tasks['waist'].controlGain.value = 45. self.setupTrace() self.trace.start() self.legsFollower.start() return def stop(self): self.legsFollower.stop() self.trace.dump() return
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)