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)
supervisor = Supervisor()
# Get the robot nodes by their DEF names
robot0 = supervisor.getFromDef("ROBOT0")
robot1 = supervisor.getFromDef("ROBOT1")
# Get the translation fields
robot0Pos = robot0.getField("translation")
robot1Pos = robot1.getField("translation")
# Get the output from the object placement supervisor
objectPlacementOutput = supervisor.getFromDef("OBJECTPLACER").getField(
"customData")
# Get this supervisor node - so that it can be rest when game restarts
mainSupervisor = supervisor.getSelf()
# Maximum time for a match
maxTime = 120
class Robot:
'''Robot object to hold values whether its in a base or holding a human'''
def __init__(self):
'''Initialises the in a base, has a human loaded and score values'''
self.inBase = True
self._humanLoaded = False
self._score = 0
self._timeStopped = 0
self._stopped = False
class InvaderBot():
# Initalize the motors.
def setup(self):
self.robot = Supervisor()
self.motor_left = self.robot.getMotor("motor_left")
self.motor_right = self.robot.getMotor("motor_right")
self.timestep = int(self.robot.getBasicTimeStep())
# Do one update step. Calls Webots' robot.step().
# Return True while simulation is running.
# Return False if simulation is ended
def step(self):
return (self.robot.step(self.timestep) != -1)
# Set the velocity of the motor [-1, 1].
def set_motor(self, motor, velocity):
mult = 1 if velocity > 0 else -1
motor.setPosition(mult * float('+inf'))
motor.setVelocity(velocity * motor.getMaxVelocity())
# Set the velocity of left and right motors [-1, 1].
def set_motors(self, left, right):
self.set_left_motor(left)
self.set_right_motor(right)
# Set the velocity of left motors [-1, 1].
def set_left_motor(self, velocity):
self.set_motor(self.motor_left, velocity)
# Set the velocity of right motors [-1, 1].
def set_right_motor(self, velocity):
self.set_motor(self.motor_right, velocity)
# Returns the current simulation time in seconds
def get_time(self):
return self.robot.getTime()
# Returns the position of the robot in [x, z, angle] format
# The coordinate system is as follows (top-down view)
# .-------------------->x
# |\ (angle)
# | \
# | \
# | \
# | \
# | \
# |
# V
# z
#
def get_position(self):
subject = self.robot.getSelf()
position = subject.getPosition()
orientation = subject.getOrientation()
orientation = math.atan2(orientation[0], orientation[2])
orientation = math.degrees(orientation)
return [position[0], position[2], orientation]
# Returns the position of the balls in the following format
# [
# [
# [green_ball_0_x, green_ball_0_z],
# [green_ball_1_x, green_ball_1_z]
# ],
#
# [
# [yellow_ball_0_x, yellow_ball_0_z],
# [yellow_ball_1_x, yellow_ball_1_z],
# [yellow_ball_2_x, yellow_ball_2_z],
# ]
# ]
def get_balls(self):
green = []
green_root = self.robot.getFromDef("_green_balls").getField("children")
for idx in reversed(range(green_root.getCount())):
try:
ball = green_root.getMFNode(idx)
pos = ball.getPosition()
green.append([pos[0], pos[2]])
except:
pass
yellow = []
yellow_root = self.robot.getFromDef("_yellow_balls").getField(
"children")
for idx in reversed(range(yellow_root.getCount())):
try:
ball = yellow_root.getMFNode(idx)
pos = ball.getPosition()
yellow.append([pos[0], pos[2]])
except:
pass
return [green, yellow]
'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:
ratio = math.pow(sensors[name].getValue() / sensors[name].getMaxValue(), 2.0)
colorFields[name].setSFColor([1.0 - ratio, 0.0, ratio])
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()
# Compute the position of the target relatively to the arm.
# x and y axis are inverted because the arm is not aligned with the Webots global axes.
x = targetPosition[0] - armPosition[0]
y = -(targetPosition[2] - armPosition[2])
z = targetPosition[1] - armPosition[1]
# Call "ikpy" to compute the inverse kinematics of the arm.
def __init__(self):
Supervisor.__init__(self)
self.currentlyPlaying = False
self.devicesNumber = self.getNumberOfDevices()
self.sup = self.getSelf()
# initialize stuff
self.findAndEnableDevices()
# get motors and its limits:
self.motor_device = dict()
self.motorLimits = self.getMotorsLimits()
self.motorSensorsNames = self.getMotorSensorsNames()
self.robot_node = Supervisor.getSelf(self)
self.display = self.getDisplay("ClockDisplay")
self.init_display()
self.trans_field = Node.getField(self.robot_node, 'translation')
self.rot_field = Node.getField(self.robot_node, 'rotation')
# self.INITIAL_TRANS = Field.getSFVec3f(self.trans_field)
# self.INITIAL_ROT = Field.getSFRotation(self.rot_field)
self.INITIAL_TRANS = [0, 0.239, 0]
self.INITIAL_ROT = [-1, 0, 0, 1.5708]
self.motor_names = list(self.motorLimits.keys())
self.INITIAL_MOTOR_POS = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.6,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.6,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.25,
'RHipPitch': -0.4,
'RKneePitch': 0.9,
'RAnklePitch': -0.8,
'RAnkleRoll': 0.4,
'LHipYawPitch': 0.25,
'LHipRoll': -0.25,
'LHipPitch': -0.4,
'LKneePitch': 0.9,
'LAnklePitch': -0.8,
'LAnkleRoll': -0.4
}
self.LEFT_MID = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.2,
'RHipPitch': 0.75,
'RKneePitch': -0.8,
'RAnklePitch': -0.2,
'RAnkleRoll': 0.35,
'LHipYawPitch': 0.25,
'LHipRoll': -0.2,
'LHipPitch': 0.05,
'LKneePitch': 0.35,
'LAnklePitch': -0.4,
'LAnkleRoll': -0.4
}
self.LEFT_STEP = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.2,
'RHipPitch': 0.85,
'RKneePitch': -0.8,
'RAnklePitch': -0.37,
'RAnkleRoll': 0.35,
'LHipYawPitch': 0.25,
'LHipRoll': -0.25,
'LHipPitch': 0,
'LKneePitch': -0.8,
'LAnklePitch': 0.52,
'LAnkleRoll': -0.35
}
self.RIGHT_MID = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.15,
'RHipPitch': 0,
'RKneePitch': 0.35,
'RAnklePitch': -0.3,
'RAnkleRoll': 0.4,
'LHipYawPitch': 0.25,
'LHipRoll': -0.2,
'LHipPitch': 0.75,
'LKneePitch': -0.8,
'LAnklePitch': -0.1,
'LAnkleRoll': -0.4
}
self.RIGHT_STEP = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.3,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.3,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.15,
'RHipPitch': 0,
'RKneePitch': -0.8,
'RAnklePitch': 0.52,
'RAnkleRoll': 0.35,
'LHipYawPitch': 0.25,
'LHipRoll': -0.2,
'LHipPitch': 0.85,
'LKneePitch': -0.8,
'LAnklePitch': -0.35,
'LAnkleRoll': -0.42
}
self.STAND_UP = {
'HeadYaw': 0.0,
'HeadPitch': 0.13235322780693037,
'RShoulderPitch': 0.8,
'RShoulderRoll': 0.75,
'RElbowYaw': 0.8,
'RElbowRoll': 0.6,
'RWristYaw': -3.43941389813196e-08,
'RPhalanx1': -1.0,
'RPhalanx2': -1.0,
'RPhalanx3': -1.0,
'RPhalanx4': -1.0,
'RPhalanx5': -1.0,
'RPhalanx6': -1.0,
'RPhalanx7': -1.0,
'RPhalanx8': -1.0,
'LShoulderPitch': 0.8,
'LShoulderRoll': -0.75,
'LElbowYaw': -0.8,
'LElbowRoll': -0.6,
'LWristYaw': 0.0,
'LPhalanx1': -1.0,
'LPhalanx2': -1.0,
'LPhalanx3': -1.0,
'LPhalanx4': -1.0,
'LPhalanx5': -1.0,
'LPhalanx6': -1.0,
'LPhalanx7': -1.0,
'LPhalanx8': -1.0,
'RHipYawPitch': 0.25,
'RHipRoll': 0.25,
'RHipPitch': 0.5,
'RKneePitch': -0.8,
'RAnklePitch': 0.1,
'RAnkleRoll': 0.4,
'LHipYawPitch': 0.25,
'LHipRoll': -0.25,
'LHipPitch': 0.5,
'LKneePitch': -0.8,
'LAnklePitch': 0.1,
'LAnkleRoll': -0.4
}
self.DEFAULT_MOTOR_POS = self.INITIAL_MOTOR_POS
'forces': forcesLabels,
'moments': momentsLabels,
'powers': powersLabels
}
for key in labelsAndCategory:
if labelsAndCategory[key]:
supervisor.wwiSendText('labels:' + key + ':' + units[key] + ':' + ' '.join(labelsAndCategory[key]).strip())
else:
supervisor.wwiSendText('labels:' + key + ':' + units[key] + ':' + 'None')
supervisor.wwiSendText('configure:' + str(supervisor.getBasicTimeStep()))
# make one step to be sure markers are not imported before pressing play
supervisor.step(timestep)
# remove possible previous marker (at regeneration for example)
markerField = supervisor.getSelf().getField('markers')
for i in range(markerField.getCount()):
markerField.removeMF(-1)
# ground reaction forces (GRF)
grfList = []
names = ['LGroundReaction', 'RGroundReaction']
for i in range(len(names)):
if names[i] + 'Force' in labels and names[i] + 'Moment' in labels:
grfList.append({'name': names[i]})
markerField.importMFNodeFromString(-1, 'C3dGroundReactionForce { translation %s %s %s}' % (sys.argv[3 + 3 * i],
sys.argv[4 + 3 * i],
sys.argv[5 + 3 * i]))
grfList[-1]['node'] = markerField.getMFNode(-1)
grfList[-1]['rotation'] = grfList[-1]['node'].getField('rotation')
grfList[-1]['cylinderTranslation'] = grfList[-1]['node'].getField('cylinderTranslation')
import time
from tornado import httpserver
from tornado import gen
from tornado.ioloop import IOLoop
import tornado.web
#from controller import Robot, Motor, DistanceSensor
# for supervisor functions
from controller import Supervisor, Motor, DistanceSensor, PositionSensor, Camera
# create the Robot instance.
# robot = Robot()
# create Robot instance with supervisor superpower
## Robot must be set to "supervisor TRUE"
robot = Supervisor()
#node = Node()
robot_node = robot.getSelf()
#TIME_STEP = 64
TIME_STEP = int(robot.getBasicTimeStep())
MAX_SPEED = float(6.28)
ROBOT_ANGULAR_SPEED_IN_DEGREES = float(360)
#ROBOT_ANGULAR_SPEED_IN_DEGREES = float(283.588111888)
# initialize sensors
ps = []
psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
for i in range(8):
ps.append(robot.getDevice(psNames[i]))
ps[i].enable(TIME_STEP)
# print('moveing')
# move2pos(supervisor.getSelf(), *target.getPosition())
# print("closing")
# closeGripper()
# target = supervisor.getFromDef('BASKET')
# x, y, z = target.getPosition()
# move2pos(supervisor.getSelf(), x, y + 0.3, z)
# openGripper()
while supervisor.step(timeStep) != -1:
targets = radar.getTargets()
if targets:
for target in targets:
distance = target.distance
print(distance)
theta = target.azimuth
x = distance * math.cos(theta)
y = 0.66
z = distance * math.sin(theta)
openGripper()
print('moveing')
move2pos(supervisor.getSelf(), x + 0.99, y, z)
print("closing")
closeGripper()
target = supervisor.getFromDef('BASKET')
x, y, z = target.getPosition()
move2pos(supervisor.getSelf(), x, y + 0.3, z)
openGripper()
passiveWait(10)
"""partyBoiPog controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Supervisor
from controller import DistanceSensor
from controller import Motor
TIME_STEP = 128
# create the Robot instance.
supervisor = Supervisor()
supervisorNode = supervisor.getSelf()
# get the time step of the current world.
#timestep = int(robot.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
def robotWheels():
wheels = []
wheelsNames = ["wheel1", "wheel2", "wheel3", "wheel4"]
for i in range(4):
class Botsy():
# Initalize the motors.
def setup(self):
self.robot = Supervisor()
self.motor_left = self.robot.getMotor("motor_left")
self.motor_right = self.robot.getMotor("motor_right")
self.timestep = int(self.robot.getBasicTimeStep())
# Do one update step. Calls Webots' robot.step().
# Return True while simulation is running.
# Return False if simulation is ended
def step(self):
return (self.robot.step(self.timestep) != -1)
# Set the velocity of the motor [-1, 1].
def set_motor(self, motor, velocity):
mult = 1 if velocity > 0 else -1
motor.setPosition(mult * float('+inf'))
motor.setVelocity(velocity * motor.getMaxVelocity())
# Set the velocity of left and right motors [-1, 1].
def set_motors(self, left, right):
self.set_left_motor(left)
self.set_right_motor(right)
# Set the velocity of left and right motors [-1, 1].
def turn_right(self, speed):
self.set_left_motor(speed)
self.set_right_motor(-speed)
def turn_left(self, speed):
self.set_left_motor(-speed)
self.set_right_motor(speed)
# Set the velocity of left motors [-1, 1].
def set_left_motor(self, velocity):
self.set_motor(self.motor_left, velocity)
# Set the velocity of right motors [-1, 1].
def set_right_motor(self, velocity):
self.set_motor(self.motor_right, velocity)
# Returns the current simulation time in seconds
def get_time(self):
return self.robot.getTime()
# Returns the position of the robot in [x, z, angle] format
# The coordinate system is as follows (top-down view)
# .-------------------->x
# |\ (angle)
# | \
# | \
# | \
# | \
# | \
# |
# V
# z
#
def get_position(self):
subject = self.robot.getSelf()
position = subject.getPosition()
orientation = subject.getOrientation()
orientation = math.atan2(orientation[0], orientation[2])
return (position[0], position[2], orientation)
# Returns the position of the green balls
def get_green_balls(self):
balls = []
balls_root = self.robot.getFromDef("_balls").getField("children")
green_balls = balls_root.getMFNode(1).getField("children")
for i in range(green_balls.getCount()):
ball = green_balls.getMFNode(i)
position = ball.getPosition()
balls.append([position[0], position[1]])
# Returns the position of the green balls
def get_yellow_balls(self):
balls = []
balls_root = self.robot.getFromDef("_balls").getField("children")
green_balls = balls_root.getMFNode(1).getField("children")
yellow_balls = balls_root.getMFNode(0).getField("children")
for i in range(yellow_balls.getCount()):
ball = yellow_balls.getMFNode(i)
position = ball.getPosition()
balls.append([position[0], position[1]])
return balls
class AutobinEnv(gym.Env):
metadata = {'render.modes': ['human']}
def __init__(self, time_step):
# time step in ms
self.time_step = time_step
# supevisor node
self.robot = Supervisor()
# self node to get velocity and position
self.bin = self.robot.getSelf()
# cameras
self.cameras = []
camerasNames = ['camera1', 'camera2']
for name in camerasNames:
self.cameras.append(self.robot.getCamera(name))
self.cameras[-1].enable(self.time_step)
self.img_size = (self.cameras[0].getWidth(),
self.cameras[0].getHeight())
# wheels
self.wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for name in wheelsNames:
self.wheels.append(self.robot.getMotor(name))
self.wheels[-1].setPosition(float('inf'))
self.wheels[-1].setVelocity(0.0)
# ball
self.ball = self.robot.getFromDef('Ball')
def step(self, action):
self.wheels[0].setVelocity(action[0])
self.wheels[1].setVelocity(action[1])
self.wheels[2].setVelocity(action[0])
self.wheels[3].setVelocity(action[1])
# step the world
if self.robot.step(self.time_step) == -1:
print('The world finishes!')
return None
imgs = [
Image.frombytes('RGB', self.img_size, cam.getImage())
for cam in self.cameras
]
vel = self.bin.getVelocity()
pos = self.bin.getPosition()
done = False
reward = 0
contact = self.ball.getNumberOfContactPoints()
if contact > 0:
vel_len = math.sqrt(vel[0] * vel[0] + vel[1] * vel[1])
if self.bin.getNumberOfContactPoints() > 0:
reward = 20 - vel_len
print(
f'Autobin successfully catches the ball! Reward is {reward:.3f}'
)
else:
contact_pos = self.ball.getContactPoint(0)
dx = contact_pos[0] - pos[0]
dy = contact_pos[1] - pos[1]
dist = math.sqrt(dx * dx + dy * dy)
reward = -vel_len - dist
print(f'Autobin misses ball! Reward is {reward:.3f}')
self.reset()
return imgs, reward, done, {'velocity': vel, 'position': pos}
def reset(self):
pos = self.ball.getField('translation')
pos.setSFVec3f([0, 5, 0])
max_vel = 2
theta = random.random() * 2 * math.pi
vel_x = max_vel * math.cos(theta)
vel_y = max_vel * math.sin(theta)
self.ball.setVelocity([vel_x, vel_y, 0, 0, 0, 0])
def render(self, mode='human'):
pass
class UR5E():
def __init__(self):
self.sup = Supervisor()
self.timeStep = int(4 * self.sup.getBasicTimeStep())
# Create the arm chain from the URDF
self.armChain = Chain.from_urdf_file('ur5e.urdf')
self.arm_motors = self.getARMJoint()
self.grip_motors = self.getGripperJoint()
'''def __init__(self):
self.robot = Supervisor()
self.arm = self.robot.getSelf()
self.timeStep = int(4 * self.robot.getBasicTimeStep())
filename = None
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(self.robot.getUrdf().encode('utf-8'))
self.armChain = Chain.from_urdf_file(filename)
print(self.armChain)
# Initialize the arm motors.
self.arm_motors = []
for link in self.armChain.links:
if 'sensor' in link.name:
motor = self.robot.getMotor(link.name.replace('sensor', 'motor'))
motor.setVelocity(1.0)
self.arm_motors.append(motor)'''
def getARMJoint(self):
base_joint = self.sup.getMotor("shoulder_pan_joint")
base_joint.setVelocity(0.5)
joint1 = self.sup.getMotor("shoulder_lift_joint")
joint1.setVelocity(0.5)
joint2 = self.sup.getMotor("elbow_joint")
joint2.setVelocity(0.5)
joint3 = self.sup.getMotor("wrist_1_joint")
joint3.setVelocity(0.5)
joint4 = self.sup.getMotor("wrist_2_joint")
joint4.setVelocity(0.5)
joint5 = self.sup.getMotor("wrist_3_joint")
joint5.setVelocity(0.5)
# send joint command
'''
base_joint.setPosition(0.2)
joint1.setPosition(-1.8)
joint2.setPosition(-1.6)
joint3.setPosition(-1.32)
joint4.setPosition(1.57)
joint5.setPosition(0.2)
'''
return [base_joint, joint1, joint2, joint3, joint4, joint5]
# set base position
def getGripperJoint(self):
finger_1_joint_1 = self.sup.getMotor('finger_1_joint_1')
finger_2_joint_1 = self.sup.getMotor('finger_1_joint_2')
finger_middle_joint_1 = self.sup.getMotor('finger_1_joint_3')
return [finger_1_joint_1, finger_2_joint_1, finger_middle_joint_1]
def simpleDemo(self, target="target2"):
target = self.sup.getFromDef(target)
arm = self.sup.getSelf()
for motor in self.grip_motors:
print(motor)
motor.setPosition(1)
#self.grip_motors[0].setPosition(1)
'''
self.arm_motors[0].setPosition(0.2)
self.arm_motors[1].setPosition(-1.8)
self.arm_motors[2].setPosition(-1.6)
self.arm_motors[3].setPosition(-1.32)
self.arm_motors[4].setPosition(1.57)
self.arm_motors[5].setPosition(0.2)
'''
'''
