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