def __init__(self):
self.robot = Robot() # 初始化Robot类以控制机器人
self.mTimeStep = int(
self.robot.getBasicTimeStep()) # 获取当前每一个仿真步所仿真时间mTimeStep
self.HeadLed = self.robot.getLED('HeadLed') # 获取头部LED灯
self.EyeLed = self.robot.getLED('EyeLed') # 获取眼部LED灯
self.HeadLed.set(0xff0000) # 点亮头部LED灯并设置一个颜色
self.EyeLed.set(0xa0a0ff) # 点亮眼部LED灯并设置一个颜色
self.mAccelerometer = self.robot.getAccelerometer(
'Accelerometer') # 获取加速度传感器
self.mAccelerometer.enable(self.mTimeStep) # 激活传感器，并以mTimeStep为周期更新数值
# self.falling_state = standingState.STANDING
self.fup = 0
self.fdown = 0 # 定义两个类变量，用于之后判断机器人是否摔倒
self.fleft = 0
self.fright = 0 # 定义两个类变量，用于之后判断机器人是否摔倒
self.mGyro = self.robot.getGyro('Gyro') # 获取陀螺仪
self.mGyro.enable(self.mTimeStep) # 激活陀螺仪，并以mTimeStep为周期更新数值
self.positionSensors = [] # 初始化关节角度传感器
self.positionSensorNames = (
'ShoulderR', 'ShoulderL', 'ArmUpperR', 'ArmUpperL', 'ArmLowerR',
'ArmLowerL', 'PelvYR', 'PelvYL', 'PelvR', 'PelvL', 'LegUpperR',
'LegUpperL', 'LegLowerR', 'LegLowerL', 'AnkleR', 'AnkleL', 'FootR',
'FootL', 'Neck', 'Head') # 初始化各传感器名
# 获取各传感器并激活，以mTimeStep为周期更新数值
for i in range(0, len(self.positionSensorNames)):
self.positionSensors.append(
self.robot.getPositionSensor(self.positionSensorNames[i] +
'S'))
self.positionSensors[i].enable(self.mTimeStep)
self.mKeyboard = self.robot.getKeyboard() # 初始化键盘读入类
self.mKeyboard.enable(self.mTimeStep) # 以mTimeStep为周期从键盘读取
self.mMotionManager = RobotisOp2MotionManager(
self.robot) # 初始化机器人动作组控制器
self.mGaitManager = RobotisOp2GaitManager(self.robot,
"config.ini") # 初始化机器人步态控制器
sys.exit(0)
# Names of position sensors needed to get the corresponding device and read the measurements.
positionSensorNames = ('ShoulderR', 'ShoulderL', 'ArmUpperR', 'ArmUpperL',
'ArmLowerR', 'ArmLowerL', 'PelvYR', 'PelvYL',
'PelvR', 'PelvL', 'LegUpperR', 'LegUpperL',
'LegLowerR', 'LegLowerL', 'AnkleR', 'AnkleL',
'FootR', 'FootL', 'Neck', 'Head')
# List of position sensor devices.
positionSensors = []
# Create the Robot instance.
robot = Robot()
basicTimeStep = int(robot.getBasicTimeStep())
# Initialize motion manager.
motionManager = RobotisOp2MotionManager(robot)
# Get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# Retrieve devices.
headLed = robot.getLED('HeadLed')
eyeLed = robot.getLED('EyeLed')
gyro = robot.getGyro('Gyro')
# Enable all the position sensors and populate the 'positionSensor' list.
for i in range(0, len(positionSensorNames)):
positionSensors.append(robot.getPositionSensor(positionSensorNames[i] + 'S'))
positionSensors[i].enable(basicTimeStep)
# Initialize the LED devices.
headLed.set(0xff0000)