fre = 0 # frequency of oscillation in Hz
w = 2 * PI * fre # Hz to sec/rad conversion
# ===================================== Webots initation ======================================
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
dt = timestep * 1e-3
keys = robot.getKeyboard()
acc = robot.getAccelerometer('accelerometer')
ori = robot.getInertialUnit('IMU')
gyro = robot.getInertialUnit('gyro')
Module = ['FL', 'FR', 'HL', 'HR']
m_H = [0, 0, 0, 0]
q_H = [0, 0, 0, 0]
m_K = [0, 0, 0, 0]
q_K = [0, 0, 0, 0]
T = [0]
for i in range(4):
# motor initializing
#m_A[i] = robot.getMotor(Module[i] + 'Am')
m_H[i] = robot.getMotor(Module[i] + 'Hm')
m_K[i] = robot.getMotor(Module[i] + 'Km')
# Encoder initializing
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# led = robot.getLED('ledname')
# ds = robot.getDistanceSensor('dsname')
keys = robot.getKeyboard()
gpad = robot.getJoystick()
acc = robot.getAccelerometer('ACC')
ori = robot.getInertialUnit('IMU')
q11 = robot.getPositionSensor('FLAps')
q12 = robot.getPositionSensor('FLHps')
q13 = robot.getPositionSensor('FLKps')
q21 = robot.getPositionSensor('FRAps')
q22 = robot.getPositionSensor('FRHps')
q23 = robot.getPositionSensor('FRKps')
q31 = robot.getPositionSensor('HLAps')
q32 = robot.getPositionSensor('HLHps')
q33 = robot.getPositionSensor('HLKps')
q41 = robot.getPositionSensor('HRAps')
q42 = robot.getPositionSensor('HRHps')
q43 = robot.getPositionSensor('HRKps')
wheels = []
wheel_names = ["wheel1", "wheel2", "wheel3", "wheel4"]
for i in range(4):
wheels.append(robot.getMotor(wheel_names[i]))
wheels[i].setPosition(float("inf"))
wheels[i].setVelocity(0.0)
# make a list of the ultrasonic sensors
US_sensors = []
US_distances = []
US_sensor_names = ["distance sensor front right", "distance sensor front left"]
for i in range(US_SENSOR_COUNT):
US_sensors.append(robot.getDistanceSensor(US_sensor_names[i]))
US_sensors[i].enable(TIMESTEP)
US_distances.append(US_sensors[i].getValue())
# initialise accelerometer
accelerometer = robot.getAccelerometer("accelerometer")
accelerometer.enable(TIMESTEP)
# initialise compass
compass = robot.getCompass("compass")
compass.enable(TIMESTEP)
desired_orientation = get_2d_direction(compass.getValues())
# Main loop
while robot.step(TIMESTEP) != -1:
# read the sensors
acceleration_vector = accelerometer.getValues()
degrees_east_of_north = get_2d_direction(compass.getValues())
if acceleration_vector[1] > 0 and robot_flipped:
robot_flipped = False
robot_speed = -robot_speed
from controller import Robot
robot = Robot()
MAXSPEED = 5
timestep = int(robot.getBasicTimeStep())
go_m = robot.getMotor('body pitch motor')
poworot = robot.getMotor('body yaw motor')
head_see = robot.getMotor('head yaw motor')
go_m.setPosition(float('inf'))
go_m.setVelocity(0.0)
poworot.setPosition(float('inf'))
poworot.setVelocity(0.0)
head_see.setPosition(float('inf'))
head_see.setVelocity(0.0)
acc = robot.getAccelerometer('body accelerometer')
acc.enable(64)
flag = True
i = 1
j = 0
while robot.step(timestep) != -1:
r = acc.getValues()
if j != 0:
j -= 1
continue
elif abs(r[0]) > 2:
go_m.setVelocity(-i / 4)
poworot.setVelocity(-i * 1.5)
j = 10
elif flag:
go_m.setVelocity(2.0)
class Walk():
# class standingState(Enum):
# STANDING = 1
# FALL_BACK = 2
# FALL_FRONT = 3
# FALL_RIGHT = 4
# FALL_LEFT = 5
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") # 初始化机器人步态控制器
def myStep(self):
ret = self.robot.step(self.mTimeStep)
if ret == -1:
exit(0)
def wait(self, ms):
startTime = self.robot.getTime()
s = ms / 1000.0
while s + startTime >= self.robot.getTime():
self.myStep()
def run(self):
print("-------Walk example of ROBOTIS OP2-------")
print("This example illustrates Gait Manager")
print("Press the space bar to start/stop walking")
print("Use the arrow keys to move the robot while walking")
self.myStep() # 仿真一个步长，刷新传感器读数
self.mMotionManager.playPage(9) # 执行动作组9号动作，初始化站立姿势，准备行走
self.wait(200) # 等待200ms
self.isWalking = False # 初始时机器人未进入行走状态
while True:
self.checkIfFallen()
self.mGaitManager.setXAmplitude(0.0) # 前进为0
self.mGaitManager.setAAmplitude(0.0) # 转体为0
key = 0 # 初始键盘读入默认为0
key = self.mKeyboard.getKey() # 从键盘读取输入
if key == 32: # 如果读取到空格，则改变行走状态
if (self.isWalking): # 如果当前机器人正在走路，则使机器人停止
self.mGaitManager.stop()
self.isWalking = False
self.wait(200)
else: # 如果机器人当前停止，则开始走路
self.mGaitManager.start()
self.isWalking = True
self.wait(200)
elif key == 315: # 如果读取到‘↑’，则前进
self.mGaitManager.setXAmplitude(1.0)
elif key == 317: # 如果读取到‘↓’，则后退
self.mGaitManager.setXAmplitude(-1.0)
elif key == 316: # 如果读取到‘←’，则左转
self.mGaitManager.setAAmplitude(-0.5)
elif key == 314: # 如果读取到‘→’，则右转
self.mGaitManager.setAAmplitude(0.5)
self.mGaitManager.step(self.mTimeStep) # 步态生成器生成一个步长的动作
self.myStep() # 仿真一个步长
def checkIfFallen(self):
acc = self.mAccelerometer.getValues() # 通过加速度传感器获取三轴的对应值
acc_tolerance = 80.0
acc_step = 100
# print(acc)
# count how many steps the accelerometer
# says that the robot is down
if (acc[1] < 512.0 - acc_tolerance):
self.fup = self.fup + 1
else:
self.fup = 0
if (acc[1] > 512.0 + acc_tolerance):
self.fdown = self.fdown + 1
else:
self.fdown = 0
if (acc[0] < 512.0 - acc_tolerance):
self.fright = self.fright + 1
else:
self.fright = 0
if (acc[0] > 512.0 + acc_tolerance):
self.fleft = self.fleft + 1
else:
self.fleft = 0
# the robot face is down
if (self.fup > acc_step):
print("1")
self.mMotionManager.playPage(10) # f_up
self.mMotionManager.playPage(9) # init position
self.fup = 0
# the back face is down
elif (self.fdown > acc_step):
print("2")
self.mMotionManager.playPage(11) # b_up
self.mMotionManager.playPage(9) # init position
self.fdown = 0
# the back face is down
elif (self.fright > acc_step):
print("3")
self.mMotionManager.playPage(13)
self.fright = 0
# the back face is down
elif (self.fleft > acc_step):
print("4")
self.mMotionManager.playPage(12)
self.fleft = 0
