def init_supervisor():
global supervisor, robot_node, target_node, enemy_node
global start_translation, start_rotation
# create the Supervisor instance.
supervisor = Supervisor()
# do this once only
root = supervisor.getRoot()
root_children_field = root.getField("children")
robot_node = None
target_node = None
for idx in range(root_children_field.getCount()):
if root_children_field.getMFNode(idx).getDef() == "Player":
robot_node = root_children_field.getMFNode(idx)
if root_children_field.getMFNode(idx).getDef() == "Goal":
target_node = root_children_field.getMFNode(idx)
if root_children_field.getMFNode(idx).getDef() == "Enemy":
enemy_node.append(root_children_field.getMFNode(idx))
enemy_init_trans.append(
copy.copy(enemy_node[-1].getField("translation").getSFVec3f()))
enemy_init_rot.append(
copy.copy(enemy_node[-1].getField("rotation").getSFRotation()))
start_translation = copy.copy(
robot_node.getField("translation").getSFVec3f())
start_rotation = copy.copy(robot_node.getField("rotation").getSFRotation())
def init_supervisor():
global robot_node, supervisor, goal_location
# create the Supervisor instance.
supervisor = Supervisor()
# do this once only
root = supervisor.getRoot()
root_children_field = root.getField("children")
for idx in range(root_children_field.getCount()):
name = root_children_field.getMFNode(idx).getTypeName()
#print('name', name)
if name == 'Mavic2Pro':
robot_node = root_children_field.getMFNode(idx)
if name == 'Solid':
goal_node = root_children_field.getMFNode(idx)
goal_location = goal_node.getField('translation').getSFVec3f()
def init_supervisor():
global supervisor, robot_node, target_node
# create the Supervisor instance.
supervisor = Supervisor()
# do this once only
root = supervisor.getRoot()
root_children_field = root.getField("children")
robot_node = None
target_node = None
for idx in range(root_children_field.getCount()):
if root_children_field.getMFNode(idx).getDef() == "EPUCK":
robot_node = root_children_field.getMFNode(idx)
if root_children_field.getMFNode(idx).getDef() == "EPUCK_TARGET":
target_node = root_children_field.getMFNode(idx)
start_translation = copy.copy(
robot_node.getField("translation").getSFVec3f())
start_rotation = copy.copy(robot_node.getField("rotation").getSFRotation())
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")
#initial guess for the knee and heel levers
#the y component of KNEE_INITIAL is the lever length
#the z component of HEEL_INITIAL is the lever length
KNEE_INITIAL = np.array([0.0, 0.325, -0.01])
HEEL_INITIAL = np.array([0.0, 0.0, 0.15])
#these are ideal values for a 80kg robot
#KNEE_TENDON_INITIAL = 143526
#HEEL_TENDON_INITIAL = 160106
#this is the initial guess for the tendon values
KNEE_TENDON_INITIAL = 25000
HEEL_TENDON_INITIAL = 25000
#get a handle to the root node and create the robot
root = supervisor.getRoot()
children = root.getField("children")
children.importMFNode(-1, "Robot.wbo")
#set the leg spring constants to the initial values
change_spring_constant(int(x[0]), int(x[1]))
#function that changes the spring constant of the robot
#edits the robot.wbo file
#changing the spring constant through the robot node causes unexpected behaviour
def change_spring_constant(knee, heel):
flag = False
a = 0
for line in fileinput.FileInput("Robot.wbo", inplace=1):
if line.strip() == "SliderJoint {":
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')
cameraNear = controller.getFromDef('CAMERA').getField('near')
if options.singleShot:
node = controller.getFromDef('OBJECTS')
if node is None:
sys.exit('No node "OBJECTS" found.')
take_original_screenshot(camera, '.' + os.sep + 'images')
take_screenshot(camera, 'objects', '.' + os.sep + 'images',
os.path.dirname(controller.getWorldPath()),
import time
import traceback
import transforms3d
import random
import numpy as np
import csv
from scipy.spatial import ConvexHull
from types import SimpleNamespace
from skopt import gp_minimize
# start the webots supervisor
supervisor = Supervisor()
time_step = int(supervisor.getBasicTimeStep())
field = Field("kid")
children = supervisor.getRoot().getField('children')
children.importMFNodeFromString(-1, f'RobocupSoccerField {{ size "kid" }}')
children.importMFNodeFromString(
-1,
f'DEF PLAYER RoboCup_GankenKun {{translation -0.3 0 0.450 rotation 0 0 1 0 controller "generate_walking_pattern" controllerArgs "0.05"}}'
)
player = supervisor.getFromDef('PLAYER')
#player_translation = supervisor.getFromDef('PLAYER').getField('translation')
#player_rotation = supervisor.getFromDef('PLAYER').getField('rotation')
#player_controller = supervisor.getFromDef('PLAYER').getField('controller')
def func(param):
global player, children, supervisor
count = 0
player.remove()
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()
q = table[s2_, a]
if q > max_q:
max_q = q
# update table
table[s1_, action] = table[s1_, action] + alpha * (reward + gamma * max_q -
table[s1_, action])
return table
# do this once only
robot_node = robot.getFromDef("firebird")
trans_field = robot_node.getField("translation")
rot_field = robot_node.getField("rotation")
root = robot.getRoot()
root_children_field = root.getField("children")
node = root_children_field.getMFNode(-3)
field = node.getField("translation")
rot = node.getField("rotation")
field.setSFVec3f(startingTranslation)
rot.setSFRotation(startingRotation[random.randint(0,
len(startingRotation) - 1)])
def list_difference(list1, list2):
difference = []
for list1_i, list2_i in zip(list1, list2):
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)
"""my_supervisor controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, LED, DistanceSensor
from controller import Supervisor, Node, Display
# create the Robot instance.
super = Supervisor()
root = super.getRoot()
emitter = super.getEmitter('emitter')
# emitter.setChannel(emitter.CHANNEL_BROADCAST)
print(super.getFromDef("e-puck"))
names = ["robo_1", "robo_2"]
robots = [super.getFromDef(name) for name in names]
# get the time step of the current world.
timestep = int(super.getBasicTimeStep())
print("Timestep:", timestep)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while super.step(timestep) != -1:
for _id, robot in enumerate(robots):
pos = robot.getField("translation")
orientation = robot.getField("rotation")
class EgoVehicle:
def __init__(self):
self.robot_ids = []
self.supervisor = Supervisor()
self.rootChildren = self.supervisor.getRoot().getField("children")
self.count = 0
with open(
'/home/dikshant/catkin_ws/src/webots_ros/worlds/CAIR_mod_net/sumo.net.xml'
) as f:
for line in f:
if "location netOffset=" in line:
word = line.split(' ')
for word_l in word:
if 'netOffset=' in word_l:
offset_value = re.findall("\d+\.\d+", word_l)
f.close()
self.xOffset = float(offset_value[0])
self.yOffset = float(offset_value[1])
#print (xOffset, yOffset)
self.edge_dict = defaultdict(dict)
#Read values from a file corresponding to the edge
with open(
'/home/dikshant/catkin_ws/src/webots_ros/worlds/CAIR_mod_net/CAIR_mod_edge_info.in'
) as f1:
for line in f1:
coord = line.split()
if len(coord) == 1:
key_value = coord[0]
x_co = []
y_co = []
elif len(coord) == 2:
#print coord
#Each x added with xOffset
x_co.append(float(coord[0]) + self.xOffset)
#Each y added with yOffset
y_co.append(float(coord[1]) + self.yOffset)
else:
self.edge_dict[key_value]['x_values'] = x_co
self.edge_dict[key_value]['y_values'] = y_co
self.run()
def run(self):
rospy.init_node('sumo_sim', anonymous=True)
self.rob_oper = AddRemoveBots()
self.robot_ids = ["r0", "r1", "r2", "r3"]
self.start_edge_ids = [
'142865550', '142865547_2', '99829041_2', '-142865547_2'
]
# Get initial vehicles in simulation
for i in range(len(self.robot_ids)):
#Generate Vehicle
defName = "EGO_VEHICLE_%s" % self.robot_ids[i]
vehicleString = "DEF " + defName + " " + "TeslaModel3" + " {\n"
x_coord = []
y_coord = []
xp, yp, zp = [], [], []
key_val = self.start_edge_ids[i]
x_coord = self.edge_dict[key_val]['x_values']
y_coord = self.edge_dict[key_val]['y_values']
height = 0.4
for index in range(2):
pos = [
round(-x_coord[index] + self.xOffset, 2), height,
y_coord[index] - self.yOffset
]
zp.append(pos[2])
xp.append(pos[0])
#Finding rotation and translation coordinates
x1 = xp[0]
x2 = xp[1]
z1 = zp[0]
z2 = zp[1]
vehicleString += " translation {} {} {}\n".format(x1, height, z1)
# angle
ang_radians = math.atan2(x2 - x1, z2 - z1)
vehicleString += " rotation 0 1 0 {}\n".format(ang_radians)
vehicleString += " color 0.18 0.50 0.72\n"
vehicleString += " name \"{}\"\n".format(self.robot_ids[i])
vehicleString += " controller \"{}\"\n".format("ros_automobile")
vehicleString += " controllerArgs \"--name={} --clock --use-sim-time\"\n".format(
self.robot_ids[i])
vehicleString += " sensorsSlotCenter [\n"
vehicleString += " GPS {\n"
vehicleString += " }\n"
vehicleString += " ]\n"
vehicleString += " }\n"
self.rootChildren.importMFNodeFromString(-1, vehicleString)
self.count = i + 1
while self.supervisor.step(TIME_STEP) != -1:
print(self.robot_ids)
msg = self.rob_oper.get_msg()
if msg != None:
if msg.opr == 'add':
if msg.rob_id not in self.robot_ids:
defName = "EGO_VEHICLE_%s" % msg.rob_id
vehicleString = "DEF " + defName + " " + "TeslaModel3" + " {\n"
x_coord = []
y_coord = []
xp, yp, zp = [], [], []
key_val = msg.edge_id
x_coord = self.edge_dict[key_val]['x_values']
y_coord = self.edge_dict[key_val]['y_values']
height = 0.4
for ind in range(2):
pos = [
round(-x_coord[ind] + self.xOffset, 2), height,
y_coord[ind] - self.yOffset
]
zp.append(pos[2])
xp.append(pos[0])
#Finding rotation and translation coordinates
x1 = xp[0]
x2 = xp[1]
z1 = zp[0]
z2 = zp[1]
vehicleString += " translation {} {} {}\n".format(
x1, height, z1)
# angle
ang_radians = math.atan2(x2 - x1, z2 - z1)
vehicleString += " rotation 0 1 0 {}\n".format(
ang_radians)
vehicleString += " color 0.18 0.50 0.72\n"
vehicleString += " name \"{}\"\n".format(msg.rob_id)
vehicleString += " controller \"{}\"\n".format(
"ros_automobile")
vehicleString += " controllerArgs \"--name={} --clock --use-sim-time\"\n".format(
msg.rob_id)
vehicleString += " sensorsSlotCenter [\n"
vehicleString += " GPS {\n"
vehicleString += " }\n"
vehicleString += " ]\n"
vehicleString += " }\n"
self.rootChildren.importMFNodeFromString(
-1, vehicleString)
self.robot_ids.append(msg.rob_id)
self.count += 1
if msg.opr == 'rem':
if msg.rob_id in self.robot_ids:
#self.robot_ids.remove("msg.rob_id")
pos = self.robot_ids.index(msg.rob_id) - len(
self.robot_ids)
self.rootChildren.removeMF(pos)
self.robot_ids.remove(msg.rob_id)
