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") # 初始化机器人步态控制器
headLed.set(0xff0000)
eyeLed.set(0xa0a0ff)
# Enable gyro device.
gyro.enable(basicTimeStep)
# Perform one simulation step to get sensors working properly.
robot.step(timestep)
# Page 1: stand up.
# Page 9: assume walking position.
motionManager.playPage(1, True)
motionManager.playPage(9, True)
# Initialize OP2 gait manager.
gaitManager = None
gaitManager = RobotisOp2GaitManager(robot, "")
gaitManager.start()
gaitManager.setXAmplitude(0.0)
gaitManager.setYAmplitude(0.0)
gaitManager.setBalanceEnable(True)
gaitAmplitude = 0.5
looptimes = 0
# Main loop: perform a simulation step until the simulation is over.
# At the beginning, start walking on the spot.
# After 45 timesteps, begin taking steps forward.
while robot.step(timestep) != -1:
if looptimes == 45:
gaitManager.setXAmplitude(gaitAmplitude)
gaitManager.step(basicTimeStep)