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