def initPostureTask(robot):
# --- TASK POSTURE --------------------------------------------------
# set a default position for the joints.
robot.features['featurePosition'] = FeaturePosture('featurePosition')
plug(robot.device.state, robot.features['featurePosition'].state)
robot.features['featurePosition'].posture.value = robot.halfSitting
# Remove the dofs of the feet.
postureTaskDofs = [True] * (len(robot.dynamic.position.value) - 6)
jla = robot.dynamic.signal('J' +
robot.OperationalPointsMap['left-ankle']).value
postureTaskDofs = removeDofUsed(jla, postureTaskDofs)
jra = robot.dynamic.signal('J' +
robot.OperationalPointsMap['right-ankle']).value
postureTaskDofs = removeDofUsed(jra, postureTaskDofs)
for dof, isEnabled in enumerate(postureTaskDofs):
robot.features['featurePosition'].selectDof(dof + 6, isEnabled)
robot.tasks['robot_task_position'] = Task('robot_task_position')
robot.tasks['robot_task_position'].add('featurePosition')
gainPosition = GainAdaptive('gainPosition')
gainPosition.set(0.1, 0.1, 125e3)
gainPosition.gain.value = 5
plug(robot.tasks['robot_task_position'].error, gainPosition.error)
plug(gainPosition.gain, robot.tasks['robot_task_position'].controlGain)
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)
from numpy import eye, hstack, identity, zeros
from dynamic_graph import plug
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.operator import Selec_of_vector
# Create the solver
# Connects the sequence player to the posture task
from dynamic_graph.sot.core.sot import SOT, Task
from dynamic_graph.sot.tools import SimpleSeqPlay
# Create the posture task
task_name = "posture_task"
taskPosture = Task(task_name)
taskPosture.dyn = robot.dynamic
taskPosture.feature = FeatureGeneric('feature_' + task_name)
taskPosture.featureDes = FeatureGeneric('feature_des_' + task_name)
taskPosture.gain = GainAdaptive("gain_" + task_name)
robotDim = robot.dynamic.getDimension()
first_6 = zeros((32, 6))
other_dof = identity(robotDim - 6)
jacobian_posture = hstack([first_6, other_dof])
taskPosture.feature.jacobianIN.value = matrixToTuple(jacobian_posture)
taskPosture.feature.setReference(taskPosture.featureDes.name)
taskPosture.add(taskPosture.feature.name)
# Create the sequence player
aSimpleSeqPlay = SimpleSeqPlay('aSimpleSeqPlay')
aSimpleSeqPlay.load(
def createTask(self):
self.task = Task('task' + self.name)
def __init__(self, dyn, name="posture"):
MetaTaskPosture.__init__(self, dyn, name)
self.task = Task('task' + name)
self.plugTask()
def __init__(self, dyn, name="com"):
MetaTaskCom.__init__(self, dyn, name)
self.task = Task('task' + name)
self.plugTask()