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