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