def __init__(self):
# Initialize the driver.
super(Driver, self).__init__()
# Instantiate emitter node to send data to receivers.
self.emitter = self.getDevice('emitter_supervisor')
# Instantiate receiver node to obtain data from emitters.
self.receiver = self.getDevice('receiver_supervisor')
self.receiver.enable(self.TIME_STEP)
# Create list for emitted message.
# ========== AGV ROBOTS ==========
# Instantiate the AGV robots.
self.AGV_ROBOT_1 = self.getFromDef('AGV_ROBOT1')
self.robot_status = False
# Instantiate suction gripper
self.SUCTION_CUP = self.getFromDef('SUCTION_CUP')
self.MAX_NUM_PALLETS = 13
self.AGV_PALLET_NO = (self.MAX_NUM_PALLETS - 1)
self.create_pallets(num_pallets=self.MAX_NUM_PALLETS)
self.picked_packets = []
# Set initial GPS values for storing of updated packet positions.
self.current_gps_pos = [0, 0, 0]
self.previous_gps_pos = [0, 0, 0]
# Enable the keyboard.
self.kb = Keyboard()
self.kb.enable(self.TIME_STEP)
self.run()
def start_simulation(self):
robot = Robot()
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
motor = robot.getMotor('propeller')
visual = robot.getMotor('visual')
sensor = robot.getPositionSensor('sensor')
sensor.enable(TIME_STEP)
motor.setVelocity(100)
motor.setPosition(self.initial_position)
visual.setVelocity(0)
visual.setPosition(float('inf'))
visual.setTorque(100)
while robot.step(TIME_STEP) != -1:
if not self.initial_position_reached(sensor):
continue
else:
motor.setVelocity(0)
motor.setPosition(float('inf'))
self.current_angle = sensor.getValue()
motor.setTorque(self.motor_velocity * self.length +
self.disturbance * (random.random() - 0.5))
visual.setVelocity(150 * self.motor_velocity)
def start_simulation(self):
self.total_energy = 0
robot = Robot()
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
#camera = robot.getCamera('view')
#camera.enable(TIME_STEP)
motor = robot.getMotor('motor')
sun_motor = robot.getMotor('sun_motor')
panel_sensor = robot.getPositionSensor('panel_sensor')
sun_sensor = robot.getPositionSensor('sun_sensor')
panel_sensor.enable(TIME_STEP)
sun_sensor.enable(TIME_STEP)
if self.moving_sun:
sun_motor.setVelocity(self.sun_velocity)
sun_motor.setPosition(self.max_sun_position)
else:
sun_motor.setVelocity(10)
random_pos = random.random() * math.pi - math.pi / 2
sun_motor.setPosition(0)
while (abs(sun_sensor.getValue() - random_pos) > 1e-2):
time.sleep(0.1)
printed = False
t = 0
i = 0
while robot.step(TIME_STEP) != -1:
sun_position = sun_sensor.getValue()
panel_position = panel_sensor.getValue()
self.sun_position = sun_position
self.panel_position = panel_position
if list(self.controller_output):
self.set_torque(self.controller_output[i])
if (self.moving_sun
and abs(sun_position - self.max_sun_position) < 1e-1):
if not printed:
print(f"Energy harvested: {self.total_energy} J")
printed = True
continue
motor.setTorque(self.motor_torque + self.disturbance *
(random.random() - 0.5))
self.total_energy += max(
[0, 1 - abs(panel_position - sun_position)]) * TIME_STEP
t += TIME_STEP * 1e-3
i += 1
def start_simulation(self):
robot = Robot()
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
motor = robot.getMotor('propeller')
visual = robot.getMotor('visual')
motor.setVelocity(0)
motor.setPosition(float('inf'))
visual.setVelocity(0)
visual.setPosition(float('inf'))
visual.setTorque(100)
while robot.step(TIME_STEP) != -1:
motor.setTorque(-self.motor_velocity * self.length)
visual.setVelocity(150 * self.motor_velocity)
def handle_gui(self):
if not self.keyboard_activated:
self.keyboard = Keyboard()
self.keyboard.enable(100)
self.keyboard_activated = True
key = self.keyboard.getKey()
if key == ord('R'):
self.reset()
elif key == ord('P'):
self.set_initial_poses()
elif key == Keyboard.SHIFT + ord('R'):
try:
self.reset_ball()
except AttributeError:
print("No ball in simulation that can be reset")
return key
def __init__(self):
# Initialize Robot
self.robot = Robot()
self.time_step = int(self.robot.getBasicTimeStep())
print("Robot time step ", self.time_step)
# Initialize sensors
self.metacamera = self.robot.getCamera("MultiSense S21 meta camera")
self.leftcamera = self.robot.getCamera("MultiSense S21 left camera")
self.rightcamera = self.robot.getCamera("MultiSense S21 right camera")
self.rangeFinder = self.robot.getRangeFinder(
"MultiSense S21 meta range finder")
self.cameras = {
"left": self.leftcamera,
"right": self.rightcamera,
"meta": self.metacamera,
"depth": self.rangeFinder
}
self.leftpositionsensor = self.robot.getPositionSensor(
'back left wheel sensor')
self.rightpositionsensor = self.robot.getPositionSensor(
'back right wheel sensor')
self.keyboard = Keyboard()
self.pen = self.robot.getPen('pen')
# Enable sensors
self.metacamera.enable(self.time_step)
self.leftcamera.enable(self.time_step)
self.rightcamera.enable(self.time_step)
self.rangeFinder.enable(self.time_step)
self.leftpositionsensor.enable(self.time_step)
self.rightpositionsensor.enable(self.time_step)
self.keyboard.enable(self.time_step)
# Initialize wheels
backrightwheel = self.robot.getMotor('back right wheel')
backleftwheel = self.robot.getMotor('back left wheel')
frontrightwheel = self.robot.getMotor('front right wheel')
frontleftwheel = self.robot.getMotor('front left wheel')
self.wheels = [
backrightwheel, backleftwheel, frontrightwheel, frontleftwheel
]
self.set_velocity_control()
self.prevlspeed = 0
self.prevrspeed = 0
self.curlspeed = 0
self.currspeed = 0
def start_simulation(self):
self.total_energy = 0
robot = Robot()
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
camera = robot.getCamera('view')
camera.enable(TIME_STEP)
motor = robot.getMotor('motor')
sun_motor = robot.getMotor('sun_motor')
panel_sensor = robot.getPositionSensor('panel_sensor')
sun_sensor = robot.getPositionSensor('sun_sensor')
panel_sensor.enable(TIME_STEP)
sun_sensor.enable(TIME_STEP)
if self.moving_sun:
sun_motor.setVelocity(0.3)
sun_motor.setPosition(self.max_sun_position)
else:
sun_motor.setVelocity(10)
random_pos = random.random() * math.pi - math.pi / 2
sun_motor.setPosition(random_pos)
while (abs(sun_sensor.getValue() - random_pos) > 1e-2):
time.sleep(0.1)
start_time = time.time()
printed = False
while robot.step(TIME_STEP) != -1:
camera.getImage()
sun_position = sun_sensor.getValue()
panel_position = panel_sensor.getValue()
if (self.moving_sun and abs(sun_position - self.max_sun_position) < 1e-1)\
or (not self.moving_sun and time.time() - start_time > self.time_limit):
if not printed:
print("Energy harvested:", self.total_energy, "J")
printed = True
continue
motor.setTorque(self.motor_torque)
self.total_energy += max(
[0, 1 - abs(panel_position - sun_position)])
class KeyboardPrinter(SupervisorEnv):
def __init__(self, controller):
self.controller = controller
self.keyboard = Keyboard()
self.keyboard.enable(self.controller.get_timestep())
def step(self, action):
observation, reward, isDone, info = self.controller.step(action)
key = self.keyboard.getKey()
# DEBUG CONTROLS
if key == Keyboard.CONTROL + ord("A"):
print()
print("Actions: ", action)
if key == Keyboard.CONTROL + ord("R"):
print()
print("Rewards: ", reward)
if key == Keyboard.CONTROL + ord("Y"):
print()
print("Observations: ", self.controller.observation)
return observation, reward, isDone, info
def is_done(self):
isDone = self.controller.is_done()
if isDone:
print("Done")
return isDone
def get_observations(self):
return self.controller.get_observations()
def get_reward(self, action):
return self.controller.get_reward(action)
def get_info(self):
return self.controller.get_info()
def reset(self):
print("RESET")
observations = self.controller.reset()
return observations
class Driver(Supervisor):
TIME_STEP = 32
x = int(0.0)
y = int(0.0)
z = int(0.0)
translation = [x, y, z]
# Instantiated objects
AGV_ROBOT_1 = None
SUCTION_CUP = None
#agv_robot_1_translation_field = agv_robot_1.getField('translation')
#suction_cup = self.getFromDef('suction_cup')
#box_1 =
# Variables for AGV, pallets and packets
picking_started = False
picking_finished = False
robot_status = False
MAX_NUM_PALLETS = None
AGV_PALLET_NO = None
count_now = False
packet_removed = False
packet_added = False
# Pallets
pallet_list = None
pallet_num = None
# Packets
picked_packets = None
packet_list = None
packet_num = None
agv_packet_num = None
# Variables to hold and update the location of packets.
current_gps_pos = None
previous_gps_pos = None
# ----------------- COMMUNICATION ---------------
# Instantiate receiver node to obtain data from emitters.
emitter = None
receiver = None
# Message sent by emitter.
emitted_message = None
previous_message = None
# ------------------- KEYBOARD --------------
# # Enable the keyboard.
# kb = Keyboard()
# kb.enable(TIME_STEP)
# Keyboard values for AGV
FORWARD = '315'
BACKWARD = '317'
TURN_LEFT = '314'
TURN_RIGHT = '316'
SPEED_INCREASE_AGV = '43'
SPEED_DECREASE_AGV = '45'
# Keyboard values for Hasselhoff Hug - Axis 1
LEFT_AXIS_1 = '65'
RIGHT_AXIS_1 = '68'
SPEED_INCREASE_SNAKEBOX = str(Keyboard.CONTROL + Keyboard.SHIFT + 43)
SPEED_DECREASE_SNAKEBOX = str(Keyboard.CONTROL + Keyboard.SHIFT + 45)
# Keyboard values for Tower - Axis 2
UP_AXIS_2 = '87'
DOWN_AXIS_2 = '83'
SPEED_INCREASE_TOWER = str(Keyboard.CONTROL + 43)
SPEED_DECREASE_TOWER = str(Keyboard.CONTROL + 45)
# Keyboard values for Snake - Axis 3
EXTEND_AXIS_3 = '81'
RETRACT_AXIS_3 = '69'
SPEED_INCREASE_SNAKE = str(Keyboard.SHIFT + 43)
SPEED_DECREASE_SNAKE = str(Keyboard.SHIFT + 45)
# Keyboard values for Snake tip - Axis 4
LEFT_AXIS_4 = '90'
RIGHT_AXIS_4 = '67'
# Keyboard values for Packet handling
PACKET_ATTACH = '32'
PACKET_DETACH = '65568'
def __init__(self):
# Initialize the driver.
super(Driver, self).__init__()
# Instantiate emitter node to send data to receivers.
self.emitter = self.getDevice('emitter_supervisor')
# Instantiate receiver node to obtain data from emitters.
self.receiver = self.getDevice('receiver_supervisor')
self.receiver.enable(self.TIME_STEP)
# Create list for emitted message.
# ========== AGV ROBOTS ==========
# Instantiate the AGV robots.
self.AGV_ROBOT_1 = self.getFromDef('AGV_ROBOT1')
self.robot_status = False
# Instantiate suction gripper
self.SUCTION_CUP = self.getFromDef('SUCTION_CUP')
self.MAX_NUM_PALLETS = 13
self.AGV_PALLET_NO = (self.MAX_NUM_PALLETS - 1)
self.create_pallets(num_pallets=self.MAX_NUM_PALLETS)
self.picked_packets = []
# Set initial GPS values for storing of updated packet positions.
self.current_gps_pos = [0, 0, 0]
self.previous_gps_pos = [0, 0, 0]
# Enable the keyboard.
self.kb = Keyboard()
self.kb.enable(self.TIME_STEP)
self.run()
# ========== PACKET ROBOTS ==========
# Instantiate the active packets/boxes.
# Continuously reads the keyboard for user inputs.
# Reads up to 7 simultaneous/combined key presses.
def read_keyboard(self):
# ------ KEYBOARD -----
# https://cyberbotics.com/doc/reference/keyboard
# Register keystrokes
keystrokes = [str(-1)] * 7
for i in range(0, 7):
keystrokes[i] = str(self.kb.getKey())
# print(keystrokes)
return keystrokes
def error_testing_thingy(self, num):
assert isinstance(num, int), INVALID_NUM_MSG
def instantiate_packets(self):
try:
pass
except Exception as e:
e = traceback.format_exc()
print("Error: ", e)
# Returns the distance of two objects as coordinate system.
# Distance calculated as difference between object 1 and object 2.
def dist_two_objects(self, obj_1, obj_2):
obj_1_pos = self.get_position(obj_1)
obj_2_pos = self.get_position(obj_2)
# Object distance in millimeters
dist = [0] * 3
dist[0] = obj_1_pos[0] - obj_2_pos[0]
dist[1] = obj_2_pos[1] - obj_1_pos[1]
dist[2] = obj_1_pos[2] - obj_2_pos[2]
return dist
def get_translation(self, object):
return object.getField('translation').getSFVec3f()
def get_position(self, object):
return object.getPosition()
def get_rotation(self, object):
return object.getOrientation()
def populate_packets(self, no_packets):
for i in no_packets:
pass
def create_pallets(self, num_pallets):
# Create pallet list.
self.pallet_list = [""] * num_pallets
for i in range(num_pallets):
pallet_name = "PALLET_" + str(i+1)
pallet_obj = self.getFromDef(pallet_name)
pallet = Pallet(pallet=pallet_obj, length=1.200, width=0.800, height=0.155)
self.pallet_list[i] = pallet
print("Pallet position ", i+1, ": ", self.pallet_list[i].get_pallet_position())
def place_pallets(self):
#pallets = self.create_pallets(num_pallets=12)
#self.pallet_list
pass
def get_pallet_pos(self, pallet_num):
pass
def get_packet_level(self, packet_num, packet_size, pallet_num):
max_packets_level = 9
def set_small_dummy_pallets(self, pallet_num, num_packets, packet_size):
# Set number of dummy packets.
length = 0
width = 0
height = 0
if packet_size == "small":
length = 0.300
width = 0.200
height = 0.180
elif packet_size == "large":
length = 0.600
width = 0.400
height = 0.369
else:
packet_size = "small"
length = 0.300
width = 0.200
height = 0.180
for i in range(num_packets):
packet_name = "PACKET_" + str(i+1)
packet = Packet(length, width, height, packet_size, packet_name)
self.pallet_list[pallet_num-1].add_packet(packet=packet)
# Populate with packets
# packet1 = Packet(0.300, 0.200, 0.180, "small")
# packet2 = Packet(0.300, 0.200, 0.180, "small")
# packet3 = Packet(0.300, 0.200, 0.180, "small")
# packet4 = Packet(0.300, 0.200, 0.180, "small")
# packet5 = Packet(0.300, 0.200, 0.180, "small")
# packet6 = Packet(0.300, 0.200, 0.180, "small")
# packet7 = Packet(0.300, 0.200, 0.180, "small")
# packet8 = Packet(0.300, 0.200, 0.180, "small")
# packet9 = Packet(0.300, 0.200, 0.180, "small")
# packet10 = Packet(0.300, 0.200, 0.180, "small")
# packet11 = Packet(0.300, 0.200, 0.180, "small")
# packet12 = Packet(0.300, 0.200, 0.180, "small")
# packet13 = Packet(0.300, 0.200, 0.180, "small")
# packet14 = Packet(0.300, 0.200, 0.180, "small")
# packet15 = Packet(0.300, 0.200, 0.180, "small")
# packet16 = Packet(0.300, 0.200, 0.180, "small")
# packet17 = Packet(0.300, 0.200, 0.180, "small")
# packet18 = Packet(0.300, 0.200, 0.180, "small")
# self.pallet_list[0].add_packet(packet=packet1)
# self.pallet_list[0].add_packet(packet=packet2)
# self.pallet_list[0].add_packet(packet=packet3)
# self.pallet_list[0].add_packet(packet=packet4)
# self.pallet_list[0].add_packet(packet=packet5)
# self.pallet_list[0].add_packet(packet=packet6)
# self.pallet_list[0].add_packet(packet=packet7)
# self.pallet_list[0].add_packet(packet=packet8)
# self.pallet_list[0].add_packet(packet=packet9)
# self.pallet_list[0].add_packet(packet=packet10)
# self.pallet_list[0].add_packet(packet=packet11)
# self.pallet_list[0].add_packet(packet=packet12)
# self.pallet_list[0].add_packet(packet=packet13)
# self.pallet_list[0].add_packet(packet=packet14)
# self.pallet_list[0].add_packet(packet=packet15)
# self.pallet_list[0].add_packet(packet=packet16)
# self.pallet_list[0].add_packet(packet=packet17)
# self.pallet_list[0].add_packet(packet=packet18)
# def check_dist
def manual_control(self, keystrokes):
# ------ MOVE AGV -----
# Increment AGV speed
# self.increment_speed_agv(keystrokes=keystrokes)
# Move AGV
# self.emitter.send()
# self.AGV_ROBOT_1.move_agv()
# self.move_agv(keystrokes=keystrokes)
# ------ HASSELHOFF HUG / SNAKE BOX - AXIS 1 -----
# Increment Axis 1 speed
# self.increment_speed_snakebox(keystrokes=keystrokes)
# Rotate snake box - Axis 1
# self.rotate_snakebox(keystrokes=keystrokes)
# ------ TOWER - AXIS 2 -----
# Increment Tower speed
# self.increment_speed_tower(keystrokes=keystrokes)
# Move tower height
# self.change_tower_height(keystrokes=keystrokes)
# ------ SNAKE - AXIS 3 -----
# Increment Snake speed
# self.increment_speed_snake_manual(keystrokes=keystrokes)
# Move snake part 1
# self.move_snake_manual(keystrokes=keystrokes)
pass
# ----------------- COMMUNICATION ---------------
# Receive message through receiver sent from emitter.
# Received in utf-8 format.
def receive_message(self):
message = ""
if self.receiver.getQueueLength() > 0:
message = self.receiver.getData().decode('utf-8')
self.receiver.nextPacket()
# Splits the message.
message = message.split()
return message
# Send message from emitter.
# Sent in utf-8 format.
def send_message(self, message):
# message_sent = False
if message != '':# and message != self.previous_message:
self.previous_message = message
# message_packed = struct.pack(message)
# self.emitter.send(message_packed)
self.emitter.send(message.encode('utf-8'))
# message_sent = True
# return message_sent
def send_message_struct(self, message_list):
# messages = [i for i in range(0,7)]
# buffer = struct.pack('%sf' % len(message_list), *message_list)
# var = struct.pack('hhl', 'test')
s = struct.Struct('4s 4s 4s 4s 4s 4s 4s')
packed = s.pack(message_list.encode('utf-8'))
return packed
def update_picked_packet_positions(self):
updated = False
if len(self.picked_packets) == 0:
updated = False
else:
for i in range(len(self.picked_packets)):
updated_pos = [0, 0, 0]
# Get the packet and current position.
packet_num = self.picked_packets[i]
# print("Packet number: ", packet_num)
packet_name = "PACKET_" + str(packet_num)
packet = self.getFromDef(packet_name)
current_pos = packet.getField('translation').getSFVec3f()
# Get difference in current and previous gps position.
gps_diff_x = self.current_gps_pos[0] - self.previous_gps_pos[0]
gps_diff_y = self.current_gps_pos[1] - self.previous_gps_pos[1]
gps_diff_z = self.current_gps_pos[2] - self.previous_gps_pos[2]
gps_pos = [gps_diff_x, gps_diff_y, gps_diff_z]
# Get updated packet position.
for i in range(3):
updated_pos[i] = current_pos[i] + gps_pos[i]
# updated_pos_x = current_pos[0] + gps_pos[0]
# updated_pos_y = current_pos[1] + gps_pos[1]
# updated_pos_z = current_pos[2] + gps_pos[2]
# Set updated packet position.
packet.getField('translation').setSFVec3f(updated_pos)
updated = True
print("Updated position for: ", self.picked_packets)
return updated
# Convert list to string. List shall contain
def list_to_string(self, list_elem):
text_string = ' '.join([str(elem) for elem in list_elem])
return text_string
def run(self):
# Set initial conditions.
self.pallet_num = 0
self.packet_num = 30
self.agv_packet_num = 0
# self.picking_started = True
# Populate pallets with packets.
self.set_small_dummy_pallets(pallet_num=(self.pallet_num + 1), num_packets=self.packet_num, packet_size="small") # Create a dummy pallet.
#pallet1 = Pallet(pallet=self.pallet_list[0],length=1200, width=800, height=145)
#pallet1.add_packet(packet1)
#pallet1.add_packet(packet2)
# print("Number of packets: ", self.pallet_list[0].get_num_packets())
# print("Packet num 1: ", self.pallet_list[0].get_packet(1))
# print("Packets: ", self.pallet_list[self.pallet_num].get_packets())
# print("Num packets: ", sum(1 for _ in filter(None.__ne__, self.pallet_list[0].get_packets())))
# print("Num packets: ", sum(x is not None for x in self.pallet_list[0].get_num_packets()))
# print("Num packets: ", self.pallet_list[self.AGV_PALLET_NO].get_num_packets())
# print("Num packets pallet 0: ", self.pallet_list[0].get_num_packets())
# print("Packets pallet 0: ", self.pallet_list[0].get_packets())
# print("Num packets pallet 1: ", self.pallet_list[1].get_num_packets())
# print("Packets pallet 1: ", self.pallet_list[1].get_packets())
# print("Num packets pallet 2: ", self.pallet_list[2].get_num_packets())
# print("Packets pallet 2: ", self.pallet_list[2].get_packets())
# print("Packet deleted: ", self.pallet_list[0].remove_packet(1))
# print("Packets: ", self.pallet_list[0].get_packets())
# Position data for connecting packets.
positioned = [False for i in range(3)]
error_margin = [0] * 3
error_margin[0] = 0.1
error_margin[1] = 0.025
error_margin[2] = 0.05
# Robot mode selection
mode_selection = 2
mode = self.robot_mode(mode_selection)
# Initial robot state
# self.state_machine("")
# Main loop:
# while self.step(timestep) != -1:
while True:
# Read keyboard values
keystrokes = self.read_keyboard()
# Update GPS positions.
self.current_gps_pos = self.getFromDef('body_agv').getPosition()
# print("Current gps pos of AGV: ", self.current_gps_pos)
# print("==================")
# print("AGV position 2: ", self.get_position(self.AGV_ROBOT_1))
# print("Suction cup position: ", self.get_position(self.SUCTION_CUP))
# print("==================")
dist = self.dist_two_objects(self.AGV_ROBOT_1, self.SUCTION_CUP)
# print("Difference AGV and Snake tip: ", dist)
# print(self.get_position(self.SUCTION_CUP))
# test = self.getFromDef('body_agv')
# print("Parent node: ", test.getPosition())
suction_cup_pos = self.get_position(self.SUCTION_CUP)
# packet_picked_pos_init = self.pallet_list[0].get_packet_position(16)
# print("Suction cup position: ", suction_cup_pos)
# print("Packet picked position: ", packet_picked_pos_init)
# print(positioned)
#self.switch_case(argument=1)
# Runs either manual by keyboard input, or in automatic or remote mode.
if mode == 'Manual mode':
manual_message = ['None'] * 12 # Clear previous message.
message = self.receive_message()
# print("Message received supervisor:", message)
# AGV keyboard input.
if (self.FORWARD in keystrokes): # Drive forwards
if (self.TURN_LEFT in keystrokes): # Turn left
agv_state = 'forward_left'
elif (self.TURN_RIGHT in keystrokes): # Turn right
agv_state = 'forward_right'
else:
agv_state = 'forward'
elif (self.BACKWARD in keystrokes): # Drive backwards
if (self.TURN_LEFT in keystrokes): # Turn left
agv_state = 'backward_left'
elif (self.TURN_RIGHT in keystrokes): # Turn right
agv_state = 'backward_right'
else:
agv_state = 'backward'
elif (self.TURN_LEFT in keystrokes): # Turn left
agv_state = 'left'
elif (self.TURN_RIGHT in keystrokes): # Turn right
agv_state = 'right'
else:
agv_state = 'idle'
manual_message[0] = agv_state
# Tower keyboard input.
if (self.UP_AXIS_2 in keystrokes):
tower_state = 'up'
elif (self.DOWN_AXIS_2 in keystrokes):
tower_state = 'down'
else:
tower_state = 'idle'
manual_message[5] = tower_state
# Snakebox keyboard input.
if (self.LEFT_AXIS_1 in keystrokes):
snakebox_state = 'left'
elif (self.RIGHT_AXIS_1 in keystrokes):
snakebox_state = 'right'
else:
snakebox_state = 'idle'
manual_message[4] = snakebox_state
# Snake keyboard input.
if (self.EXTEND_AXIS_3 in keystrokes):
snake_state = 'extend'
elif (self.RETRACT_AXIS_3 in keystrokes):
snake_state = 'retract'
else:
snake_state = 'idle'
manual_message[6] = snake_state
# Snaketip keyboard input.
if (self.LEFT_AXIS_4 in keystrokes):
snaketip_state = 'left'
elif (self.RIGHT_AXIS_4 in keystrokes):
snaketip_state = 'right'
else:
snaketip_state = 'idle'
manual_message[7] = snaketip_state
# if (self.axis_2_pos > self.motor_axis_2.getMaxPosition()):
# self.axis_2_pos = round(self.motor_axis_2.getMaxPosition(), 2)
# # print("Desired pos: ", self.axis_2_pos)
# # print("Maximum position: ", motor_axis_2.getMaxPosition())
# sys.stderr.write("Axis 2 has reached maximum height.\n")
# elif (self.axis_2_pos < self.motor_axis_2.getMinPosition()):
# self.axis_2_pos = round(self.motor_axis_2.getMinPosition(), 2)
# # print("Min. pos: ", motor_axis_2.getMaxPosition())
# sys.stderr.write("Axis 2 has reached minimum height.\n")
# else:
# self.motor_axis_2.setPosition(self.axis_2_pos)
self.emitted_message = self.list_to_string(manual_message)
elif mode == "Remote mode":
remote_message = ['None'] * 20 # Clear previous message.
message = self.receive_message()
# print("Message received supervisor:", message)
# print("=========================================")
# print("OTHER pallet packets: ", self.pallet_list[self.pallet_num].get_packets())
# print(" -------------- ")
# print("AGV pallet packets: ", self.pallet_list[self.AGV_PALLET_NO].get_packets())
# print("=========================================")
# robot_status = 'true' # message[2].lower()
try:
self.robot_status = message[2].lower()
self.picking_started = True
except:
pass
print("Currently packed: ", self.picked_packets)
if self.packet_num <= 0:
self.picking_finished = True
else:
# Picked packets.
packet_picked_pos_init = self.pallet_list[self.pallet_num].get_packet_position(self.packet_num)
remote_message[8] = packet_picked_pos_init[0]
remote_message[9] = packet_picked_pos_init[1]
remote_message[10] = packet_picked_pos_init[2]
# pallet_num = 0
#
# packet_num = 16
# print("AGV Pallet: ", "Num. packets: ", self.pallet_list[self.AGV_PALLET_NO].get_num_packets(),
# " and stored: ", self.pallet_list[self.AGV_PALLET_NO].get_packets())
# print("Main Pallet: ", "Num. packets: ", self.pallet_list[self.pallet_num].get_num_packets(),
# " and stored: ", self.pallet_list[self.pallet_num].get_packets())
if self.picking_started and not self.picking_finished:
if (self.robot_status == 'true'):
# print("This is remaining packets: ", self.pallet_list[self.pallet_num].get_packets())
if all(positioned):
if (message[0] == 'attach'):
# print("We're about to connect!")
packet_name = self.pallet_list[self.pallet_num].get_packet(self.packet_num).get_name()
packet = self.getFromDef(packet_name)
x = round(suction_cup_pos[0], 7)
y = round(suction_cup_pos[1], 7) - self.pallet_list[self.pallet_num].get_packet(self.packet_num).get_height() / 2
z = round(suction_cup_pos[2], 7)
pos = [x, y, z]
packet.getField('translation').setSFVec3f(pos)
# print("New packet position: ", self.get_position(packet))
# print("Used position: ", pos)
# packet_picked_pos
# setSFVec3f(suction_cup_pos)
remote_message[11] = 'True'
print("Picked packet: ", packet_name)
print("Picked packet number: ", self.packet_num)
# print("We're connected!")
# print("=============")
# Rotate the package in accordance with the snake angle.
gripper_angle = math.radians(float(message[1]))
gripper_angle_list = [0, 1, 0, gripper_angle]
# gripper = self.getFromDef('motor_axis_4')
# gripper_angle_1 = gripper.getField('rotation')
# print("Gripper angle first: ", gripper_angle_1)
# gripper_angle_2 = gripper.getField('rotation').getSFRotation()
# print("Gripper angle first: ", gripper_angle_2)
# gripper_angle = gripper.getSFRotation()
# print("Gripper angle: ", gripper_angle)
packet.getField('rotation').setSFRotation(gripper_angle_list)
# Sets status that packet has been picked.
remote_message[11] = "True"
if not self.packet_added:
# Packet to be moved.
packet = self.pallet_list[self.pallet_num].get_packet(packet_num=self.packet_num)
self.picked_packets.append(self.packet_num)
print("PACKET ADDED: ", packet)
# Find position on new pallet and place packet.
# self.pallet_list[self.AGV_PALLET_NO].add_packet(packet=packet)
self.agv_packet_num += 1
# self.agv_packet_num = self.pallet_list[self.AGV_PALLET_NO].get_num_packets()
# Packet has been added.
self.packet_added = True
if self.packet_added:
# print("AGV pallet packets: ", self.pallet_list[self.AGV_PALLET_NO].get_packets())
# print("Other pallet packets: ", self.pallet_list[self.pallet_num].get_packets())
# Get the next position for the packet - Only preliminary position. AGV to decide final position.
# packet_picked_pos_final = self.pallet_list[self.AGV_PALLET_NO].get_packet_position(self.agv_packet_num)
# remote_message[12] = packet_picked_pos_final[0]
# remote_message[13] = packet_picked_pos_final[1]
# remote_message[14] = packet_picked_pos_final[2]
remote_message[15] = self.agv_packet_num
packet_dimensions = self.pallet_list[self.pallet_num].get_packet_dimensions(self.packet_num)
# packet_dimensions = self.pallet_list[self.AGV_PALLET_NO].get_packet_dimensions(self.agv_packet_num)
remote_message[16] = packet_dimensions[0] # Width
remote_message[17] = packet_dimensions[1] # Length
remote_message[18] = packet_dimensions[2] # Height
# print("Packet dimensions: ", "Width = ", remote_message[16], "Length = ", remote_message[17], "Height = ", remote_message[18])
# Ready to do a count when detaching packet.
self.count_now = True
elif (message[0] == 'detach'):
if self.count_now:
if not self.packet_removed:
# Remove packet from initial pallet.
# self.pallet_list[self.pallet_num].remove_packet(packet_num=self.packet_num)
self.packet_removed = True
self.packet_num -= 1
print("I counted now up to: ", self.packet_num)
self.count_now = False
self.packet_added = False
remote_message[11] = "False"
elif (message[0] == 'idle'):
remote_message[11] = "False"
print("Currently I have: ", self.packet_num, " packets.")
else:
for i in range(3):
# Check x, y and z position
if suction_cup_pos[i] >= (packet_picked_pos_init[i] - error_margin[i]) and suction_cup_pos[i] <= \
(packet_picked_pos_init[i] + error_margin[i]):
positioned[i] = True
else:
positioned[i] = False
else:
print("AGV not ready.")
elif self.picking_finished:
print("Picking order has finished.")
remote_message[0] = 'idle'
else:
print("AGV robot is not ready.")
self.picking_started = False
# Updating packet position on AGV.
if self.update_picked_packet_positions():
print("Packet positions on AGV updated.")
# print("Packet position: ", positioned)
self.emitted_message = self.list_to_string(remote_message)
elif mode == "Automatic mode":
pass
else:
# Print error
sys.stderr.write("No or incorrect mode selected.\n")
# The program will exit
sys.exit("The program will now be terminated.")
pass
# time1 = time.time()
# count = 0
# for i in range(100):
# count += 1
# s = self.emitted_message+str(count)
# self.send_message(s)
# time2 = time.time()
# print("Time 1: ", time1)
# print("Time 2: ", time2)
# print("========")
# print("Keys: ", keystrokes)
# print("Type: ", type(keystrokes[0]))
# print(*keystrokes[0])
# bufferTest = self.send_message_struct(keystrokes)
# Send the message at the end of the iteration.
self.send_message(self.emitted_message)
# Update previous GPS position.
self.previous_gps_pos = self.current_gps_pos
try:
pass
# print(self.pallet_list[0].get_pallet_position())
# print(self.pallet_list[0].get_packet_position(16))
except AttributeError:
print("Pallet number is incorrect.")
# Reset emitted message.
self.emitted_message = ""
# Breaks the simulation.
while self.step(self.TIME_STEP) == -1:
print("============== I'm breaking up! ==============")
break
def switch_case_robot(self, argument):
# Creating a dictionary of the states.
switcher = {
1: self.robot_idle_sc,
2: self.move_agv_sc,
3: self.move_tower_sc,
4: self.rotate_snakebox_sc,
5: self.extend_snake_sc,
6: self.attach_packet_sc,
7: self.detach_packet_sc,
8: self.retract_snake_sc
}
# Get the function from switcher dictionary
func = switcher.get(argument, lambda: "Invalid robot state")
# Execute the function
state = func()
return state
def robot_idle_sc(self):
print("Robot is in idle mode!")
pass
def move_agv_sc(self):
pass
def move_tower_sc(self):
pass
def rotate_snakebox_sc(self):
pass
def extend_snake_sc(self):
pass
def attach_packet_sc(self):
pass
def detach_packet_sc(self):
pass
def retract_snake_sc(self):
pass
# ------ Switch case -----
def manual_mode_sc(self):
return "Manual mode"
def remote_mode_sc(self):
return "Remote mode"
def automatic_mode_sc(self):
return "Automatic mode"
# Set the type of robot mode.
def robot_mode(self, argument):
switcher = {
1: self.manual_mode_sc,
2: self.remote_mode_sc,
3: self.automatic_mode_sc
}
# Get the function from switcher dictionary
func = switcher.get(argument, lambda: "Invalid robot mode")
# Execute the function
mode = func()
return mode
from propeller_keyboard_helper import Propeller_Simulation, TIME_STEP
from controller import Keyboard
import time
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
sim = Propeller_Simulation()
while True:
key = keyboard.getKey()
if key == ord('A'):
sim.set_velocity(0.6)
elif key == ord('D'):
sim.set_velocity(-0.6)
else:
sim.set_velocity(0)
time.sleep(1/TIME_STEP)
def initialize_webots_keyboard(self):
""" Initialize webots keyboard """
self.key_press = Keyboard()
self.key_press.enable(self.TIMESTEP * 25)
def __init__(self, joints):
self.msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""
self.moveBindings = {
'73':(1,0,0,0),
'79':(1,0,0,-1),
'74':(0,0,0,1),
'76':(0,0,0,-1),
'85':(1,0,0,1),
'44':(-1,0,0,0),
'46':(-1,0,0,1),
'77':(-1,0,0,-1),
'65615':(1,-1,0,0),
'65609':(1,0,0,0),
'65610':(0,1,0,0),
'65612':(0,-1,0,0),
'65621':(1,1,0,0),
'65596':(-1,0,0,0),
'65598':(-1,-1,0,0),
'65613':(-1,1,0,0),
'84':(0,0,1,0),
'66':(0,0,-1,0),
}
self.speedBindings = {
'81':(1.1,1.1),
'90':(.9,.9),
'87':(1.1,1),
'88':(.9,1),
'69':(1,1.1),
'67':(1,.9),
}
self.keyboard = Keyboard()
self.keyboard.enable(32)
self.speed = 0.5
self.turn = 1
self.x = 0
self.y = 0
self.th = 0
self.status = 0
self.twist = [0, 0, 0]
# Base parameters
self.A = 74.87/1000 # Wheel width
self.B = 100.0/1000 # Wheel diameter
self.C = 471.0/1000 # Between from and rear wheels
self.D = 300.46/1000 # Base width
self.E = 28.0/1000 # Roll diameter
self.slideRatio = 1
self.joints = joints
print(self.msg)
print("currently:\tspeed %s\tturn %s " % (self.speed, self.turn))
def run(self):
# initate lidar
timeStep = 64
lidar = self.getDevice('LDS-01')
lidar.enable(timeStep)
# lidarWidth = lidar.getHorizontalResolution()
"""Set the Pedestrian pose and position."""
opt_parser = optparse.OptionParser()
opt_parser.add_option("--trajectory", default="", help="Specify the trajectory in the format [x1 y1, x2 y2, ...]")
opt_parser.add_option("--speed", type=float, help="Specify walking speed in [m/s]")
opt_parser.add_option("--step", type=int, help="Specify time step (otherwise world time step is used)")
opt_parser.add_option("--caneMovement", type=int, help="Should the cane move sideways?")
options, args = opt_parser.parse_args()
if not options.trajectory or len(options.trajectory.split(',')) < 2:
print("You should specify the trajectory using the '--trajectory' option.")
print("The trajectory shoulld have at least 2 points.")
return
if options.speed and options.speed > 0:
self.speed = options.speed
if options.step and options.step > 0:
self.time_step = options.step
else:
self.time_step = int(self.getBasicTimeStep())
if options.caneMovement == 0:
self.caneMovement = bool(options.caneMovement)
point_list = options.trajectory.split(',')
self.number_of_waypoints = len(point_list)
self.waypoints = []
for i in range(0, self.number_of_waypoints):
self.waypoints.append([])
self.waypoints[i].append(float(point_list[i].split()[0]))
self.waypoints[i].append(float(point_list[i].split()[1]))
self.root_node_ref = self.getSelf()
self.root_translation_field = self.root_node_ref.getField("translation")
self.root_rotation_field = self.root_node_ref.getField("rotation")
for joint in self.joint_names:
self.joints_position_field.append(self.root_node_ref.getField(joint))
# compute waypoints distance
self.updateWaypointsDistance()
# enable keyboard
keyboard = Keyboard()
keyboard.enable(self.time_step)
# main cycle
while not self.step(self.time_step) == -1:
# lidar
# inf if nothing, outputs a float if there is a solid object in way
imageData = lidar.getRangeImage()
# choose number of partitions and field of view
partitionMinima, partitionBorders = lou.findPartitionMinima(imageData, 5, 180)
print("Border angles of partitions: " + str (partitionBorders))
print("Closest distance for each partition: " + str(["%.2f"%x for x in partitionMinima]))
# choose min and max values for distance to be detected and converted to feedback
# LDS-01 range on manufacturer website: 0.12m ~ 3.5m
feedbackLevels = lou.getFeedbackLevels(partitionMinima, 0.5, 3)
print("Feedback levels for each partition: " + str(["%.2f"%x for x in feedbackLevels]))
if not self.clearLidarFeedback:
self.commandOutput = feedbackLevels
# print table
lou.printFeedbackLevels(self.commandOutput, 5)
# listen for keyboard press
key = keyboard.getKey()
# action: voice recognition
if (key == ord('V')):
self.buttonPressed = True
self.clearLidarFeedback = True
commands = ["where is location one", "check the battery"]
phrases = [(command, 1.0) for command in commands]
locations = {}
locations["location one"] = self.locationOne
command = listen(phrases)
if command != "":
executionOutput = executeCommand(command, locations, {})
if isinstance(executionOutput, int):
self.commandOutput = battery_to_haptic(executionOutput)
elif isinstance(executionOutput, list):
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
currentLocation = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
self.commandOutput = haptic_guide(currentLocation, executionOutput, -currentAngle - self.relativeCaneAngle)
# action: show and go to location
if (key == ord('G')):
self.buttonPressed = True
self.clearLidarFeedback = True
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
currentLocation = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = self.locationOne
self.commandOutput = haptic_guide(currentLocation, newDestination, -currentAngle - self.relativeCaneAngle)
self.add_new_current_waypoint = True
self.new_current_waipoint = newDestination
# action: show location
if (key == ord('H')):
self.buttonPressed = True
self.clearLidarFeedback = True
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
currentLocation = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = self.locationOne
self.commandOutput = haptic_guide(currentLocation, newDestination, -currentAngle - self.relativeCaneAngle)
# action: stop
if (key == ord(' ')):
self.buttonPressed = True
# action: show battery
if (key == ord('B')):
self.buttonPressed = True
self.clearLidarFeedback = True
self.commandOutput = battery_to_haptic(35)
if (key == ord(' ')):
self.buttonPressed = True
# action: turn right 90
if (key == ord('D')):
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = [x + 100 * math.cos(math.radians(-180 - currentAngle)), z + 100 * math.sin(math.radians(-180 - currentAngle))]
self.add_new_current_waypoint = True
self.new_current_waipoint = newDestination
# action: turn right 45
if (key == ord('E')):
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = [x + 100 * math.cos(math.radians(-225 - currentAngle)), z + 100 * math.sin(math.radians(-225 - currentAngle))]
self.add_new_current_waypoint = True
self.new_current_waipoint = newDestination
# action: turn left 90
if (key == ord('A')):
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = [x + 100 * math.cos(math.radians(-currentAngle)), z + 100 * math.sin(math.radians(-currentAngle))]
self.add_new_current_waypoint = True
self.new_current_waipoint = newDestination
# action: turn left 45
if (key == ord('Q')):
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = [x + 100 * math.cos(math.radians(45 - currentAngle)), z + 100 * math.sin(math.radians(45 - currentAngle))]
self.add_new_current_waypoint = True
self.new_current_waipoint = newDestination
# action: turn back 180
if (key == ord('S')):
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = [x + 100 * math.cos(math.radians(-90 - currentAngle)), z + 100 * math.sin(math.radians(-90 - currentAngle))]
self.add_new_current_waypoint = True
self.new_current_waipoint = newDestination
# action: continue further forward 0
if (key == ord('W')):
currentAngle = math.degrees(self.root_rotation_field.getSFRotation()[3])
x, z = [self.root_translation_field.getSFVec3f()[i] for i in [0, 2]]
newDestination = [x + 100 * math.cos(math.radians(-270 - currentAngle)), z + 100 * math.sin(math.radians(-270 - currentAngle))]
self.add_new_current_waypoint = True
self.new_current_waipoint = newDestination
# get front partition that relative to the user
relativeCaneAngle = 0
for i in [7, 8]:
relativeCaneAngle += self.joints_position_field[i].getSFFloat()
self.relativeCaneAngle = math.degrees(relativeCaneAngle)
relativeFrontPartitionIndex = [idx for idx, border in enumerate(partitionBorders) if border > relativeCaneAngle][0] - 1
# following stops on any feedback in any of the patitions
#if any(map(lambda x: x > 0, feedbackLevels)):
if feedbackLevels[relativeFrontPartitionIndex] > self.stopMovingFeedbackLevel or self.buttonPressed:
# stop movement, reset delay
self.stopMoving = True
self.delayMoving = False
self.buttonPressed = False
self.timeNotMoving += self.timeAfterDelayStart
self.delayStartTimestamp = 0
self.timeAfterDelayStart = 0
if self.stopTimestamp == 0:
self.stopTimestamp = self.getTime()
else:
# path is clear
if self.delayMoving:
self.timeAfterDelayStart = self.getTime() - self.delayStartTimestamp
if self.timeAfterDelayStart >= self.resumeMovingDelay:
# delay ended, resume movement
self.timeNotMoving += self.timeAfterDelayStart
self.delayMoving = False
self.clearLidarFeedback = False
self.delayStartTimestamp = 0
self.timeAfterDelayStart = 0
if self.stopMoving:
# path just got cleared, start delay
self.timeNotMoving += self.getTime() - self.stopTimestamp
self.stopMoving = False
self.stopTimestamp = 0
self.delayMoving = True
self.delayStartTimestamp = self.getTime()
# internal total runtime of robot
time = self.getTime() - self.timeNotMoving
if ((not self.stopMoving) and (not self.delayMoving)):
# free to move
current_sequence = int(((time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO) % self.WALK_SEQUENCES_NUMBER)
# compute the ratio 'distance already covered between way-point(X) and way-point(X+1)'
# / 'total distance between way-point(X) and way-point(X+1)'
ratio = (time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO - \
int(((time * self.speed) / self.CYCLE_TO_DISTANCE_RATIO))
# body parts movement
for i in range(0, len(self.angles)):
if i in [3,4,6]:
current_angle = self.angles[i][0]
self.joints_position_field[i].setSFFloat(current_angle)
elif (not self.caneMovement and i in [5,7,8]):
if i in [7,8]:
current_angle = 0
else:
current_angle = self.angles[i][1]
self.joints_position_field[i].setSFFloat(current_angle)
else:
current_angle = self.angles[i][current_sequence] * (1 - ratio) + \
self.angles[i][(current_sequence + 1) % self.WALK_SEQUENCES_NUMBER] * ratio
self.joints_position_field[i].setSFFloat(current_angle)
# adjust height
self.current_height_offset = self.height_offsets[current_sequence] * (1 - ratio) + \
self.height_offsets[(current_sequence + 1) % self.WALK_SEQUENCES_NUMBER] * ratio
# total distance covered
distance = time * self.speed
# subtract distance covered in previous full cycles
relative_distance = distance - int(distance / self.waypoints_distance[self.number_of_waypoints - 1]) * \
self.waypoints_distance[self.number_of_waypoints - 1]
# find current waypoint that we are heading to
for i in range(0, self.number_of_waypoints):
if self.waypoints_distance[i] > relative_distance:
break
# if new waypoint is to be added
if self.add_new_current_waypoint:
self.add_new_current_waypoint = False
self.number_of_waypoints += 2
current_translation = self.root_translation_field.getSFVec3f()
x = current_translation[0]
z = current_translation[2]
self.waypoints = self.waypoints[:i + 1] + [[x, z]] + [self.new_current_waipoint] + self.waypoints[i + 1:]
# calculate the new waypoint distances list
self.updateWaypointsDistance()
# note: relative distance does not need to be updated after waypoint is added
# how much distance is covered between the last and current waypoint
distance_ratio = 0
if i == 0:
distance_ratio = relative_distance / self.waypoints_distance[0]
else:
distance_ratio = (relative_distance - self.waypoints_distance[i - 1]) / \
(self.waypoints_distance[i] - self.waypoints_distance[i - 1])
# new coordinates
x = distance_ratio * self.waypoints[(i + 1) % self.number_of_waypoints][0] + \
(1 - distance_ratio) * self.waypoints[i][0]
z = distance_ratio * self.waypoints[(i + 1) % self.number_of_waypoints][1] + \
(1 - distance_ratio) * self.waypoints[i][1]
root_translation = [x, self.ROOT_HEIGHT + self.current_height_offset, z]
# new angle
angle = math.atan2(self.waypoints[(i + 1) % self.number_of_waypoints][0] - self.waypoints[i][0],
self.waypoints[(i + 1) % self.number_of_waypoints][1] - self.waypoints[i][1])
rotation = [0, 1, 0, angle]
# update position and rotation
self.root_translation_field.setSFVec3f(root_translation)
self.root_rotation_field.setSFRotation(rotation)
def WithNoise(input_vector):
mean = 0
std = 0.005
n = len(input_vector)
noise = np.random.normal(mean,std,n)
return list(np.array(input_vector) + noise)
dir_path = os.path.dirname(os.path.realpath(__file__))
L = Logger()
env = Mitsos(ACTIONS='CONTINUOUS')
state = env.reset()
keyboard = Keyboard() # to control training from keyboard input
keyboard.enable(env.timestep)
# # USING BOTH CAMERA AND IR SENSORS
# n_inputs = 2
# dqn = ComplexDQN
# mem = MemoryDouble
# USING ONLY IR SENSORS
n_inputs = 1
dqn = SimpleDQN
mem = Memory
# # USING ONLY CAMERA
# n_inputs = 1
# dqn = ConvDQN
class BaseController:
def __init__(self, joints):
self.msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""
self.moveBindings = {
'73':(1,0,0,0),
'79':(1,0,0,-1),
'74':(0,0,0,1),
'76':(0,0,0,-1),
'85':(1,0,0,1),
'44':(-1,0,0,0),
'46':(-1,0,0,1),
'77':(-1,0,0,-1),
'65615':(1,-1,0,0),
'65609':(1,0,0,0),
'65610':(0,1,0,0),
'65612':(0,-1,0,0),
'65621':(1,1,0,0),
'65596':(-1,0,0,0),
'65598':(-1,-1,0,0),
'65613':(-1,1,0,0),
'84':(0,0,1,0),
'66':(0,0,-1,0),
}
self.speedBindings = {
'81':(1.1,1.1),
'90':(.9,.9),
'87':(1.1,1),
'88':(.9,1),
'69':(1,1.1),
'67':(1,.9),
}
self.keyboard = Keyboard()
self.keyboard.enable(32)
self.speed = 0.5
self.turn = 1
self.x = 0
self.y = 0
self.th = 0
self.status = 0
self.twist = [0, 0, 0]
# Base parameters
self.A = 74.87/1000 # Wheel width
self.B = 100.0/1000 # Wheel diameter
self.C = 471.0/1000 # Between from and rear wheels
self.D = 300.46/1000 # Base width
self.E = 28.0/1000 # Roll diameter
self.slideRatio = 1
self.joints = joints
print(self.msg)
print("currently:\tspeed %s\tturn %s " % (self.speed, self.turn))
def getKey(self):
key = self.keyboard.getKey()
return str(key)
def vels(self):
return "currently:\tspeed %s\tturn %s " % (self.speed, self.turn)
def getTwist(self):
try:
key = self.getKey()
if key in self.moveBindings.keys():
self.x = self.moveBindings[key][0]
self.y = self.moveBindings[key][1]
self.th = self.moveBindings[key][3]
elif key in self.speedBindings.keys():
self.speed = self.speed * self.speedBindings[key][0]
self.turn = self.turn * self.speedBindings[key][1]
print(self.vels())
if (self.status == 14):
print(self.msg)
self.status = (self.status + 1) % 15
else:
self.x = 0
self.y = 0
self.th = 0
self.twist[0] = self.x*self.speed
self.twist[1] = self.y*self.speed
self.twist[2] = self.th*self.turn
return
except Exception as e:
print(e)
self.twist[0] = 0; self.twist[1] = 0; self.twist[2] = 0;
def getMotorVelocities(self):
velFromX = self.twist[2]/self.B
velFromY = self.twist[1]/(self.B*self.slideRatio)
velFromTheta = self.twist[0] * (self.C + self.D) / self.B
wheelVelocities = [0, 0, 0, 0]
wheelVelocities[0] = velFromX + velFromY + velFromTheta
wheelVelocities[1] = -velFromX - velFromY + velFromTheta
wheelVelocities[2] = velFromX - velFromY + velFromTheta
wheelVelocities[3] = -velFromX + velFromY + velFromTheta
return wheelVelocities;
def rotateBaseMotors(self):
self.getTwist()
#print(self.twist)
wheelVelocities = self.getMotorVelocities()
self.joints[0].setVelocity(wheelVelocities[0])
self.joints[1].setVelocity(wheelVelocities[1])
self.joints[2].setVelocity(wheelVelocities[2])
self.joints[3].setVelocity(wheelVelocities[3])
driver = Driver()
basicTimeStep = int(driver.getBasicTimeStep())
TIME_STEP = 100
UNKNOWN = 99999.9
FILTER_SIZE = 3
steering_angle = 0.0
KP = 0.25
KI = 0.00
KD = 2
gps_coords = [0.0 for i in range(3)]
gps_speed = 0.0
speed = 0
sick_fov = -1.0
keyboard = Keyboard()
def setSpeed(kmh):
global speed
speed = 250.0 if kmh > 250.0 else kmh
print("setting speed to {} km/h".format(speed))
driver.setCruisingSpeed(speed)
def set_autodrive(onoff):
global autodrive
if autodrive == onoff:
return
autodrive = onoff
if autodrive == False:
class CustomRobotClass:
def __init__(self):
# Initialize Robot
self.robot = Robot()
self.time_step = int(self.robot.getBasicTimeStep())
print("Robot time step ", self.time_step)
# Initialize sensors
self.metacamera = self.robot.getCamera("MultiSense S21 meta camera")
self.leftcamera = self.robot.getCamera("MultiSense S21 left camera")
self.rightcamera = self.robot.getCamera("MultiSense S21 right camera")
self.rangeFinder = self.robot.getRangeFinder(
"MultiSense S21 meta range finder")
self.cameras = {
"left": self.leftcamera,
"right": self.rightcamera,
"meta": self.metacamera,
"depth": self.rangeFinder
}
self.leftpositionsensor = self.robot.getPositionSensor(
'back left wheel sensor')
self.rightpositionsensor = self.robot.getPositionSensor(
'back right wheel sensor')
self.keyboard = Keyboard()
self.pen = self.robot.getPen('pen')
# Enable sensors
self.metacamera.enable(self.time_step)
self.leftcamera.enable(self.time_step)
self.rightcamera.enable(self.time_step)
self.rangeFinder.enable(self.time_step)
self.leftpositionsensor.enable(self.time_step)
self.rightpositionsensor.enable(self.time_step)
self.keyboard.enable(self.time_step)
# Initialize wheels
backrightwheel = self.robot.getMotor('back right wheel')
backleftwheel = self.robot.getMotor('back left wheel')
frontrightwheel = self.robot.getMotor('front right wheel')
frontleftwheel = self.robot.getMotor('front left wheel')
self.wheels = [
backrightwheel, backleftwheel, frontrightwheel, frontleftwheel
]
self.set_velocity_control()
self.prevlspeed = 0
self.prevrspeed = 0
self.curlspeed = 0
self.currspeed = 0
def set_velocity_control(self):
for wheel in self.wheels:
wheel.setPosition((float('inf')))
wheel.setVelocity(0)
def step(self):
self.robot.step(self.time_step)
def set_speed(self, l, r):
print("Inside set speed", SPEED_UNIT * l, SPEED_UNIT * r)
if self.prevlspeed != l or self.prevrspeed != r:
self.wheels[0].setVelocity(SPEED_UNIT * r)
self.wheels[1].setVelocity(SPEED_UNIT * l)
self.wheels[2].setVelocity(SPEED_UNIT * r)
self.wheels[3].setVelocity(SPEED_UNIT * l)
self.prevlspeed = self.curlspeed
self.prevrspeed = self.currspeed
self.curlspeed = l
self.currspeed = r
def get_image(self, camera_name, depth_option=False):
if (depth_option == True):
return self.cameras[camera_name].getRangeImageArray()
else:
return self.cameras[camera_name].getImageArray()
"""
Webots controller to manually drive a car using the arrow keys.
Authors: John Shamoon and Wisam Bunni
"""
from numpy import pi
from controller import Keyboard
from vehicle import Driver
ego_vehicle = Driver()
keyboard = Keyboard()
keyboard.enable(int(ego_vehicle.getBasicTimeStep()))
SPEED = 25
FORWARD_RIGHT = pi / 8
FORWARD_LEFT = -FORWARD_RIGHT
BACKWARD_RIGHT = -FORWARD_RIGHT
BACKWARD_LEFT = -FORWARD_LEFT
RIGHT = pi / 4
LEFT = -RIGHT
FORWARD = 0
BACKWARD = -FORWARD
NEUTRAL = 0
SIGNALS = {
(-1, -1): (NEUTRAL, NEUTRAL),
(keyboard.LEFT, -1): (NEUTRAL, LEFT),
(-1, keyboard.LEFT): (NEUTRAL, LEFT),
(keyboard.RIGHT, -1): (NEUTRAL, RIGHT),
(-1, keyboard.RIGHT): (NEUTRAL, RIGHT),
(keyboard.UP, -1): (SPEED, FORWARD),
(-1, keyboard.UP): (SPEED, FORWARD),
def retrieveData():
myClient = pymongo.MongoClient(
"mongodb+srv://test:[email protected]/directions?retryWrites=true&w=majority"
)
myDb = myClient["directions"]
myCol = myDb["directions"]
x = myCol.find()
print(x[0])
TIME_STEP = 64
robot = Robot()
timestep = int(robot.getBasicTimeStep())
keyboard = Keyboard()
keyboard.enable(timestep)
ds = []
dsNames = ['ds_right', 'ds_left']
for i in range(2):
ds.append(robot.getDistanceSensor(dsNames[i]))
ds[i].enable(TIME_STEP)
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
while robot.step(TIME_STEP) != -1:
_, fr = video.read()
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
def __init__(self, robot, n_iterations, parameters, logs="logs/log.npz"):
super(SalamanderCMC, self).__init__()
self.robot = robot
timestep = int(robot.getBasicTimeStep())
self.network = SalamanderNetwork(1e-3*timestep, parameters)
# Position sensors
self.position_sensors = [
self.robot.getPositionSensor('position_sensor_{}'.format(i+1))
for i in range(self.N_BODY_JOINTS)
] + [
self.robot.getPositionSensor('position_sensor_leg_{}'.format(i+1))
for i in range(self.N_LEGS)
]
for sensor in self.position_sensors:
sensor.enable(timestep)
# GPS
self.gps = robot.getGPS("fgirdle_gps")
self.gps.enable(timestep)
# Get motors
self.motors_body = [
self.robot.getMotor("motor_{}".format(i+1))
for i in range(self.N_BODY_JOINTS)
]
self.motors_legs = [
self.robot.getMotor("motor_leg_{}".format(i+1))
for i in range(self.N_LEGS)
]
# Set motors
for motor in self.motors_body:
motor.setPosition(0)
motor.enableForceFeedback(timestep)
motor.enableTorqueFeedback(timestep)
for motor in self.motors_legs:
motor.setPosition(-np.pi/2)
motor.enableForceFeedback(timestep)
motor.enableTorqueFeedback(timestep)
# Iteration counter
self.iteration = 0
# Logging
self.log = ExperimentLogger(
n_iterations,
n_links=1,
n_joints=self.N_BODY_JOINTS+self.N_LEGS,
filename=logs,
timestep=1e-3*timestep,
**parameters
)
#GPS stuff
self.waterPosx = 0
self.NetworkParameters = self.network.parameters
self.SimulationParameters = parameters
self.doTransition = False
self.keyboard = Keyboard()
self.keyboard.enable(samplingPeriod=100)
self.lastkey = 0
def log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(
self.iteration, i,
self.position_sensors[i].getValue()
)
# # Velocity
# self.log.log_joint_velocity(
# self.iteration, i,
# motor.getVelocity()
# )
# Command
self.log.log_joint_cmd(
self.iteration, i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, i,
motor.getTorqueFeedback()
)
for i, motor in enumerate(self.motors_legs):
# Position
self.log.log_joint_position(
self.iteration, 10+i,
self.position_sensors[10+i].getValue()
)
# # Velocity
# self.log.log_joint_velocity(
# self.iteration, i,
# motor.getVelocity()
# )
# Command
self.log.log_joint_cmd(
self.iteration, 10+i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
def step(self):
"""Step"""
# Increment iteration
self.iteration += 1
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(
positions[self.N_BODY_JOINTS+i] - np.pi/2
)
key=self.keyboard.getKey()
if (key==ord('A') and key is not self.lastkey):
print('Turning left')
self.SimulationParameters.turnRate = [0.5,1]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.lastkey = key
if (key==ord('D') and key is not self.lastkey):
print('Turning right')
self.SimulationParameters.turnRate = [1,0.5]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.lastkey = key
if (key==ord('W') and key is not self.lastkey):
print('Going forward')
self.SimulationParameters.turnRate = [1,1]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.SimulationParameters.Backwards = False
self.NetworkParameters.set_phase_bias(self.SimulationParameters)
self.lastkey = key
if (key==ord('S') and key is not self.lastkey):
print('Going backward')
self.SimulationParameters.Backwards = True
self.NetworkParameters.set_phase_bias(self.SimulationParameters)
self.SimulationParameters.turnRate = [1,1]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.lastkey = key
if (key==ord('T') and key is not self.lastkey):
if self.doTransition:
print('Disabling transition')
self.doTransition = False
else:
print('Enabling transition')
self.doTransition = True
self.lastkey = key
if self.doTransition:
gpsPos = self.gps.getValues()
if gpsPos[0] < self.waterPosx+2 and gpsPos[0] > self.waterPosx -0.5:
gain = 4/2.5*(gpsPos[0]+0.5) + 1
self.SimulationParameters.drive = gain
#print('Transitioning')
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.NetworkParameters.set_frequencies(self.SimulationParameters)
# Log data
self.log_iteration()
def __init__(self, robot, n_iterations, parameters, logs="logs/log.npz"):
super(SalamanderCMC, self).__init__()
self.robot = robot
timestep = int(robot.getBasicTimeStep())
self.network = SalamanderNetwork(1e-3*timestep, parameters)
# Position sensors
self.position_sensors = [
self.robot.getPositionSensor('position_sensor_{}'.format(i+1))
for i in range(self.N_BODY_JOINTS)
] + [
self.robot.getPositionSensor('position_sensor_leg_{}'.format(i+1))
for i in range(self.N_LEGS)
]
for sensor in self.position_sensors:
sensor.enable(timestep)
# GPS
self.gps = robot.getGPS("fgirdle_gps")
self.gps.enable(timestep)
# Get motors
self.motors_body = [
self.robot.getMotor("motor_{}".format(i+1))
for i in range(self.N_BODY_JOINTS)
]
self.motors_legs = [
self.robot.getMotor("motor_leg_{}".format(i+1))
for i in range(self.N_LEGS)
]
# Set motors
for motor in self.motors_body:
motor.setPosition(0)
motor.enableForceFeedback(timestep)
motor.enableTorqueFeedback(timestep)
for motor in self.motors_legs:
motor.setPosition(-np.pi/2)
motor.enableForceFeedback(timestep)
motor.enableTorqueFeedback(timestep)
# Iteration counter
self.iteration = 0
# Logging
self.log = ExperimentLogger(
n_iterations,
n_links=1,
n_joints=self.N_BODY_JOINTS+self.N_LEGS,
filename=logs,
timestep=1e-3*timestep,
**parameters
)
#GPS stuff
self.waterPosx = 0
self.NetworkParameters = self.network.parameters
self.SimulationParameters = parameters
self.doTransition = False
self.keyboard = Keyboard()
self.keyboard.enable(samplingPeriod=100)
self.lastkey = 0
for m in range(4):
motors[m].setPosition(math.inf)
motors[m].setVelocity(1)
print("Motors set!\n")
imu = robot.getDevice('inertial unit')
imu.enable(timestep)
gps = robot.getDevice('gps')
gps.enable(timestep)
gyro = robot.getDevice('gyro')
gyro.enable(timestep)
kb = Keyboard()
try:
kb.enable(timestep)
except:
print("keyboard fail")
print("Instruments and control initiated!\n")
while robot.step(timestep) != -1:
if robot.getTime() > 1.0:
break
# Display manual control message.
print("You can control the drone with your computer keyboard:\n")
print("- 'up': move forward.\n")
print("- 'down': move backward.\n")
"""Simple robot controller."""
from controller import Robot, Keyboard, Pen, Display
import sys, time, math
from rovecomm import RoveComm, RoveCommPacket
import socket, cv2, pickle, struct
import numpy as np
from rover import Rover
rovecomm_node = RoveComm(11001, ("", 11112))
# Get pointer to the robot.
robot = Robot()
keyboard = Keyboard()
keyboard.enable(64)
# Initialize the rover class, with our rovecomm node
rover = Rover(robot, rovecomm_node)
rovecomm_node.set_callback(1000, rover.drive_callback)
# Get simulation step length.
timeStep = int(robot.getBasicTimeStep())
while robot.step(timeStep) != -1:
## Check if we need to trigger watchdog and stop driving
rover.drive_watchdog_check()
# send the sensor data to the autonomy program
rover.send_sensor_update()
# Stream the current frame (at fixed FPS)
class Mouse(Supervisor):
"""Main class for Mouse control. """
def __init__(self):
super(Mouse, self).__init__()
self.biomech = MusculoSkeletalSystem.MusculoSkeletalSystem(
os.path.join('musculoskeletal', 'mouse.json')
)
self.TIMESTEP = int(self.getBasicTimeStep())
self.motors = {}
self.position_sensors = {}
self.ground_contact_sensors = {}
self.joint_positions = {}
self.muscle_forces = {}
self.ground_contacts = {}
# Initialize webots
self.key_press = None
self.initialize_webots_motors()
self.initialize_webots_position_sensors()
self.initialize_webots_ground_contact_sensors()
self.initialize_webots_keyboard()
# Muscle visualization
self.gps = {}
self.mv_transform = {}
self.mv_color = {}
self.mv_geom = {}
self.muscle_viz = None
self.initialize_gps()
self.initialize_muscle_visualizations()
# Foot Trajectories [time, axis]
self.ankle_r_trajectory = np.zeros([BUFFER_SIZE_TRAJECTORIES, 3])
self.ankle_l_trajectory = np.zeros([BUFFER_SIZE_TRAJECTORIES, 3])
self.iteration = 0
def __del__(self):
""" Deletion """
try:
os.stat(RESULTS_DIRECTORY)
except:
os.mkdir(RESULTS_DIRECTORY)
np.save(
RESULTS_DIRECTORY + "ankle_l_trajectory.npy",
self.ankle_l_trajectory[:self.iteration, :]
)
np.save(
RESULTS_DIRECTORY + "ankle_r_trajectory.npy",
self.ankle_r_trajectory[:self.iteration, :]
)
return
def run(self):
""" Run """
reflex = Reflexes(self.biomech.sim_muscle_names)
while self.step(self.TIMESTEP) != -1:
# Reflex model
reflex.step(
self.update_joint_positions(),
self.update_muscle_forces(),
self.update_ground_contacts()
)
activations = reflex.activations
# Update the biomechanical model
self.biomech.update(
self.TIMESTEP / 1000.,
self.update_joint_positions(),
activations
)
# Get the Torque
torque = self.biomech.joint_torque()
for name, motor in self.motors.iteritems():
motor.setTorque(torque[name])
if MUSCLE_VISUALIZATION:
self.muscle_viz.step(activations, viz=True)
else:
self.muscle_viz.step(activations, viz=False)
# Save data
self.iteration += 1
self.ankle_l_trajectory[self.iteration, :] = (
self.gps["LH_G1_ANKLE"].getValues()
)
self.ankle_r_trajectory[self.iteration, :] = (
self.gps["RH_G1_ANKLE"].getValues()
)
def initialize_webots_keyboard(self):
""" Initialize webots keyboard """
self.key_press = Keyboard()
self.key_press.enable(self.TIMESTEP * 25)
def initialize_webots_motors(self):
"""Set-up leg joints in the system."""
for joint in self.biomech.sim_joints:
print('Initializing webots motor : {}'.format(joint))
self.motors[joint] = self.getMotor(str(joint))
def initialize_webots_position_sensors(self):
"""Set-up leg joints in the system."""
for joint in self.biomech.sim_joints:
print('Initializing webots motor : {}'.format(joint))
self.position_sensors[joint] = self.getPositionSensor(
str(joint) + '_POS'
)
self.position_sensors[joint].enable(self.TIMESTEP)
def initialize_webots_ground_contact_sensors(self):
"""Initialize groung contact sensors."""
print('Initializing webots ground contact sensors ')
self.ground_contact_sensors['LEFT_TOE_TOUCH'] = self.getTouchSensor(
'LEFT_TOE_TOUCH'
)
self.ground_contact_sensors['LEFT_TOE_TOUCH'].enable(self.TIMESTEP)
self.ground_contact_sensors['RIGHT_TOE_TOUCH'] = self.getTouchSensor(
'RIGHT_TOE_TOUCH'
)
self.ground_contact_sensors['RIGHT_TOE_TOUCH'].enable(self.TIMESTEP)
# MUSCLE VISUALIZATION
def initialize_muscle_visualizations(self):
"""Initialize necessary attributes for muscle visualization."""
# Get muscle transform, appearance and geom
for muscle in self.biomech.sim_muscle_names:
muscle_split = muscle.split('_')
side = muscle_split[0]
name = muscle_split[-1]
# TRANSFORM
transform = str(side + '_MV_TRANSFORM_' + name)
self.mv_transform[transform] = self.getFromDef(transform)
# COLOR
appearance = str(side + '_MV_COLOR_' + name)
self.mv_color[appearance] = self.getFromDef(appearance)
# GEOM
geom = str(side + '_MV_GEOM_' + name)
self.mv_geom[geom] = self.getFromDef(geom)
# Creat muscle muscle visualization object
self.muscle_viz = MuscleVisualization(
self.muscle_visualization_attachment(),
self.gps,
self.mv_transform,
self.mv_color,
self.mv_geom)
return
def initialize_gps(self):
"""Initialize gps nodes for muscle visualization."""
GPS_NAMES = ['G1_PELVIS', 'G2_PELVIS',
'G1_HIP', 'G2_HIP',
'G1_KNEE', 'G2_KNEE',
'G1_ANKLE']
sides = ['LH', 'RH']
for side in sides:
for gps in GPS_NAMES:
name = side + '_' + gps
self.gps[name] = self.getGPS(name)
self.gps[name].enable(self.TIMESTEP)
return
def muscle_visualization_attachment(self):
"""Returns the dictionaries muscle origin and
insertion with respect to GPS."""
muscle_attach = {
'PMA': ['G1_PELVIS', 'G1_HIP'],
'CF': ['G2_PELVIS', 'G1_HIP'],
'SM': ['G2_PELVIS', 'G1_KNEE'],
'POP': ['G1_HIP', 'G1_KNEE'],
'RF': ['G1_HIP', 'G2_HIP'],
'TA': ['G1_KNEE', 'G1_ANKLE'],
'SOL': ['G1_KNEE', 'G2_KNEE'],
'LG': ['G1_HIP', 'G2_KNEE']
}
return muscle_attach
def update_joint_positions(self):
""" Initialize the array to store joint positions."""
for name, sensor in self.position_sensors.iteritems():
self.joint_positions[name] = (
sensor.getValue()
+ self.biomech.sim_joints[name].reference_angle
)
return self.joint_positions
def update_muscle_forces(self):
""" Initialize the array to store joint positions."""
for muscle in self.biomech.sim_muscles:
self.muscle_forces[muscle] = self.biomech.sim_muscles[
muscle
].tendonForce
return self.muscle_forces
def update_ground_contacts(self):
""" Update ground contacts """
self.ground_contacts['L'] = self.ground_contact_sensors[
'LEFT_TOE_TOUCH'
].getValue()
self.ground_contacts['R'] = self.ground_contact_sensors[
'RIGHT_TOE_TOUCH'
].getValue()
return self.ground_contacts
def default_high_pos():
for i in range(6):
motor_lst[0 + i*3].setPosition(0)
motor_lst[0 + i*3].setVelocity(1.0)
motor_lst[2 + i*3].setPosition(math.pi * -1 / 8)
motor_lst[2 + i*3].setVelocity(1.0)
motor_lst[1 + i*3].setPosition(math.pi * -3 / 8)
motor_lst[1 + i*3].setVelocity(1.0)
default_high_pos()
keyboard = Keyboard()
keyboard.enable(60)
while robot.step(timestep) != -1:
#motor_lst[0].setPosition(10.0)
#motor_lst[0].setVelocity(1.0)
key=keyboard.getKey()
if (key==ord('D')):
print('D is pressed')
rotate(math.pi * -2 / 8)
elif (key==ord('F')):
print('F is pressed')
rotate(math.pi * 2 / 8)
class SupervisorController:
def __init__(self,
ros_active=False,
mode='normal',
do_ros_init=True,
base_ns='',
model_states_active=True):
"""
The SupervisorController, a Webots controller that can control the world.
Set the environment variable WEBOTS_ROBOT_NAME to "supervisor_robot" if used with 1_bot.wbt or 4_bots.wbt.
:param ros_active: Whether to publish ROS messages
:param mode: Webots mode, one of 'normal', 'paused', or 'fast'
:param do_ros_init: Whether rospy.init_node should be called
:param base_ns: The namespace of this node, can normally be left empty
"""
# requires WEBOTS_ROBOT_NAME to be set to "supervisor_robot"
self.ros_active = ros_active
self.model_states_active = model_states_active
self.time = 0
self.clock_msg = Clock()
self.supervisor = Supervisor()
self.keyboard_activated = False
if mode == 'normal':
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
elif mode == 'paused':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
elif mode == 'fast':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST)
else:
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
self.motors = []
self.sensors = []
self.timestep = int(self.supervisor.getBasicTimeStep())
self.robot_nodes = {}
self.translation_fields = {}
self.rotation_fields = {}
self.joint_nodes = {}
self.link_nodes = {}
root = self.supervisor.getRoot()
children_field = root.getField('children')
children_count = children_field.getCount()
for i in range(children_count):
node = children_field.getMFNode(i)
name_field = node.getField('name')
if name_field is not None and node.getType() == Node.ROBOT:
# this is a robot
name = name_field.getSFString()
if name == "supervisor_robot":
continue
self.robot_nodes[name] = node
self.translation_fields[name] = node.getField("translation")
self.rotation_fields[name] = node.getField("rotation")
self.joint_nodes[name], self.link_nodes[
name] = self.collect_joint_and_link_node_references(
node, {}, {})
if self.ros_active:
# need to handle these topics differently or we will end up having a double //
if base_ns == "":
clock_topic = "/clock"
model_topic = "/model_states"
else:
clock_topic = base_ns + "clock"
model_topic = base_ns + "model_states"
if do_ros_init:
rospy.init_node("webots_ros_supervisor",
argv=['clock:=' + clock_topic])
self.clock_publisher = rospy.Publisher(clock_topic,
Clock,
queue_size=1)
self.model_state_publisher = rospy.Publisher(model_topic,
ModelStates,
queue_size=1)
self.reset_service = rospy.Service(base_ns + "reset", Empty,
self.reset)
self.reset_pose_service = rospy.Service(base_ns + "reset_pose",
Empty,
self.set_initial_poses)
self.set_robot_pose_service = rospy.Service(
base_ns + "set_robot_pose", SetObjectPose,
self.robot_pose_callback)
self.reset_ball_service = rospy.Service(base_ns + "reset_ball",
Empty, self.reset_ball)
self.set_ball_position_service = rospy.Service(
base_ns + "set_ball_position", SetObjectPosition,
self.ball_pos_callback)
self.world_info = self.supervisor.getFromDef("world_info")
self.ball = self.supervisor.getFromDef("ball")
def collect_joint_and_link_node_references(self, node, joint_dict,
link_dict):
# this is a recursive function that iterates through the whole robot as this seems to be the only way to
# get all joints
# add node if it is a joint
if node.getType() == Node.SOLID:
name = node.getDef()
link_dict[name] = node
if node.getType() == Node.HINGE_JOINT:
name = node.getDef()
# substract the "Joint" keyword due to naming convention
name = name[:-5]
joint_dict[name] = node
# the joints dont have children but an "endpoint" that we need to search through
if node.isProto():
endpoint_field = node.getProtoField('endPoint')
else:
endpoint_field = node.getField('endPoint')
endpoint_node = endpoint_field.getSFNode()
self.collect_joint_and_link_node_references(
endpoint_node, joint_dict, link_dict)
# needs to be done because Webots has two different getField functions for proto nodes and normal nodes
if node.isProto():
children_field = node.getProtoField('children')
else:
children_field = node.getField('children')
if children_field is not None:
for i in range(children_field.getCount()):
child = children_field.getMFNode(i)
self.collect_joint_and_link_node_references(
child, joint_dict, link_dict)
return joint_dict, link_dict
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_clock()
if self.model_states_active:
self.publish_model_states()
def handle_gui(self):
if not self.keyboard_activated:
self.keyboard = Keyboard()
self.keyboard.enable(100)
self.keyboard_activated = True
key = self.keyboard.getKey()
if key == ord('R'):
self.reset()
elif key == ord('P'):
self.set_initial_poses()
elif key == Keyboard.SHIFT + ord('R'):
try:
self.reset_ball()
except AttributeError:
print("No ball in simulation that can be reset")
return key
def publish_clock(self):
self.clock_msg.clock = rospy.Time.from_seconds(self.time)
self.clock_publisher.publish(self.clock_msg)
def set_gravity(self, active):
if active:
self.world_info.getField("gravity").setSFFloat(9.81)
else:
self.world_info.getField("gravity").setSFFloat(0)
def set_self_collision(self, active, name="amy"):
self.robot_nodes[name].getField("selfCollision").setSFBool(active)
def reset_robot_pose(self, pos, quat, name="amy"):
self.set_robot_pose_quat(pos, quat, name)
if name in self.robot_nodes:
self.robot_nodes[name].resetPhysics()
def reset_robot_pose_rpy(self, pos, rpy, name="amy"):
self.set_robot_pose_rpy(pos, rpy, name)
if name in self.robot_nodes:
self.robot_nodes[name].resetPhysics()
def reset(self, req=None):
self.supervisor.simulationReset()
self.supervisor.simulationResetPhysics()
return EmptyResponse()
def reset_robot_init(self, name="amy"):
self.robot_nodes[name].loadState('__init__')
self.robot_nodes[name].resetPhysics()
def set_initial_poses(self, req=None):
self.reset_robot_pose_rpy([-1, 3, 0.42], [0, 0.24, -1.57], name="amy")
self.reset_robot_pose_rpy([-1, -3, 0.42], [0, 0.24, 1.57], name="rory")
self.reset_robot_pose_rpy([-3, 3, 0.42], [0, 0.24, -1.57], name="jack")
self.reset_robot_pose_rpy([-3, -3, 0.42], [0, 0.24, 1.57],
name="donna")
self.reset_robot_pose_rpy([0, 6, 0.42], [0, 0.24, -1.57],
name="melody")
return EmptyResponse()
def robot_pose_callback(self, req=None):
self.reset_robot_pose(
[req.pose.position.x, req.pose.position.y, req.pose.position.z], [
req.pose.orientation.x, req.pose.orientation.y,
req.pose.orientation.z, req.pose.orientation.w
], req.object_name)
return SetObjectPoseResponse()
def reset_ball(self, req=None):
self.ball.getField("translation").setSFVec3f([0, 0, 0.0772])
self.ball.getField("rotation").setSFRotation([0, 0, 1, 0])
self.ball.resetPhysics()
return EmptyResponse()
def ball_pos_callback(self, req=None):
self.set_ball_pose([req.position.x, req.position.y, req.position.z])
return SetObjectPositionResponse()
def set_ball_pose(self, pos):
self.ball.getField("translation").setSFVec3f(list(pos))
self.ball.resetPhysics()
def get_ball_pose(self):
return self.ball.getField("translation").getSFVec3f()
def get_ball_velocity(self):
if self.ball:
return self.ball.getVelocity()
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_axis_angle(self, axis, angle, name="amy"):
if name in self.rotation_fields:
self.rotation_fields[name].setSFRotation(
list(np.append(axis, angle)))
def set_robot_rpy(self, rpy, name="amy"):
axis, angle = transforms3d.euler.euler2axangle(rpy[0],
rpy[1],
rpy[2],
axes='sxyz')
self.set_robot_axis_angle(axis, angle, name)
def set_robot_quat(self, quat, name="amy"):
axis, angle = transforms3d.quaternions.quat2axangle(
[quat[3], quat[0], quat[1], quat[2]])
self.set_robot_axis_angle(axis, angle, name)
def set_robot_position(self, pos, name="amy"):
if name in self.translation_fields:
self.translation_fields[name].setSFVec3f(list(pos))
def set_robot_pose_rpy(self, pos, rpy, name="amy"):
self.set_robot_position(pos, name)
self.set_robot_rpy(rpy, name)
def set_robot_pose_quat(self, pos, quat, name="amy"):
self.set_robot_position(pos, name)
self.set_robot_quat(quat, name)
def get_robot_position(self, name="amy"):
if name in self.translation_fields:
return self.translation_fields[name].getSFVec3f()
def get_robot_orientation_axangles(self, name="amy"):
if name in self.rotation_fields:
return self.rotation_fields[name].getSFRotation()
def get_robot_orientation_rpy(self, name="amy"):
ax_angle = self.get_robot_orientation_axangles(name)
return list(
transforms3d.euler.axangle2euler(ax_angle[:3],
ax_angle[3],
axes='sxyz'))
def get_robot_orientation_quat(self, name="amy"):
ax_angle = self.get_robot_orientation_axangles(name)
# transforms 3d uses scalar (i.e. the w part in the quaternion) first notation of quaternions, ros uses scalar last
quat_scalar_first = transforms3d.quaternions.axangle2quat(
ax_angle[:3], ax_angle[3])
quat_scalar_last = np.append(quat_scalar_first[1:],
quat_scalar_first[0])
return list(quat_scalar_last)
def get_robot_pose_rpy(self, name="amy"):
return self.get_robot_position(name), self.get_robot_orientation_rpy(
name)
def get_robot_pose_quat(self, name="amy"):
return self.get_robot_position(name), self.get_robot_orientation_quat(
name)
def get_robot_velocity(self, name="amy"):
velocity = self.robot_nodes[name].getVelocity()
return velocity[:3], velocity[3:]
def get_link_pose(self, link, name="amy"):
link_node = self.robot_nodes[name].getFromProtoDef(link)
if not link_node:
return None
link_position = link_node.getPosition()
mat = link_node.getOrientation()
link_orientation = transforms3d.quaternions.mat2quat(np.array(mat))
return link_position, np.append(link_orientation[1:],
link_orientation[0])
def get_link_velocities(self, link, name="amy"):
"""Returns linear and angular velocities"""
link_node = self.robot_nodes[name].getFromProtoDef(link)
velocity = link_node.getVelocity()
return velocity[:3], velocity[3:]
def publish_model_states(self):
if self.model_state_publisher.get_num_connections() != 0:
msg = ModelStates()
for robot_name, robot_node in self.robot_nodes.items():
position, orientation = self.get_robot_pose_quat(
name=robot_name)
robot_pose = Pose()
robot_pose.position = Point(*position)
robot_pose.orientation = Quaternion(*orientation)
msg.name.append(robot_name)
msg.pose.append(robot_pose)
lin_vel, ang_vel = self.get_robot_velocity(robot_name)
twist = Twist()
twist.linear.x = lin_vel[0]
twist.linear.y = lin_vel[1]
twist.linear.z = lin_vel[2]
twist.angular.x = ang_vel[0]
twist.angular.y = ang_vel[1]
twist.angular.z = ang_vel[2]
msg.twist.append(twist)
head_node = robot_node.getFromProtoDef("head")
head_position = head_node.getPosition()
head_orientation = head_node.getOrientation()
head_orientation_quat = transforms3d.quaternions.mat2quat(
np.reshape(head_orientation, (3, 3)))
head_pose = Pose()
head_pose.position = Point(*head_position)
head_pose.orientation = Quaternion(head_orientation_quat[1],
head_orientation_quat[2],
head_orientation_quat[3],
head_orientation_quat[0])
msg.name.append(robot_name + "_head")
msg.pose.append(head_pose)
if self.ball is not None:
ball_position = self.ball.getField("translation").getSFVec3f()
ball_pose = Pose()
ball_pose.position = Point(*ball_position)
ball_pose.orientation = Quaternion()
msg.name.append("ball")
msg.pose.append(ball_pose)
self.model_state_publisher.publish(msg)
# KEYBOARD CONTROLLED VEHICLE
"""
In this simulation, The vehicle gets input from the user's
keyboard command and acts accordingly.The purpose of this simulation is
to get used to the Webots environment.
"""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, Keyboard
from controller import Robot, Motor, Keyboard
import time
TIME_STEP = 64 #timestep should be the same as in the WorldInfo
# create the Robot and Keyboard instance.
robot = Robot()
keyboard = Keyboard()
keyboard.enable(TIME_STEP)
# assign the motors to wheels.
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(
float('inf')) #set position infinity not to restrict motion
wheels[i].setVelocity(0.0) #set initial speed as 0
leftSpeed = 0.0
rightSpeed = 0.0
print('Click the simulation window before the keyboard command!')
print('Press [W] or [w] to move forward')
def __init__(self, controller):
self.controller = controller
self.keyboard = Keyboard()
self.keyboard.enable(self.controller.get_timestep())
right_motor = robot.getMotor("right motor")
left_motor_sensor = left_motor.getPositionSensor()
right_motor_sensor = right_motor.getPositionSensor()
left_motor_sensor.enable(timestep)
right_motor_sensor.enable(timestep)
left_motor_init = left_motor_sensor.getValue()
right_motor_init = right_motor_sensor.getValue()
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
#KEYBOARD INITIALIZATION
keyboard = Keyboard()
keyboard.enable(timestep)
print("Select the 3D window and control the robot using the W, A, S, D keys.")
#LIDAR INITIALIZATION
lidar = robot.getLidar("lidar")
lidar.enable(timestep)
lidar.enablePointCloud()
#FRONT CAMERA INITIALIZATION
front_camera = robot.getCamera("front_camera")
front_camera.enable(timestep)
#REAR CAMERA INITIALIZATION
rear_camera = robot.getCamera("rear_camera")
rear_camera.enable(timestep)
while robot.step(timestep) != -1:
"""uav_control controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot, Camera, InertialUnit, GPS, Compass, Gyro, Motor, Keyboard, Emitter
import math
import numpy as np
import struct
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
keyboard = Keyboard()
keyboard.enable(timestep)
emitter = robot.getEmitter("emitter")
# send data
message = struct.pack('5f', 0.0, 0.0, 0.0, 0.0, 0.0)
emitter.send(message)
imu = robot.getInertialUnit("inertial unit")
imu.enable(timestep)
camera = robot.getCamera("camera")
camera.enable(timestep)
gps = robot.getGPS("gps")
gps.enable(timestep)
compass = robot.getCompass("compass")
compass.enable(timestep)
gyro = robot.getGyro("gyro")
gyro.enable(timestep)
camera_roll_motor = robot.getCamera("camera roll")
camera_pitch_motor = robot.getCamera("camera pitch")
