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) ''' '''