def main():
parser = argparse.ArgumentParser()
parser.add_argument('--duration',
type=float,
default=10,
help='Duration of the animation in seconds')
parser.add_argument('--output',
default='../../animation/index.html',
help='Path at which the animation will be saved')
args = parser.parse_args()
robot = Supervisor()
timestep = int(robot.getBasicTimeStep())
receiver = robot.getDevice('receiver')
receiver.enable(timestep)
robot.step(timestep)
robot.animationStartRecording(args.output)
step_i = 0
done = False
n_steps = (1000 * args.duration) / robot.getBasicTimeStep()
while not done and robot.step(timestep) != -1 and step_i < n_steps:
step_i += 1
if receiver.getQueueLength() > 0:
if receiver.getData().decode('utf-8') == 'done':
done = True
receiver.nextPacket()
robot.animationStopRecording()
for _ in range(10):
robot.step(timestep)
print('The animation is saved')
robot.simulationQuit(0)
class Robot:
def __init__(self):
self.supervisor = Supervisor()
self.timeStep = int(4 * self.supervisor.getBasicTimeStep())
# Create the arm chain from the URDF
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(self.supervisor.getUrdf().encode('utf-8'))
armChain = Chain.from_urdf_file(filename)
armChain.active_links_mask[0] = False
# Initialize the arm motors and encoders.
self.motors = []
for link in armChain.links:
if 'motor' in link.name:
motor = self.supervisor.getDevice(link.name)
motor.setVelocity(1.0)
position_sensor = motor.getPositionSensor()
position_sensor.enable(self.timeStep)
self.motors.append(motor)
self.arm = self.supervisor.getSelf()
self.positions = [0 for _ in self.motors]
def update(self):
for motor, position in zip(self.motors, self.positions):
motor.setPosition(position * 0.01745277777)
def simulate(self):
self.supervisor.step(self.timeStep)
def get_simulation_run_mode(supervisor: Supervisor) -> 'SimulationMode':
# webots 2020b is buggy and can raise TypeError when getDevice is passed a str
if supervisor.getDevice("2021a-compatibility") is None:
# we are running version 2020b so the old command is used
return Supervisor.SIMULATION_MODE_RUN
else:
# webots-2021a removed the RUN mode and now uses FAST
print(
"This simulator is running a different version of Webots to the "
"one that will be used for the next official competition matches "
"(You can check the docs to see which version will be used)",
file=sys.stderr,
)
print(
"As such it is possible that some behaviour may not "
"match that of the official competition matches",
file=sys.stderr,
)
return Supervisor.SIMULATION_MODE_FAST
#!/usr/bin/env python3
import os
import sys
from controller import Robot, CameraRecognitionObject, Display, Supervisor
import random
import cv2
import numpy as np
deviceImagePath = os.getcwd()
#robot = Robot()
supervisor = Supervisor()
timestep = int(supervisor.getBasicTimeStep()*10)
camera = supervisor.getDevice('camera_sensor')
camera.enable(timestep)
camera.recognitionEnable(timestep)
camera.enableRecognitionSegmentation()
number = 0
print("hasRecognition(): " + str(camera.hasRecognition()))
print("hasRecognitionSegmentation(): " + str(camera.hasRecognitionSegmentation()))
cv2.startWindowThread()
cv2.namedWindow("preview")
ball_color = ( 0, 0 , 255, 255)
enemy1_color = ( 0, 255, 0, 255)
enemy2_color = (255, 0, 0, 255)
enemy3_color = (255, 255, 0, 255)
while supervisor.step(timestep) != -1:
sys.path.append(
os.path.abspath(os.path.join(os.path.dirname(__file__), '../..')))
import pycontroller as ctrl
import numpy as np
DEBUG = True
MAX_SPEED = 440
mult = 1
odd = -mult * MAX_SPEED
even = mult * MAX_SPEED
super = Supervisor()
drone = super.getFromDef('drone')
prop1 = super.getDevice("prop1")
prop2 = super.getDevice("prop2")
prop3 = super.getDevice("prop3")
prop4 = super.getDevice("prop4")
prop5 = super.getDevice("prop5")
prop6 = super.getDevice("prop6")
gyro = super.getDevice('GYRO')
gps = super.getDevice('GPS')
compass = super.getDevice('COMPASS')
prop1.setPosition(float('-inf'))
prop2.setPosition(float('+inf'))
prop3.setPosition(float('-inf'))
prop4.setPosition(float('+inf'))
prop5.setPosition(float('-inf'))
prop6.setPosition(float('+inf'))
EPS = 0.2
def getPoints(dist):
poi_points = 0
for j in range(10):
if dist < EPS * (j + 1):
poi_points += 1
return poi_points
referee = Supervisor()
timestep = int(referee.getBasicTimeStep())
robot_node = referee.getFromDef('PARTICIPANT_ROBOT')
emitter = referee.getDevice('emitter')
poi_list = []
poi_string_list = robot_node.getField('customData').getSFString().split()
for i in range(10):
poi_element = [
float(poi_string_list[2 * i]),
float(poi_string_list[2 * i + 1])
]
poi_list.append(poi_element)
min_dist = [20] * 10
points = 0
while referee.step(timestep) != -1 and SPENT_TIME < MAXIMUM_TIME:
#mode = 'repair'
# create the Robot instance.
robot = Supervisor()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# Initialize leg motors
motorNames = [
'RPC', 'RPF', 'RPT', 'RMC', 'RMF', 'RMT', 'RAC', 'RAF', 'RAT', 'LPC',
'LPF', 'LPT', 'LMC', 'LMF', 'LMT', 'LAC', 'LAF', 'LAT'
]
motors = []
for i in range(0, 18):
motors.append(robot.getDevice(motorNames[i]))
# Initialize lidar sensor
lidar = robot.getDevice('Hokuyo URG-04LX-UG01')
lidar.enable(timestep)
lidar.enablePointCloud()
# Initialize GPS unit
gps = robot.getDevice('gps')
gps.enable(timestep)
# Initialize Compass unit
compass = robot.getDevice('compass')
compass.enable(timestep)
# Initialize display and arrays for sensor readings and lidar beam angles
'front left 1',
'front left 2',
'rear',
'rear left',
'rear right',
'right',
'left']
sensors = {}
colorFields = {}
supervisor = Supervisor()
# get and enable the distance sensors
for name in sensorsNames:
sensors[name] = supervisor.getDevice('distance sensor ' + name)
sensors[name].enable(TIME_STEP)
defName = name.upper().replace(' ', '_')
colorFields[name] = supervisor.getFromDef(defName + '_VISUALIZATION').getField('diffuseColor')
# get the color fields
childrenField = supervisor.getSelf().getField('children')
for i in range(childrenField.getCount()):
node = childrenField.getMFNode(i)
if node.getTypeName() == 'DistanceSensorVisualization':
colorFields[node.getDef()] = node.getField('diffuseColor')
colorFields[node.getDef()].setSFColor([0.0, 1.0, 0.0])
while supervisor.step(TIME_STEP) != -1:
# adjust the color according to the value returned by the front distance sensor
for name in sensorsNames:
# create the Robot instance.
robot = Supervisor()
robot_node = robot.getFromDef("BIWAKO-X")
pos = robot_node.getField("translation") #+ type Field
fluid_node = robot.getFromDef("STILL_WATER") # type Node
stream_vel = fluid_node.getField("streamVelocity") # type Field
parameter = parameter()
workspace = parameter.workspace
# TIME_STEP = int(robot.getBasicTimeStep())
TIME_STEP = parameter.TIME_STEP
gps_sampling_rate = 1000 # [msec]
compass_sampling_rate = 100 # [msec]
thruster1 = robot.getDevice("thruster1")
thruster2 = robot.getDevice("thruster2")
thruster3 = robot.getDevice("thruster3")
thruster4 = robot.getDevice("thruster4")
# set thrusters to infinity rotation mode
thruster1.setPosition(float('+inf'))
thruster2.setPosition(float('+inf'))
thruster3.setPosition(float('+inf'))
thruster4.setPosition(float('+inf'))
way_point_file = parameter.way_point_file
target_point = np.genfromtxt(way_point_file,
delimiter=',',
dtype='float',
encoding='utf-8')
class DarwinWebotsController:
def __init__(self,
namespace='',
ros_active=False,
ros_anonymous=True,
mode='normal'):
self.ros_active = ros_active
self.time = 0
self.clock_msg = Clock()
self.namespace = namespace
self.supervisor = Supervisor()
self.motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR",
"ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR",
"LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR",
"FootL", "Neck", "Head"
]
self.walkready = [0] * 20
self.names_webots_to_bitbots = {
"ShoulderR": "RShoulderPitch",
"ShoulderL": "LShoulderPitch",
"ArmUpperR": "RShoulderRoll",
"ArmUpperL": "LShoulderRoll",
"ArmLowerR": "RElbow",
"ArmLowerL": "LElbow",
"PelvYR": "RHipYaw",
"PelvYL": "LHipYaw",
"PelvR": "RHipRoll",
"PelvL": "LHipRoll",
"LegUpperR": "RHipPitch",
"LegUpperL": "LHipPitch",
"LegLowerR": "RKnee",
"LegLowerL": "LKnee",
"AnkleR": "RAnklePitch",
"AnkleL": "LAnklePitch",
"FootR": "RAnkleRoll",
"FootL": "LAnkleRoll",
"Neck": "HeadPan",
"Head": "HeadTilt"
}
self.names_bitbots_to_webots = {
v: k
for k, v in self.names_webots_to_bitbots.items()
}
self.motors = []
self.sensors = []
self.timestep = int(self.supervisor.getBasicTimeStep())
# self.timestep = 10
if mode == 'normal':
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
elif mode == 'paused':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
elif mode == 'run':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_RUN)
elif mode == 'fast':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST)
else:
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
for motor_name in self.motor_names:
self.motors.append(self.supervisor.getDevice(motor_name))
self.motors[-1].enableTorqueFeedback(self.timestep)
self.sensors.append(self.supervisor.getDevice(motor_name + "S"))
self.sensors[-1].enable(self.timestep)
self.accel = self.supervisor.getDevice("Accelerometer")
self.accel.enable(self.timestep)
self.gyro = self.supervisor.getDevice("Gyro")
self.gyro.enable(self.timestep)
self.camera = self.supervisor.getDevice("Camera")
self.camera.enable(self.timestep)
if self.ros_active:
rospy.init_node("webots_darwin_ros_interface",
anonymous=ros_anonymous,
argv=['clock:=/' + self.namespace + '/clock'])
self.pub_js = rospy.Publisher(self.namespace + "/joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher(self.namespace + "/imu/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher(self.namespace + "/image_raw",
Image,
queue_size=1)
self.pub_clock = rospy.Publisher(self.namespace + "/clock",
Clock,
queue_size=1)
rospy.Subscriber(self.namespace + "/DynamixelController/command",
JointCommand, self.command_cb)
self.world_info = self.supervisor.getFromDef("world_info")
self.hinge_joint = self.supervisor.getFromDef("barrier_hinge")
self.robot_node = self.supervisor.getFromDef("Darwin")
self.translation_field = self.robot_node.getField("translation")
self.rotation_field = self.robot_node.getField("rotation")
@property
def ros_time(self) -> rospy.Time:
return rospy.Time.from_seconds(self.time)
def step_sim(self):
self.time += self.timestep / 1000
self.supervisor.step(self.timestep)
def step(self):
self.step_sim()
if self.ros_active:
self.publish_imu()
self.publish_joint_states()
self.publish_clock()
self.publish_camera()
def publish_clock(self):
self.clock_msg.clock = self.ros_time
self.pub_clock.publish(self.clock_msg)
def command_cb(self, command: JointCommand):
for i, name in enumerate(command.joint_names):
try:
motor_index = self.motor_names.index(
self.names_bitbots_to_webots[name])
self.motors[motor_index].setPosition(command.positions[i])
except ValueError:
rospy.logerr(
f"invalid motor specified ({self.names_bitbots_to_webots[name]})"
)
def set_head_tilt(self, pos):
self.motors[-1].setPosition(pos)
def set_arms_zero(self):
positions = [
-0.8399999308200574, 0.7200000596634105, -0.3299999109923385,
0.35999992683575216, 0.5099999812500172, -0.5199999789619728
]
for i in range(0, 6):
self.motors[i].setPosition(positions[i])
def publish_joint_states(self):
msg = JointState()
msg.name = []
msg.header.stamp = self.ros_time
msg.position = []
msg.effort = []
for i in range(len(self.sensors)):
msg.name.append(self.names_webots_to_bitbots[self.motor_names[i]])
value = self.sensors[i].getValue()
msg.position.append(value)
msg.effort.append(self.motors[i].getTorqueFeedback())
self.pub_js.publish(msg)
def publish_imu(self):
msg = Imu()
msg.header.stamp = self.ros_time
msg.header.frame_id = "imu_frame"
accel_vels = self.accel.getValues()
msg.linear_acceleration.x = ((accel_vels[0] - 512.0) / 512.0) * 3 * G
msg.linear_acceleration.y = ((accel_vels[1] - 512.0) / 512.0) * 3 * G
msg.linear_acceleration.z = ((accel_vels[2] - 512.0) / 512.0) * 3 * G
gyro_vels = self.gyro.getValues()
msg.angular_velocity.x = ((gyro_vels[0] - 512.0) / 512.0) * 1600 * (
math.pi / 180) # is 400 deg/s the real value
msg.angular_velocity.y = (
(gyro_vels[1] - 512.0) / 512.0) * 1600 * (math.pi / 180)
msg.angular_velocity.z = (
(gyro_vels[2] - 512.0) / 512.0) * 1600 * (math.pi / 180)
# todo compute
msg.orientation.x = 0
msg.orientation.y = 0
msg.orientation.z = 0
msg.orientation.w = 1
self.pub_imu.publish(msg)
def publish_camera(self):
msg = Image()
msg.header.stamp = self.ros_time
msg.header.frame_id = "MP_PMDCAMBOARD"
msg.height = self.camera.getHeight()
msg.width = self.camera.getWidth()
msg.encoding = "bgra8"
msg.step = 4 * self.camera.getWidth()
img = self.camera.getImage()
msg.data = img
self.pub_cam.publish(msg)
def set_gravity(self, active):
if active:
self.world_info.getField("gravity").setSFVec3f([0.0, -9.81, 0.0])
self.world_info.getField("gravity").setSFFloat(9.81)
else:
self.world_info.getField("gravity").setSFVec3f([0.0, 0.0, 0.0])
self.world_info.getField("gravity").setSFFloat(0)
def reset_robot_pose(self, pos, quat):
rpy = tf.transformations.euler_from_quaternion(quat)
self.set_robot_pose_rpy(pos, rpy)
self.robot_node.resetPhysics()
def reset_robot_pose_rpy(self, pos, rpy):
self.set_robot_pose_rpy(pos, rpy)
self.robot_node.resetPhysics()
def reset(self):
# self.supervisor.simulationReset()
# reactivate camera after reset https://github.com/cyberbotics/webots/issues/1778
# self.supervisor.getSelf().restartController()
# self.camera = self.supervisor.getCamera("Camera")
# self.camera.enable(self.timestep)
self.supervisor.simulationResetPhysics()
def node(self):
s = self.supervisor.getSelected()
if s is not None:
print(f"id: {s.getId()}, type: {s.getType()}, def: {s.getDef()}")
def set_robot_pose_rpy(self, pos, rpy):
self.translation_field.setSFVec3f(pos_ros_to_webots(pos))
self.rotation_field.setSFRotation(rpy_to_axis(*rpy))
def set_robot_rpy(self, rpy):
self.rotation_field.setSFRotation(rpy_to_axis(*rpy))
def get_robot_pose_rpy(self):
pos = self.translation_field.getSFVec3f()
rot = self.rotation_field.getSFRotation()
return pos_webots_to_ros(pos), axis_to_rpy(*rot)
timeStep = int(4 * supervisor.getBasicTimeStep())
# Create the arm chain from the URDF
filename = None
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(supervisor.getUrdf().encode('utf-8'))
armChain = Chain.from_urdf_file(filename)
for i in [0, 6]:
armChain.active_links_mask[0] = False
# Initialize the arm motors and encoders.
motors = []
for link in armChain.links:
if 'motor' in link.name:
motor = supervisor.getDevice(link.name)
motor.setVelocity(1.0)
position_sensor = motor.getPositionSensor()
position_sensor.enable(timeStep)
motors.append(motor)
# Get the arm and target nodes.
target = supervisor.getFromDef('TARGET')
arm = supervisor.getSelf()
# Loop 1: Draw a circle on the paper sheet.
print('Draw a circle on the paper sheet...')
while supervisor.step(timeStep) != -1:
t = supervisor.getTime()
# Use the circle equation relatively to the arm base as an input of the IK algorithm.
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()
EPS = 0.2
def getPoints(dist):
poi_points = 0
for j in range(10):
if dist < EPS * (j + 1):
poi_points += 1
return poi_points
referee = Supervisor()
timestep = int(referee.getBasicTimeStep())
robot_node = referee.getFromDef('PARTICIPANT_ROBOT')
emitter = referee.getDevice('emitter')
touch_sensor = referee.getDevice('touch sensor')
poi_list = []
poi_string_list = robot_node.getField('customData').getSFString().split()
for i in range(10):
poi_element = [
float(poi_string_list[2 * i]),
float(poi_string_list[2 * i + 1])
]
poi_list.append(poi_element)
min_dist = [20] * 10
points = 0
"""lab3_controller_py controller."""
from controller import Supervisor, DistanceSensor, Gyro, Compass, Motor
#supervisor object
supervisor = Supervisor()
#robot Node
robot = supervisor.getFromDef('Segway')
#time step definition
TIME_STEP = int(supervisor.getBasicTimeStep())
#motor sensors, nodes
front_sensor = supervisor.getDevice('lidar_F')
right_sensor = supervisor.getDevice('lidar_R')
compass = supervisor.getDevice('compass')
#gyro, nodes
gyro = supervisor.getDevice('gyro')
#enable sensors
front_sensor.enable(TIME_STEP)
right_sensor.enable(TIME_STEP)
compass.enable(TIME_STEP)
gyro.enable(TIME_STEP)
#non-position control mode
left_motor = supervisor.getDevice('motor_L')
right_motor = supervisor.getDevice('motor_R')
left_motor.setPosition(float('inf'))
class Agent(object):
"""docstring for Agent."""
def __init__(self, mdNames, objName):
super().__init__()
self.robot = Supervisor()
self.objName = objName
self.energy = 10000
self.timestep = int(self.robot.getBasicTimeStep())
self.camera = self.robot.getDevice('camera')
self.camera.enable(self.timestep)
self.camera.recognitionEnable(self.timestep)
self.MAX_SPEED = 10
self.LOW_SPEED = -10
self.MAX_ENERGY = 10000
self.consumptionEnergy = 2000
self.ds = []
self.md = []
self.dsNames = ['ds_left', 'ds_right', 'ds_left(1)', 'ds_right(1)']
for i in range(4):
self.ds.append(self.robot.getDevice(self.dsNames[i]))
self.ds[i].enable(self.timestep)
self.length = len(self.mdNames)
for i in range(self.length):
self.md.append(self.robot.getDevice(self.mdNames[i]))
def multiMoveMotorPos(self, devices, dPos):
for device in devices:
device.setPosition(dPos)
def setMultiMotorVel(self, devices, vel):
for device in devices:
device.setVelocity(vel*self.MAX_SPEED)
self.velocity = vel
def getEnergy(self):
return self.energy
def setEnergy(self, energy):
self.energy = energy
def eat(self, prey):
prey = prey
pField = prey.getField('translation')
randX = random.uniform(-2.45, 2.45)
randZ = random.uniform(-2.45, 2.45)
newPos = [randX,0.05,randZ]
pField.setSFVec3f(newPos)
self.energy = self.energy + self.consumptionEnergy
if self.energy > self.MAX_ENERGY:
self.energy = self.MAX_ENERGY
def checkEnergyCollision(self, preyName):
if preyName:
objPos = self.getPosition(self.objName)
objPos = np.array(objPos)
objPos = np.array(objPos)
for i in preyName:
self.prey = self.robot.getFromDef(i)
preyPos = self.prey.getPosition()
preyPos = self.prey.getPosition()
preyPos = np.array(preyPos)
dist = np.linalg.norm(objPos - preyPos)
if dist < 0.4:
return self.prey
else:
return False
def getPosition(self, name):
thing = self.robot.getFromDef(name)
pos = thing.getPosition()
return pos
def checkObstacle(self):
dsValues = []
for i in range(4):
dsValues.append(self.ds[i].getValue())
left_obstacle = dsValues[0] < 1000.0 or dsValues[2] < 1000.0
right_obstacle = dsValues[1] < 1000.0 or dsValues[3] < 1000.0
if left_obstacle:
return 1
elif right_obstacle:
return 2
else:
return False
def checkRecogniseSource(self):
currClosest = [1000,100, 1000]
minDist = float('inf')
recognisedObj = self.camera.getRecognitionObjects()
for obj in recognisedObj:
currObj = obj.get_position()
distance = math.sqrt(currObj[0]*currObj[0]+currObj[2]*currObj[2])
if distance < minDist:
minDist = distance
currClosest = currObj
if recognisedObj:
x = currClosest[0]
angle = x *minDist
return angle
else:
return False
return False
def getMotorDevices(self):
return self.md
def avoidObstacle(self, value):
if value == 1:
self.turnRight()
elif value == 2:
self.turnLeft()
def setMaxEnergy(self, energy):
self.MAX_ENERGY = energy
def setConsumptionEnergy(self, energy):
self.consumptionEnergy = energy
"""bingomaster controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
# from controller import Robot
from controller import Supervisor
from controller import Emitter
from controller import Receiver
import random
import struct
# create the Robot instance.
# robot = Robot()
supervisor = Supervisor()
emitter = supervisor.getDevice("emitter")
receiver = supervisor.getDevice("receiver")
emitter.setChannel(0)
receiver.enable(1)
receiver.setChannel(1)
# get the time step of the current world.
timestep = int(supervisor.getBasicTimeStep())
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
NUMSQUARES = 16 # board size
colors = ["blue", "green", "orange", "pink", "yellow"]
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot
from controller import Supervisor
import numpy as np
import csv
import matplotlib.pyplot as plt
# create the Robot instance.
robot = Supervisor()
segway = robot.getFromDef('robot')
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
motor_R = robot.getDevice('motor_R')
motor_L = robot.getDevice('motor_L')
lidar_F = robot.getDevice('lidar_F')
lidar_R = robot.getDevice('lidar_R')
gyro = robot.getDevice('gyro')
compass = robot.getDevice('compass')
lidar_F.enable(timestep)
lidar_R.enable(timestep)
gyro.enable(timestep)
compass.enable(timestep)
motor_R.setPosition(float('+inf'))
motor_L.setPosition(float('-inf'))
INPUT_FILE = "Inputs/analytic_inputs/path_7"
robot = Supervisor()
#node = Node()
robot_node = robot.getSelf()
#TIME_STEP = 64
TIME_STEP = int(robot.getBasicTimeStep())
MAX_SPEED = float(6.28)
ROBOT_ANGULAR_SPEED_IN_DEGREES = float(360)
#ROBOT_ANGULAR_SPEED_IN_DEGREES = float(283.588111888)
# initialize sensors
ps = []
psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
for i in range(8):
ps.append(robot.getDevice(psNames[i]))
ps[i].enable(TIME_STEP)
# initialize and set motor devices
leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')
leftSensor = robot.getDevice("left wheel sensor")
leftSensor.enable(TIME_STEP)
rightSensor = robot.getDevice("right wheel sensor")
rightSensor.enable(TIME_STEP)
# Set to endless rotational motion
leftMotor.setPosition(float('+inf'))
rightMotor.setPosition(float('+inf'))
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)
"""weighted_landmark_supervisor controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Supervisor
import struct
from functions import *
# Import Simulations Stuff
from simulation import *
# Set up Webots stuff
WALL_THR = 300
supervisor = Supervisor()
timestep = int(supervisor.getBasicTimeStep())
emitter = supervisor.getDevice('emitter')
# Set up Simulation stuff
simulation = Simulations(supervisor, timestep)
simulation.plt_only_weights_and_vectors(to_plot='W', fig_tag=-1)
for t in range(0, int(total_time / dt)):
dumy = supervisor.step(timestep)
timestep = int(supervisor.getBasicTimeStep())
simulation.sim_pre_step(t, emitter, supervisor, timestep)
max_tdelta = simulation.agents.get_max_webot_tdelta()
for i in range(0, max_tdelta):
dumy = supervisor.step(timestep)
timeStep = int(4 * supervisor.getBasicTimeStep())
# Create the arm chain from the URDF
filename = None
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(supervisor.getUrdf().encode('utf-8'))
armChain = Chain.from_urdf_file(filename)
for i in [0, 6]:
armChain.active_links_mask[0] = False
# Initialize the arm motors and encoders.
motors = []
for link in armChain.links:
if 'motor' in link.name:
motor = supervisor.getDevice(link.name)
motor.setVelocity(1.0)
position_sensor = motor.getPositionSensor()
position_sensor.enable(timeStep)
motors.append(motor)
# Get the arm and target nodes.
target = supervisor.getFromDef('TARGET')
arm = supervisor.getSelf()
# Loop: Move the arm hand to the target.
print('Move the yellow and black sphere to move the arm...')
while supervisor.step(timeStep) != -1:
# Get the absolute postion of the target and the arm base.
targetPosition = target.getPosition()
armPosition = arm.getPosition()
from Gripper import *
from Base import *
import numpy as np
import math
robot = Supervisor()
timestep = int(robot.getBasicTimeStep())
# Initialize the base, arm and gripper of the youbot robot
base = Base(robot)
arm = Arm(robot)
gripper = Gripper(robot)
# Enable compass/gps modules
compass = robot.getDevice('compass')
compass.enable(timestep)
gps = robot.getDevice('gps')
gps.enable(timestep)
# Initialize waypoints and state machine initial state
waypoints = [(22.26, 24.61), (22.2, 24.6), (22.03, 26.06), (26.0, 26.4),
(28.7, 25.0)] #(25.5, 25.0),
current_waypoint = waypoints.pop(0)
state = 'lower arm'
# Establish the gains for feedback controls
x_gain = 1.5
theta_gain = 2.0
sys.stderr.write("Warning: 'robotbenchmark' module not found.\n")
sys.exit(0)
# Get random generator seed value from 'controllerArgs' field
seed = 1
if len(sys.argv) > 1 and sys.argv[1].startswith('seed='):
seed = int(sys.argv[1].split('=')[1])
robot = Supervisor()
timestep = int(robot.getBasicTimeStep())
jointParameters = robot.getFromDef("PENDULUM_PARAMETERS")
positionField = jointParameters.getField("position")
emitter = robot.getDevice("emitter")
time = 0
force = 0
forceStep = 800
random.seed(seed)
run = True
while robot.step(timestep) != -1:
if run:
time = robot.getTime()
robot.wwiSendText("time:%-24.3f" % time)
robot.wwiSendText("force:%.2f" % force)
# Detect status of inverted pendulum
position = positionField.getSFFloat()
if position < -1.58 or position > 1.58:
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")
right_hip_roll = supervisor.getDevice("RHipRoll")
left_hip_roll = supervisor.getDevice("LHipRoll")
right_knee_pitch = supervisor.getDevice("RKneePitch")
MAX_ANGULAR_SPEED = 1.5708 # [rad.s-1]
LINEAR_ACCEL = 0.5 # [m.s-2]
ANGULAR_ACCEL = LINEAR_ACCEL / WHEEL_BASE # [rad.s-2]
# time in [ms] of a simulation step
TIME_STEP = 32
# create the Robot instance.
supervisor = Supervisor()
# Init keyboard
keyboard = supervisor.getKeyboard()
keyboard.enable(TIME_STEP)
# Init motors
leftMotor = supervisor.getDevice('left wheel motor')
rightMotor = supervisor.getDevice('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)
linearVelocity = 0
angularVelocity = 0
def check_keyboard_inputs():
keys = [keyboard.getKey()]
while keys[-1] != -1:
keys.append(keyboard.getKey())
