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