def __init__(self): """ Initializes the controller's motor which behaves like an oscillator and creates two connectors. """ Robot.__init__(self) # Derive the oscillator id from its name, which is usually s.th. like "module_1" self.id = int(self.getName()[-1]) - 1 # Get access to the motor self.motor = self.getMotor('motor') # Get access to the connectors self.rear_connector = self.getConnector('rear_connector') self.front_connector = self.getConnector('front_connector') # The configuration of the oscillator is stored in a file which can be changed by a supervisor self.config = {} try: time.sleep(0.5) self.load_configuration() except: self.config = { 'active': False }
def __init__(self): Robot.__init__(self) self.currentlyPlaying = False # initialize stuff self.findAndEnableDevices() self.loadMotionFiles() self.printHelp()
def __init__(self): Robot.__init__(self) # initialize stuff self.number_of_params = 5 self.params = [ 'location', 'hand_configuration', 'hand_orientation', 'movement', 'movement_speed' ] self.findAndEnableDevices()
def __init__(self): Robot.__init__(self) self.currentlyPlaying = False self.shouldTurn120Degrees = False self.turnsMade = 0 # initialize stuff self.findAndEnableDevices() self.loadMotionFiles() self.printHelp()
def __init__(self, noise): Robot.__init__(self) self.noise = noise self.left_wheel = self.getMotor('left wheel motor') self.right_wheel = self.getMotor('right wheel motor') self.front_slider = self.getMotor('front slider motor') self.bottom_slider = self.getMotor('bottom slider motor') self.left_wheel.setPosition(float('inf')) self.right_wheel.setPosition(float('inf')) self.front_slider.setPosition(float('inf')) self.bottom_slider.setPosition(float('inf'))
def __init__(self): Robot.__init__(self) self.currentlyPlaying = False # initialize stuff self.findAndEnableDevices() # self.receive_motion_file() # to receive messages: self.receiver = self.getReceiver('receiver') self.receiver.enable(self.timeStep) self.printHelp()
def __init__(self): Robot.__init__(self) self.currentlyPlaying = False # initialize stuff self.findAndEnableDevices() self.loadMotionFiles() #self.printHelp() # ROS rospy.init_node('controller', anonymous=True) pub = rospy.Publisher('motor', Float64, queue_size=10) rospy.Subscriber("spikes", String, self.callback)
def __init__(self): Robot.__init__(self) # You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # led = self.getLed('ledname') print "Program Start" # --------------------------------------------- # ------------- Robot Start Setup ------------- # --------------------------------------------- def runCommand(self, key): self.RHP = self.getServo('RHP') #self.setPosition(RHP, 45.2) # --------------------------------------------- # ------------- Robot End Setup --------------- # --------------------------------------------- # from http://wiki.python.org/moin/UdpCommunication UDP_IP="127.0.0.1" UDP_PORT=11000 sock = socket.socket( socket.AF_INET, # internet socket.SOCK_DGRAM ) # UDP sock.bind( (UDP_IP,UDP_PORT) ) # Main loop while True: print "waiting for UDP" rxData, addr = sock.recvfrom( 1024 ) # buffer size is 1024 bytes print "received message Length:", len(rxData) print "Data 1:", ord(rxData[1]) #print "Data 1:", data; N = ord(rxData[2]) # message length Nn = len(rxData) # received message length NN = Nn - N # difference between received length and message length (should be 4) #Servo.setPosition("RHP",20) #for i in range(0,N) # Servo.setPosition("RHP",20) self.RHP.setPosition(25.2)
def __init__(self): Robot.__init__(self) #timestep info self.timeStep = 40 self.f = finfo(float) #Choose Humanoid Robot self.bot = Nao(self, self.timeStep) #Keep track of starting and stopping of walk cycle self.idle = True self.stepStop = True self.stepStart = True self.stepCount = 0 self.stepSign = -1 # -1 for right support, +1 for left support self.tStep = 0.5 self.upPhase = 0.3 self.downPhase = 0.8 self.t0 = self.getTime() self.t = 0 self.dt = 0 self.tPhase = 0. self.phShift = 0. self.key = 0 # define the motion which is currently playing self.currentlyPlaying = None self.start() #Print Menu self.bot.printHelp() self.walkForward() while not self.key: self.key = self.keyboardGetKey() self.bot.update() self.simulationStep()
def __init__(self): Robot.__init__(self) #timestep info self.timeStep = 40 self.f = finfo(float) #Choose Humanoid Robot self.bot = Nao(self, self.timeStep) #Keep track of starting and stopping of walk cycle self.idle = True self.stepStop = True self.stepStart = True self.stepCount = 0 self.stepSign = -1 # -1 for right support, +1 for left support self.tStep = 0.5 self.upPhase = 0.3 self.downPhase = 0.8 self.t0 = self.getTime() self.t = 0 self.dt = 0 self.tPhase = 0. self.phShift = 0. self.key = 0 # define the motion which is currently playing self.currentlyPlaying = None self.start() #Print Menu self.bot.printHelp() self.walkForward() while not self.key: self.key = self.keyboardGetKey() self.bot.update() self.simulationStep()
def __init__(self): Robot.__init__(self) self.tick = 0 self.parse_args() # initialize stuff self.findAndEnableDevices() if self.args.camera: # initialize top image sender w = self.cameraTop.getWidth() h = self.cameraTop.getHeight() self.topImageServer = ImageServer(self.args.tcam_addr, w, h, CamIndex.TOP) # initialize bottom image sender w = self.cameraBottom.getWidth() h = self.cameraBottom.getHeight() self.bottomImageServer = ImageServer(self.args.bcam_addr, w, h, CamIndex.BOTTOM)
def __init__(self): # initialise the underlying Robot Robot.__init__(self) # get the world's timestep and initialise the sensors self.timestep = int(self.getBasicTimeStep()) # variables that help the robot work self.pos = np.zeros(3) self.vel = np.zeros(3) self.angle = 0 # angle to x axis self.angvel = 0 self.forward = np.zeros( 3) # unit vector in the direction we are facing self.posHist = np.zeros( (10, 3)) # stores last 10 positions of robot - maybe unneccesary self.distances = np.zeros( (10, 2)) # last 10 distances, 10 rows of [leftDistance, rightDistance] self.clawAngle = 0.0 self.simTime = self.timestep self.scheduleTuples = [] self.behaviour = lambda: None self.greenLevel = 0.0 self.redLevel = 0.0 self.boxFirstEdgeTime = None self.receivedData = np.array([]) self.otherRobot = np.array([[0, 0], [0, 0], [0, 0]]) self.otherRobotPos = np.array([0, 0, 0]) self.pointsSearched = 0 # number of points in the grid that we have spun around completely and cleared self.directionCleared = np.array( [1, 0, 0] ) # direction that we have cleared up to on the current point - starting from 1, 0, 0 self.ourColourBoxes = [] self.otherColourBoxes = [] self.othersPoints = 0 self.reverseTime = 0 self.objectAvoider = 0 self.angleSearchSum = 0 # Robot dependant variables if sys.argv[1] == "red": self.colour = Colour.RED self.home = redCentre elif sys.argv[1] == "green": self.colour = Colour.GREEN self.home = greenCentre # x, y and z coords for convenience self.x, self.y, self.z = self.pos # some functions to help set wheel speeds self.stop = lambda: self.setWheelSpeeds(0.0, 0.0) self.spin = lambda: self.setWheelSpeeds(1.5, 3.0) # function to get distance sensor readings self.getDistance = lambda: distanceFromReading( np.array([ self.leftDistanceSensor.getValue(), self.rightDistanceSensor.getValue() ])) # initialise the sensors and enter the main loop self.initialiseSensors() self.mainLoop()
def __init__(self): Robot.__init__(self) # define the motion which is currently playing self.currentlyPlaying = None self.key = 0 self.timeStep = 40 self.joints = [] self.findAndEnableDevices() self.f = finfo(float) ###AshGavs edit self.fsr = self.getFsr() self.idle = True self.stepStop = True self.stepStart = True self.stepCount = 0 self.stepSign = -1 # -1 for right support, +1 for left support self.tStep = 0.5 self.upPhase = 0.3 self.downPhase = 0.8 # Nao details: self.bodyHeight = 0.28 self.dHeight = 0.00 self.tZmp = 0.17 self.stepHeight = 0.025 self.bodyRoll = 0 * pi / 180 self.bodyTilt = 0 * pi / 180 self.supportX = 0.015 self.supportY = 0 self.velCurrent = array([0., 0., 0.0]) self.velCommand = array([0., 0., 0.]) #self.setVelocity(array([1., 0, 0.])) self.velMax = 0.06 self.velScale = array([1., 2.5, .15]) self.velChange = 0.25 self.velOdometry = array([0., 0., 0.]) self.odometry = array([0., 0., 0.]) self.odometryScale = 0.99 # self.footX = 0 self.footX = -self.supportX self.footY = 0.0525 self.uLeft0 = array([self.footX, self.footY, 0.]) self.uLeft = self.uLeft0 self.uLeft1 = self.uLeft self.uLeft2 = self.uLeft self.uRight0 = array([self.footX, -self.footY, 0.]) self.uRight = self.uRight0 self.uRight1 = self.uRight self.uRight2 = self.uRight self.uBody = array([0., 0., 0.]) self.uBody1 = self.uBody self.uBody2 = self.uBody self.qLegs = zeros((12, 1), float) self.legsIndex = range(6, 18) #[6:18] #changed 7 to 6 self.qLegsIndexStart = 6 self.qLegsIndexStop = 18 self.hsupT = array([.8, .8, .8, .8, .6, .6]) self.hardnessSupport = self.hsupT.transpose() self.hswingT = array([.8, .6, .6, .6, .2, .2]) self.hardnessSwing = self.hswingT.transpose() self.qArms = zeros((8, 1), float) self.qArmsT = array([120, 15, -90, -60, 120, -15, 90, 60]) self.qArmsT2 = self.qArmsT.transpose() self.qArms0 = (pi / 180) * self.qArmsT2 print "self.qArms0: " print self.qArms0 self.armsIndex1Start = 0 self.armsIndex1Stop = 3 self.armsIndex2Start = 13 self.armsIndex2Stop = 15 self.hardnessArms = ones((8, 1), float) * .2 #self.setJointName("LAnklePitch", "RAnklePitch", -.2) #self.setJointName("LHipPitch", "RHipPitch", -.2) #self.setJointName("LKneePitch", "RKneePitch", -.2) #self.setJointName("LHipPitch", "RHipPitch", -.3) #self.setJointName("LHipRoll", "RHipRoll", -.2) self.t0 = self.getTime() self.tPhase = 0. self.phShift = 0. self.start() self.printHelp() #Move Left self.setVelocity([0.0, 0.03, 0.0]) while not self.key: self.key = self.keyboardGetKey() self.update() self.simulationStep()
def __init__(self): Robot.__init__(self) self.findAndEnableDevices()
def __init__(self): Robot.__init__(self) # define the motion which is currently playing self.currentlyPlaying = None self.key = 0 self.timeStep = 40 self.joints = [] self.findAndEnableDevices() self.f = finfo(float) ###AshGavs edit self.fsr = self.getFsr() self.idle = True self.stepStop = True self.stepStart = True self.stepCount = 0 self.stepSign = -1 # -1 for right support, +1 for left support self.tStep = 0.5 self.upPhase = 0.3 self.downPhase = 0.8 # Nao details: self.bodyHeight = 0.28 self.dHeight = 0.00 self.tZmp = 0.17 self.stepHeight = 0.025 self.bodyRoll = 0*pi/180 self.bodyTilt = 0*pi/180 self.supportX = 0.015 self.supportY = 0 self.velCurrent = array([0., 0., 0.0]) self.velCommand = array([0., 0., 0.]) #self.setVelocity(array([1., 0, 0.])) self.velMax = 0.06 self.velScale = array([1., 2.5, .15]) self.velChange = 0.25 self.velOdometry = array([0., 0., 0.]) self.odometry = array([0., 0., 0.]) self.odometryScale = 0.99 # self.footX = 0 self.footX = -self.supportX self.footY = 0.0525 self.uLeft0 = array([self.footX, self.footY, 0.]) self.uLeft = self.uLeft0 self.uLeft1 = self.uLeft self.uLeft2 = self.uLeft self.uRight0 = array([self.footX, -self.footY, 0.]) self.uRight = self.uRight0 self.uRight1 = self.uRight self.uRight2 = self.uRight self.uBody = array([0., 0., 0.]) self.uBody1 = self.uBody self.uBody2 = self.uBody self.qLegs = zeros((12,1),float) self.legsIndex = range(6,18)#[6:18] #changed 7 to 6 self.qLegsIndexStart = 6 self.qLegsIndexStop = 18 self.hsupT = array([.8, .8, .8, .8, .6, .6]) self.hardnessSupport = self.hsupT.transpose() self.hswingT = array([.8, .6, .6, .6, .2, .2]) self.hardnessSwing = self.hswingT.transpose() self.qArms = zeros((8,1),float) self.qArmsT = array([120, 15, -90, -60, 120, -15, 90, 60]) self.qArmsT2 = self.qArmsT.transpose() self.qArms0 = (pi/180)*self.qArmsT2 print "self.qArms0: " print self.qArms0 self.armsIndex1Start = 0 self.armsIndex1Stop = 3 self.armsIndex2Start = 13 self.armsIndex2Stop = 15 self.hardnessArms = ones((8,1),float)*.2 #self.setJointName("LAnklePitch", "RAnklePitch", -.2) #self.setJointName("LHipPitch", "RHipPitch", -.2) #self.setJointName("LKneePitch", "RKneePitch", -.2) #self.setJointName("LHipPitch", "RHipPitch", -.3) #self.setJointName("LHipRoll", "RHipRoll", -.2) self.t0 = self.getTime() self.tPhase = 0. self.phShift = 0. self.start() self.printHelp() #Move Left self.setVelocity([0.0, 0.03, 0.0]) while not self.key: self.key = self.keyboardGetKey() self.update() self.simulationStep()