def main(): parser = argparse.ArgumentParser() parser.add_argument('--duration', type=float, default=10, help='Duration of the animation in seconds') parser.add_argument('--output', default='../../animation/index.html', help='Path at which the animation will be saved') args = parser.parse_args() robot = Supervisor() timestep = int(robot.getBasicTimeStep()) receiver = robot.getDevice('receiver') receiver.enable(timestep) robot.step(timestep) robot.animationStartRecording(args.output) step_i = 0 done = False n_steps = (1000 * args.duration) / robot.getBasicTimeStep() while not done and robot.step(timestep) != -1 and step_i < n_steps: step_i += 1 if receiver.getQueueLength() > 0: if receiver.getData().decode('utf-8') == 'done': done = True receiver.nextPacket() robot.animationStopRecording() for _ in range(10): robot.step(timestep) print('The animation is saved') robot.simulationQuit(0)
def main(): """Main""" # Duration of each simulation simulation_duration = 20 # Get supervisor to take over the world world = Supervisor() n_joints = 10 timestep = int(world.getBasicTimeStep()) freqs = 1 # Get and control initial state of salamander reset = RobotResetControl(world, n_joints) # Simulation example amplitude = None phase_lag = None turn = None parameter_set = [[freqs, amplitude, phase_lag, turn], [freqs, amplitude, phase_lag, turn]] for simulation_i, parameters in enumerate(parameter_set): reset.reset() run_simulation( world, parameters, timestep, int(1000 * simulation_duration / timestep), logs="./logs/example/simulation_{}.npz".format(simulation_i)) # Pause world.simulationSetMode(world.SIMULATION_MODE_PAUSE) pylog.info("Simulations complete") world.simulationQuit(0)
def move_walls_after(seconds: int) -> None: robot = Supervisor() timestep = robot.getBasicTimeStep() walls = [ webots_utils.node_from_def(robot, 'west_moving_wall'), webots_utils.node_from_def(robot, 'west_moving_triangle'), webots_utils.node_from_def(robot, 'east_moving_wall'), webots_utils.node_from_def(robot, 'east_moving_triangle'), ] if controller_utils.get_robot_mode() == 'comp': # Interact with the supervisor "robot" to wait for the start of the match. while robot.getCustomData() != 'start': robot.step(int(timestep)) # wait for the walls to start moving in ms robot.step(seconds * 1000) print('Moving arena walls') # noqa: T001 for wall in walls: wall.setVelocity(LINEAR_DOWNWARDS) # wait for the walls to reach their final location robot.step(1000) for wall in walls: wall.resetPhysics()
class Robot: def __init__(self): self.supervisor = Supervisor() self.timeStep = int(4 * self.supervisor.getBasicTimeStep()) # Create the arm chain from the URDF with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: filename = file.name file.write(self.supervisor.getUrdf().encode('utf-8')) armChain = Chain.from_urdf_file(filename) armChain.active_links_mask[0] = False # Initialize the arm motors and encoders. self.motors = [] for link in armChain.links: if 'motor' in link.name: motor = self.supervisor.getDevice(link.name) motor.setVelocity(1.0) position_sensor = motor.getPositionSensor() position_sensor.enable(self.timeStep) self.motors.append(motor) self.arm = self.supervisor.getSelf() self.positions = [0 for _ in self.motors] def update(self): for motor, position in zip(self.motors, self.positions): motor.setPosition(position * 0.01745277777) def simulate(self): self.supervisor.step(self.timeStep)
class WebotsNode(Node): def __init__(self, name, args=None): super().__init__(name) self.declare_parameter( 'synchronization', Parameter('synchronization', Parameter.Type.BOOL, False)) parser = argparse.ArgumentParser() parser.add_argument( '--webots-robot-name', dest='webotsRobotName', default='', help='Specifies the "name" field of the robot in Webots.') # use 'parse_known_args' because ROS2 adds a lot of internal arguments arguments, unknown = parser.parse_known_args() if arguments.webotsRobotName: os.environ['WEBOTS_ROBOT_NAME'] = arguments.webotsRobotName self.robot = Supervisor() self.timestep = int(self.robot.getBasicTimeStep()) self.clockPublisher = self.create_publisher(Clock, 'clock', 10) timer_period = 0.001 * self.timestep # seconds self.stepService = self.create_service(SetInt, 'step', self.step_callback) self.timer = self.create_timer(timer_period, self.timer_callback) self.sec = 0 self.nanosec = 0 def step(self, ms): if self.robot is None or self.get_parameter('synchronization').value: return # Robot step if self.robot.step(ms) < 0.0: del self.robot self.robot = None sys.exit(0) # Update time time = self.robot.getTime() self.sec = int(time) # rounding prevents precision issues that can cause problems with ROS timers self.nanosec = int(round(1000 * (time - self.sec)) * 1.0e+6) # Publish clock msg = Clock() msg.clock.sec = self.sec msg.clock.nanosec = self.nanosec self.clockPublisher.publish(msg) def timer_callback(self): self.step(self.timestep) def step_callback(self, request, response): self.step(request.value) response.success = True return response def now(self): sim_time = self.robot.getTime() seconds = int(sim_time) nanoseconds = int((sim_time - seconds) * 1.0e+6) return Time(sec=seconds, nanosec=nanoseconds)
def wait_until_robots_ready(supervisor: Supervisor) -> None: time_step = int(supervisor.getBasicTimeStep()) for zone_id, robot in get_robots(supervisor, skip_missing=True): # Robot.wait_start sets this to 'ready', then waits to see 'start' field = robot.getField('customData') if field.getSFString() != 'ready': print("Waiting for {}".format(zone_id)) while field.getSFString() != 'ready': supervisor.step(time_step) print("Zone {} ready".format(zone_id))
def main(): """Main""" # Get supervisor to take over the world world = Supervisor() n_joints = 10 timestep = int(world.getBasicTimeStep()) # Get and control initial state of salamander reset = RobotResetControl(world, n_joints) # Simulation arguments arguments = world.getControllerArguments() pylog.info("Arguments passed to smulation: {}".format(arguments)) # Exercise example to show how to run a grid search if "example" in arguments: exercise_example(world, timestep, reset) # Exercise 9b - Phase lag + amplitude study if "9b" in arguments: pylog.info("Running 9b.....") #exercise_example(world, timestep, reset) exercise_9b(world, timestep, reset) # Exercise 9c - Gradient amplitude study if "9c" in arguments: exercise_9c(world, timestep, reset) # Exercise 9d1 - Turning if "9d1" in arguments: exercise_9d1(world, timestep, reset) # Exercise 9d2 - Backwards swimming if "9d2" in arguments: exercise_9d2(world, timestep, reset) # Exercise 9f - Walking if "9f" in arguments: exercise_9f(world, timestep, reset) # Exercise 9g - Transitions if "9g" in arguments: exercise_9g(world, timestep, reset) # Pause world.simulationSetMode(world.SIMULATION_MODE_PAUSE) pylog.info("Simulations complete") world.simulationQuit(0)
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_REAL_TIME) time_step = int(supervisor.getBasicTimeStep()) duration_ms = time_step * int(1000 * GAME_DURATION_SECONDS // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
class SupervisorEmitterReceiver(SupervisorEnv): def __init__(self, emitter_name="emitter", receiver_name="receiver", time_step=None): super(SupervisorEmitterReceiver, self).__init__() self.supervisor = Supervisor() if time_step is None: self.timestep = int(self.supervisor.getBasicTimeStep()) else: self.timestep = time_step self.initialize_comms(emitter_name, receiver_name) def initialize_comms(self, emitter_name, receiver_name): self.emitter = self.supervisor.getEmitter(emitter_name) self.receiver = self.supervisor.getReceiver(receiver_name) self.receiver.enable(self.timestep) return self.emitter, self.receiver def step(self, action): self.supervisor.step(self.timestep) self.handle_emitter(action) return ( self.get_observations(), self.get_reward(action), self.is_done(), self.get_info(), ) @abstractmethod def handle_emitter(self, action): pass @abstractmethod def handle_receiver(self): pass def get_timestep(self): return self.timestep
def wait_until_robots_ready(supervisor: Supervisor) -> None: time_step = int(supervisor.getBasicTimeStep()) for zone_id, robot in get_robots(supervisor, skip_missing=True): # Robot.wait_start sets this to 'ready', then waits to see 'start' field = robot.getField('customData') if field.getSFString() != 'ready': print("Waiting for {}".format(zone_id)) end_time = supervisor.getTime() + 5 while field.getSFString() != 'ready': # 5 second initialisation timeout if supervisor.getTime() > end_time: raise RuntimeError( f"Robot in zone {zone_id} failed to initialise. " "Check whether the robot code is correctly reaching and " "calling `wait_start`.", ) supervisor.step(time_step) print("Zone {} ready".format(zone_id))
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") # First signal the robot controllers that they're able to start ... for _, robot in get_robots(supervisor): robot.getField('customData').setSFString('start') # ... then un-pause the simulation, so they all start together supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_REAL_TIME) time_step = int(supervisor.getBasicTimeStep()) duration_ms = time_step * int(1000 * GAME_DURATION_SECONDS // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") # First signal the robot controllers that they're able to start ... for _, robot in get_robots(supervisor, skip_missing=True): robot.getField('customData').setSFString('start') # ... then un-pause the simulation, so they all start together supervisor.simulationSetMode(get_simulation_run_mode(supervisor)) time_step = int(supervisor.getBasicTimeStep()) duration = controller_utils.get_match_duration_seconds() duration_ms = time_step * int(1000 * duration // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
def run_match(supervisor: Supervisor) -> None: print("===========") print("Match start") print("===========") # First signal the robot controllers that they're able to start ... for _, robot in get_robots(supervisor, skip_missing=True): inform_start(robot) inform_start(webots_utils.node_from_def(supervisor, 'WALL_CTRL')) # ... then un-pause the simulation, so they all start together supervisor.simulationSetMode(get_simulation_run_mode(supervisor)) time_step = int(supervisor.getBasicTimeStep()) duration = controller_utils.get_match_duration_seconds() duration_ms = time_step * int(1000 * duration // time_step) supervisor.step(duration_ms) print("==================") print("Game over, pausing") print("==================") supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
class Agent(object): """docstring for Agent.""" def __init__(self, mdNames, objName): super().__init__() self.robot = Supervisor() self.objName = objName self.energy = 10000 self.timestep = int(self.robot.getBasicTimeStep()) self.camera = self.robot.getDevice('camera') self.camera.enable(self.timestep) self.camera.recognitionEnable(self.timestep) self.MAX_SPEED = 10 self.LOW_SPEED = -10 self.MAX_ENERGY = 10000 self.consumptionEnergy = 2000 self.ds = [] self.md = [] self.dsNames = ['ds_left', 'ds_right', 'ds_left(1)', 'ds_right(1)'] for i in range(4): self.ds.append(self.robot.getDevice(self.dsNames[i])) self.ds[i].enable(self.timestep) self.length = len(self.mdNames) for i in range(self.length): self.md.append(self.robot.getDevice(self.mdNames[i])) def multiMoveMotorPos(self, devices, dPos): for device in devices: device.setPosition(dPos) def setMultiMotorVel(self, devices, vel): for device in devices: device.setVelocity(vel*self.MAX_SPEED) self.velocity = vel def getEnergy(self): return self.energy def setEnergy(self, energy): self.energy = energy def eat(self, prey): prey = prey pField = prey.getField('translation') randX = random.uniform(-2.45, 2.45) randZ = random.uniform(-2.45, 2.45) newPos = [randX,0.05,randZ] pField.setSFVec3f(newPos) self.energy = self.energy + self.consumptionEnergy if self.energy > self.MAX_ENERGY: self.energy = self.MAX_ENERGY def checkEnergyCollision(self, preyName): if preyName: objPos = self.getPosition(self.objName) objPos = np.array(objPos) objPos = np.array(objPos) for i in preyName: self.prey = self.robot.getFromDef(i) preyPos = self.prey.getPosition() preyPos = self.prey.getPosition() preyPos = np.array(preyPos) dist = np.linalg.norm(objPos - preyPos) if dist < 0.4: return self.prey else: return False def getPosition(self, name): thing = self.robot.getFromDef(name) pos = thing.getPosition() return pos def checkObstacle(self): dsValues = [] for i in range(4): dsValues.append(self.ds[i].getValue()) left_obstacle = dsValues[0] < 1000.0 or dsValues[2] < 1000.0 right_obstacle = dsValues[1] < 1000.0 or dsValues[3] < 1000.0 if left_obstacle: return 1 elif right_obstacle: return 2 else: return False def checkRecogniseSource(self): currClosest = [1000,100, 1000] minDist = float('inf') recognisedObj = self.camera.getRecognitionObjects() for obj in recognisedObj: currObj = obj.get_position() distance = math.sqrt(currObj[0]*currObj[0]+currObj[2]*currObj[2]) if distance < minDist: minDist = distance currClosest = currObj if recognisedObj: x = currClosest[0] angle = x *minDist return angle else: return False return False def getMotorDevices(self): return self.md def avoidObstacle(self, value): if value == 1: self.turnRight() elif value == 2: self.turnLeft() def setMaxEnergy(self, energy): self.MAX_ENERGY = energy def setConsumptionEnergy(self, energy): self.consumptionEnergy = energy
class LightingController: def __init__(self, duration: float, cue_stack: List[LightingEffect]) -> None: self._robot = Supervisor() self.timestep = self._robot.getBasicTimeStep() self.start_offset: float = 0 self.duration = duration self.cue_stack = cue_stack self.ambient_node = webots_utils.node_from_def(self._robot, 'AMBIENT') self.luminosity_fade = LuminosityFade(0, 0, 0.35, 0.35) self.lighting_fades: List[LightFade] = [] # fetch all nodes used in effects, any missing nodes will be flagged here self.light_nodes: Dict[str, Node] = {} for cue in cue_stack: for light in cue.lighting: if self.light_nodes.get(light.light_def) is None: self.light_nodes[light.light_def] = webots_utils.node_from_def( self._robot, light.light_def, ) def set_node_luminosity(self, node: Node, luminosity: float) -> None: node.getField('luminosity').setSFFloat(luminosity) def set_node_intensity(self, node: Node, intensity: float) -> None: node.getField('intensity').setSFFloat(intensity) def set_node_colour(self, node: Node, colour: Tuple[float, float, float]) -> None: node.getField('color').setSFColor(list(colour)) def get_current_luminosity(self) -> float: return self.ambient_node.getField('luminosity').getSFFloat() def get_current_lighting_values(self, light_def: str) -> ArenaLighting: light = self.light_nodes[light_def] return ArenaLighting( light_def, light.getField('intensity').getSFFloat(), light.getField('color').getSFColor(), # type: ignore ) def increment_colour( self, colour: Tuple[float, float, float], step: Tuple[float, float, float], ) -> Tuple[float, float, float]: return tuple(colour[i] + step[i] for i in range(3)) # type: ignore def current_match_time(self) -> float: return self._robot.getTime() - self.start_offset def remaining_match_time(self) -> float: return self.duration - self.current_match_time() def start_lighting_effect(self, effect: LightingEffect) -> None: print( # noqa: T001 f"Running lighting effect: {effect.name} @ {self.current_match_time()}", ) if effect.fade_time is None: self.set_node_luminosity(self.ambient_node, effect.luminosity) for light in effect.lighting: self.set_node_intensity(self.light_nodes[light.light_def], light.intensity) self.set_node_colour(self.light_nodes[light.light_def], light.colour) print(f"Lighting effect '{effect.name}' complete") # noqa: T001 else: steps = int((effect.fade_time * 1000) / self.timestep) # get starting values current_luminosity = self.get_current_luminosity() luminosity_step = (effect.luminosity - current_luminosity) / steps self.luminosity_fade = LuminosityFade( steps, luminosity_step, current_luminosity, effect.luminosity, ) for light in effect.lighting: # get starting values ( _, current_intensity, current_colour, ) = self.get_current_lighting_values(light.light_def) # figure out steps of each value intensity_step = (light.intensity - current_intensity) / steps colour_step: Tuple[float, float, float] = tuple( # type: ignore light.colour[i] - current_colour[i] for i in range(3) ) # add fade to queue to have steps processed self.lighting_fades.append(LightFade( self.light_nodes[light.light_def], steps, intensity_step, colour_step, current_intensity, current_colour, light, )) def do_lighting_step(self) -> None: if self.luminosity_fade.remaining_steps != 0: self.luminosity_fade.current_luminosity += self.luminosity_fade.luminosity_step self.luminosity_fade.remaining_steps -= 1 if self.luminosity_fade.remaining_steps == 0: # final step self.luminosity_fade.current_luminosity = self.luminosity_fade.final_luminosity self.set_node_luminosity( self.ambient_node, self.luminosity_fade.current_luminosity, ) for fade in self.lighting_fades: if fade.remaining_steps > 1: fade.current_intensity += fade.intensity_step fade.current_colour = self.increment_colour( fade.current_colour, fade.colour_step, ) fade.remaining_steps -= 1 self.set_node_intensity(fade.light, fade.current_intensity) self.set_node_colour(fade.light, fade.current_colour) else: self.set_node_intensity(fade.light, fade.effect.intensity) self.set_node_colour(fade.light, fade.effect.colour) print(f"Lighting effect for '{fade.effect.light_def}' complete") # noqa: T001 self.lighting_fades.remove(fade) # remove completed fade def schedule_lighting(self) -> None: if controller_utils.get_robot_mode() != 'comp': return print('Scheduled cues:') # noqa: T001 for cue in self.cue_stack: print(cue) # noqa: T001 # run pre-start snap changes for cue in self.cue_stack.copy(): if cue.start_time == 0 and cue.fade_time is None: self.start_lighting_effect(cue) self.cue_stack.remove(cue) # Interact with the supervisor "robot" to wait for the start of the match. while self._robot.getCustomData() != 'start': self._robot.step(int(self.timestep)) self.start_offset = self._robot.getTime() while self.cue_stack: for cue in self.cue_stack.copy(): if ( cue.start_time >= 0 and self.current_match_time() >= cue.start_time ): # cue relative to start self.start_lighting_effect(cue) self.cue_stack.remove(cue) elif ( cue.start_time < 0 and self.remaining_match_time() <= -(cue.start_time) ): # cue relative to end self.start_lighting_effect(cue) self.cue_stack.remove(cue) self.do_lighting_step() self._robot.step(int(self.timestep))
class Mitsos(): # Webots-to-environment-agnostic def __init__(self,max_steps=200,ACTIONS='DISCRETE'): self.name = "Mitsos" self.max_steps = max_steps self.robot = Supervisor() # create the Robot instance self.timestep = int(self.robot.getBasicTimeStep()) # get the time step of the current world. # crash sensor self.bumper = self.robot.getDeviceByIndex(36) #### <--------- self.bumper.enable(self.timestep) # camera sensor self.camera = self.robot.getDevice("camera") self.camera.enable(self.timestep) # ir sensors IR_names = ["ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"] self.InfraredSensors = [self.robot.getDevice(s) for s in IR_names] for ir in self.InfraredSensors: ir.enable(self.timestep) # wheels motors motors = ["left wheel motor","right wheel motor"] self.wheels = [] for i in range(len(motors)): self.wheels.append(self.robot.getDevice(motors[i])) self.wheels[i].setPosition(float('inf')) self.wheels[i].setVelocity(0) self.robot.step(self.timestep) self.cam_shape = (self.camera.getWidth(),self.camera.getHeight()) self.sensors_shape = (14,) self.stepCounter = 0 self.shaping = None # Actions self.ACTIONS = ACTIONS self.RELATIVE_ROTATIONS = True self.FIXED_ORIENTATIONS = False # State self.POSITION = True self.IRSENSORS = True self.CAMERA = False if self.FIXED_ORIENTATIONS: self.discrete_actions = [0,1,2,3] if self.RELATIVE_ROTATIONS: self.discrete_actions = [0,1,2] if self.ACTIONS=='DISCRETE': self.action_size = len(self.discrete_actions) else: self.action_size = 2 self.path = [] self.substeps = 10 self.n_obstacles = 25 self.d = 0.3 self.create_world() self.map = DynamicMap(self.START[0],self.START[1],0.02) def reset(self,reset_position=True,reset_world=True): self.stepCounter = 0 if reset_world: self.create_world() self.map.reset(self.START[0],self.START[1],0.02) self.path = [(self.START[0],self.START[1])] else: x,y,z = self.get_robot_position() if D((x,y,z),self.GOAL) < 0.05: rho = d phi = random.random()*2*np.pi xg,yg = pol2cart(rho,phi) self.GOAL = xg,yg if reset_position: self.set_position(self.START[0],self.START[1],0.005) self.set_orientation(np.random.choice([0,np.pi/2,np.pi,-np.pi/2])) self.shaping = None if self.ACTIONS=='DISCRETE': state,_,_,_ = self.step(1) else: state,_,_,_ = self.step([1,0]) return state def step(self,action): [xg,yg,_] = self.GOAL x,y,z = self.get_robot_position() self.path.append((x,y)) self.map.visit(x,y) if self.ACTIONS=='DISCRETE': if self.FIXED_ORIENTATIONS: # Take action if action == 0: a = 0 if action == 1: a = 90 if action == 2: a = 180 if action == 3: a = -90 self.turn0(a) self.set_wheels_speed(1,0) elif self.RELATIVE_ROTATIONS: if action == 0: a = -45 if action == 1: a = 0 if action == 2: a = 45 self.turn(a) self.set_wheels_speed(1,0) elif self.ACTIONS=='CONTINUOUS': u,w = action self.set_wheels_speed(u,w) camera_stack = np.zeros(shape=self.cam_shape+(4,)) sensor_data = [] position_data = [] sensor_data = list(self.read_ir()) for i in range(self.substeps): self.robot.step(self.timestep) x1,y1,z1 = self.get_robot_position() collision = self.collision() position_data = [x-xg,y-yg,x1-xg,y1-yg] sensor_data += list(self.read_ir()) # rho0,phi0 = cart2pol(x-xg,y-yg) # rho1,phi1 = cart2pol(x1-xg,y1-yg) # state = [rho0,phi0,rho1,phi1] state = [] if self.POSITION: state += position_data if self.IRSENSORS: state += sensor_data if self.CAMERA: state = [camera_stack,state] # REWARD reward,done,self.shaping = reward_function(position_data,self.shaping,collision) if reward == 100: print('goal') if self.stepCounter >= self.max_steps: done = True if done: vars = [self.path,self.GOAL,self.obstacles] vars = np.array(vars,dtype=object) f = open(episode_filename,'wb') np.save(f,vars) f.close() self.stepCounter += 1 info = '' return state,reward,done,info def create_world(self): mode = 1 if mode == 0: self.GOAL = [0,0,0] self.START = [0.2,0.2,0] if mode == 1: self.GOAL = [0,0,0] a = random.random()*np.pi*2 x,y = pol2cart(self.d,a) self.START = [x+self.GOAL[0],y+self.GOAL[1],0] while(True): try: obs = self.robot.getFromDef('OBS') obs.remove() except: break self.set_obstacle_positions() p = self.obstacles for pos in p: nodeString = self.get_object_proto(pos=pos) root = self.robot.getRoot() node = root.getField('children') node.importMFNodeFromString(-1,nodeString) def set_obstacle_positions(self): n = self.n_obstacles self.obstacles = [] while len(self.obstacles) < n: # r = (random.random() + 0.25) / 1.25 # 0.2 < r < 0.8 # d = D(self.GOAL,self.START) * r d = random.uniform(0.15,1) a = random.random()*np.pi*2 x,y = pol2cart(d,a) self.obstacles.append([x+self.GOAL[0],y+self.GOAL[1],0]) def render(self): return def read_ir(self): ir_sensors = np.array([ i.getValue() for i in self.InfraredSensors]) max_ = 2500.0 for i in range(len(ir_sensors)): if ir_sensors[i] < 0: ir_sensors[i] = 0.0 elif ir_sensors[i] > max_: ir_sensors[i] = 1.0 else: ir_sensors[i] = ir_sensors[i] / max_ return ir_sensors def read_camera(self): image = np.uint8(self.camera.getImageArray()) gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) grayN = gray / 255.0 return gray def collision(self): return bool(self.bumper.getValue()) def set_position(self,x,y,z): #return object = self.robot.getFromDef(self.name) positionField = object.getField("translation") Field.setSFVec3f(positionField,[y,z,x]) for _ in range(5): self.robot.step(self.timestep) # if not nan values in first iteration def set_orientation(self,a): a += np.pi object = self.robot.getFromDef(self.name) rotationField = object.getField("rotation") #object.getPosition() Field.setSFRotation(rotationField,[0,1,0,a]) self.robot.step(self.timestep) # if not nan values in first iteration def set_wheels_speed(self,u,w): # u: velocity # w: angular velocity u1 = u + w u2 = u - w self.wheels[0].setVelocity(float(u1)) self.wheels[1].setVelocity(float(u2)) def turn(self,a): phi = np.rad2deg(self.get_robot_rotation()[-1]) phi1 = phi + a w=-2 w = int(w * np.sign(a)) self.set_wheels_speed(0,w) while(np.abs(phi - phi1)%360 > 5): self.robot.step(self.timestep) phi = np.rad2deg(self.get_robot_rotation()[-1]) def turn0(self,a): phi = np.rad2deg(self.get_robot_rotation()[-1]) w = 5 if phi - a > 180: w = -w self.set_wheels_speed(0,w) while( np.abs(phi - a) >= 3 ): self.robot.step(self.timestep) phi = np.rad2deg(self.get_robot_rotation()[-1]) def get_robot_position(self): object = self.robot.getFromDef(self.name) y,z,x = object.getPosition() return [x,y,z] def get_robot_rotation(self): object = self.robot.getFromDef(self.name) rotationField = object.getField("rotation") a=rotationField.getSFRotation() return a def rotation_to_goal(self,G,X1,X2): (xg,yg),(x1,y1),(x2,y2) = G,X1,X2 if xg == x1: theta1 = np.pi/2 + (yg<y1)*np.pi else: lambda1 = (yg-y1)/(xg-x1) if xg > x1: theta1 = np.arctan(lambda1) else: theta1 = np.arctan(lambda1) + np.pi if x2 == x1: theta2 = np.pi/2 + (y2<y1)*np.pi else: lambda2 = (y2-y1)/(x2-x1) if x2 > x1: theta2 = np.arctan(lambda2) else: theta2 = np.arctan(lambda2) + np.pi theta = theta1 - theta2 return theta def wait(self,timesteps): for _ in range(timesteps): self.robot.step(self.timestep) return def random_position(self): x = (random.random()*2 - 1) * 0.95 y = (random.random()*2 - 1) * 0.95 z = 0 return [x,y,z] def get_object_proto(self,object='',pos=[0,0,0]): translation = str(pos[1])+' '+str(pos[2]+0.025)+' '+str(pos[0]) return "DEF OBS SolidBox { translation "+translation+" size 0.05 0.05 0.05}" # # needs change # objects = {'Chair':'0.2 ', # 'Ball':'1.6 ', # 'ComputerMouse':'1.4 ', # 'OilBarrel':'0.17 ', # 'Toilet':'0.13 ', # 'SolidBox':''} # if object=='': # object = np.random.choice(list(objects.keys())) # if object=='SolidBox': # translation = str(pos[1])+' '+str(pos[2]+0.05)+' '+str(pos[0]) # return "DEF OBS SolidBox { translation "+translation+" size 0.05 0.05 0.05}" # translation = str(pos[1])+' '+str(pos[2])+' '+str(pos[0]) # scale = objects[object]*3 # proto = "DEF OBS Solid { translation "+translation+" scale "+scale+" children [ "+object+" { } ]}" # return proto def store_path(self): filename = 'paths' keep_variables = [self.path,self.START,self.GOAL] keep_variables = np.array(keep_variables,dtype=object) f = open(filename,'a') np.save(f,keep_variables) f.close()
def getPointsList(reader, name): """Get a group of points and extract it's labels as a list of strings.""" if name not in reader.groups['POINT'].params: return None list = reader.groups['POINT'].get_string(name) elementSize = reader.groups['POINT'].get(name).dimensions[0] newlist = [list[i:i + elementSize] for i in range(0, len(list), elementSize)] for i in range(len(newlist)): newlist[i] = newlist[i].strip() return newlist supervisor = Supervisor() timestep = int(supervisor.getBasicTimeStep()) enableValueGraphs = [] # parse arguments if len(sys.argv) < 3 or sys.argv[1] == 'None': sys.exit('C3D file not defined.') if not os.path.isfile(sys.argv[1]): sys.exit('\'%s\' does not exist.' % sys.argv[1]) playbackSpeed = float(sys.argv[2]) # parse C3D file reader = c3d.Reader(open(sys.argv[1], 'rb')) # get C3D files settings
class TerritoryController: _emitters: Dict[StationCode, Emitter] _receivers: Dict[StationCode, Receiver] def __init__(self, claim_log: ClaimLog) -> None: self._claim_log = claim_log self._robot = Supervisor() self._claim_starts: Dict[Tuple[StationCode, Claimant], float] = {} self._emitters = { station_code: get_robot_device(self._robot, station_code + "Emitter", Emitter) for station_code in StationCode } self._receivers = { station_code: get_robot_device(self._robot, station_code + "Receiver", Receiver) for station_code in StationCode } for receiver in self._receivers.values(): receiver.enable(RECEIVE_TICKS) def begin_claim( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: self._claim_starts[station_code, claimed_by] = claim_time def has_begun_claim_in_time_window( self, station_code: StationCode, claimant: Claimant, current_time: float, ) -> bool: try: start_time = self._claim_starts[station_code, claimant] except KeyError: return False time_delta = current_time - start_time return 1.8 <= time_delta <= 2.1 def claim_territory( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: if self._claim_log.get_claimant(station_code) == claimed_by: # This territory is already claimed by this claimant. return new_colour = ZONE_COLOURS[claimed_by] self._robot.getFromDef(station_code).getField("zoneColour").setSFColor( list(new_colour), ) self._claim_log.log_territory_claim(station_code, claimed_by, self._robot.getTime()) def process_packet( self, station_code: StationCode, packet: bytes, receive_time: float, ) -> None: try: robot_id, is_conclude = struct.unpack( "!BB", packet) # type: Tuple[int, int] claimant = Claimant(robot_id) if is_conclude: if self.has_begun_claim_in_time_window( station_code, claimant, receive_time, ): self.claim_territory( station_code, claimant, receive_time, ) else: self.begin_claim( station_code, claimant, receive_time, ) except ValueError: print( # noqa:T001 f"Received malformed packet at {receive_time} on {station_code}: {packet!r}", ) def receive_territory(self, station_code: StationCode, receiver: Receiver) -> None: simulation_time = self._robot.getTime() while receiver.getQueueLength(): try: data = receiver.getData() self.process_packet(station_code, data, simulation_time) finally: # Always advance to the next packet in queue: if there has been an exception, # it is safer to advance to the next. receiver.nextPacket() def receive_robot_captures(self) -> None: for station_code, receiver in self._receivers.items(): self.receive_territory(station_code, receiver) self._claim_log.record_captures() def transmit_pulses(self) -> None: for station_code, emitter in self._emitters.items(): emitter.send( struct.pack("!2sb", station_code.encode('ASCII'), int(self._claim_log.get_claimant(station_code)))) def main(self) -> None: timestep = self._robot.getBasicTimeStep() steps_per_broadcast = (1 / BROADCASTS_PER_SECOND) / (timestep / 1000) counter = 0 while True: counter += 1 self.receive_robot_captures() if counter > steps_per_broadcast: self.transmit_pulses() counter = 0 self._robot.step(int(timestep))
) import math from controller import Supervisor if ikpy.__version__[0] < '3': sys.exit( 'The "ikpy" Python module version is too old. ' 'Please upgrade "ikpy" Python module to version "3.0" or newer with this command: "pip install --upgrade ikpy"' ) IKPY_MAX_ITERATIONS = 4 # Initialize the Webots Supervisor. supervisor = Supervisor() timeStep = int(4 * supervisor.getBasicTimeStep()) # Create the arm chain from the URDF filename = None with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: filename = file.name file.write(supervisor.getUrdf().encode('utf-8')) armChain = Chain.from_urdf_file(filename) for i in [0, 6]: armChain.active_links_mask[0] = False # Initialize the arm motors and encoders. motors = [] for link in armChain.links: if 'motor' in link.name: motor = supervisor.getDevice(link.name)
class WebotsSupervisor(object): """Control the Webots simulation""" def __init__(self): self.number_of_blocks = 5 # define the controller name we want to connect to os.environ['WEBOTS_ROBOT_NAME'] = 'supervisor' # get the controller self.supervisor = Supervisor() # get timestep of the simulation self.timestep = int(self.supervisor.getBasicTimeStep()) # position and rotation of the cobot in the simulation self.ur3e_position = [0.69, 0.74, 0] self.ur3e_rotation = Rot.from_rotvec(-(np.pi / 2) * np.array([1.0, 0.0, 0.0])) # get all blocks of the simulation self.blocks = [] for i in range(self.number_of_blocks): self.blocks.append(self.supervisor.getFromDef( "block{0}".format(i))) # get the position of the end-effector self.endEffector = self.supervisor.getFromDef("gps") # initialize the variable to grab objects self.grabbed = [] for i in range(self.number_of_blocks): self.grabbed.append(False) self.is_grabbed = False # get the positions field for all blocks self.block_positions_field = [] self.block_rotations_field = [] for i in range(self.number_of_blocks): # get the fields self.block_positions_field.append( self.blocks[i].getField("translation")) self.block_rotations_field.append( self.blocks[i].getField("rotation")) # get the name of the blocks self.block_names = [] for i in range(self.number_of_blocks): self.block_names.append( self.blocks[i].getField("name").getSFString()) self.is_recording = False self.table_color_field = self.supervisor.getFromDef("table").getField( 'trayAppearance').getSFNode().getField('baseColor') rospy.Subscriber('touchsensor_status', Int8, self.touch_callback, queue_size=1) self.touchsensors = 0 def touch_callback(self, data): self.touchsensors = data.data def grab_service(self, data): """ Service call to grab and release objects in the simulation """ c = data.obj if c == 'c': self.clean_table() return True, self.number_of_blocks for i in range(self.number_of_blocks): if c == str(i): if self.is_grabbed: print('Please, release the other object') return False, self.number_of_blocks else: self.grabbed[i] = True self.is_grabbed = True return True, self.number_of_blocks if c == 'r': for i in range(self.number_of_blocks): self.grabbed[i] = False self.is_grabbed = False self.blocks[i].resetPhysics() return True, self.number_of_blocks if c == 'n': return True, self.number_of_blocks if c == 'reset': # self.supervisor.simulationReset() # self.supervisor.simulationRevert() mouse = Controller() # print(mouse.position) mouse.position = (971, 96) mouse.click(Button.left, 1) return True, self.number_of_blocks if c == 'clean': self.clean_table() return True, self.number_of_blocks if c == 'prepare': self.prepare_table(self.number_of_blocks - 1) return True, self.number_of_blocks if c == 'prepare1': self.prepare_table(1) return True, self.number_of_blocks if c == 'prepare2': self.prepare_table(2) return True, self.number_of_blocks if c == 'prepare3': self.prepare_table(3) return True, self.number_of_blocks if c == 'prepare4': self.prepare_table(4) return True, self.number_of_blocks if c == 'start_video': timestr = time.strftime("%Y%m%d-%H%M%S") file = '/home/ubuntu/Videos/' + timestr + ' Simulation.mp4' self.supervisor.movieStartRecording(file, 854, 480, quality=97) self.is_recording = True return True if c == 'stop_video': if self.is_recording: self.supervisor.movieStopRecording() self.is_recording = False return not self.supervisor.movieFailed() return False, self.number_of_blocks def position_service(self, data): """ Service call to the position of objects in the simulation """ c = int(data.obj) block_msgs = BlockPose() # get the name of the block block_msgs.name = self.block_names[c] # get the position and orientation of the block block_msgs.position = np.array( self.block_positions_field[c].getSFVec3f()) block_msgs.rotation = self.block_rotations_field[c].getSFRotation() # change the reference of the block # rotation rot_vec = block_msgs.rotation[3] * np.array([ block_msgs.rotation[0], block_msgs.rotation[1], block_msgs.rotation[2] ]) block_msgs.rotation = Rot.from_rotvec(rot_vec) * self.ur3e_rotation # position block_msgs.position = block_msgs.position - np.array( self.ur3e_position) block_msgs.position = self.ur3e_rotation.inv().apply( block_msgs.position) block_msgs.position = Rot.from_rotvec( np.pi * np.array([0.0, 0.0, 1.0])).apply(block_msgs.position) # if i == 1: print(block_msgs[i].position) # convert to lists block_msgs.rotation = block_msgs.rotation.as_quat() block_msgs.position = block_msgs.position.tolist() return block_msgs def clean_table(self): """ Remove all objects from table """ self.table_color_field.setSFColor([0.6, 0.6, 0.6]) for i in range(self.number_of_blocks): if not self.grabbed[i]: # get position above the basket position = [0.3, 1.2, 0.55] # set the position of the block self.block_positions_field[i].setSFVec3f(position) self.blocks[i].resetPhysics() self.supervisor.step(40 * self.timestep) def position_collision(self, positions, position): """ Check if there is chance of collisions between objects """ collision = False for pos in positions: if (pos[0] - position[0])**2 + (pos[2] - position[2])**2 < 0.09**2: collision = True return collision def prepare_table(self, obj_num): """ Place objects in the table """ # self.clean_table() for i in range(self.number_of_blocks): if not self.grabbed[i]: # get position above the basket position = [-i, 1, 1] # set the position of the block self.block_positions_field[i].setSFVec3f(position) self.blocks[i].resetPhysics() positions = [] for i in range(1, obj_num + 1, 1): # start, stop, step # print(i) if not self.grabbed[i]: # get position middle of the workspace y = 0.773 if len(positions) == 0: x = random.uniform(0.250, 0.530) z = random.uniform(-0.140, 0.140) # center = [0.390, 0.773, 0.0] position = [x, y, z] positions.append(position) # set the position of the block if obj_num == 1: i = random.randint(1, 4) self.block_positions_field[i].setSFVec3f(position) self.blocks[i].resetPhysics() # self.supervisor.step(40 * self.timestep) else: x = random.uniform(0.250, 0.530) z = random.uniform(-0.140, 0.140) if i == 3: y = 0.76 # estimate a position position = [x, y, z] # check if there are collisions while self.position_collision(positions, position): # if affirmative create another estimate and check again # print(position) x = random.uniform(0.250, 0.530) z = random.uniform(-0.140, 0.140) position = [x, y, z] positions.append(position) # set the position of the block self.block_positions_field[i].setSFVec3f(position) self.blocks[i].resetPhysics() # self.supervisor.step(40 * self.timestep) r = random.uniform(0.0, 1.0) g = random.uniform(0.0, 1.0) b = random.uniform(0.0, 1.0) self.table_color_field.setSFColor([r, g, b])
class InvaderBot(): # Initalize the motors. def setup(self): self.robot = Supervisor() self.motor_left = self.robot.getMotor("motor_left") self.motor_right = self.robot.getMotor("motor_right") self.timestep = int(self.robot.getBasicTimeStep()) # Do one update step. Calls Webots' robot.step(). # Return True while simulation is running. # Return False if simulation is ended def step(self): return (self.robot.step(self.timestep) != -1) # Set the velocity of the motor [-1, 1]. def set_motor(self, motor, velocity): mult = 1 if velocity > 0 else -1 motor.setPosition(mult * float('+inf')) motor.setVelocity(velocity * motor.getMaxVelocity()) # Set the velocity of left and right motors [-1, 1]. def set_motors(self, left, right): self.set_left_motor(left) self.set_right_motor(right) # Set the velocity of left motors [-1, 1]. def set_left_motor(self, velocity): self.set_motor(self.motor_left, velocity) # Set the velocity of right motors [-1, 1]. def set_right_motor(self, velocity): self.set_motor(self.motor_right, velocity) # Returns the current simulation time in seconds def get_time(self): return self.robot.getTime() # Returns the position of the robot in [x, z, angle] format # The coordinate system is as follows (top-down view) # .-------------------->x # |\ (angle) # | \ # | \ # | \ # | \ # | \ # | # V # z # def get_position(self): subject = self.robot.getSelf() position = subject.getPosition() orientation = subject.getOrientation() orientation = math.atan2(orientation[0], orientation[2]) orientation = math.degrees(orientation) return [position[0], position[2], orientation] # Returns the position of the balls in the following format # [ # [ # [green_ball_0_x, green_ball_0_z], # [green_ball_1_x, green_ball_1_z] # ], # # [ # [yellow_ball_0_x, yellow_ball_0_z], # [yellow_ball_1_x, yellow_ball_1_z], # [yellow_ball_2_x, yellow_ball_2_z], # ] # ] def get_balls(self): green = [] green_root = self.robot.getFromDef("_green_balls").getField("children") for idx in reversed(range(green_root.getCount())): try: ball = green_root.getMFNode(idx) pos = ball.getPosition() green.append([pos[0], pos[2]]) except: pass yellow = [] yellow_root = self.robot.getFromDef("_yellow_balls").getField( "children") for idx in reversed(range(yellow_root.getCount())): try: ball = yellow_root.getMFNode(idx) pos = ball.getPosition() yellow.append([pos[0], pos[2]]) except: pass return [green, yellow]
class TerritoryController: _emitters: Dict[StationCode, Emitter] _receivers: Dict[StationCode, Receiver] def __init__(self, claim_log: ClaimLog, attached_territories: AttachedTerritories) -> None: self._claim_log = claim_log self._attached_territories = attached_territories self._robot = Supervisor() self._claim_timer = ActionTimer(2, self.handle_claim_timer_tick) self._connected_territories = self._attached_territories.build_attached_capture_trees( ) self._emitters = { station_code: get_robot_device(self._robot, station_code + "Emitter", Emitter) for station_code in StationCode } self._receivers = { station_code: get_robot_device(self._robot, station_code + "Receiver", Receiver) for station_code in StationCode } for receiver in self._receivers.values(): receiver.enable(RECEIVE_TICKS) for station_code in StationCode: self.set_node_colour(station_code, ZONE_COLOURS[Claimant.UNCLAIMED]) for claimant in Claimant.zones(): self.set_score_display(claimant, 0) def set_node_colour(self, node_id: str, new_colour: Tuple[float, float, float]) -> None: node = self._robot.getFromDef(node_id) if node is None: logging.error(f"Failed to fetch node {node_id}") else: set_node_colour(node, new_colour) def set_territory_ownership( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: station = self._robot.getFromDef(station_code) if station is None: logging.error(f"Failed to fetch territory node {station_code}", ) return new_colour = ZONE_COLOURS[claimed_by] set_node_colour(station, new_colour) self._claim_log.log_territory_claim(station_code, claimed_by, claim_time) def prune_detached_stations( self, connected_territories: Tuple[Set[StationCode], Set[StationCode]], claim_time: float, ) -> None: broken_links = False # skip regenerating capture trees unless something changed # find territories which lack connections back to their claimant's corner for station in StationCode: # for territory in station_codes if self._claim_log.get_claimant(station) == Claimant.UNCLAIMED: # unclaimed territories can't be pruned continue if station in connected_territories[0]: # territory is linked back to zone 0's starting corner continue if station in connected_territories[1]: # territory is linked back to zone 1's starting corner continue # all disconnected territory is unclaimed self.set_territory_ownership(station, Claimant.UNCLAIMED, claim_time) broken_links = True if broken_links: self._connected_territories = \ self._attached_territories.build_attached_capture_trees() def claim_territory( self, station_code: StationCode, claimed_by: Claimant, claim_time: float, ) -> None: if not self._attached_territories.can_capture_station( station_code, claimed_by, self._connected_territories, ): # This claimant doesn't have a connection back to their starting zone logging.error( f"Robot in zone {claimed_by} failed to capture {station_code}") return if claimed_by == self._claim_log.get_claimant(station_code): logging.error( f"{station_code} already owned by {claimed_by.name}, resetting to UNCLAIMED", ) claimed_by = Claimant.UNCLAIMED self.set_territory_ownership(station_code, claimed_by, claim_time) # recalculate connected territories to account for # the new capture and newly created islands self._connected_territories = self._attached_territories.build_attached_capture_trees( ) self.prune_detached_stations(self._connected_territories, claim_time) def process_packet( self, station_code: StationCode, packet: bytes, receive_time: float, ) -> None: try: robot_id, is_conclude = struct.unpack( "!BB", packet) # type: Tuple[int, int] claimant = Claimant(robot_id) operation_args = (station_code, claimant, receive_time) if is_conclude: if self._claim_timer.has_begun_action_in_time_window( *operation_args): self.claim_territory(*operation_args) else: self._claim_timer.begin_action(*operation_args) except ValueError: logging.error( f"Received malformed packet at {receive_time} on {station_code}: {packet!r}", ) def receive_territory(self, station_code: StationCode, receiver: Receiver) -> None: simulation_time = self._robot.getTime() while receiver.getQueueLength(): try: data = receiver.getData() self.process_packet(station_code, data, simulation_time) finally: # Always advance to the next packet in queue: if there has been an exception, # it is safer to advance to the next. receiver.nextPacket() def update_territory_links(self) -> None: for stn_a, stn_b in TERRITORY_LINKS: if isinstance(stn_a, TerritoryRoot): # starting zone is implicitly owned if stn_a == TerritoryRoot.z0: stn_a_claimant = Claimant.ZONE_0 else: stn_a_claimant = Claimant.ZONE_1 else: stn_a_claimant = self._claim_log.get_claimant(stn_a) stn_b_claimant = self._claim_log.get_claimant(stn_b) # if both ends are owned by the same Claimant if stn_a_claimant == stn_b_claimant: claimed_by = stn_a_claimant else: claimed_by = Claimant.UNCLAIMED self.set_node_colour(f'{stn_a}-{stn_b}', LINK_COLOURS[claimed_by]) def update_displayed_scores(self) -> None: scores = self._claim_log.get_scores() for zone, score in scores.items(): self.set_score_display(zone, score) def set_score_display(self, zone: Claimant, score: int) -> None: # the text is not strictly monospace # but the subset of characters used roughly approximates this character_width = 40 character_spacing = 4 starting_spacing = 2 score_display = get_robot_device( self._robot, f'SCORE_DISPLAY_{zone.value}', Display, ) # fill with background colour score_display.setColor(0x183acc) score_display.fillRectangle( 0, 0, score_display.getWidth(), score_display.getHeight(), ) # setup score text score_display.setColor(0xffffff) score_display.setFont('Arial Black', 48, True) score_str = str(score) # Approx center value x_used = ( len(score_str) * character_width + # pixels used by characters (len(score_str) - 1) * character_spacing # pixels used between characters ) x_offset = int( (score_display.getWidth() - x_used) / 2) - starting_spacing # Add the score value score_display.drawText(score_str, x_offset, 8) def get_tower_led(self, station_code: StationCode, led: int) -> LED: return get_robot_device( self._robot, f"{station_code.value}Territory led{led}", LED, ) def handle_claim_timer_tick( self, station_code: StationCode, claimant: Claimant, progress: float, prev_progress: float, ) -> None: zone_colour = convert_to_led_colour(ZONE_COLOURS[claimant]) if progress in {ActionTimer.TIMER_EXPIRE, ActionTimer.TIMER_COMPLETE}: for led in range(NUM_TOWER_LEDS): tower_led = self.get_tower_led(station_code, led) if tower_led.get() == zone_colour: tower_led.set(0) else: # map the progress value to the LEDs led_progress = min(int(progress * NUM_TOWER_LEDS), NUM_TOWER_LEDS - 1) led_prev_progress = min(int(prev_progress * NUM_TOWER_LEDS), NUM_TOWER_LEDS - 1) if led_progress == led_prev_progress and prev_progress != 0: # skip setting an LED that was already on return tower_led = self.get_tower_led(station_code, led_progress) if led_progress == NUM_TOWER_LEDS - 1: if not self._attached_territories.can_capture_station( station_code, claimant, self._connected_territories, ): # station can't be captured by this team, the claim will fail # skip setting top LED return tower_led.set(zone_colour) def receive_robot_captures(self) -> None: for station_code, receiver in self._receivers.items(): self.receive_territory(station_code, receiver) if self._claim_log.is_dirty(): self.update_territory_links() self.update_displayed_scores() self._claim_log.record_captures() def transmit_pulses(self) -> None: for station_code, emitter in self._emitters.items(): emitter.send( struct.pack( "!2sb", station_code.encode("ASCII"), int(self._claim_log.get_claimant(station_code)), ), ) def main(self) -> None: timestep = self._robot.getBasicTimeStep() steps_per_broadcast = (1 / BROADCASTS_PER_SECOND) / (timestep / 1000) counter = 0 while True: counter += 1 self.receive_robot_captures() current_time = self._robot.getTime() self._claim_timer.tick(current_time) if counter > steps_per_broadcast: self.transmit_pulses() counter = 0 self._robot.step(int(timestep))
take_screenshot(camera, category, objectDirectory, os.path.dirname(protoPath), protoName, options, background, colorThreshold, shadowColor) # remove the object supervisor.step(timeStep) count = rootChildrenfield.getCount() importedNode.remove() supervisor.step(timeStep) if rootChildrenfield.getCount() != count - 1: sys.exit(protoName + ' was not removed sucessfully.') # Initialize the Supervisor. controller = Supervisor() timeStep = int(controller.getBasicTimeStep()) camera = controller.getCamera('camera') camera.enable(timeStep) options = get_options() if os.path.exists('.' + os.sep + 'images'): shutil.rmtree('.' + os.sep + 'images') # Get required fields rootChildrenfield = controller.getRoot().getField('children') supervisorTranslation = controller.getFromDef('SUPERVISOR').getField( 'translation') supervisorRotation = controller.getFromDef('SUPERVISOR').getField('rotation') viewpointPosition = controller.getFromDef('VIEWPOINT').getField('position') viewpointOrientation = controller.getFromDef('VIEWPOINT').getField( 'orientation')
def main(): """Main""" # Duration of each simulation simulation_duration = 10 # Get supervisor to take over the world world = Supervisor() n_joints = 10 timestep = int(world.getBasicTimeStep()) #freqs = 1 # Get and control initial state of salamander reset = RobotResetControl(world, n_joints) ''' Simulation setup - (choose an question to run)''' question = '8d' # 8a, 8b, 8c, 8d if question == '8a': # Experiment 0 freqs1 = np.ones(20) # 20 amplitudes to be specified amplitude1 = [1/10,1/10] # [Rhead,Rtail] (specify head and tail amplitude) phase_lag1 = 2*np.pi/10 # Positive = forward, negative = backward turn1 = 0 # Positive = left, negative = right (must not be too large) parameter_set = [ [freqs1, amplitude1, phase_lag1, turn1] ] elif question == '8b': pass elif question == '8c': # Experiment 0 freqs0 = np.ones(20) amplitude0 = [0,1/5] # Linear distribution of amplitude phase_lag0 = 2*np.pi/10 turn0 = 0 parameter_set = [ [freqs0, amplitude0, phase_lag0, turn0] ] elif question == '8d': # Experiment 0 freqs0 = np.ones(20) amplitude0 = [0,1/5] # Linear distribution of amplitude phase_lag0 = -2*np.pi/10 # Swimming backward turn0 = 0 # Experiment 1 freqs1 = np.ones(20) amplitude1 = [0,1/5] # Linear distribution of amplitude phase_lag1 = 2*np.pi/10 turn1 = 0.1 # turning left parameter_set = [ [freqs0, amplitude0, phase_lag0, turn0], [freqs1, amplitude1, phase_lag1, turn1] ] else: parameter_set = [] print('ERROR - Invalid question selected') # Store the data of the specified question for simulation_i, parameters in enumerate(parameter_set): reset.reset() run_simulation( world, parameters, timestep, int(1000*simulation_duration/timestep), logs="./logs/" + question + "/simulation_{}.npz".format(simulation_i) #logs="./logs/example/simulation_{}.npz".format(simulation_i) ) # Pause world.simulationSetMode(world.SIMULATION_MODE_PAUSE) world.worldReload() pylog.info("Simulations complete")
MAP_HEIGHT = 400 MAP_WIDTH = 400 DAMAGE_THRESHOLD = .95 INCREMENT_CMAP = 0.005 NOISE_THRESHOLD = 1.0 FORTY_FIVE_DEGREES = math.pi / 4 NINETY_DEGREES = math.pi / 2 SINUSOIDAL_FUNCTION_MULTIPLIER = 2.0 * math.pi * 0.5 mode = 'mapping' #mode = 'repair' # create the Robot instance. robot = Supervisor() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # Initialize leg motors motorNames = [ 'RPC', 'RPF', 'RPT', 'RMC', 'RMF', 'RMT', 'RAC', 'RAF', 'RAT', 'LPC', 'LPF', 'LPT', 'LMC', 'LMF', 'LMT', 'LAC', 'LAF', 'LAT' ] motors = [] for i in range(0, 18): motors.append(robot.getDevice(motorNames[i])) # Initialize lidar sensor lidar = robot.getDevice('Hokuyo URG-04LX-UG01') lidar.enable(timestep) lidar.enablePointCloud()
class DarwinWebotsController: def __init__(self, namespace='', ros_active=False, ros_anonymous=True, mode='normal'): self.ros_active = ros_active self.time = 0 self.clock_msg = Clock() self.namespace = namespace self.supervisor = Supervisor() self.motor_names = [ "ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR", "FootL", "Neck", "Head" ] self.walkready = [0] * 20 self.names_webots_to_bitbots = { "ShoulderR": "RShoulderPitch", "ShoulderL": "LShoulderPitch", "ArmUpperR": "RShoulderRoll", "ArmUpperL": "LShoulderRoll", "ArmLowerR": "RElbow", "ArmLowerL": "LElbow", "PelvYR": "RHipYaw", "PelvYL": "LHipYaw", "PelvR": "RHipRoll", "PelvL": "LHipRoll", "LegUpperR": "RHipPitch", "LegUpperL": "LHipPitch", "LegLowerR": "RKnee", "LegLowerL": "LKnee", "AnkleR": "RAnklePitch", "AnkleL": "LAnklePitch", "FootR": "RAnkleRoll", "FootL": "LAnkleRoll", "Neck": "HeadPan", "Head": "HeadTilt" } self.names_bitbots_to_webots = { v: k for k, v in self.names_webots_to_bitbots.items() } self.motors = [] self.sensors = [] self.timestep = int(self.supervisor.getBasicTimeStep()) # self.timestep = 10 if mode == 'normal': self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) elif mode == 'paused': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE) elif mode == 'run': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_RUN) elif mode == 'fast': self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST) else: self.supervisor.simulationSetMode( Supervisor.SIMULATION_MODE_REAL_TIME) for motor_name in self.motor_names: self.motors.append(self.supervisor.getDevice(motor_name)) self.motors[-1].enableTorqueFeedback(self.timestep) self.sensors.append(self.supervisor.getDevice(motor_name + "S")) self.sensors[-1].enable(self.timestep) self.accel = self.supervisor.getDevice("Accelerometer") self.accel.enable(self.timestep) self.gyro = self.supervisor.getDevice("Gyro") self.gyro.enable(self.timestep) self.camera = self.supervisor.getDevice("Camera") self.camera.enable(self.timestep) if self.ros_active: rospy.init_node("webots_darwin_ros_interface", anonymous=ros_anonymous, argv=['clock:=/' + self.namespace + '/clock']) self.pub_js = rospy.Publisher(self.namespace + "/joint_states", JointState, queue_size=1) self.pub_imu = rospy.Publisher(self.namespace + "/imu/data", Imu, queue_size=1) self.pub_cam = rospy.Publisher(self.namespace + "/image_raw", Image, queue_size=1) self.pub_clock = rospy.Publisher(self.namespace + "/clock", Clock, queue_size=1) rospy.Subscriber(self.namespace + "/DynamixelController/command", JointCommand, self.command_cb) self.world_info = self.supervisor.getFromDef("world_info") self.hinge_joint = self.supervisor.getFromDef("barrier_hinge") self.robot_node = self.supervisor.getFromDef("Darwin") self.translation_field = self.robot_node.getField("translation") self.rotation_field = self.robot_node.getField("rotation") @property def ros_time(self) -> rospy.Time: return rospy.Time.from_seconds(self.time) def step_sim(self): self.time += self.timestep / 1000 self.supervisor.step(self.timestep) def step(self): self.step_sim() if self.ros_active: self.publish_imu() self.publish_joint_states() self.publish_clock() self.publish_camera() def publish_clock(self): self.clock_msg.clock = self.ros_time self.pub_clock.publish(self.clock_msg) def command_cb(self, command: JointCommand): for i, name in enumerate(command.joint_names): try: motor_index = self.motor_names.index( self.names_bitbots_to_webots[name]) self.motors[motor_index].setPosition(command.positions[i]) except ValueError: rospy.logerr( f"invalid motor specified ({self.names_bitbots_to_webots[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 publish_joint_states(self): msg = JointState() msg.name = [] msg.header.stamp = self.ros_time msg.position = [] msg.effort = [] for i in range(len(self.sensors)): msg.name.append(self.names_webots_to_bitbots[self.motor_names[i]]) value = self.sensors[i].getValue() msg.position.append(value) msg.effort.append(self.motors[i].getTorqueFeedback()) self.pub_js.publish(msg) def publish_imu(self): msg = Imu() msg.header.stamp = self.ros_time msg.header.frame_id = "imu_frame" accel_vels = self.accel.getValues() msg.linear_acceleration.x = ((accel_vels[0] - 512.0) / 512.0) * 3 * G msg.linear_acceleration.y = ((accel_vels[1] - 512.0) / 512.0) * 3 * G msg.linear_acceleration.z = ((accel_vels[2] - 512.0) / 512.0) * 3 * G gyro_vels = self.gyro.getValues() msg.angular_velocity.x = ((gyro_vels[0] - 512.0) / 512.0) * 1600 * ( math.pi / 180) # is 400 deg/s the real value msg.angular_velocity.y = ( (gyro_vels[1] - 512.0) / 512.0) * 1600 * (math.pi / 180) msg.angular_velocity.z = ( (gyro_vels[2] - 512.0) / 512.0) * 1600 * (math.pi / 180) # todo compute msg.orientation.x = 0 msg.orientation.y = 0 msg.orientation.z = 0 msg.orientation.w = 1 self.pub_imu.publish(msg) def publish_camera(self): msg = Image() msg.header.stamp = self.ros_time msg.header.frame_id = "MP_PMDCAMBOARD" msg.height = self.camera.getHeight() msg.width = self.camera.getWidth() msg.encoding = "bgra8" msg.step = 4 * self.camera.getWidth() img = self.camera.getImage() msg.data = img self.pub_cam.publish(msg) def set_gravity(self, active): if active: self.world_info.getField("gravity").setSFVec3f([0.0, -9.81, 0.0]) self.world_info.getField("gravity").setSFFloat(9.81) else: self.world_info.getField("gravity").setSFVec3f([0.0, 0.0, 0.0]) self.world_info.getField("gravity").setSFFloat(0) def reset_robot_pose(self, pos, quat): rpy = tf.transformations.euler_from_quaternion(quat) self.set_robot_pose_rpy(pos, rpy) self.robot_node.resetPhysics() def reset_robot_pose_rpy(self, pos, rpy): self.set_robot_pose_rpy(pos, rpy) self.robot_node.resetPhysics() def reset(self): # self.supervisor.simulationReset() # reactivate camera after reset https://github.com/cyberbotics/webots/issues/1778 # self.supervisor.getSelf().restartController() # self.camera = self.supervisor.getCamera("Camera") # self.camera.enable(self.timestep) self.supervisor.simulationResetPhysics() def node(self): s = self.supervisor.getSelected() if s is not None: print(f"id: {s.getId()}, type: {s.getType()}, def: {s.getDef()}") def set_robot_pose_rpy(self, pos, rpy): self.translation_field.setSFVec3f(pos_ros_to_webots(pos)) self.rotation_field.setSFRotation(rpy_to_axis(*rpy)) def set_robot_rpy(self, rpy): self.rotation_field.setSFRotation(rpy_to_axis(*rpy)) def get_robot_pose_rpy(self): pos = self.translation_field.getSFVec3f() rot = self.rotation_field.getSFRotation() return pos_webots_to_ros(pos), axis_to_rpy(*rot)
virtualMarkers.append(prefix + 'A') virtualMarkers.append(prefix + 'L') virtualMarkers.append(prefix + 'P') if name in virtualMarkers: return True return False @staticmethod def removeMarkers(): markerField = supervisor.getSelf().getField('markers') for i in range(markerField.getCount()): markerField.removeMF(-1) supervisor = Supervisor() timestep = int(supervisor.getBasicTimeStep()) enableValueGraphs = [] # parse arguments if len(sys.argv) < 3 or sys.argv[1] == 'None': sys.exit('C3D file not defined.') playbackSpeed = float(sys.argv[2]) # make one step to be sure markers are not imported before pressing play supervisor.step(timestep) # remove possible previous marker (at regeneration for example) c3dFile.removeMarkers() c3dfile = c3dFile(sys.argv[1])
class EndP_env3D(object): """docstring for Robot_arm""" def __init__(self): # ---create robot as supervisor--- self.robot = Supervisor() # ---get simulation timestep--- self.timestep = int(self.robot.getBasicTimeStep()) '''Position Motors''' self.velocity = 1 self.dt = 0.032 ## to be modified : init_position '''2D 2Motor''' # self.rm1_init_position = 0 # self.rm4_init_position = -0.0698 # '''3D 4Motor train''' # self.rm2_init_position = 0 # self.rm3_init_position = 0 # self.rm5_init_position = 0 # self.rm6_init_position = 0 # self.rm7_init_position = 0 self.rm1_init_position = self.robot.getFromDef("Franka_Emika_Panda.joint01.j1p").getField('position').getSFFloat() print("rm1_initpos: {}".format(self.rm1_init_position)) self.rm2_init_position = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.j2p").getField('position').getSFFloat() self.rm3_init_position = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.joint23.j3p").getField('position').getSFFloat() self.rm4_init_position = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.joint23.link3.joint34.j4p").getField('position').getSFFloat() self.rm5_init_position = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.joint23.link3.joint34.link4.joint45.j5p").getField('position').getSFFloat() self.rm6_init_position = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.joint23.link3.joint34.link4.joint45.link5.joint56.j6p").getField('position').getSFFloat() self.rm7_init_position = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.joint23.link3.joint34.link4.joint45.link5.joint56.link6.joint67.j7p").getField('position').getSFFloat() '''Orientation Motor''' ## for franka self.P2 = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.M2P") self.P4 = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.joint23.link3.joint34.link4.M4P") self.ENDP = self.robot.getFromDef("Franka_Emika_Panda.joint01.link1.joint12.link2.joint23.link3.joint34.link4.joint45.link5.joint56.link6.joint67.link7.joint7H.hand.endEffector.endp") self.origin = np.array(self.P2.getPosition()) # self.elbowP = np.array(self.P4.getPosition()) self.endpointP = np.array(self.ENDP.getPosition()) self.edge = self.endpointP[0]-self.origin[0] print("edge : {}".format(self.edge)) print("NEW") self.beaker = self.robot.getFromDef('Beaker') print("beaker_ori: {}".format(self.beaker.getOrientation()[4])) ## to be modified : modify the random space to fit in our robot's work place ## we should use 3D random goal ## for franka ## use a box space for now #x_range = np.linspace(0.4, 0.7, 2) #x = np.random.choice(x_range) #z_range = np.linspace(-0.25, 0.25, 3) #z = np.random.choice(z_range) x = 0.4 z = 0.1 # y_range = np.linspace(0.0, 0.3, 3) # y = np.random.choice(y_range) y = 0.15 self.goal_position = self.robot.getFromDef('TARGET.tar').getPosition() print("goal : {}".format(self.goal_position)) '''3D goal''' self.goal_np_p = np.array(self.goal_position) d, _ = self._get_elbow_position() print("(elbow_local/self.edge) : {}".format(d)) # self.GOAL = self.robot.getFromDef('goal') # self.GOAL_P = self.GOAL.getField('translation') # self.GOAL_P.setSFVec3f(self.goal_position) ## for franka self.arm_motor_name = [ # 'motor1', 'motor2', 'motor3', 'motor4', 'motor5', 'motor6'] '''3D 4Motor''' self.arm_position = dict(zip(self.arm_motor_name, [ # self.rm1_init_position, self.rm2_init_position, self.rm3_init_position, self.rm4_init_position, self.rm5_init_position, self.rm6_init_position])) # self.rm7_init_position])) self.arm_motors, self.arm_ps = self.create_motors(self.arm_motor_name, self.arm_position) ## to be modified : set the position range self.arm_position_range = np.array([ # [-2.897, 2.897], [-1.763, 1.763], [-2.8973, 2.8973], [-3.072, -0.0698], [-2.8973, 2.8973], [-0.0175, 3.7525]]) # [-2.897, 2.897]]) self.act_mid = dict(zip(self.arm_motor_name, np.mean(self.arm_position_range, axis=1))) self.act_rng = dict(zip(self.arm_motor_name, 0.5*(self.arm_position_range[:, 1] - self.arm_position_range[:, 0]))) self.arm_position_range = dict(zip(self.arm_motor_name, [ # [-2.897, 2.897], [-1.763, 1.763], [-2.8973, 2.8973], [-3.072, -0.0698], [-2.8973, 2.8973], [-0.0175, 3.7525]])) # [-2.897, 2.897]])) '''Orientation Motors''' # ---create camera dict--- self.cameras = {} self.camera_name = ['vr_camera_r', # "camera_center" ] self.cameras = self.create_sensors(self.camera_name, self.robot.getCamera) # ---get image height and width--- self.camera_height = self.cameras["vr_camera_r"].getHeight() self.camera_width = self.cameras["vr_camera_r"].getWidth() # ---set game over for game start--- self.done = False self.lives = 0 # ---action set for motor to rotate clockwise/counterclockwise or stop self.action_num = len(self.arm_motors) print("action_num: {}".format(self.action_num)) self.action_space = spaces.Box(-1, 1, shape=(self.action_num,), dtype=np.float32) # self.action_space = spaces.Box(-0.016, 0.016, shape=(self.action_num, ), dtype=np.float32) self.on_goal = 0 # self.crash = 0 self.accumulated_reward = 0 self.episode_len_reward = 0 self.episode_ori_reward = 0 self.arrival_time = 0 # print(self.ENDP.getOrientation()) self.robot.step(self.timestep) self.start_time = self.getTime() '''constant''' self.NEAR_DISTANCE = 0.05 self.BONUS_REWARD = 10 self.ON_GOAL_FINISH_COUNT = 5 self.MAX_STEP = 100 '''variable''' self.finished_step = 0 self.prev_d = self.goal_np_p - np.array(self.ENDP.getPosition()) self.left_bonus_count = self.ON_GOAL_FINISH_COUNT def reset_eval_to_down(self): self.on_goal = 0 # self.crash = 0 self.accumulated_reward = 0 self.episode_len_reward = 0 self.episode_ori_reward = 0 self.arrival_time = 0 self.finished_step = 0 self.prev_d = self.goal_np_p - np.array(self.ENDP.getPosition()) self.left_bonus_count = self.ON_GOAL_FINISH_COUNT self.start_time = self.getTime() self.done = False self.lives = 0 def _get_robot(self): return self.robot def create_motors(self, names, position): obj = {} ps_obj = {} for name in names: motor = self.robot.getMotor(name) motor.setVelocity(self.velocity) motor.setPosition(position[name]) ps = motor.getPositionSensor() ps.enable(self.timestep) obj[name] = motor ps_obj[name] = ps return obj, ps_obj def create_sensors(self, names, instant_func): obj = {} for name in names: sensor = instant_func(name) sensor.enable(self.timestep) obj[name] = sensor return obj def set_goal_position(self, arg): #x_range = np.linspace(0.4, 0.7, 2) #x = np.random.choice(x_range) #z_range = np.linspace(-0.25, 0.25, 3) #z = np.random.choice(z_range) # y_range = np.linspace(0.0, 0.3, 3) # y = np.random.choice(y_range) x = 0.5665 z = 0.0241 y = 0.66 # self.goal_position = [x, y, z] self.goal_position = self.robot.getFromDef('TARGET.tar').getPosition() if arg=='down': self.goal_position = [x, y, z] self.robot.getFromDef('TARGET').getField('translation').setSFVec3f(self.goal_position) self.goal_np_p = np.array(self.goal_position) # self.GOAL_P.setSFVec3f(self.goal_position) self.robot.step(self.timestep) def _get_image(self): return self.cameras["vr_camera_r"].getImage() def _get_obs(self): return self._get_image() def _get_motor_position(self): motor_position = [] for name in self.arm_motor_name: data = self.arm_ps[name].getValue() # normalized data = (data - self.act_mid[name])/self.act_rng[name] motor_position.append(data) return motor_position def _get_endpoint_orientation(self): orientation = np.array(self.ENDP.getOrientation()) # print(orientation[::4]) d = self.on_goal - orientation[::4] return orientation.tolist(), (d/2).tolist() def _get_state(self): state = [] '''Position''' elp, elbd = self._get_elbow_position() arm_p = self._get_motor_position() ep, ed = self._get_endpoint_position() ## to be modified : choose what to be our state # state += elp+ep+elbd+ed+[1. if self.on_goal else 0.] state += arm_p + ed '''Orientation''' # orient, d = self._get_endpoint_orientation() # state += orient+d+[1. if self.on_goal else 0.] return state def _get_elbow_position(self): goal = self.goal_np_p '''3D Position Distance''' ## to be modified : set elbow; why edge? elbow_global = np.array(self.P4.getPosition()) elbow_local = elbow_global - self.origin d = (goal - elbow_global) return (elbow_local/self.edge).tolist(), (d/self.edge).tolist() def _get_endpoint_position(self): goal = self.goal_np_p '''3D Position Distance''' end_global = np.array(self.ENDP.getPosition()) end_local = end_global - self.origin d = goal - end_global return (end_local/self.edge).tolist(), (d/self.edge).tolist() ## to be modified def _get_reward(self): '''Position Reward''' _, d = self._get_endpoint_position() d = np.array(d)*self.edge # d = np.array(d)*0.565*2 '''3D reward''' # r = -np.sqrt(d[0]**2+d[1]**2+d[2]**2) r = np.linalg.norm(self.prev_d) - np.linalg.norm(d) distance = np.linalg.norm(d) if distance < self.NEAR_DISTANCE : if self.left_bonus_count > 0 : bonus = self.BONUS_REWARD * (self.MAX_STEP - 2*self.finished_step) / self.MAX_STEP r += bonus self.left_bonus_count -= 1 print('bonus: ', bonus) self.on_goal += 1 print('on goal: ', self.on_goal) if self.on_goal >= self.ON_GOAL_FINISH_COUNT : self.done = True print('ON GOAL') else: self.on_goal = 0 print('on goal: 0') self.episode_len_reward += r self.orientation_reward = 1000*(self.beaker.getOrientation()[4] - 0.001) # print("ori: {}".format(self.beaker.getOrientation()[4])) print("ori_reward: {}".format(self.orientation_reward)) r += self.orientation_reward self.episode_ori_reward += self.orientation_reward self.accumulated_reward += r self.prev_d = d self.finished_step += 1 return r @property def obsrvation_space(self): return spaces.Box(low=0, high=255, shape=(self.camera_height, self.camera_width, 3), dtype=np.uint8) # for rgb def action_space(self): return spaces.Box(-1, 1, shape=(self.action_num, ), dtype=np.float32) def get_lives(self): return self.lives def getTime(self): return self.robot.getTime() def reset(self): fp = open("Episode-len-score.txt","a") fp.write(str(self.episode_len_reward)+'\n') fp.close() fp = open("Episode-orientation-score.txt","a") fp.write(str(self.episode_ori_reward)+'\n') fp.close() fp = open("Episode-score.txt","a") fp.write(str(self.accumulated_reward)+'\n') fp.close() self.robot.worldReload() def step(self, action_dict): reward = 0 action_dict = np.clip(action_dict, -1, 1) for name, a in zip(self.arm_motor_name, action_dict): self.arm_position[name] += a * self.dt if self.arm_position_range[name][0] > self.arm_position[name]: self.arm_position[name] = self.arm_position_range[name][0] elif self.arm_position_range[name][1] < self.arm_position[name]: self.arm_position[name] = self.arm_position_range[name][1] self.arm_motors[name].setPosition(self.arm_position[name]) self.robot.step(self.timestep) reward = self._get_reward() state = self._get_state() obs = False return obs, state, reward, self.done, {"goal_achieved":self.on_goal>0}
from os import truncate from controller import Robot, Supervisor, Display from shapely.geometry import Polygon #for rectangle intersections from shapely.geometry import Point import time supervisor = Supervisor() global TIMESTEP TIMESTEP = int(supervisor.getBasicTimeStep()) robotNode = supervisor.getRoot() children = robotNode.getField("children") robot = children.getMFNode(5) startTime = time.time() #get devices gyro = supervisor.getDevice("gyro") accel = supervisor.getDevice("accelerometer") gyro.enable(TIMESTEP) accel.enable(TIMESTEP) right_shoulder_pitch = supervisor.getDevice("RShoulderPitch") left_shoulder_pitch = supervisor.getDevice("LShoulderPitch") right_torso_pitch = supervisor.getDevice("RHipPitch") left_torso_pitch = supervisor.getDevice("LHipPitch")
from tornado.ioloop import IOLoop import tornado.web #from controller import Robot, Motor, DistanceSensor # for supervisor functions from controller import Supervisor, Motor, DistanceSensor, PositionSensor, Camera # create the Robot instance. # robot = Robot() # create Robot instance with supervisor superpower ## Robot must be set to "supervisor TRUE" robot = Supervisor() #node = Node() robot_node = robot.getSelf() #TIME_STEP = 64 TIME_STEP = int(robot.getBasicTimeStep()) MAX_SPEED = float(6.28) ROBOT_ANGULAR_SPEED_IN_DEGREES = float(360) #ROBOT_ANGULAR_SPEED_IN_DEGREES = float(283.588111888) # initialize sensors ps = [] psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7'] for i in range(8): ps.append(robot.getDevice(psNames[i])) ps[i].enable(TIME_STEP) # initialize and set motor devices leftMotor = robot.getDevice('left wheel motor') rightMotor = robot.getDevice('right wheel motor')