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 __init__(self, dyn, name="com"):
self.dyn = dyn
self.name = name
# dyn.setProperty('ComputeCoM','true')
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
plug(dyn.com, self.feature.errorIN)
plug(dyn.Jcom, self.feature.jacobianIN)
self.feature.setReference(self.featureDes.name)
def __init__(self, dyn, config=None, name="config"):
self.dyn = dyn
self.name = name
self.config = config
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
plug(dyn.position, self.feature.errorIN)
robotDim = dyn.getDimension()
self.feature.jacobianIN.value = matrixToTuple(identity(robotDim))
self.feature.setReference(self.featureDes.name)
if config is not None:
self.feature.selec.value = toFlags(self.config)
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 __init__(self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EEPosture, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
pinmodel = sotrobot.dynamic.model
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(
list(position))
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
# Define the reference and the selected DoF
rank = 0
ranks = []
for name in self.jointNames:
idJ = pinmodel.getJointId(name)
assert idJ < pinmodel.njoints
joint = pinmodel.joints[idJ]
idx_v = joint.idx_v
nv = joint.nv
ranks += range(idx_v, idx_v + nv)
q[idx_v:idx_v + nv] = position[rank:rank + nv]
rank += nv
assert rank == len(position)
self.tp.feature.posture.value = q
for i in ranks:
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + n)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
def __init__ (self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import identity, hstack, zeros
super(EEPosture, self).__init__()
self.gripper = gripper
# Get joint position in posture
pinmodel = sotrobot.dynamic.model
idJ = pinmodel.getJointId(gripper.sotjoint)
assert idJ < pinmodel.njoints
joint = sotrobot.dynamic.model.joints[idJ]
assert joint.nq == len(position)
idx_q = joint.idx_q + 1
idx_v = joint.idx_v + 1
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(position)
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture ('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
q[idx_v:idx_v + joint.nv] = position
self.tp.feature.posture.value = q
self.tp.gain = GainAdaptive("gain_"+n)
robotDim = sotrobot.dynamic.getDimension()
# for i in range (6, robotDim):
# self.tp.feature.selectDof (i, False)
# print idx_q, idx_v
for i in range(joint.nv):
self.tp.feature.selectDof (idx_v + i, True)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain,(4.9,0.9,0.01,0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [ self.tp ]
def __init__(self, name, sotrobot):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity
n = Posture.sep + name
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
q = list(sotrobot.dynamic.position.value)
self.tp.feature.state.value = q
self.tp.feature.posture.value = q
robotDim = sotrobot.dynamic.getDimension()
for i in range(6, robotDim):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + n)
self.tp.add(self.tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self.tp.feature.state)
self.tp.setWithDerivative(True)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [self.tp]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": [self._signalPositionRef]
},
"vel_" + name: {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": [self._signalVelocityRef]
},
}
def __init__(self, sotrobot, gripper, name_suffix):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EndEffector, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
self.robot = sotrobot
pinmodel = sotrobot.dynamic.model
self.name = Posture.sep.join(["ee", gripper.name, name_suffix])
self.tp = Task('task' + self.name)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + self.name)
plug(sotrobot.dynamic.position, self.tp.feature.state)
# Select the dofs
self.tp.feature.posture.value = sotrobot.dynamic.position.value
# Define the reference and the selected DoF
self.jointRanks = []
for name in self.jointNames:
idJ = pinmodel.getJointId(name)
assert idJ < pinmodel.njoints
joint = pinmodel.joints[idJ]
idx_v = joint.idx_v
nv = joint.nv
self.jointRanks.append((idx_v, nv))
for idx_v, nv in self.jointRanks:
for i in range(idx_v, idx_v + nv):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + self.name)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
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 __init__(self, dyn, joint, name=None):
if name is None:
name = "joint" + str(joint)
self.dyn = dyn
self.name = name
self.joint = joint
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
self.selec = Selec_of_vector("selec" + name)
self.selec.selec(joint, joint + 1)
plug(dyn.position, self.selec.sin)
plug(self.selec.sout, self.feature.errorIN)
robotDim = len(dyn.position.value)
Id = identity(robotDim)
J = Id[joint:joint + 1]
self.feature.jacobianIN.value = matrixToTuple(J)
self.feature.setReference(self.featureDes.name)
def __init__ (self, name, sotrobot):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import identity, hstack, zeros
n = Posture.sep + name
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeatureGeneric('feature_'+n)
self.tp.featureDes = FeatureGeneric('feature_des_'+n)
self.tp.gain = GainAdaptive("gain_"+n)
robotDim = sotrobot.dynamic.getDimension()
self.tp.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
self.tp.feature.setReference(self.tp.featureDes.name)
self.tp.add(self.tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self.tp.feature.errorIN)
self.tp.setWithDerivative (True)
# Set the gain of the posture task
setGain(self.tp.gain,10)
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [ self.tp ]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": [ self._signalPositionRef ] },
"vel_" + name: {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": [ self._signalVelocityRef ] },
}
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 dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.tools import SimpleSeqPlay
from numpy import eye
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity, hstack, zeros
# 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(
"/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz"
)
aSimpleSeqPlay.setTimeToStart(10.0)
# Connects the sequence player to the posture task
def createGain(self):
self.gain = GainAdaptive('gain'+self.name)
self.gain.set(1050,45,125e3)
def createGain(self):
self.gain = GainAdaptive('gain' + self.name)
self.gain.set(0.1, 0.1, 125e3)
tp.dyn = robot.dynamic
tp.feature = FeaturePosture('feature_' + n)
q = list(robot.device.robotState.value)
tp.feature.state.value = q
tp.feature.posture.value = q
robotDim = robot.dynamic.getDimension()
assert robotDim == len(q)
with_wheels = robotDim == 19
for i in range(8 if with_wheels else 6, robotDim):
tp.feature.selectDof(i, True)
tp.gain = GainAdaptive("gain_" + n)
tp.add(tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(robot.dynamic.position, tp.feature.state)
# Set the gain of the posture task
setGain(tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(tp.gain.gain, tp.controlGain)
plug(tp.error, tp.gain.error)
def getJointIdxQ(name):
model = robot.pinocchioModel
jid = model.getJointId(name)
return model.joints[jid].idx_q - 1
