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()
