Ejemplo n.º 1
0
class RobotBuilder(AbstractHumanoidRobot):
    """
    This class instanciates a robot.
    """

    #OperationalPoints = ['right-wrist','left-wrist','waist','right-ankle','left-ankle']
    #OperationalPoints = ['right-wrist','left-wrist','waist','gaze']

    tracedSignals = {
        'dynamic': ["com", "zmp", "position", "velocity", "acceleration"],
        'device': ['zmp', 'control', 'state']
        }

    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()
Ejemplo n.º 2
0
class RobotBuilder(AbstractHumanoidRobot):
    """
    This class instanciates a robot.
    """

    #OperationalPoints = ['right-wrist','left-wrist','waist','right-ankle','left-ankle']
    #OperationalPoints = ['right-wrist','left-wrist','waist','gaze']

    tracedSignals = {
        'dynamic': ["com", "zmp", "position", "velocity", "acceleration"],
        'device': ['zmp', 'control', 'state']
    }

    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()
Ejemplo n.º 3
0
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',['matrixHomo','pg_H_sf',pg.rightfootref,pg.leftfootref],['matrixHomo','wa_H_sf',geom.rf,geom.lf])
plug(pg.SupportFoot,selecSupportFoot.selec)
Ejemplo n.º 4
0
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',
    ['matrixHomo', 'pg_H_sf', pg.rightfootref, pg.leftfootref],