def __init__(self, name, dimension, damping=None, timer=False):
from dynamic_graph.entity import VerbosityLevel
from dynamic_graph.sot.core.sot import SOT
sot = SOT(name)
sot.setSize(dimension)
sot.setMaxControlIncrementSquaredNorm(10.)
sot.setLoggerVerbosityLevel(VerbosityLevel.VERBOSITY_ALL)
if damping is not None: sot.damping.value = damping
self.sot = sot
self.tasks = []
if timer:
from .tools import insertTimerOnOutput
self.timer = insertTimerOnOutput(sot.control, "vector")
else:
self.timer = None
def init_appli(robot):
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh',
robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
# --- CONTACTS
contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
robot.OperationalPointsMap['left-ankle'])
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
locals()['contactLF'] = contactLF
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
locals()['contactRF'] = contactRF
from dynamic_graph import plug
from dynamic_graph.sot.core.sot import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
from dynamic_graph.ros import RosPublish
ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_control/state")
from dynamic_graph import plug
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push(taskCom.task.name)
robot.device.control.recompute(0)
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)
plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN)
# Connects the dynamics to the current feature of the posture task
getPostureValue = Selec_of_vector("current_posture")
getPostureValue.selec(6, robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(getPostureValue.sout, aSimpleSeqPlay.currentPosture)
# Set the gain of the posture task
setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
taskPosture.featureDes.errorIN.recompute(0)
# Push the posture task in the solver
