def createBalanceTask (self, taskName, gain = 1.):
task = Task (taskName)
task.add (self.featureCom.name)
task.add (self.leftAnkle.name)
task.add (self.rightAnkle.name)
task.controlGain.value = gain
return task
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 setupJointLimitTask():
featureJl = FeatureJointLimits('featureJl')
featureJl.actuate()
dyn = robot.dynamic
plug(dyn.signal('position'), featureJl.signal('joint'))
plug(dyn.signal('upperJl'), featureJl.signal('upperJl'))
plug(dyn.signal('lowerJl'), featureJl.signal('lowerJl'))
taskJl = Task('taskJl')
taskJl.add('featureJl')
taskJl.signal('controlGain').value = .1
def setupCustomOpPointTask(op):
robot.dynamic.createCustomOpPoint(op, 27, (0, 0, -0.2))
robot.dynamic.signal(op).recompute(0)
robot.dynamic.signal('J' + op).recompute(0)
robot.features[op] = \
FeaturePosition(robot.name + '_feature_' + op,
robot.dynamic.signal(op),
robot.dynamic.signal('J' + op),
robot.dynamic.signal(op).value)
robot.tasks[op] = Task(robot.name + '_task_' + op)
robot.tasks[op].add(robot.name + '_feature_' + op)
robot.tasks[op].controlGain.value = .2
robot.OperationalPoints.append(op)
def __init__(self, name, filename):
AbstractHumanoidRobot.__init__(self, name)
self.OperationalPoints.append('waist')
p = Parser(self.name + '_dynamic', filename)
self.dynamic = p.parse()
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose")
self.initializeRobot()
self.compoundDrive = CompoundDrive(self.name + '_compoundDrive',
self.device)
self.compoundDriveTask = Task(self.name + '_compoundDriveTask')
self.compoundDriveTask.add(self.name + '_compoundDrive')
self.compoundDriveTask.signal('controlGain').value = 1.
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 setupPostureTask():
global postureTask, postureError, postureFeature
initialGain = 0.1
postureTask = Task(robot.name + '_posture')
postureFeature = FeatureGeneric(robot.name + '_postureFeature')
postureError = PostureError('PostureError')
posture = list(robot.halfSitting)
posture[6 + 17] -= 1
posture[6 + 24] += 1
# postureError.setPosture(tuple(posture))
plug(robot.device.state, postureError.state)
plug(postureError.error, postureFeature.errorIN)
postureFeature.jacobianIN.value = computeJacobian()
postureTask.add(postureFeature.name)
postureTask.controlGain.value = initialGain
def createOperationalPointFeatureAndTask(self,
operationalPointName,
featureName,
taskName,
gain = .2):
jacobianName = 'J{0}'.format(operationalPointName)
self.dynamic.signal(operationalPointName).recompute(0)
self.dynamic.signal(jacobianName).recompute(0)
feature = \
FeaturePosition(featureName,
self.dynamic.signal(operationalPointName),
self.dynamic.signal(jacobianName),
self.dynamic.signal(operationalPointName).value)
task = Task(taskName)
task.add(featureName)
task.controlGain.value = gain
return (feature, task)
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, 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 createCenterOfMassFeatureAndTask(self,
featureName, featureDesName,
taskName,
selec = '011',
gain = 1.):
self.dynamic.com.recompute(0)
self.dynamic.Jcom.recompute(0)
featureCom = FeatureGeneric(featureName)
plug(self.dynamic.com, featureCom.errorIN)
plug(self.dynamic.Jcom, featureCom.jacobianIN)
featureCom.selec.value = selec
featureComDes = FeatureGeneric(featureDesName)
featureComDes.errorIN.value = self.dynamic.com.value
featureCom.setReference(featureComDes.name)
comTask = Task(taskName)
comTask.add(featureName)
comTask.controlGain.value = gain
return (featureCom, featureComDes, comTask)
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, 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)
def __init__(self, dyn, config=None, name="config"):
MetaTaskConfig.__init__(self, dyn, config, name)
self.task = Task('task' + name)
self.plugTask()
"""
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
'''
I4 = ((1.,0,0,0.637),(0,1.,0,0.0),(0,0,1.,0.275),(0,0,0,1.),)
'''
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')
tracer.setBufferSize(2**20)
controller.tauDes.value = [0.0]
controller.init(timeStep, 1)
controller.setPosition([robot.device.state.value[LeftRollJoint + 6]])
robot.leftRollAnkleController = controller
robot.leftAnkleRollTask = MetaTaskKineJoint(robot.dynamic, LeftRollJoint)
robot.leftAnkleRollTask.task.controlGain.value = 0
robot.leftAnkleRollTask.task.setWithDerivative(True)
plug(robot.leftRollAnkleController.qRef,
robot.leftAnkleRollTask.featureDes.errorIN)
plug(robot.leftRollAnkleController.dqRef,
robot.leftAnkleRollTask.featureDes.errordotIN)
# -------------------------- SOT CONTROL --------------------------
# --- Posture
robot.taskPosture = Task('taskPosture')
robot.taskPosture.feature = FeaturePosture('featurePosture')
q = list(robot.dynamic.position.value)
robot.taskPosture.feature.state.value = q
robot.taskPosture.feature.posture.value = q
for i in range(18, 38):
robot.taskPosture.feature.selectDof(i, True)
robot.taskPosture.controlGain.value = 100.0
robot.taskPosture.add(robot.taskPosture.feature.name)
plug(robot.dynamic.position, robot.taskPosture.feature.state)
# --- CONTACTS
# define contactLF and contactRF
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)
#feature_lift.selec.value = '000000'
task_lift = Task('lift_task')
def __init__(self, dyn, joint, name=None):
MetaTaskJoint.__init__(self, dyn, joint, name)
self.task = Task('task' + self.name)
self.plugTask()
def __init__(self, dyn, name="com"):
MetaTaskCom.__init__(self, dyn, name)
self.task = Task('task' + name)
self.plugTask()
# flake8: noqa
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT, FeatureGeneric, FeaturePosture, GainAdaptive, Task
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.tiago.diff_drive_controller import HolonomicProjection
n = "posture"
tp = Task('task' + n)
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)
#from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
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)
robot.com_admittance_control = com_admittance_control
Kp_adm = [15.0, 15.0, 0.0] # this value is employed later
# --- Control Manager
robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
robot.cm.addCtrlMode('sot_input')
robot.cm.setCtrlMode('all', 'sot_input')
robot.cm.addEmergencyStopSIN('zmp')
robot.cm.addEmergencyStopSIN('distribute')
# -------------------------- SOT CONTROL --------------------------
# --- Upper body
robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
q = list(robot.dynamic.position.value)
robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q
robotDim = robot.dynamic.getDimension() # 38
for i in range(18, robotDim):
robot.taskUpperBody.feature.selectDof(i, True)
robot.taskUpperBody.controlGain.value = 100.0
robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
# --- CONTACTS
def createTask(self):
self.task = Task('task' + self.name)
