# Create task for the wrist3 I4 = ((1.,0,0,0.321), (0,1.,0,0.109), (0,0,1.,0.848), (0,0,0,1.),) feature_wrist = FeaturePosition ('position_wrist', robot.dynamic.wrist_3_joint, robot.dynamic.Jwrist_3_joint, I4) task_wrist = Task ('wrist_task') task_wrist.controlGain.value = 1 task_wrist.add (feature_wrist.name) #Create tracer tracer = TracerRealTime ('trace') tracer.setBufferSize(2**20) tracer.open('/tmp/','dg_','.dat') # Make sure signals are recomputed even if not used in the control graph robot.device.after.addSignal('{0}.triger'.format(tracer.name)) addTrace (robot.device, tracer, robot.device.name, "state") addTrace (robot.device, tracer, feature_wrist._feature.name, "position") addTrace (robot.device, tracer, feature_wrist._reference.name, "position") # solver solver = SOT ('solver') solver.setSize (dimension) solver.push (task_waist.name) solver.push (task_wrist.name) solver.push (task_elbow.name) solver.push (task_lift.name) solver.push (task_wrist1.name) solver.push (task_wrist2.name) plug (solver.control, robot.device.control)
def add_trace(self, entityName, signalName): if self.tracer: addTrace(self, self.tracer, entityName, signalName)
def addTrace(self, entityName, signalName): if self.tracer: self.autoRecomputedSignals.append( '{0}.{1}'.format(entityName, signalName)) addTrace(self, self.tracer, entityName, signalName)
(0, 1., 0, 0.109), (0, 0, 1., 0.848), (0, 0, 0, 1.), ) feature_wrist = FeaturePosition('position_wrist', robot.dynamic.wrist_3_joint, robot.dynamic.Jwrist_3_joint, I4) task_wrist = Task('wrist_task') task_wrist.controlGain.value = 1 task_wrist.add(feature_wrist.name) #Create tracer tracer = TracerRealTime('trace') tracer.setBufferSize(2**20) tracer.open('/tmp/', 'dg_', '.dat') # Make sure signals are recomputed even if not used in the control graph robot.device.after.addSignal('{0}.triger'.format(tracer.name)) addTrace(robot.device, tracer, robot.device.name, "state") addTrace(robot.device, tracer, feature_wrist._feature.name, "position") addTrace(robot.device, tracer, feature_wrist._reference.name, "position") # solver solver = SOT('solver') solver.setSize(dimension) solver.push(task_waist.name) solver.push(task_wrist.name) solver.push(task_elbow.name) solver.push(task_lift.name) solver.push(task_wrist1.name) solver.push(task_wrist2.name) plug(solver.control, robot.device.control) #robot.device.increment (0.01)