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