def __init__(self, name, device=None, tracer=None, ns='sot_controller'): AbstractHumanoidRobot.__init__(self, name, tracer) self.device = device self.dynamic = RosSotRobotModel("{0}_dynamic".format(name)) self.dynamic.setNamespace(ns) self.dynamic.loadFromParameterServer() self.halfSitting = self.dynamic.curConf() self.dimension = self.dynamic.getDimension() self.initializeRobot()
pg.parseCmd(":armparameters 0.5") pg.parseCmd(":LimitsFeasibility 0.0") pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015") pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0") pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0") pg.parseCmd(":comheight 0.814") pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa") plug(robot.dynamic.position, pg.position) plug(robot.dynamic.com, pg.com) pg.motorcontrol.value = robot.dynamic.getDimension() * (0, ) pg.zmppreviouscontroller.value = (0, 0, 0) pg.initState() geom = RosSotRobotModel('geom') geom.setNamespace('sot_controller') geom.loadFromParameterServer() geom.createOpPoint('rf', 'right-ankle') geom.createOpPoint('lf', 'left-ankle') plug(robot.dynamic.position, geom.position) geom.ffposition.value = 6 * (0, ) geom.velocity.value = robot.dynamic.getDimension() * (0, ) geom.acceleration.value = robot.dynamic.getDimension() * (0, ) comRef = Selector('comRef', ['vector', 'ref', robot.dynamic.com, pg.comref]) plug(pg.inprocess, comRef.selec) selecSupportFoot = Selector( 'selecSupportFoot',