class Paul(object): def __init__(self): # Name of the controller self.name = "PAUL controller for Webots" # Current Speed of the robot (Used for exception handling recovery) self.speed = 0 self.up_speed = 5 self.down_speed = -5 # May want to consider making both of these final variables self.top_threshold = 1000 self.bottom_threshold = 1000 # create the Robot instance. self.robot = Robot() # get the time step of the current world. self.timestep = int(self.robot.getBasicTimeStep()) # initialise the 3 wheels self.wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3'] for i in range(3): self.wheels.append(self.robot.getDevice(wheelsNames[i])) self.wheels[i].setPosition(float('inf')) self.wheels[i].setVelocity(0.0) # unit: rad/s # initialise distance sensors self.top_ds = self.robot.getDevice('ds_top') self.bottom_ds = self.robot.getDevice('ds_bottom') self.top_ds.enable(self.timestep) self.bottom_ds.enable(self.timestep) # initialise LED self.uv_led = self.robot.getDevice('uv_led') # In the case that at init both distance sensors were already within a threshold, don't start up self.completed_init = True if self.check_top_ds_reading() and self.check_bottom_ds_reading(): self.completed_init = False # Movement Functions # Function to start the robot moving at a speed defined in the speed parameter def start_motors(self, speed): print("Starting motors...") self.speed = speed for i in range(3): self.wheels[i].setVelocity(speed) # Function to stop the robot moving def stop_motors(self): print("Stopping motors...") self.speed = 0 for i in range(3): self.wheels[i].setVelocity(0) # Motors and Sensors Readings # Function to display all readings from the motors and sensors def display_all_readings(self): self.display_motor_readings() self.display_top_ds_reading() self.display_bottom_ds_reading() # Function to display any readings from the motors def display_motor_readings(self): print("Motor speed: " + str(self.speed)) # Function get the current speed of the robot # TODO: Potentially remove, equivalent to getSpeed? def get_motor_readings(self): return self.speed # Function to check if the top distance sensor reading is within the threshold to trigger a change in direction # Returns True if the distance should change # Returns False if the distance is not within the threshold def check_top_ds_reading(self): return self.get_top_ds_reading() < self.get_top_threshold() # Function to display any readings from the top distance sensor def display_top_ds_reading(self): print("Top DS Reading: " + str(self.get_top_ds_reading())) # Function to display get readings from the top distance sensor def get_top_ds_reading(self): return self.top_ds.getValue() # Function to check if the top distance sensor reading is within the threshold to trigger a change in direction # Returns True if the distance should change # Returns False if the distance is not within the threshold def check_bottom_ds_reading(self): return self.get_bottom_ds_reading() < self.get_bottom_threshold() # Function to display any readings from the bottom distance sensor def display_bottom_ds_reading(self): print("Bottom DS Reading: " + str(self.get_bottom_ds_reading())) # Function to display get readings from the bottom distance sensor def get_bottom_ds_reading(self): return self.bottom_ds.getValue() def turn_uv_on(self): self.uv_led.set(255) def turn_uv_off(self): self.uv_led.set(0) def wait_for_time(self, time): self.robot.waitForUserInputEvent(self.robot.EVENT_NO_EVENT, time) # Getters and Setters def get_name(self): return self.name def set_name(self, name): self.name = name def get_speed(self): return self.speed def set_speed(self, speed): self.speed = speed def get_up_speed(self): return self.up_speed def set_up_speed(self, up_speed): self.up_speed = up_speed def get_down_speed(self): return self.down_speed def set_down_speed(self, down_speed): self.down_speed = down_speed def get_top_threshold(self): return self.top_threshold def set_top_threshold(self, top_threshold): self.top_threshold = top_threshold def get_bottom_threshold(self): return self.bottom_threshold def set_bottom_threshold(self, bottom_threshold): self.bottom_threshold = bottom_threshold
from controller import Camera from controller import CameraRecognitionObject import math #Function definitions def clamp(n, smallest, largest): return max(smallest, min(n, largest)) # create the Robot instance. agro_drone = Robot() # get the time step of the current world. timestep = int(agro_drone.getBasicTimeStep()) camera = agro_drone.getCamera("camera") # Initialise devices imu = agro_drone.getInertialUnit('inertial unit') imu.enable(timestep) gps = agro_drone.getGPS('gps') gps.enable(timestep) gyro = agro_drone.getGyro('gyro') gyro.enable(timestep) front_left_motor = agro_drone.getMotor("front left propeller") front_right_motor = agro_drone.getMotor("front right propeller")
class RobotEmitterReceiver(ABC): """ This RobotEmitterReceiver implements only the most basic run method, that steps the robot and calls the handleEmitter, handleReceiver methods that are needed for communication with the supervisor. This class must be inherited by all robot controllers created by the user and the handleEmitter, handleReceiver, initialize_comms methods are all abstract and need to be implemented, according to their docstrings. For a simpler RobotController that implements the methods in a basic form inherit the RobotEmitterReceiver class. """ def __init__(self, emitter_name="emitter", receiver_name="receiver", timestep=None): """ The basic robot constructor. Initializes the Webots Robot and sets up a basic timestep if None is supplied. Also initializes the emitter and the receiver used to communicate with the supervisor, using the initialize_comms() method which must be implemented by the user. The two methods handle_emitter() and handle_receiver() must also be implemented by the user. For the step argument see relevant Webots documentation: https://cyberbotics.com/doc/guide/controller-programming#the-step-and-wb_robot_step-functions :param timestep: float, positive or None """ self.robot = Robot() if timestep is None: self.timestep = int(self.robot.getBasicTimeStep()) else: self.timestep = timestep self.emitter, self.receiver = self.initialize_comms( emitter_name, receiver_name) def get_timestep(self): return self.timestep @abstractmethod def initialize_comms(self, emitter_name, receiver_name): """ This method should initialize and the return emitter and receiver in a tuple as expected by the constructor. A basic example implementation can be: emitter = self.robot.getEmitter("emitter") receiver = self.robot.getReceiver("receiver") receiver.enable(self.timestep) return emitter, receiver :return: (emitter, receiver) tuple, as initialized """ pass @abstractmethod def handle_emitter(self): """ This method should take data from the robot, eg. sensor data, parse it into a message and use the robot's emitter to send it to the supervisor. This message will be used as the observation of the robot. """ pass @abstractmethod def handle_receiver(self): """ This method should take data through the receiver in the form of a message and parse into data usable by the robot. For example the message might include a motor speed, which should be parsed and applied to the robot's motor. """ pass def run(self): """ This method is required by Webots to update the robot in the simulation. It steps the robot and in each step it runs the two handler methods to use the emitter and receiver components. This method should be called by a robot manager to run the robot. """ while self.robot.step(self.timestep) != -1: self.handle_receiver() self.handle_emitter()
import math import threading from tornado import httpserver from tornado import gen from tornado.ioloop import IOLoop import tornado.web #from controller import Robot, Motor, DistanceSensor from controller import Robot, Motor, DistanceSensor, InertialUnit from controller import LED # create the Robot instance. robot = Robot() lock = threading.Lock() #TIME_STEP = 64 TIME_STEP = int(robot.getBasicTimeStep()) MAX_SPEED = float(6) WHEEL_DIAMETER = 0.096 axle_length = 0.33 radius_wheel = 0.048 DEFAULT_SPEED = 6.28 DRIVE_FACTOR = 12.2 # positions to meters TURN_FACTOR = 30 port = robot.getCustomData() robot_name = robot.getName() print("Port: ", port) # init inertialunit #inertialUnit = robot.getDevice('inertial unit') #inertialUnit.enable(TIME_STEP)
TRANSLATE_FACTOR = 0.096 # ratio between the value reported by the wheel sensor and the actual # angle traveled by the robot while turning, in radian. ROTATE_FACTOR = 0.566 # maximum speed for the velocity value of the wheels. MAX_SPEED = 5.24 # minimum speed used while slowing down and trying to stop at a precise # location. MIN_SPEED = 0.1 robot = Robot() timestep = int(robot.getBasicTimeStep()) leftWheel = robot.getDevice('left wheel') rightWheel = robot.getDevice('right wheel') leftWheel.setVelocity(0) rightWheel.setVelocity(0) leftWheel.setPosition(float('inf')) rightWheel.setPosition(float('inf')) leftWheelSensor = robot.getDevice('left wheel sensor') leftWheelSensor.enable(timestep) rightWheelSensor = robot.getDevice('right wheel sensor') rightWheelSensor.enable(timestep)
class CustomRobotClass: def __init__(self): # Initialize Robot self.robot = Robot() self.time_step = int(self.robot.getBasicTimeStep()) print("Robot time step ", self.time_step) # Initialize sensors self.metacamera = self.robot.getCamera("MultiSense S21 meta camera") self.leftcamera = self.robot.getCamera("MultiSense S21 left camera") self.rightcamera = self.robot.getCamera("MultiSense S21 right camera") self.rangeFinder = self.robot.getRangeFinder( "MultiSense S21 meta range finder") self.cameras = { "left": self.leftcamera, "right": self.rightcamera, "meta": self.metacamera, "depth": self.rangeFinder } self.leftpositionsensor = self.robot.getPositionSensor( 'back left wheel sensor') self.rightpositionsensor = self.robot.getPositionSensor( 'back right wheel sensor') self.keyboard = Keyboard() self.pen = self.robot.getPen('pen') # Enable sensors self.metacamera.enable(self.time_step) self.leftcamera.enable(self.time_step) self.rightcamera.enable(self.time_step) self.rangeFinder.enable(self.time_step) self.leftpositionsensor.enable(self.time_step) self.rightpositionsensor.enable(self.time_step) self.keyboard.enable(self.time_step) # Initialize wheels backrightwheel = self.robot.getMotor('back right wheel') backleftwheel = self.robot.getMotor('back left wheel') frontrightwheel = self.robot.getMotor('front right wheel') frontleftwheel = self.robot.getMotor('front left wheel') self.wheels = [ backrightwheel, backleftwheel, frontrightwheel, frontleftwheel ] self.set_velocity_control() self.prevlspeed = 0 self.prevrspeed = 0 self.curlspeed = 0 self.currspeed = 0 def set_velocity_control(self): for wheel in self.wheels: wheel.setPosition((float('inf'))) wheel.setVelocity(0) def step(self): self.robot.step(self.time_step) def set_speed(self, l, r): print("Inside set speed", SPEED_UNIT * l, SPEED_UNIT * r) if self.prevlspeed != l or self.prevrspeed != r: self.wheels[0].setVelocity(SPEED_UNIT * r) self.wheels[1].setVelocity(SPEED_UNIT * l) self.wheels[2].setVelocity(SPEED_UNIT * r) self.wheels[3].setVelocity(SPEED_UNIT * l) self.prevlspeed = self.curlspeed self.prevrspeed = self.currspeed self.curlspeed = l self.currspeed = r def get_image(self, camera_name, depth_option=False): if (depth_option == True): return self.cameras[camera_name].getRangeImageArray() else: return self.cameras[camera_name].getImageArray()
class ManualTimestepRobot: """ Primary API for access to robot parts. This robot requires that the consumer manage the progession of time manually by calling the `sleep` method. """ def __init__(self, quiet: bool = False, init: bool = True) -> None: self._initialised = False self._quiet = quiet self.webot = WebotsRobot() # returns a float, but should always actually be an integer value self._timestep = int(self.webot.getBasicTimeStep()) self.mode = environ.get("SR_ROBOT_MODE", "dev") self.zone = int(environ.get("SR_ROBOT_ZONE", 0)) self.arena = "A" self.usbkey = path.normpath(path.join(environ["SR_ROBOT_FILE"], "../")) # Lock used to guard access to Webot's time stepping machinery, allowing # us to safely advance simulation time from *either* the competitor's # code (in the form of our `sleep` method) or from our background # thread, but not both. self._step_lock = Lock() if init: self.init() self.wait_start() @classmethod def setup(cls): return cls(init=False) def init(self) -> None: self._init_devs() self._initialised = True self.display_info() def _get_user_code_info(self) -> Optional[str]: user_version_path = path.join(self.usbkey, '.user-rev') if path.exists(user_version_path): with open(user_version_path) as f: return f.read().strip() return None def display_info(self) -> None: user_code_version = self._get_user_code_info() parts = [ "Zone: {}".format(self.zone), "Mode: {}".format(self.mode), ] if user_code_version: parts.append("User code: {}".format(user_code_version)) print("Robot Initialized. {}.".format(", ".join(parts))) # noqa:T001 def webots_step_and_should_continue(self, duration_ms: int) -> bool: """ Run a webots step of the given duration in milliseconds. Returns whether or not the simulation should continue (based on Webots telling us whether or not the simulation is about to end). """ if duration_ms <= 0: raise ValueError( "Duration must be greater than zero, not {!r}".format( duration_ms), ) with self._step_lock: # We use Webots in synchronous mode (specifically # `synchronization` is left at its default value of `TRUE`). In # that mode, Webots returns -1 from step to indicate that the # simulation is terminating, or 0 otherwise. result = self.webot.step(duration_ms) return result != -1 def wait_start(self) -> None: "Wait for the start signal to happen" if self.mode not in ["comp", "dev"]: raise Exception( "mode of '%s' is not supported -- must be 'comp' or 'dev'" % self.mode, ) if self.zone < 0 or self.zone > 3: raise Exception( "zone must be in range 0-3 inclusive -- value of %i is invalid" % self.zone, ) if self.arena not in ["A", "B"]: raise Exception("arena must be A or B") def _init_devs(self) -> None: "Initialise the attributes for accessing devices" # Motor boards self._init_motors() # Ruggeduinos self._init_ruggeduinos() # Camera self._init_camera() def _init_motors(self) -> None: self.motors = motor.init_motor_array(self.webot) def _init_ruggeduinos(self) -> None: self.ruggeduinos = ruggeduino.init_ruggeduino_array(self.webot) def _init_camera(self) -> None: # See comment in Camera.see for why we need to pass the step lock here. self.camera = camera.Camera(self.webot, self._step_lock) self.see = self.camera.see def time(self) -> float: """ Roughly equivalent to `time.time` but for simulation time. """ return self.webot.getTime() def sleep(self, secs: float) -> None: """ Roughly equivalent to `time.sleep` but accounting for simulation time. """ # Checks that secs is positive or zero if secs < 0: raise ValueError('sleep length must be non-negative') # Ensure the time delay is a valid step increment, while also ensuring # that small values remain nonzero. n_steps = math.ceil((secs * 1000) / self._timestep) duration_ms = n_steps * self._timestep # We're in the main thread here, so we don't really need to do any # cleanup if Webots tells us the simulation is terminating. When webots # kills the process all the proper tidyup will happen anyway. self.webots_step_and_should_continue(duration_ms)
def __init__(self, webot: Robot): self._compass = get_robot_device(webot, "robot compass", WebotsCompass) self._compass.enable(int(webot.getBasicTimeStep()))
from controller import Robot from controller import Camera TIME_STEP = 64 robot = Robot() #Define motors motors = [] motorNames = ['left motor', 'right motor'] for i in range(2): motors.append(robot.getMotor(motorNames[i])) motors[i].setPosition(float('inf')) motors[i].setVelocity(0.0) motors[i].setAcceleration(25) camera = Camera('camera') camera.enable(int(robot.getBasicTimeStep())) SPEED = 2 while robot.step(TIME_STEP) != -1: leftSpeed = 0.0 rightSpeed = 0.0 #get image and process it image = camera.getImage() leftSum = 0 rightSum = 0 cameraData = camera.getImage() for x in range(0, camera.getWidth()): for y in range(int(camera.getHeight() * 0.9), camera.getHeight()): gray = Camera.imageGetGray(cameraData, camera.getWidth(), x, y) if x < camera.getWidth() / 2: leftSum += gray else:
def main(): global robot, _timestep, _pop, _gbest, _w, _wdamp, _c1, _c2, iter ph = 0.0 chi = 0.0 outbuffer = [0.0 for i in range(0, 3)] # setting parameters for PSO _w = _IW _wdamp = _IW_Damp _c1 = _CT1 _c2 = _CT2 # If you would like to use Constriction Coefficients for PSO, # uncomment the following block and comment the above set of parameters. # Constriction Coefficients # ------------------------------------------------------- ph = _PH1 + _PH2 # was _PH1+_PH1 chi = 2.0 / (ph - 2 + math.sqrt(ph * ph - 4 * ph) ) # velocity constrain factor _w = chi # inertia Weight _wdamp = 1 # inertia Weight Damping Ratio _c1 = chi * _PH1 # personal Learning Coefficient _c2 = chi * _PH2 # global Learning Coefficient was _c1=chi*_PH2 # ------------------------------------------------------- # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) _timestep = timestep # 每16个基本步算一步 InitRobot() # Main loop: InitSwarm() Report(0) for iter in range(1, _MaxIter + 1): # 开始 实验 _MaxIter 次看看会不会成功 for i in range(1, _SwarmSize + 1): # 我们每一次Iter 其实都有_SwarmSize 个 particle _pop[i].update_velocity() # 公式15 _pop[i].update_position() # 公式16 # Evaluation _pop[i].fitness = Evaluation(_pop[i]) # evaluate 获取fitness # Update Global Best if _pop[i].fitness < _gbest.fitness: _gbest.fitness = _pop[i].fitness for j in range(1, _LenChrom + 1): _gbest.pos[j] = _pop[i].pos[j] else: #如果globle fitness没有变化或者还变差了,那就用firework炸一波 # 用FA更新swarm FA_results = FWA(_pop, 5) for i in range(len(_pop) - 1): _pop[i + 1].FA_pop(FA_results['fireworks'][i], FA_results['new_fitness'][i]) _gbest.fitness = _pop[1].fitness for j in range(1, _LenChrom + 1): _gbest.pos[j] = _pop[1].pos[j] # update inertia weight with damping ratio _w = _w * _wdamp # update inertia weight with damping ratio # save and report information of gbest Report(iter) # If the robot is capable of following the boundary more than one round, # then PSO terminates. if _gbest.fitness <= 1.0 / _MaxTimestep: iter = _MaxIter + 1 outbuffer[0] = 100 # terminating simulation command outbuffer[1] = _gbest.fitness # fitness value robot_send_data(outbuffer, 2) break # loop for iteration ends # reload the environment. outbuffer[0] = -1 # reset command outbuffer[1] = 0 # fitness value robot_send_data(outbuffer, 2) return 0
from controller import Robot from controller import Motor from controller import PositionSensor from controller import InertialUnit import math #create a robot instace arm = Robot() target1 = 0 target2 = 0 #target2=0 #kp=50 #kd=5 count = 0 r = 0 timestep = int(arm.getBasicTimeStep()) dt = 0.001 * timestep #print(timestep) logfile_name = "twolink_pd.log" #initialise every component motor1 = arm.getMotor("rm@flt") motor2 = arm.getMotor("rm@flk") motor3 = arm.getMotor("rm@blt") motor4 = arm.getMotor("rm@blk") motor5 = arm.getMotor("rm@frt") motor6 = arm.getMotor("rm@frk") motor7 = arm.getMotor("rm@brt") motor8 = arm.getMotor("rm@brk") ps1 = arm.getPositionSensor("ps@flt") ps2 = arm.getPositionSensor("ps@flk")
ENABLE_SOCKET = True ENABLE_VISION_DISPLAY = False VISION_DISPLAY_NAME = 'vision_display' # create the Robot instance. robot = Robot() # initialize vision display vision_display = False if ENABLE_VISION_DISPLAY: vision_display = VisionDisplay(robot.getDisplay(VISION_DISPLAY_NAME), robot.getCamera('camera'), Display.RGB) # get the time step of the current world. Constants.TIMESTEP = int(robot.getBasicTimeStep()) # get the robot movement logic rbc = RobotControls(robot) # get the keyboard for user input kb = robot.getKeyboard() kb.enable(Constants.TIMESTEP) socket = False if ENABLE_SOCKET: socket = SocketClient('localhost', 4444) # configure Controller class to handle all logic (robot movement and user input) RobotController = RobotController(rbc, url="ws://localhost:3333",
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
'f4_pos', 't4_pos', # LEG 4 'c5_pos', 'f5_pos', 't5_pos', # LEG 5 'c6_pos', 'f6_pos', 't6_pos', ] # LEG 6 for i in range(18): ps.append(robot.getPositionSensor(psName[i])) ps[i].enable(POSITION_SENSOR_SAMPLE_PERIOD) robot = Robot() timeStep = int(4 * robot.getBasicTimeStep()) keyboard = robot.getKeyboard() init_servos() init_positional_sensors() def run(): positions = [ 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2 ] while robot.step(timeStep) != 1: for servo, position in zip(servos, positions): servo.setPosition(position) break
class SimCamera(object): """Acquire image and depth from a camera sensor and publish on ROS""" def __init__(self): # webots os.environ['WEBOTS_ROBOT_NAME'] = 'camera' self.robot = Robot() self.bridge = CvBridge() self.timestep = int(self.robot.getBasicTimeStep()) # timestep is 4ms equals to 250FPS # nopub 4ms aprox 250FPS avg 0.5x simulation time # nopub 33ms aprox 30FPS avg 4x simulation time # nopub 67ms aprox 15FPS avg 7x simulation time # nopub 100ms aprox 10FPS avg 8x simulation time # nopub 500ms aprox 2FPS avg 8.5x simulation time self.camera = self.robot.getDeviceByIndex(1) self.depthcamera = self.robot.getDeviceByIndex(2) # ROS rospy.init_node('camera_server', anonymous=False) self.service_image = rospy.Service('image_camera_service', SimImageCameraService, self.getImage) self.service_depth = rospy.Service('depth_camera_service', SimDepthCameraService, self.getDepth) self.flag = False def getImage(self, data): """ respond to image request """ # enable the camera self.camera.enable(self.timestep) # take a time step # for i in range(5): self.robot.step(self.timestep) # get the image self.camera.getImage() self.camera.saveImage('tmp-img.png', 100) img = cv2.imread('tmp-img.png') # os.remove('tmp-img.png') self.camera_msg = self.bridge.cv2_to_imgmsg(img) # disable the camera to save processing power self.camera.disable() self.flag = True # send the image rospy.loginfo('Sending Image ') return self.camera_msg def getDepth(self, data): """ respond to depth image request """ # enable the camera self.depthcamera.enable(self.timestep) # take a time step # for i in range(5): self.robot.step(self.timestep) # get the depth image self.depthcamera.getRangeImage() self.depthcamera.saveImage('tmp-dep.png', 100) dep = self.depthcamera.getRangeImageArray() # print(type(dep)) # print(len(dep),len(dep[0])) dep = np.array(dep).transpose() self.depthcamera_msg = self.bridge.cv2_to_imgmsg(dep) # disable the camera to save processing power self.depthcamera.disable() # send the image rospy.loginfo('Sending Depth') return self.depthcamera_msg def update(self): """ publish the image and depth """ # image return self.camera.getImage() self.camera.saveImage('tmp-img.png', 100) img = cv2.imread('tmp-img.png') self.camera_msg = self.bridge.cv2_to_imgmsg(img) self.camera_pub.publish(self.camera_msg) # depth self.depthcamera.getRangeImage() dep = self.depthcamera.getRangeImageArray() dep = np.array(dep).transpose() self.depthcamera_msg = self.bridge.cv2_to_imgmsg(dep) self.depthcamera_pub.publish(self.depthcamera_msg)
class RobotController: def __init__(self, ros_active=False, robot='wolfgang', do_ros_init=True, robot_node=None, base_ns='', recognize=False, camera_active=True, foot_sensors_active=True): """ The RobotController, a Webots controller that controls a single robot. The environment variable WEBOTS_ROBOT_NAME should be set to "amy", "rory", "jack" or "donna" if used with 4_bots.wbt or to "amy" if used with 1_bot.wbt. :param ros_active: Whether ROS messages should be published :param robot: The name of the robot to use, currently one of wolfgang, darwin, nao, op3 :param do_ros_init: Whether to call rospy.init_node (only used when ros_active is True) :param external_controller: Whether an external controller is used, necessary for RobotSupervisorController :param base_ns: The namespace of this node, can normally be left empty """ self.ros_active = ros_active self.recognize = recognize self.camera_active = camera_active self.foot_sensors_active = foot_sensors_active if robot_node is None: self.robot_node = Robot() else: self.robot_node = robot_node self.walkready = [0] * 20 self.time = 0 self.motors = [] self.sensors = [] # for direct access self.motors_dict = {} self.sensors_dict = {} self.timestep = int(self.robot_node.getBasicTimeStep()) self.switch_coordinate_system = True self.is_wolfgang = False self.pressure_sensors = None if robot == 'wolfgang': self.is_wolfgang = True self.proto_motor_names = [ "RShoulderPitch [shoulder]", "LShoulderPitch [shoulder]", "RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw", "RHipRoll [hip]", "LHipRoll [hip]", "RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan", "HeadTilt" ] self.external_motor_names = [ "RShoulderPitch", "LShoulderPitch", "RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw", "RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan", "HeadTilt" ] self.initial_joint_positions = { "LAnklePitch": -30, "LAnkleRoll": 0, "LHipPitch": 30, "LHipRoll": 0, "LHipYaw": 0, "LKnee": 60, "RAnklePitch": 30, "RAnkleRoll": 0, "RHipPitch": -30, "RHipRoll": 0, "RHipYaw": 0, "RKnee": -60, "LShoulderPitch": 75, "LShoulderRoll": 0, "LElbow": 36, "RShoulderPitch": -75, "RShoulderRoll": 0, "RElbow": -36, "HeadPan": 0, "HeadTilt": 0 } self.sensor_suffix = "_sensor" accel_name = "imu accelerometer" gyro_name = "imu gyro" camera_name = "camera" self.pressure_sensor_names = [] if self.foot_sensors_active: self.pressure_sensor_names = [ "llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb" ] self.pressure_sensors = [] for name in self.pressure_sensor_names: sensor = self.robot_node.getDevice(name) sensor.enable(self.timestep) self.pressure_sensors.append(sensor) elif robot == 'darwin': self.proto_motor_names = [ "ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR", "FootL", "Neck", "Head" ] self.external_motor_names = [ "RShoulderPitch", "LShoulderPitch", "RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw", "RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan", "HeadTilt" ] self.sensor_suffix = "S" accel_name = "Accelerometer" gyro_name = "Gyro" camera_name = "Camera" elif robot == 'nao': self.proto_motor_names = [ "RShoulderPitch", "LShoulderPitch", "RShoulderRoll", "LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch", "LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch", "LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw", "HeadPitch" ] self.external_motor_names = self.proto_motor_names self.sensor_suffix = "S" accel_name = "accelerometer" gyro_name = "gyro" camera_name = "CameraTop" self.switch_coordinate_system = False elif robot == 'op3': self.proto_motor_names = [ "ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR", "FootL", "Neck", "Head" ] self.external_motor_names = [ "r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll", "r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll", "l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee", "r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll", "head_pan", "head_tilt" ] self.sensor_suffix = "S" accel_name = "Accelerometer" gyro_name = "Gyro" camera_name = "Camera" self.switch_coordinate_system = False self.motor_names_to_external_names = {} self.external_motor_names_to_motor_names = {} for i in range(len(self.proto_motor_names)): self.motor_names_to_external_names[ self.proto_motor_names[i]] = self.external_motor_names[i] self.external_motor_names_to_motor_names[ self.external_motor_names[i]] = self.proto_motor_names[i] self.current_positions = {} self.joint_limits = {} for motor_name in self.proto_motor_names: motor = self.robot_node.getDevice(motor_name) motor.enableTorqueFeedback(self.timestep) self.motors.append(motor) self.motors_dict[ self.motor_names_to_external_names[motor_name]] = motor sensor = self.robot_node.getDevice(motor_name + self.sensor_suffix) sensor.enable(self.timestep) self.sensors.append(sensor) self.sensors_dict[ self.motor_names_to_external_names[motor_name]] = sensor self.current_positions[self.motor_names_to_external_names[ motor_name]] = sensor.getValue() # min, max and middle position (precomputed since it will be used at each step) self.joint_limits[ self.motor_names_to_external_names[motor_name]] = ( motor.getMinPosition(), motor.getMaxPosition(), 0.5 * (motor.getMinPosition() + motor.getMaxPosition())) self.accel = self.robot_node.getDevice(accel_name) self.accel.enable(self.timestep) self.gyro = self.robot_node.getDevice(gyro_name) self.gyro.enable(self.timestep) if self.is_wolfgang: self.accel_head = self.robot_node.getDevice( "imu_head accelerometer") self.accel_head.enable(self.timestep) self.gyro_head = self.robot_node.getDevice("imu_head gyro") self.gyro_head.enable(self.timestep) self.camera = self.robot_node.getDevice(camera_name) self.camera_counter = 0 if self.camera_active: self.camera.enable(self.timestep * CAMERA_DIVIDER) if self.recognize: self.camera.recognitionEnable(self.timestep) self.last_img_saved = 0.0 self.img_save_dir = "/tmp/webots/images" + \ time.strftime("%Y-%m-%d-%H-%M-%S") + \ os.getenv('WEBOTS_ROBOT_NAME') if not os.path.exists(self.img_save_dir): os.makedirs(self.img_save_dir) self.imu_frame = rospy.get_param("~imu_frame", "imu_frame") if self.ros_active: if base_ns == "": clock_topic = "/clock" else: clock_topic = base_ns + "clock" if do_ros_init: rospy.init_node("webots_ros_interface", argv=['clock:=' + clock_topic]) self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole") self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole") self.camera_optical_frame = rospy.get_param( "~camera_optical_frame", "camera_optical_frame") self.head_imu_frame = rospy.get_param("~head_imu_frame", "imu_frame_2") self.pub_js = rospy.Publisher(base_ns + "joint_states", JointState, queue_size=1) self.pub_imu = rospy.Publisher(base_ns + "imu/data_raw", Imu, queue_size=1) self.pub_imu_head = rospy.Publisher(base_ns + "imu_head/data", Imu, queue_size=1) self.pub_cam = rospy.Publisher(base_ns + "camera/image_proc", Image, queue_size=1) self.pub_cam_info = rospy.Publisher(base_ns + "camera/camera_info", CameraInfo, queue_size=1, latch=True) self.pub_pres_left = rospy.Publisher(base_ns + "foot_pressure_left/raw", FootPressure, queue_size=1) self.pub_pres_right = rospy.Publisher(base_ns + "foot_pressure_right/raw", FootPressure, queue_size=1) self.cop_l_pub_ = rospy.Publisher(base_ns + "cop_l", PointStamped, queue_size=1) self.cop_r_pub_ = rospy.Publisher(base_ns + "cop_r", PointStamped, queue_size=1) rospy.Subscriber(base_ns + "DynamixelController/command", JointCommand, self.command_cb) # publish camera info once, it will be latched self.cam_info = CameraInfo() self.cam_info.header.stamp = rospy.Time.from_seconds(self.time) self.cam_info.header.frame_id = self.camera_optical_frame self.cam_info.height = self.camera.getHeight() self.cam_info.width = self.camera.getWidth() f_y = self.mat_from_fov_and_resolution( self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height, self.cam_info.width), self.cam_info.height) f_x = self.mat_from_fov_and_resolution(self.camera.getFov(), self.cam_info.width) self.cam_info.K = [ f_x, 0, self.cam_info.width / 2, 0, f_y, self.cam_info.height / 2, 0, 0, 1 ] self.cam_info.P = [ f_x, 0, self.cam_info.width / 2, 0, 0, f_y, self.cam_info.height / 2, 0, 0, 0, 1, 0 ] self.pub_cam_info.publish(self.cam_info) if robot == "op3": # start pose command = JointCommand() command.joint_names = ["r_sho_roll", "l_sho_roll"] command.positions = [-math.tau / 8, math.tau / 8] self.command_cb(command) # needed to run this one time to initialize current position, otherwise velocity will be nan self.get_joint_values(self.external_motor_names) def mat_from_fov_and_resolution(self, fov, res): return 0.5 * res * (math.cos((fov / 2)) / math.sin((fov / 2))) def h_fov_to_v_fov(self, h_fov, height, width): return 2 * math.atan(math.tan(h_fov * 0.5) * (height / width)) def step_sim(self): self.time += self.timestep / 1000 self.robot_node.step(self.timestep) def step(self): self.step_sim() if self.ros_active: self.publish_ros() def publish_ros(self): self.publish_imu() self.publish_joint_states() if self.camera_active and self.camera_counter == 0: self.publish_camera() self.publish_pressure() if self.recognize: self.save_recognition() self.camera_counter = (self.camera_counter + 1) % CAMERA_DIVIDER def convert_joint_radiant_to_scaled(self, joint_name, pos): # helper method to convert to scaled position between [-1,1] for this joint using min max scaling lower_limit, upper_limit, mid_position = self.joint_limits[joint_name] return 2 * (pos - mid_position) / (upper_limit - lower_limit) def convert_joint_scaled_to_radiant(self, joint_name, position): # helper method to convert to scaled position for this joint using min max scaling lower_limit, upper_limit, mid_position = self.joint_limits[joint_name] return position * (upper_limit - lower_limit) / 2 + mid_position def set_joint_goal_position(self, joint_name, goal_position, goal_velocity=-1, scaled=False, relative=False): motor = self.motors_dict[joint_name] if scaled: goal_position = self.convert_joint_radiant_to_scaled( joint_name, goal_position) if relative: goal_position = goal_position + self.get_joint_values([joint_name ])[0] motor.setPosition(goal_position) if goal_velocity == -1: motor.setVelocity(motor.getMaxVelocity()) else: motor.setVelocity(goal_velocity) def set_joint_goals_position(self, joint_names, goal_positions, goal_velocities=[]): for i in range(len(joint_names)): try: if len(goal_velocities) != 0: self.set_joint_goal_position(joint_names[i], goal_positions[i], goal_velocities[i]) else: self.set_joint_goal_position(joint_names[i], goal_positions[i]) except ValueError: print(f"invalid motor specified ({joint_names[i]})") def command_cb(self, command: JointCommand): if len(command.positions) != 0: # position control # todo maybe needs to match external motor names to interal ones fist? self.set_joint_goals_position(command.joint_names, command.positions, command.velocities) else: # torque control for i, name in enumerate(command.joint_names): try: self.motors_dict[name].setTorque(command.accelerations[i]) except ValueError: print(f"invalid motor specified ({name})") def set_head_tilt(self, pos): self.motors[-1].setPosition(pos) def set_arms_zero(self): positions = [ -0.8399999308200574, 0.7200000596634105, -0.3299999109923385, 0.35999992683575216, 0.5099999812500172, -0.5199999789619728 ] for i in range(0, 6): self.motors[i].setPosition(positions[i]) def get_joint_values(self, used_joint_names, scaled=False): joint_positions = [] joint_velocities = [] joint_torques = [] for joint_name in used_joint_names: value = self.sensors_dict[joint_name].getValue() if scaled: value = self.convert_joint_radiant_to_scaled(joint_name, value) joint_positions.append(value) joint_velocities.append(self.current_positions[joint_name] - value) joint_torques.append( self.motors_dict[joint_name].getTorqueFeedback()) self.current_positions[joint_name] = value return joint_positions, joint_velocities, joint_torques def get_joint_state_msg(self): js = JointState() js.name = [] js.header.stamp = rospy.Time.from_seconds(self.time) js.position = [] js.effort = [] for joint_name in self.external_motor_names: js.name.append(joint_name) value = self.sensors_dict[joint_name].getValue() js.position.append(value) js.velocity.append(self.current_positions[joint_name] - value) js.effort.append(self.motors_dict[joint_name].getTorqueFeedback()) self.current_positions[joint_name] = value return js def publish_joint_states(self): self.pub_js.publish(self.get_joint_state_msg()) def get_imu_msg(self, head=False): msg = Imu() msg.header.stamp = rospy.Time.from_seconds(self.time) if head: msg.header.frame_id = self.head_imu_frame else: msg.header.frame_id = self.imu_frame # change order because webots has different axis if head: accel_vels = self.accel_head.getValues() msg.linear_acceleration.x = accel_vels[2] msg.linear_acceleration.y = -accel_vels[0] msg.linear_acceleration.z = -accel_vels[1] else: accel_vels = self.accel.getValues() msg.linear_acceleration.x = accel_vels[0] msg.linear_acceleration.y = accel_vels[1] msg.linear_acceleration.z = accel_vels[2] # make sure that acceleration is not completely zero or we will get error in filter. # Happens if robot is moved manually in the simulation if msg.linear_acceleration.x == 0 and msg.linear_acceleration.y == 0 and msg.linear_acceleration.z == 0: msg.linear_acceleration.z = 0.001 if head: gyro_vels = self.gyro_head.getValues() msg.angular_velocity.x = gyro_vels[2] msg.angular_velocity.y = -gyro_vels[0] msg.angular_velocity.z = -gyro_vels[1] else: gyro_vels = self.gyro.getValues() msg.angular_velocity.x = gyro_vels[0] msg.angular_velocity.y = gyro_vels[1] msg.angular_velocity.z = gyro_vels[2] return msg def publish_imu(self): self.pub_imu.publish(self.get_imu_msg(head=False)) if self.is_wolfgang: self.pub_imu_head.publish(self.get_imu_msg(head=True)) def publish_camera(self): img_msg = Image() img_msg.header.stamp = rospy.Time.from_seconds(self.time) img_msg.header.frame_id = self.camera_optical_frame img_msg.height = self.camera.getHeight() img_msg.width = self.camera.getWidth() img_msg.encoding = "bgra8" img_msg.step = 4 * self.camera.getWidth() img = self.camera.getImage() img_msg.data = img self.pub_cam.publish(img_msg) def save_recognition(self): if self.time - self.last_img_saved < 1.0: return self.last_img_saved = self.time annotation = "" img_stamp = f"{self.time:.2f}".replace(".", "_") img_name = f"img_{os.getenv('WEBOTS_ROBOT_NAME')}_{img_stamp}.PNG" recognized_objects = self.camera.getRecognitionObjects() # variables for saving not in image later found_ball = False found_wolfgang = False for e in range(self.camera.getRecognitionNumberOfObjects()): model = recognized_objects[e].get_model() position = recognized_objects[e].get_position_on_image() size = recognized_objects[e].get_size_on_image() if model == b"soccer ball": found_ball = True vector = f"""{{"x1": {position[0] - 0.5 * size[0]}, "y1": {position[1] - 0.5 * size[1]}, "x2": {position[0] + 0.5 * size[0]}, "y2": {position[1] + 0.5 * size[1]}}}""" annotation += f"{img_name}|" annotation += "ball|" annotation += vector annotation += "\n" if model == b"wolfgang": found_wolfgang = True vector = f"""{{"x1": {position[0] - 0.5 * size[0]}, "y1": {position[1] - 0.5 * size[1]}, "x2": {position[0] + 0.5 * size[0]}, "y2": {position[1] + 0.5 * size[1]}}}""" annotation += f"{img_name}|" annotation += "robot|" annotation += vector annotation += "\n" if not found_ball: annotation += f"{img_name}|ball|not in image\n" if not found_wolfgang: annotation += f"{img_name}|robot|not in image\n" with open(os.path.join(self.img_save_dir, "annotations.txt"), "a") as f: f.write(annotation) self.camera.saveImage(filename=os.path.join(self.img_save_dir, img_name), quality=100) def get_image(self): return self.camera.getImage() def get_pressure_message(self): current_time = rospy.Time.from_sec(self.time) if not self.foot_sensors_active: cop_r = PointStamped() cop_r.header.frame_id = self.r_sole_frame cop_r.header.stamp = current_time cop_l = PointStamped() cop_l.header.frame_id = self.l_sole_frame cop_l.header.stamp = current_time return FootPressure(), FootPressure(), cop_l, cop_r left_pressure = FootPressure() left_pressure.header.stamp = current_time left_pressure.left_back = self.pressure_sensors[0].getValue() left_pressure.left_front = self.pressure_sensors[1].getValue() left_pressure.right_front = self.pressure_sensors[2].getValue() left_pressure.right_back = self.pressure_sensors[3].getValue() right_pressure = FootPressure() right_pressure.header.stamp = current_time right_pressure.left_back = self.pressure_sensors[4].getValue() right_pressure.left_front = self.pressure_sensors[5].getValue() right_pressure.right_front = self.pressure_sensors[6].getValue() right_pressure.right_back = self.pressure_sensors[7].getValue() # compute center of pressures of the feet pos_x = 0.085 pos_y = 0.045 # we can take a very small threshold, since simulation gives more accurate values than reality threshold = 1 cop_l = PointStamped() cop_l.header.frame_id = self.l_sole_frame cop_l.header.stamp = current_time sum = left_pressure.left_back + left_pressure.left_front + left_pressure.right_front + left_pressure.right_back if sum > threshold: cop_l.point.x = (left_pressure.left_front + left_pressure.right_front - left_pressure.left_back - left_pressure.right_back) * pos_x / sum cop_l.point.x = max(min(cop_l.point.x, pos_x), -pos_x) cop_l.point.y = (left_pressure.left_front + left_pressure.left_back - left_pressure.right_front - left_pressure.right_back) * pos_y / sum cop_l.point.y = max(min(cop_l.point.x, pos_y), -pos_y) else: cop_l.point.x = 0 cop_l.point.y = 0 cop_r = PointStamped() cop_r.header.frame_id = self.r_sole_frame cop_r.header.stamp = current_time sum = right_pressure.right_back + right_pressure.right_front + right_pressure.right_front + right_pressure.right_back if sum > threshold: cop_r.point.x = (right_pressure.left_front + right_pressure.right_front - right_pressure.left_back - right_pressure.right_back) * pos_x / sum cop_r.point.x = max(min(cop_r.point.x, pos_x), -pos_x) cop_r.point.y = (right_pressure.left_front + right_pressure.left_back - right_pressure.right_front - right_pressure.right_back) * pos_y / sum cop_r.point.y = max(min(cop_r.point.x, pos_y), -pos_y) else: cop_r.point.x = 0 cop_r.point.y = 0 return left_pressure, right_pressure, cop_l, cop_r def publish_pressure(self): left, right, cop_l, cop_r = self.get_pressure_message() self.pub_pres_left.publish(left) self.pub_pres_right.publish(right) self.cop_l_pub_.publish(cop_l) self.cop_r_pub_.publish(cop_r)
import numpy as np import pickle # Import evalution functions from eval import showPlots # Import functions from other scripts in controller folder from lqr_controller import LQRController from adaptive_controller import AdaptiveController #from lqr_controller import CustomController # Instantiate dron driver supervisor driver = Robot() # Get the time step of the current world timestep = int(driver.getBasicTimeStep()) # Set your percent loss of thrust lossOfThust = 0.5 # Instantiate controller and start sensors customController = AdaptiveController(driver, lossOfThust) customController.initializeMotors() customController.startSensors(timestep) # Initialize state storage vectors stateHistory = [] referenceHistory = [] # flag for motor failure motor_failure = False
import os import time import psutil os.environ['WEBOTS_ROBOT_NAME'] = 'icebox' found_icebox = False for proc in psutil.process_iter(): try: if 'icebox' in proc.name().lower(): found_icebox = True break except (psutil.NoSuchProcess, psutil.AccessDenied, psutil.ZombieProcess): pass if found_icebox is False: # print('icebox controller: killing previous instances') # os.system("killall -9 icebox") print('icebox controller: running new') os.system("icebox --Ice.Config=icebox.conf &") from controller import Robot robot = Robot() while robot.step(int(robot.getBasicTimeStep())) != -1: pass
class Controller(): jointNames = [ # coxa, femur, tibia 'c1', 'f1', 't1', # LEG 1 'c2', 'f2', 't2', # LEG 2 'c3', 'f3', 't3', # LEG 3 'c4', 'f4', 't4', # LEG 4 'c5', 'f5', 't5', # LEG 5 'c6', 'f6', 't6' ] # LEG 6 def __init__(self): # Initialize the Webots Supervisor. self.robot = Robot() self.timeStep = int(4 * self.robot.getBasicTimeStep()) self.keyboard = self.robot.getKeyboard() # Define list for motors and position sensors self.motors = [] self.position_sensors = [] # Initialise the motors and position sensors (could be moved into __init__ into a single for-loop) self.init_motors() self.init_positional_sensors() def init_motors(self): for name in Controller.jointNames: motor = self.robot.getMotor(name + '_motor') # motor.setPosition(float('inf')) # motor.setVelocity(0) self.motors.append(motor) def init_positional_sensors(self): for name in Controller.jointNames: positional_sensor = self.robot.getPositionSensor( name + '_position_sensor') positional_sensor.enable(POSITION_SENSOR_SAMPLE_PERIOD) self.position_sensors.append(positional_sensor) def positionN(self): while self.robot.step(self.timeStep) != 1: for motor, position in zip(self.motors, positions): motor.setPosition(position) break def readPos(self): value = [] while self.robot.step(self.timeStep) != 1: for i in range(len(self.jointNames)): value = self.position_sensors[i].getValue() all_positions.append(value) print(all_positions) #if all_positions == positions: # all_positions = [] # return #all_positions = [] return # not sure about that one def walk(self): while self.robot.step(self.timeStep) != -1: tripodGait(0, 20, 10, 1)
'--recognize', action='store_true', help="if true, recognition is active (for training data collection)") args, unknown = parser.parse_known_args() rospy.init_node("webots_ros_interface", argv=['clock:=/clock']) pid_param_name = "/webots_pid" + args.sim_id while not rospy.has_param(pid_param_name): rospy.logdebug("Waiting for parameter " + pid_param_name + " to be set..") time.sleep(2.0) webots_pid = rospy.get_param(pid_param_name) os.environ["WEBOTS_PID"] = str(webots_pid) os.environ["WEBOTS_ROBOT_NAME"] = args.robot_name if args.void_controller: rospy.logdebug("Starting void interface for " + args.robot_name) from controller import Robot r = Robot() while not rospy.is_shutdown(): r.step(int(r.getBasicTimeStep())) else: rospy.logdebug("Starting ros interface for " + args.robot_name) r = RobotController(ros_active=True, do_ros_init=False, recognize=args.recognize, camera_active=(not args.disable_camera)) while not rospy.is_shutdown(): r.step()
# Update: 17 September 2021 - add comments and adjust variable names # Update: 03 March 2022 - change the coordinate system to ENU to match the default of Webots R2022a from controller import Robot, DistanceSensor, Motor import numpy as np #------------------------------------------------------- # Initialize variables MAX_SPEED = 6.28 # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # [ms] delta_t = robot.getBasicTimeStep() / 1000.0 # [s] # states states = ['forward', 'turn_right', 'turn_left'] current_state = states[0] # counter: used to maintain an active state for a number of cycles counter = 0 COUNTER_MAX = 3 # Robot pose # Adjust the initial values to match the initial robot pose in your simulation x = -0.06 # position in x [m] y = 0.436 # position in y [m] phi = 0.0531 # orientation [rad]
class RobotController: def __init__(self, ros_active=False, robot='wolfgang', do_ros_init=True): # requires WEBOTS_ROBOT_NAME to be set to "amy" or "rory" self.ros_active = ros_active self.robot_node = Robot() self.walkready = [0] * 20 self.time = 0 self.motors = [] self.sensors = [] self.timestep = int(self.robot_node.getBasicTimeStep()) self.switch_coordinate_system = True self.is_wolfgang = False self.pressure_sensors = None if robot == 'wolfgang': self.is_wolfgang = True self.motor_names = [ "RShoulderPitch", "LShoulderPitch", "RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw", "RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan", "HeadTilt" ] self.external_motor_names = self.motor_names sensor_postfix = "_sensor" accel_name = "imu accelerometer" gyro_name = "imu gyro" camera_name = "camera" pressure_sensor_names = [ "llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb" ] self.pressure_sensors = [] for name in pressure_sensor_names: sensor = self.robot_node.getDevice(name) sensor.enable(30) self.pressure_sensors.append(sensor) elif robot == 'darwin': self.motor_names = [ "ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR", "FootL", "Neck", "Head" ] self.external_motor_names = [ "RShoulderPitch", "LShoulderPitch", "RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw", "RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan", "HeadTilt" ] sensor_postfix = "S" accel_name = "Accelerometer" gyro_name = "Gyro" camera_name = "Camera" elif robot == 'nao': self.motor_names = [ "RShoulderPitch", "LShoulderPitch", "RShoulderRoll", "LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch", "LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch", "LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw", "HeadPitch" ] self.external_motor_names = self.motor_names sensor_postfix = "S" accel_name = "accelerometer" gyro_name = "gyro" camera_name = "CameraTop" self.switch_coordinate_system = False elif robot == 'op3': self.motor_names = [ "ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR", "FootL", "Neck", "Head" ] self.external_motor_names = [ "r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll", "r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll", "l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee", "r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll", "head_pan", "head_tilt" ] sensor_postfix = "S" accel_name = "Accelerometer" gyro_name = "Gyro" camera_name = "Camera" self.switch_coordinate_system = False # self.robot_node = self.supervisor.getFromDef(self.robot_node_name) for motor_name in self.motor_names: self.motors.append(self.robot_node.getDevice(motor_name)) self.motors[-1].enableTorqueFeedback(self.timestep) self.sensors.append( self.robot_node.getDevice(motor_name + sensor_postfix)) self.sensors[-1].enable(self.timestep) self.accel = self.robot_node.getDevice(accel_name) self.accel.enable(self.timestep) self.gyro = self.robot_node.getDevice(gyro_name) self.gyro.enable(self.timestep) if self.is_wolfgang: self.accel_head = self.robot_node.getDevice( "imu_head accelerometer") self.accel_head.enable(self.timestep) self.gyro_head = self.robot_node.getDevice("imu_head gyro") self.gyro_head.enable(self.timestep) self.camera = self.robot_node.getDevice(camera_name) self.camera.enable(self.timestep) if self.ros_active: if do_ros_init: rospy.init_node("webots_ros_interface", argv=['clock:=/clock']) self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole") self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole") self.camera_optical_frame = rospy.get_param( "~camera_optical_frame", "camera_optical_frame") self.head_imu_frame = rospy.get_param("~head_imu_frame", "imu_frame_2") self.imu_frame = rospy.get_param("~imu_frame", "imu_frame") self.pub_js = rospy.Publisher("joint_states", JointState, queue_size=1) self.pub_imu = rospy.Publisher("imu/data_raw", Imu, queue_size=1) self.pub_imu_head = rospy.Publisher("imu_head/data", Imu, queue_size=1) self.pub_cam = rospy.Publisher("camera/image_proc", Image, queue_size=1) self.pub_cam_info = rospy.Publisher("camera/camera_info", CameraInfo, queue_size=1, latch=True) self.pub_pres_left = rospy.Publisher("foot_pressure_left/filtered", FootPressure, queue_size=1) self.pub_pres_right = rospy.Publisher( "foot_pressure_right/filtered", FootPressure, queue_size=1) self.cop_l_pub_ = rospy.Publisher("cop_l", PointStamped, queue_size=1) self.cop_r_pub_ = rospy.Publisher("cop_r", PointStamped, queue_size=1) rospy.Subscriber("DynamixelController/command", JointCommand, self.command_cb) # publish camera info once, it will be latched self.cam_info = CameraInfo() self.cam_info.header.stamp = rospy.Time.from_seconds(self.time) self.cam_info.header.frame_id = self.camera_optical_frame self.cam_info.height = self.camera.getHeight() self.cam_info.width = self.camera.getWidth() f_y = self.mat_from_fov_and_resolution( self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height, self.cam_info.width), self.cam_info.height) f_x = self.mat_from_fov_and_resolution(self.camera.getFov(), self.cam_info.width) self.cam_info.K = [ f_x, 0, self.cam_info.width / 2, 0, f_y, self.cam_info.height / 2, 0, 0, 1 ] self.cam_info.P = [ f_x, 0, self.cam_info.width / 2, 0, 0, f_y, self.cam_info.height / 2, 0, 0, 0, 1, 0 ] if self.ros_active: self.pub_cam_info.publish(self.cam_info) def mat_from_fov_and_resolution(self, fov, res): return 0.5 * res * (math.cos((fov / 2)) / math.sin((fov / 2))) def h_fov_to_v_fov(self, h_fov, height, width): return 2 * math.atan(math.tan(h_fov * 0.5) * (height / width)) def step_sim(self): self.time += self.timestep / 1000 self.robot_node.step(self.timestep) def step(self): self.step_sim() if self.ros_active: self.publish_imu() self.publish_joint_states() self.publish_camera() self.publish_pressure() def command_cb(self, command: JointCommand): for i, name in enumerate(command.joint_names): try: motor_index = self.external_motor_names.index(name) self.motors[motor_index].setPosition(command.positions[i]) if command.velocities[i] == -1: self.motors[motor_index].setVelocity( self.motors[motor_index].getMaxVelocity()) else: self.motors[motor_index].setVelocity(command.velocities[i]) self.motors[motor_index].setAcceleration( command.accelerations[i]) except ValueError: print(f"invalid motor specified ({name})") def set_head_tilt(self, pos): self.motors[-1].setPosition(pos) def set_arms_zero(self): positions = [ -0.8399999308200574, 0.7200000596634105, -0.3299999109923385, 0.35999992683575216, 0.5099999812500172, -0.5199999789619728 ] for i in range(0, 6): self.motors[i].setPosition(positions[i]) def get_joint_state_msg(self): js = JointState() js.name = [] js.header.stamp = rospy.Time.from_seconds(self.time) js.position = [] js.effort = [] for i in range(len(self.sensors)): js.name.append(self.external_motor_names[i]) value = self.sensors[i].getValue() js.position.append(value) js.effort.append(self.motors[i].getTorqueFeedback()) return js def publish_joint_states(self): self.pub_js.publish(self.get_joint_state_msg()) def get_imu_msg(self, head=False): msg = Imu() msg.header.stamp = rospy.Time.from_seconds(self.time) if head: msg.header.frame_id = self.head_imu_frame else: msg.header.frame_id = self.imu_frame # change order because webots has different axis if head: accel_vels = self.accel_head.getValues() msg.linear_acceleration.x = accel_vels[2] msg.linear_acceleration.y = -accel_vels[0] msg.linear_acceleration.z = -accel_vels[1] else: accel_vels = self.accel.getValues() msg.linear_acceleration.x = accel_vels[0] msg.linear_acceleration.y = accel_vels[1] msg.linear_acceleration.z = accel_vels[2] if head: gyro_vels = self.gyro_head.getValues() msg.angular_velocity.x = gyro_vels[2] msg.angular_velocity.y = -gyro_vels[0] msg.angular_velocity.z = -gyro_vels[1] else: gyro_vels = self.gyro.getValues() msg.angular_velocity.x = gyro_vels[0] msg.angular_velocity.y = gyro_vels[1] msg.angular_velocity.z = gyro_vels[2] return msg def publish_imu(self): self.pub_imu.publish(self.get_imu_msg(head=False)) self.pub_imu_head.publish(self.get_imu_msg(head=True)) def publish_camera(self): img_msg = Image() img_msg.header.stamp = rospy.Time.from_seconds(self.time) img_msg.header.frame_id = self.camera_optical_frame img_msg.height = self.camera.getHeight() img_msg.width = self.camera.getWidth() img_msg.encoding = "bgra8" img_msg.step = 4 * self.camera.getWidth() img = self.camera.getImage() img_msg.data = img self.pub_cam.publish(img_msg) def get_image(self): return self.camera.getImage() def get_pressure_message(self): current_time = rospy.Time.from_sec(self.time) left_pressure = FootPressure() left_pressure.header.stamp = current_time left_pressure.left_back = self.pressure_sensors[0].getValues()[2] left_pressure.left_front = self.pressure_sensors[1].getValues()[2] left_pressure.right_front = self.pressure_sensors[2].getValues()[2] left_pressure.right_back = self.pressure_sensors[3].getValues()[2] right_pressure = FootPressure() left_pressure.header.stamp = current_time right_pressure.left_back = self.pressure_sensors[4].getValues()[2] right_pressure.left_front = self.pressure_sensors[5].getValues()[2] right_pressure.right_front = self.pressure_sensors[6].getValues()[2] right_pressure.right_back = self.pressure_sensors[7].getValues()[2] # compute center of pressures of the feet pos_x = 0.085 pos_y = 0.045 # we can take a very small threshold, since simulation gives more accurate values than reality threshold = 1 cop_l = PointStamped() cop_l.header.frame_id = self.l_sole_frame cop_l.header.stamp = current_time sum = left_pressure.left_back + left_pressure.left_front + left_pressure.right_front + left_pressure.right_back if sum > threshold: cop_l.point.x = (left_pressure.left_front + left_pressure.right_front - left_pressure.left_back - left_pressure.right_back) * pos_x / sum cop_l.point.x = max(min(cop_l.point.x, pos_x), -pos_x) cop_l.point.y = (left_pressure.left_front + left_pressure.left_back - left_pressure.right_front - left_pressure.right_back) * pos_y / sum cop_l.point.y = max(min(cop_l.point.x, pos_y), -pos_y) else: cop_l.point.x = 0 cop_l.point.y = 0 cop_r = PointStamped() cop_r.header.frame_id = self.r_sole_frame cop_r.header.stamp = current_time sum = right_pressure.right_back + right_pressure.right_front + right_pressure.right_front + right_pressure.right_back if sum > threshold: cop_r.point.x = (right_pressure.left_front + right_pressure.right_front - right_pressure.left_back - right_pressure.right_back) * pos_x / sum cop_r.point.x = max(min(cop_r.point.x, pos_x), -pos_x) cop_r.point.y = (right_pressure.left_front + right_pressure.left_back - right_pressure.right_front - right_pressure.right_back) * pos_y / sum cop_r.point.y = max(min(cop_r.point.x, pos_y), -pos_y) else: cop_r.point.x = 0 cop_r.point.y = 0 return left_pressure, right_pressure, cop_l, cop_r def publish_pressure(self): left, right, cop_l, cop_r = self.get_pressure_message() self.pub_pres_left.publish(left) self.pub_pres_right.publish(right) self.cop_l_pub_.publish(cop_l) self.cop_r_pub_.publish(cop_r)
"""An open loop controller to make a robot follow a square motion path.""" from controller import Robot from math import pi # from the python "math" module import the "pi" variable # Create an instance of the robot: robot = Robot() # get the time step of the simulation (See WorldInfo > basicTimeStep # in the scene tree): timestep = int(robot.getBasicTimeStep()) # in milliseconds # Create an object for each of the robots wheels: leftWheel = robot.getMotor('left wheel motor') rightWheel = robot.getMotor('right wheel motor') # some key robot geometries and equations of motion calculations: wheel_diameter = 0.05 # meters robot_diameter = 0.09 # meters (the distance between the two wheels) # We can find out the maximum velocity of our robots wheels by # accessing the .getMaxVelocity() function from either of our wheels # (it is the same for both) max_velocity = leftWheel.getMaxVelocity() # radians per second # print this to the console so that we can see what it is: print('The Robot max wheel velocity is {} rad/s'.format(max_velocity)) wheel_circumference = pi * wheel_diameter # meters # calculate the time required to move 0.5 meters in a straight line: fwd_displacement = 0.5 / wheel_circumference # wheel revolutions # one wheel revolution is 2*pi radians, so: fwd_displacement = 2 * pi * fwd_displacement # in radians # time required to travel this far:
class Controller(): jointNames = [ # coxa, femur, tibia 'c1', 'f1', 't1', # LEG 1 'c2', 'f2', 't2', # LEG 2 'c3', 'f3', 't3', # LEG 3 'c4', 'f4', 't4', # LEG 4 'c5', 'f5', 't5', # LEG 5 'c6', 'f6', 't6' ] # LEG 6 touchNames = ['f1', 'f2', 'f3', 'f4', 'f5', 'f6'] def __init__(self): # Initialize the Webots Supervisor. self.robot = Robot() self.timeStep = int(4 * self.robot.getBasicTimeStep()) self.keyboard = self.robot.getKeyboard() # Define list for motors and position sensors self.motors = [] self.position_sensors = [] self.touch_sensors = [] # Initialise the motors and position sensors (could be moved into __init__ into a single for-loop) self.init_motors() self.init_positional_sensors() self.init_touching_sensors() def init_motors(self): for name in Controller.jointNames: motor = self.robot.getMotor(name + '_motor') # motor.setPosition(float('inf')) # motor.setVelocity(0) self.motors.append(motor) def init_positional_sensors(self): for name in Controller.jointNames: positional_sensor = self.robot.getPositionSensor( name + '_position_sensor') positional_sensor.enable(SENSOR_SAMPLE_PERIOD) self.position_sensors.append(positional_sensor) def init_touching_sensors(self): for name in Controller.jointNames[1::3]: touching_sensor = self.robot.getTouchSensor(name + '_touch_sensor') touching_sensor.enable(SENSOR_SAMPLE_PERIOD) self.touch_sensors.append(touching_sensor) # Test initialiser before I got the above version working. #def init_touching_sensors(self): # for name in Controller.touchNames: # touching_sensor = self.robot.getTouchSensor(name + '_touch_sensor') # touching_sensor.enable(SENSOR_SAMPLE_PERIOD) # self.touch_sensors.append(touching_sensor) def positionAll(self, positions): while self.robot.step(self.timeStep) != 1: for motor, position in zip(self.motors, positions): motor.setPosition(position) C.reachedAllPos(positions) break def positionN(self, id_list, positions): id_list = [x - 1 for x in id_list] while self.robot.step(self.timeStep) != 1: for i in range(len(id_list)): j = id_list[i] self.motors[j].setPosition(positions[i]) C.reachedNPos(id_list, positions) break def readPos(self): all_positions = [] valuePos = [] while self.robot.step(self.timeStep) != 1: for i in range(len(self.jointNames)): valuePos = self.position_sensors[i].getValue() all_positions.append(valuePos) #print(all_positions) # just for debugging return all_positions def readTouch(self): all_touches = [] valueTouch = [] while self.robot.step(self.timeStep) != 1: for i in range(len(self.jointNames[1::3])): valueTouch = self.touch_sensors[i].getValue() all_touches.append(valueTouch) #print(all_touches) # just for debugging return all_touches def reachedAllPos(self, positions): all_positions = [] #while self.robot.step(self.timeStep) != 1: while all_positions != positions: all_positions = [round(x, 2) for x in C.readPos()] positions = [round(y, 2) for y in positions] #print(all_positions) # just for debugging #print(positions) # just for debugging def reachedNPos(self, id_list, positions): #print('start id list:', id_list) #print('start pos list:', positions) temp_positions = [] all_positions = [0] * len(id_list) #while self.robot.step(self.timeStep) != 1: while all_positions != positions: temp_positions = [round(x, 2) for x in C.readPos()] positions = [round(y, 2) for y in positions] for i in range(len(id_list)): j = id_list[i] all_positions[i] = temp_positions[j] #all_positions = [0.0 if i == -0.0 else i for i in all_positions] # for fixing -0 value, but not needed #print('all_positions:', all_positions) # just for debugging #print(' positions:', positions) # just for debugging return def walk(self): standUp() # different types of gaits #tripod_gait_test_for_lars(0,10,10,1) #tripodGait(0, 20, 10, 10) #waveGait(0, 20, 10, 1) #rippleGait(0, 40, 10, 5) # ripple gait manual imported #singleLeg(0, 20, 10, 0, 0, 0, 1) #singleLeg(0, 20, 10, 0, 0, 0, 4) #singleLeg(0, -40/2, 0, 0, 0, 0, 2) #singleLeg(0, -40/2, 0, 0, 0, 0, 5) #singleLeg(0, -40/2, 0, 0, 0, 0, 3) #singleLeg(0, -40/2, 0, 0, 0, 0, 6) #print('0') #singleLeg(0, 0, -10, 0, 0, 0, 1) #print('1') #singleLeg(0, 0, 40, 0, 0, 0, 4) # some other translations #translationZ(-50) #parallelGait(0, 0, 3, 0, 0, 0) self.robot.step(1000) # reading and printing for debugging all_positions = C.readPos() K.printForward(all_positions) print(K.rad_to_step(all_positions))
WHEEL_BACKWARD = -1 state = "send_pose" # End with the more complex feedback control method! sub_state = "bearing" # TODO: It may be helpful to use sub_state to designate operation modes within the "turn_drive_turn_control" state # create the Robot instance. # csci3302_lab3_supervisor.init_supervisor() # robot = csci3302_lab3_supervisor.supervisor robot = Robot() EPUCK_MAX_WHEEL_SPEED = 0.12880519 # Unnecessarily precise ePuck speed in m/s. REMINDER: motor.setVelocity() takes ROTATIONS/SEC as param, not m/s. EPUCK_AXLE_DIAMETER = 0.053 # ePuck's wheels are 53mm apart. EPUCK_WHEEL_RADIUS = 0.0205 # ePuck's wheels are 0.041m in diameter. # get the time step of the current world. SIM_TIMESTEP = int(robot.getBasicTimeStep()) # Initialize Motors leftMotor = robot.getMotor('left wheel motor') rightMotor = robot.getMotor('right wheel motor') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) def update_odometry(left_wheel_direction, right_wheel_direction, time_elapsed): ''' Given the amount of time passed and the direction each wheel was rotating, update the robot's pose information accordingly '''
class Controller(): servos = [] psNames = [] ps = [] psValues = [] def __init__(self): self.robot = Robot() self.timeStep = int(4 * self.robot.getBasicTimeStep()) self.keyboard = self.robot.getKeyboard() self.init_servos() self.init_positional_sensors() def init_servos(self): # coxa, femur, tibia for servosName in ( 'c1_ser', 'f1_ser', 't1_ser', # LEG 1 'c2_ser', 'f2_ser', 't2_ser', # LEG 2 'c3_ser', 'f3_ser', 't3_ser', # LEG 3 'c4_ser', 'f4_ser', 't4_ser', # LEG 4 'c5_ser', 'f5_ser', 't5_ser', # LEG 5 'c6_ser', 'f6_ser', 't6_ser', ): # LEG 6 servos = self.robot.getMotor(servosName) #servos.setPosition(float('inf')) #servos.setVelocity(0) self.servos.append(servos) # def init_positional_sensors(self): # # coxa, femur, tibia # for positionsensorName in ('c1_pos', 'f1_pos', 't1_pos', # LEG 1 # 'c2_pos', 'f2_pos', 't2_pos', # LEG 2 # 'c3_pos', 'f3_pos', 't3_pos', # LEG 3 # 'c4_pos', 'f4_pos', 't4_pos', # LEG 4 # 'c5_pos', 'f5_pos', 't5_pos', # LEG 5 # 'c6_pos', 'f6_pos', 't6_pos',): # LEG 6 # positional_sensor = self.robot.getPositionSensor(positionsensorName) # positional_sensor.enable(POSITION_SENSOR_SAMPLE_PERIOD) # self.position_sensors.append(positional_sensor) def init_positional_sensors(self): psNames = [ 'c1_pos', 'f1_pos', 't1_pos', # LEG 1 'c2_pos', 'f2_pos', 't2_pos', # LEG 2 'c3_pos', 'f3_pos', 't3_pos', # LEG 3 'c4_pos', 'f4_pos', 't4_pos', # LEG 4 'c5_pos', 'f5_pos', 't5_pos', # LEG 5 'c6_pos', 'f6_pos', 't6_pos', ] # LEG 6 for i in range(18): self.ps.append(self.robot.getPositionSensor(psNames[i])) self.ps[i].enable(POSITION_SENSOR_SAMPLE_PERIOD) def run(self): positions = [ 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2 ] while self.robot.step(self.timeStep) != 1: for servos, position in zip(self.servos, positions): servos.setPosition(position) break def read(self): for i in range(18): self.psValues.append(self.ps[i].getValue()) return self.psValues