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)) 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))
position = sumoFirstVehicle.getPosition() # check if first sumo vehicle has moved if (position[0] == sumoFirstVehicleInitialPosition[0] and position[1] == sumoFirstVehicleInitialPosition[1] and position[2] == sumoFirstVehicleInitialPosition[2]): sumoFailure = True # check if robot is inside the emergency lane position = vehicleNode.getPosition() positionPoint = Point((position[0], position[2])) if emergencyLanePath.distance(positionPoint) < 0.5 * laneWidth: inEmergencyLane = True # check for collision numberOfContactPoints = vehicleNode.getNumberOfContactPoints() if numberOfContactPoints > 0: collided = True time = supervisor.getTime() distance = roadPath.project(positionPoint) - initialDistance supervisor.wwiSendText("update: " + str(time) + " {0:.3f}".format(distance)) # check if sensors visualization should be enabled/disabled message = supervisor.wwiReceiveText() if message and message.startswith("sensors visualization:"): if message.endswith("false"): enable_sensors_visualization(supervisor, False) elif message.endswith("true"): enable_sensors_visualization(supervisor, True) supervisor.wwiSendText("stop") if inEmergencyLane: supervisor.setLabel(0,
class UniversalController: def __init__(self, network, robot_name="standard", target_name="TARGET", num_of_inputs=4, eval_run_time=100, time_step=32): self.neural_network = network self.TIME_STEP = time_step #standard time step is 32 or 64 self.supervisor = Supervisor() self.inputs = num_of_inputs self.solution_threshold = 0.95 #call methods functions to intialise robot and target self.get_and_set_robot(robot_name) self.get_and_set_target(target_name) self.eval_run_time = eval_run_time #default to 1m x 1m space but can be edited directly or using method below self.max_distance = 1.4142 self.data_reporting = DataReporting() def get_and_set_robot(self, robot_name): # initialise robot node and components self.robot_node = self.supervisor.getFromDef( robot_name) #TARGET ROBOT MUST BE NAMED IN DEF FIELD self.robot_trans = self.robot_node.getField("translation") self.robot_rotation = self.robot_node.getField("rotation") #call start location function to store initial position self.get_and_set_robotStart() #call motors function self.get_and_set_motors() def get_and_set_robotStart(self): #establish robot starting position self.robot_starting_location = self.robot_trans.getSFVec3f() self.robot_starting_rotation = self.robot_rotation.getSFRotation() '''EDIT HERE TO EXPAND TO DIFFERENT NUMBERS OF WHEELS/MOTORS''' def get_and_set_motors(self): self.motors = [] self.motor_max_Vs = [] #get robot motors - currently works for all two wheel motor morphologies self.left_motor = self.supervisor.getMotor('left wheel motor') self.right_motor = self.supervisor.getMotor('right wheel motor') self.left_motor.setPosition(float('inf')) self.right_motor.setPosition(float('inf')) #get the max velocity each motor is capable of and set the max velocity self.left_motor_max = self.left_motor.getMaxVelocity() self.right_motor_max = self.right_motor.getMaxVelocity() #append necessary additional motors and max velocities here after enabling as above self.motors.append(self.left_motor) self.motors.append(self.right_motor) self.motor_max_Vs.append(self.left_motor_max) self.motor_max_Vs.append(self.right_motor_max) def get_and_set_target(self, target_name): #get target location self.target = self.supervisor.getFromDef( target_name) #TARGET MUST BE NAMED IN DEF FIELD #get and set the translation and location for retrieval when needed self.target_trans = self.target.getField("translation") self.target_location = self.target_trans.getSFVec3f() def set_dimensions(self, space_dimensions): #basic pythagorean calculation to find max distance possible in square space self.max_distance = np.sqrt(space_dimensions[0]**2 + space_dimensions[1]**2) #print(self.max_distance) def set_distance_sensors(self, distance_sensors): #takes in array of strings - names of distance sensors self.distance_sensors = [] self.min_DS_values = [] self.DS_value_range = [] for i in range(len(distance_sensors)): #set and enable each sensor self.distance_sensors.append( self.supervisor.getDistanceSensor(distance_sensors[i])) self.distance_sensors[i].enable(self.TIME_STEP) #get and store the min reading value of each sensor self.min_DS_values.append(self.distance_sensors[i].getMinValue()) #get and store the possible value range of each sensor self.DS_value_range.append(self.distance_sensors[i].getMaxValue() - self.min_DS_values[i]) #print(self.DS_value_range) def get_DS_values(self): #get distance sensor values values = [] for i in range(len(self.distance_sensors)): value = self.distance_sensors[i].getValue() #value = value/self.maxDSReading #practical max value value = value - self.min_DS_values[i] if value < 0.0: value = 0.0 #to account for gaussian noise value = value / (self.DS_value_range[i]) #account for gaussian noise providing higher than max reading if value > 1.0: value = 1.0 values.append(value) #return a list of the normalised sensor readings return values def compute_motors(self, DS_values): #get the outputs of the neural network and convert into wheel motor speeds #already fully flexible for multiple motors results = self.neural_network.forwardPass(DS_values) for i in range(len(self.motors)): self.motors[i].setVelocity(results[i] * self.motor_max_Vs[i]) def reset_all_physics(self): #reset robot physics and return to starting translation ready for next run self.robot_rotation.setSFRotation(self.robot_starting_rotation) self.robot_trans.setSFVec3f(self.robot_starting_location) self.robot_node.resetPhysics() '''SIMULATION FUNCTION - can also be used directly for manual DataVisualisationandTesting of weight arrays (to manually repeat successful solutions etc.)''' def evaluate_robot(self, individual, map_elites=False, all_novelty=False): self.neural_network.decodeEA( individual) #individual passed from algorithm #note simulator start time t = self.supervisor.getTime() if map_elites or all_novelty: velocity = [] angular_V = [] #run simulation for eval_run_time seconds while self.supervisor.getTime() - t < self.eval_run_time: #calculate the motor speeds from the sensor readings self.compute_motors(self.get_DS_values()) #check current objective fitness current_fit = self.calculate_fitness() if map_elites or all_novelty: currentV = self.robot_node.getVelocity() velocity.append( np.sqrt((currentV[0]**2) + (currentV[1]**2) + (currentV[2]**2))) angular_V.append(np.sqrt(currentV[4]**2)) #break if robot has reached the target if current_fit > self.solution_threshold: time_taken = self.supervisor.getTime() - t #safety measure due to time_step and thread lag if time_taken > 0.0: #then a legitimate solution fit = self.calculate_fitness() break if self.supervisor.step(self.TIME_STEP) == -1: quit() #Get only the X and Y coordinates to create the endpoint vector endpoint = self.robot_trans.getSFVec3f()[0:3:2] distance_FS = np.sqrt( (endpoint[0] - self.robot_starting_location[0])**2 + (endpoint[1] - self.robot_starting_location[2])**2) #reset the simulation self.reset_all_physics() #find fitness fit = self.calculate_fitness() if map_elites: average_velocity = np.average(velocity) #average_angular_V = np.average(angular_V) return average_velocity, distance_FS, fit if all_novelty: average_velocity = np.average(velocity) median_velocity = np.median(velocity) #print("fit = " + str(fit) + ", endpoint = " + str(endpoint[0]) + "," + str(endpoint[1]) + ", AV = " + str(average_velocity) + ", distanceFS = " + str(distance_FS)) return fit, endpoint, average_velocity, distance_FS, median_velocity return fit, endpoint def calculate_fitness(self): values = self.robot_trans.getSFVec3f() distance_from_target = np.sqrt( (self.target_location[0] - values[0])**2 + (self.target_location[2] - values[2])**2) fit = 1.0 - (distance_from_target / self.max_distance) return fit def sigma_test(self, my_EA, upper_limit=1.0, lower_limit=0.1): self.data_reporting.sigma_test(my_EA, upper_limit, lower_limit) def algorithm_test(self, my_EA, generations, total_runs, nipes=False, map_elites=False): self.data_reporting.algorithm_test(my_EA, generations, total_runs, nipes, map_elites) def control_group_test(self, generations=10000, total_runs=1): self.data_reporting.control_group_test( individual_size=self.neural_network.solution_size, eval_function=self.evaluate_robot, generations=generations, total_runs=total_runs)
class WebotsNode(Node): def __init__(self, name, args=None, enableTfPublisher=False, enableJointState=False): super().__init__(name) self.declare_parameter('synchronization', False) self.declare_parameter('use_joint_state_publisher', 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 self.__device_manager = None if enableTfPublisher: if self.robot.getSupervisor(): self.tfPublisher = TfPublisher(self.robot, self) else: self.get_logger().warn( 'Impossible to publish transforms because the "supervisor"' ' field is false.') if self.get_parameter( 'use_joint_state_publisher').value or enableJointState: self.jointStatePublisher = JointStatePublisher( self.robot, '', self) def step(self, ms): if self.get_parameter('use_joint_state_publisher').value: self.jointStatePublisher.publish() if self.__device_manager: self.__device_manager.step() 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 msg = Clock() msg.clock = Time(seconds=self.robot.getTime()).to_msg() 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 start_device_manager(self, config=None): """ Start automatic ROSification of Webots devices available in the robot. Kwargs: config (dict): Dictionary of properties in format:: { [device_name]: { [property_name]: [property_value] } } """ self.__device_manager = DeviceManager(self, config)
h = value[1].lstrip('#') color = [int(h[i:i + 2], 16) / 255.0 for i in (0, 2, 4)] for i in range(2, len(value)): c3dfile.pointRepresentations[value[i]]['color'].setSFColor( color) elif action == 'graphs': if value[2] == 'true': enableValueGraphs.append(value[1]) else: enableValueGraphs.remove(value[1]) elif action == 'body_transparency': c3dfile.bodyTransparency = float(value[1]) c3dfile.bodyTransparencyField.setSFFloat(c3dfile.bodyTransparency) elif action == 'speed': playbackSpeed = float(value[1]) offsetTime = supervisor.getTime() totalFrameCoutner = 0 elif action == 'c3dfile': file = tempfile.NamedTemporaryFile(mode='wb', delete=False, suffix='.c3d') file.write(base64.b64decode(value[1])) file.close() del c3dfile c3dfile = c3dFile(file.name) os.remove(file.name) else: print(message) message = supervisor.wwiReceiveText() # play the required frame (if needed)
shoulderOffset = 0.8 kneeOffset = -2.4 offsetVector = [ -baseOffset, shoulderOffset, kneeOffset, 0.0, shoulderOffset, kneeOffset, baseOffset, shoulderOffset, kneeOffset, baseOffset, shoulderOffset, kneeOffset, 0.0, shoulderOffset, kneeOffset, -baseOffset, shoulderOffset, kneeOffset ] goal_theta = math.pi # Main controller loop: while robot.step(timestep) != -1: if (mode == 'mapping'): # Retrieve the pose coordinates from the GPS and Compass units time = robot.getTime() coord = gps.getValues() n = compass.getValues() pose_x = coord[0] pose_y = coord[2] pose_theta = -((math.atan2(n[0], n[2])) - (NINETY_DEGREES)) bearing_err = pose_theta - goal_theta if abs(bearing_err) > 0.2 and pose_y < 7 and goal_theta != 0: #south for i in range(0, 10): #all the right legs motors[i].setPosition(ampVector[i] * math.sin( SINUSOIDAL_FUNCTION_MULTIPLIER * time + phaseVector[i]) + offsetVector[i]) for i in range(9, 18): motors[i].setPosition(ampVector[i] * math.sin( -SINUSOIDAL_FUNCTION_MULTIPLIER * time + phaseVector[i]) +
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]
supervisor = Supervisor() TIMESTEP = int(supervisor.getBasicTimeStep()) COMM_CHANNEL = 1 # Supervisor interpret world soccerball = supervisor.getFromDef("BALL") trans_field = soccerball.getField("translation") ball_radius = 0.113 # soccerball.getField("radius") INITIAL_TRANS = [0, ball_radius, 0] # Emitter setup emitter = supervisor.getEmitter('emitter') emitter.setChannel(COMM_CHANNEL) tInitial = supervisor.getTime() while supervisor.step(TIMESTEP) != -1: values = trans_field.getSFVec3f() t = supervisor.getTime() # Emit ball position if (t - tInitial) >= 1: # print(t-tInitial) print("Ball is at position: %g %g %g" % (values[0], values[1], values[2])) message = struct.pack("ddd", values[0], values[1], values[2]) emitter.send(message) time_difference = 1 - (t - tInitial)
positionx = str(position[0]) positionz = str(position[2]) f_d = str(front_dist) r_d = str(right_dist) comp1 = str(compass_data[0]) comp2 = str(compass_data[2]) gyro_dx = str(gyro_data[0]) gyro_dy = str(gyro_data[1]) gyro_dz = str(gyro_data[2]) s = positionx + "," + positionz + "," + f_d + "," + r_d + "," + comp1 + "," + comp2 + "," + gyro_dy f.write(s) f.write('\n') #simulation time t = supervisor.getTime() t2 = supervisor.getTime() #motion scenario case = 1 if case == 1: while supervisor.getTime() - t < 1: if supervisor.step(TIME_STEP) == -1: quit() left_motor.setVelocity((supervisor.getTime() - t)*1.5) right_motor.setVelocity((supervisor.getTime() - t)*-2.0) # read sensors front_dist = front_sensor.getValue()
positionx = str(position[0]) positionz = str(position[2]) f_d = str(front_dist) r_d = str(right_dist) comp1 = str(compass_data[0]) comp2 = str(compass_data[2]) gyro_dx = str(gyro_data[0]) gyro_dy = str(gyro_data[1]) gyro_dz = str(gyro_data[2]) s = positionx + "," + positionz + "," + f_d + "," + r_d + "," + comp1 + "," + comp2 + "," + gyro_dy f.write(s) f.write('\n') #simulation time t = supervisor.getTime() t2 = supervisor.getTime() #motion scenario case = 1 if case == 1: while supervisor.getTime() - t < 1: if supervisor.step(TIME_STEP) == -1: quit() left_motor.setVelocity(-1.5 * (supervisor.getTime() - t)) right_motor.setVelocity(-1.0 * (supervisor.getTime() - t)) # read sensors front_dist = front_sensor.getValue()
trans_field = soccerball.getField("translation") ball_radius = 0.113# soccerball.getField("radius") INITIAL_TRANS = [0, ball_radius, 0] # Emitter setup emitter = supervisor.getEmitter('emitter') emitter.setChannel(COMM_CHANNEL) timeLastMessage = -1 while supervisor.step(TIMESTEP) != -1: values = trans_field.getSFVec3f() # Emit ball position if supervisor.getTime() > timeLastMessage + 1: message = struct.pack("ddd", values[0], values[1], values[2]) emitter.send(message) timeLastMessage = int(supervisor.getTime()) # determine out of bounds64 if values[0] > 5: trans_field.setSFVec3f([5, ball_radius, values[2]]) soccerball.resetPhysics() elif values[0] < -5: trans_field.setSFVec3f([-5, ball_radius, values[2]]) soccerball.resetPhysics() elif values[2] > 3: trans_field.setSFVec3f([values[0], ball_radius, 3]) soccerball.resetPhysics() elif values[2] < -3:
KI = float(os.environ.get('I_GAIN', '100.5')) KD = float(os.environ.get('D_GAIN', '0')) integral = 0.0 previous_position = 0.0 # Initialize the robot speed (left wheel, right wheel). leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) # Main loop: perform a simulation step until the simulation is over. while robot.step(timestep) != -1: # Read the sensor measurement. position = ps.getValue() # Stop the robot when the pendulum falls. if math.fabs(position) > math.pi * 0.5 or robot.getTime() > 180.0: leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) print("Score: %lf" % robot.getTime()) print("P_GAIN: %lf" % KP) print("I_GAIN: %lf" % KI) print("D_GAIN: %lf" % KD) with open('result_%lf_%lf_%lf.txt' % (KP, KI, KD), 'w') as f: f.write('%lf %lf %lf %lf' % (robot.getTime(), KP, KI, KD)) robot.simulationQuit(0) # PID control. integral = integral + (position + previous_position) * 0.5 / timestep derivative = (position - previous_position) / timestep speed = KP * position + KI * integral + KD * derivative
class Botsy(): # 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 and right motors [-1, 1]. def turn_right(self, speed): self.set_left_motor(speed) self.set_right_motor(-speed) def turn_left(self, speed): self.set_left_motor(-speed) self.set_right_motor(speed) # 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]) return (position[0], position[2], orientation) # Returns the position of the green balls def get_green_balls(self): balls = [] balls_root = self.robot.getFromDef("_balls").getField("children") green_balls = balls_root.getMFNode(1).getField("children") for i in range(green_balls.getCount()): ball = green_balls.getMFNode(i) position = ball.getPosition() balls.append([position[0], position[1]]) # Returns the position of the green balls def get_yellow_balls(self): balls = [] balls_root = self.robot.getFromDef("_balls").getField("children") green_balls = balls_root.getMFNode(1).getField("children") yellow_balls = balls_root.getMFNode(0).getField("children") for i in range(yellow_balls.getCount()): ball = yellow_balls.getMFNode(i) position = ball.getPosition() balls.append([position[0], position[1]]) return balls
def toggle_visibility_for_all_parts(visible): for part in phone_part_objects: part.setVisibility(camera_node, visible) def transform_image(img_array): np_img = np.array(img_array, dtype=np.uint8) np_img = np_img.transpose((1, 0, 2)) np_img_bgr = cv2.cvtColor(np_img, cv2.COLOR_RGB2BGR) return np_img_bgr randomize_phone_parts() wait_time = 3.5 # seconds last_run_time = supervisor.getTime() take_image = True is_first_run = True images_to_take = 200 image_index = 0 camera.enable(1) supervisor.step(1) toggle_visibility_for_all_parts(False) supervisor.step(1) background_img = transform_image(camera.getImageArray()) cv2.imwrite('background.png', background_img) toggle_visibility_for_all_parts(True) restore_colors() camera.disable() dataset_path = 'dataset'
motors = [] for motorName in [ 'A motor', 'B motor', 'C motor', 'D motor', 'E motor', 'F motor' ]: motor = supervisor.getMotor(motorName) motor.setVelocity(1.0) motors.append(motor) # Get the arm and target nodes. target = supervisor.getFromDef('TARGET') arm = supervisor.getSelf() # Loop 1: Draw a circle on the paper sheet. print('Draw a circle on the paper sheet...') while supervisor.step(timeStep) != -1: t = supervisor.getTime() # Use the circle equation relatively to the arm base as an input of the IK algorithm. x = 0.25 * math.cos(t) + 1.1 y = 0.25 * math.sin(t) - 0.95 z = 0.23 # Call "ikpy" to compute the inverse kinematics of the arm. ikResults = armChain.inverse_kinematics([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]]) # Actuate the 3 first arm motors with the IK results. for i in range(3): motors[i].setPosition(ikResults[i + 1]) # Keep the hand orientation down. motors[4].setPosition(-ikResults[2] - ikResults[3] + math.pi / 2)
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))
positionx = str(position[0]) positionz = str(position[2]) f_d = str(front_dist) r_d = str(right_dist) comp1 = str(compass_data[0]) comp2 = str(compass_data[2]) gyro_dx = str(gyro_data[0]) gyro_dy = str(gyro_data[1]) gyro_dz = str(gyro_data[2]) s = positionx + "," + positionz + "," + f_d + "," + r_d + "," + comp1 + "," + comp2 + "," + gyro_dy f.write(s) f.write('\n') #simulation time t = supervisor.getTime() t2 = supervisor.getTime() #motion scenario case = 1 if case == 1: while supervisor.getTime() - t < 1: if supervisor.step(TIME_STEP) == -1: quit() left_motor.setVelocity((supervisor.getTime() - t) * -0.5) right_motor.setVelocity((supervisor.getTime() - t) * -2.0) # read sensors front_dist = front_sensor.getValue()
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))
elif action == 'color': h = value[1].lstrip('#') color = [int(h[i:i + 2], 16) / 255.0 for i in (0, 2, 4)] for i in range(2, len(value)): pointRepresentations[value[i]]['color'].setSFColor(color) elif action == 'graphs': if value[2] == 'true': enableValueGraphs.append(value[1]) else: enableValueGraphs.remove(value[1]) elif action == 'body_transparency': bodyTransparency = float(value[1]) bodyTransparencyField.setSFFloat(bodyTransparency) elif action == 'speed': playbackSpeed = float(value[1]) offsetTime = supervisor.getTime() totalFrameCoutner = 0 else: print(message) message = supervisor.wwiReceiveText() # play the required frame (if needed) step = int(playbackSpeed * (supervisor.getTime() - offsetTime) / frameStep - totalFrameCoutner) if step > 0: toSend = '' frame = frameAndPoints[frameCoutner][0] points = frameAndPoints[frameCoutner][1] # update the GRF visualization for grf in grfList: index1 = labels.index(grf['name'] + 'Force') index2 = labels.index(grf['name'] + 'Moment')
while (robot.step(timestep) != -1): # Get robot pose values coord = gps.getValues() bearing = compass.getValues() pose_x = coord[0] pose_y = coord[2] pose_theta = -math.atan2(bearing[0], bearing[2]) + math.pi / 2 #-1.5708) # Initial state: robot moves into position to grab the repair materials if state == 'lower arm': gripper.release() arm.pick_up() if robot.getTime() > 3.0: state = 'grab' # Second state: robot grabs repair materials from the 'shelf' elif state == 'grab': pose_x = gps.getValues()[0] pose_y = gps.getValues()[2] dist_error = math.sqrt( math.pow(pose_x - current_waypoint[0], 2) + math.pow(pose_y - current_waypoint[1], 2)) base.base_forwards(dist_error * x_gain) if dist_error <= 0.01: base.base_stop() gripper.grip()
box1 = robot.getFromDef("box1") box2 = robot.getFromDef("box2") print(box1.getPosition()) print(robot_node.getPosition()) print(robot_node.getOrientation()) print((robot_node.getField("rotation")).getSFRotation()) r = R.from_rotvec(np.reshape(robot_node.getOrientation(), (3, 3))) print(r) print(r.as_euler('zxy')) trans_field = robot_node.getField("translation") # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) print(timestep) print(robot.getTime()) # You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # motor = robot.getMotor('motorname') # ds = robot.getDistanceSensor('dsname') # print(robot) # ds.enable(timestep) # print(robot.getControllerArguments()) m = (robot_node.getField("translation")).getSFVec3f() x = round(-m[0] + 2.5 - 0.5) y = round(m[2] + 2.5 - 0.5) print("->", m, x, y)
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))
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}
# Message to indicate that data has correctly been taken objectPlacementOutput.setSFString("received") # Get the message in from the robot window(if there is one) message = supervisor.wwiReceiveText() # If there is a message if message != "": # split into parts parts = message.split(",") # If there are parts if len(parts) > 0: if parts[0] == "run": # Start running the match currentlyRunning = True lastTime = supervisor.getTime() gameStarted = True if parts[0] == "pause": # Pause the match currentlyRunning = False if parts[0] == "reset": # Reset the simulation supervisor.simulationReset() simulationRunning = False # Restart this supervisor mainSupervisor.restartController() # Send the update information to the robot window supervisor.wwiSendText("update," + str(robot0Obj.getScore()) + "," + str(robot1Obj.getScore()) + "," + str(timeElapsed))
positionx = str(position[0]) positionz = str(position[2]) f_d = str(front_dist) r_d = str(right_dist) comp1 = str(compass_data[0]) comp2 = str(compass_data[2]) gyro_dx = str(gyro_data[0]) gyro_dy = str(gyro_data[1]) gyro_dz = str(gyro_data[2]) s = positionx + "," + positionz + "," + f_d + "," + r_d + "," + comp1 + "," + comp2 + "," + gyro_dy f.write(s) f.write('\n') #simulation time t = supervisor.getTime() t2 = supervisor.getTime() #motion scenario case = 1 if case == 1: while supervisor.getTime() - t < 1: if supervisor.step(TIME_STEP) == -1: quit() left_motor.setVelocity((supervisor.getTime() - t) * -0.5) right_motor.setVelocity((supervisor.getTime() - t) * -1.0) # read sensors front_dist = front_sensor.getValue()