def createBalanceTask(robot, application, taskName, ingain=1.):
task = Task(taskName)
task.add(application.featureCom.name)
task.add(application.leftAnkle.name)
task.add(application.rightAnkle.name)
gain = GainAdaptive('gain' + taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (task, gain)
def createPostureTask(robot, taskName, ingain=1.):
robot.dynamic.position.recompute(0)
feature = FeatureGeneric('feature' + taskName)
featureDes = FeatureGeneric('featureDes' + taskName)
featureDes.errorIN.value = robot.halfSitting
plug(robot.dynamic.position, feature.errorIN)
feature.setReference(featureDes.name)
robotDim = len(robot.dynamic.position.value)
feature.jacobianIN.value = matrixToTuple(identity(robotDim))
task = Task(taskName)
task.add(feature.name)
gain = GainAdaptive('gain' + taskName)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
gain.setConstant(ingain)
return (feature, featureDes, task, gain)
def createOperationalPointFeatureAndTask(robot, operationalPointName, featureName, taskName, ingain=.2):
operationalPointMapped = operationalPointName
jacobianName = 'J{0}'.format(operationalPointMapped)
robot.dynamic.signal(operationalPointMapped).recompute(0)
robot.dynamic.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
robot.dynamic.signal(operationalPointMapped),
robot.dynamic.signal(jacobianName),
robot.dynamic.signal(operationalPointMapped).value)
task = Task(taskName)
task.add(featureName)
gain = GainAdaptive('gain' + taskName)
gain.setConstant(ingain)
plug(gain.gain, task.controlGain)
plug(task.error, gain.error)
return (feature, task, gain)
def createCenterOfMassFeatureAndTask(robot, featureName, featureDesName, taskName, selec='111', ingain=1.):
robot.dynamic.com.recompute(0)
robot.dynamic.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(robot.dynamic.com, featureCom.errorIN)
plug(robot.dynamic.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = robot.dynamic.com.value
featureCom.setReference(featureComDes.name)
taskCom = Task(taskName)
taskCom.add(featureName)
gainCom = GainAdaptive('gain' + taskName)
gainCom.setConstant(ingain)
plug(gainCom.gain, taskCom.controlGain)
plug(taskCom.error, gainCom.error)
return (featureCom, featureComDes, taskCom, gainCom)
# WAIST
t.set((0., 0.095, 0.563816))
# HAND
t.set((0.25, -0.5, .85))
plug(t.signal('out'), comp.signal('sin2'))
plug(comp.signal('sout'), p6d.signal('position'))
plug(dyn.signal('J0'), p6.signal('Jq'))
plug(dyn.signal('0'), p6.signal('position'))
p6.signal('sdes').value = p6d
task = Task('task')
task.add('p6')
gain = GainAdaptive('gain')
gain.setConstant(.2)
plug(task.signal('error'), gain.signal('error'))
plug(gain.signal('gain'), task.signal('controlGain'))
R3 = ((0., 0., -1.),
(0., 1., 0.),
(1., 0., 0.))
eye3.set(R3)
p6.signal('selec').value = '000111'
p6.frame('current')
# --- COM
dyn.setProperty('ComputeCoM', 'true')
# dyn.setProperty ComputeVelocity true
# dyn.setProperty ComputeMomentum true
# WAIST
t.set((0., 0.095, 0.563816))
# HAND
t.set((0.25, -0.5, .85))
plug(t.signal('out'), comp.signal('in2'))
plug(comp.signal('out'), p6d.signal('position'))
plug(dyn.signal('J0'), p6.signal('Jq'))
plug(dyn.signal('0'), p6.signal('position'))
p6.signal('sdes').value = p6d
task = Task('task')
task.add('p6')
gain = GainAdaptive('gain')
gain.setConstant(.2)
plug(task.signal('error'), gain.signal('error'))
plug(gain.signal('gain'), task.signal('controlGain'))
R3 = ((0., 0., -1.), (0., 1., 0.), (1., 0., 0.))
eye3.set(R3)
p6.signal('selec').value = '000111'
p6.frame('current')
# --- COM
dyn.setProperty('ComputeCoM', 'true')
# dyn.setProperty ComputeVelocity true
# dyn.setProperty ComputeMomentum true
# dyn.setProperty ComputeZMP true