def __init__(self, robotfile): """ Initialize the simulator. """ # Initialize the Cluster self.cluster = Cluster() # Load robot data self.loadRobotData(robotfile) self.gait = 0 # Get the module ID correspondence self.getKeyOrders(self.config) # Initialize the time self.starttime = time.time()
def __init__(self,robotfile): """ Initialize the simulator. """ # Initialize the Cluster self.cluster = Cluster() # Load robot data self.loadRobotData(robotfile) self.gait = 0 # Get the module ID correspondence self.getKeyOrders(self.config) # Initialize the time self.starttime = time.time()
class CKBotRun: """ CKBot Hardware Runtime Class """ def __init__(self, robotfile): """ Initialize the simulator. """ # Initialize the Cluster self.cluster = Cluster() # Load robot data self.loadRobotData(robotfile) self.gait = 0 # Get the module ID correspondence self.getKeyOrders(self.config) # Initialize the time self.starttime = time.time() def loadRobotData(self, filename): """ Loads full robot information from a text file that specifies: 1. Configuration Matrix. 2. Relative rotation and translation from the origin, in CKBot length units. 3. A set of gaits and target gait execution times. """ # Clear all previous information self.config = "none" self.connM = [] self.gaits = [] self.cluster.populate() # Open the text file. data = open(filename, "r") # Go through the text file line by line. reading = "none" for line in data: linesplit = line.split() # If we are currently reading nothing, then we are looking for the next attribute to read. if reading == "none" and linesplit != []: if linesplit[0] == "ConfigName:": reading = "name" elif linesplit[0] == "ConnMatrix:": reading = "matrix" elif linesplit[0] == "ForwardVector:": reading = "fwd_vector" elif linesplit[0] == "Gaits:": reading = "gait" # If we are currently reading something, the continue to do so until finished. # If we are reading the name of the configuration, then it is simply the word that makes the line under "ConfigName:" elif reading == "name": self.config = linesplit[0] reading = "none" # If we are reading the connectivity matrix, then read the matrix row by row until we reach whitespace. elif reading == "matrix": if linesplit == []: reading = "none" else: temprow = [] for num in linesplit: temprow.append(int(num)) self.connM.append(temprow) # Forward vector reading thingy elif reading == "fwd_vector": if linesplit == []: reading = "none" else: sign = linesplit[0] axis = linesplit[1] if sign == "+": coeff = 1 else: coeff = -1 if axis == "x": self.fwdvec = [coeff * 1, 0, 0] elif axis == "y": self.fwdvec = [0, coeff * 1, 0] elif axis == "z": self.fwdvec = [0, 0, coeff * 1] # If we are reading gaits, it is more complicated. We must ensure to read all the gaits specified. # For each gait, we must be able to tell whether the gait is Periodic or Fixed type and # parse it accordingly. elif reading == "gait": # Read the Proportional control gain specified in the text file. if linesplit[0] == "Gain": self.gain = float(linesplit[1]) # Figure out whether the gaits are fixed or periodic if linesplit[0] == "Type": self.gaittype = linesplit[1] if self.gaittype == "Periodic": reading = "periodic_gait" else: reading = "fixed_gait" # If we are reading periodic gaits, we know our gait table is just 3 lines. # The first line is the set of amplitudes (in degrees*100) of each hinge. # The second line is the set of frequencies (in rad/s) of each hinge. # The third line is the set of phase angles (in degrees*100) of each hinge. elif reading == "periodic_gait" and linesplit != []: if linesplit[0] == "Gait": amplitudes = [] frequencies = [] phases = [] reading = "amplitude" elif reading == "amplitude": for elem in linesplit: amplitudes.append( float(elem) * (math.pi / 180.0) * (1 / 100.0)) reading = "frequency" elif reading == "frequency": for elem in linesplit: frequencies.append(float(elem)) reading = "phase" elif reading == "phase": for elem in linesplit: phases.append( float(elem) * (math.pi / 180.0) * (1 / 100.0)) tempgait = [amplitudes] tempgait.append(frequencies) tempgait.append(phases) self.gaits.append(tempgait) reading = "periodic_gait" # If we are reading fixed gaits, the gait table can be an arbitrary number of lines. # For each gait we will read all the steps until we find the last line for the gait # (which the time that the gait should loop in). elif reading == "fixed_gait" and linesplit != []: if linesplit[0] == "Gait": gaitrows = [] gaittime = 0 reading = "fixed_gait_rows" elif reading == "fixed_gait_rows": if len(linesplit) == 1: gaittime = [float(linesplit[0])] gaittime.extend(gaitrows) self.gaits.append(gaittime) reading = "fixed_gait" else: temprow = [] for elem in linesplit: temprow.append( float(elem) * (math.pi / 180.0) * (1 / 100.0)) gaitrows.append(temprow) def getKeyOrders(self, config): """ Get the Key Orders from the KeyOrders.txt file for Module ID correspondence. """ curdir = os.getcwd() if ("platforms" in curdir) and ("ckbot" in curdir): f = open("KeyOrders.txt", 'r') else: f = open('lib/platforms/ckbot/KeyOrders.txt', 'r') self.key_orders = [] for line in f: linesplit = line.split() if linesplit != []: if linesplit[0] == self.config: for idx in range(1, len(linesplit)): self.key_orders.append(int(linesplit[idx])) print self.key_orders def setGait(self, gait): """ Set the gait number for simulation """ self.gait = gait def rungait(self): """ Runs the gait specified by the object variable "self.gait" """ t = time.time() - self.starttime gait = self.gaits[self.gait - 1] # If the gait is set to zero, stop moving all hinges. if self.gait == 0: pass else: #If the gait is of periodic type, read the rows in the following format. #ROW 1: Amplitudes #ROW 2: Frequencies #ROW 3: Phases if self.gaittype == "Periodic": for module_idx in range(len(self.connM)): amplitude = gait[0][module_idx] frequency = gait[1][module_idx] phase = gait[2][module_idx] ref_ang = amplitude * math.sin(frequency * t * 0.25 + phase) ref_ang = ref_ang * (0.5 * 18000.0 / math.pi) self.cluster[self.key_orders[module_idx]].set_pos(ref_ang) # If the gait is of fixed type, run the function "gaitangle" to interpolate between # reference hinge angles at the current time. else: for module_idx in range(len(self.connM)): ref_ang = self.gaitangle(gait, t, module_idx) ref_ang = ref_ang * (0.5 * 18000.0 / math.pi) self.cluster[self.key_orders[module_idx]].set_pos(ref_ang) def gaitangle(self, gait, time, module): """ Takes in a gait matrix and returns the reference angle at that point in time. """ nummoves = len(gait) - 1 gaittime = gait[0] singletime = float(gaittime) / (nummoves - 1) timearray = [] for i in range(0, nummoves): if i == 0: timearray.append(0) else: timearray.append(timearray[i - 1] + singletime) currenttime = (time % gaittime) / singletime globalref = gait[int(math.ceil(currenttime)) + 1][module] globalprev = gait[int(math.floor(currenttime)) + 1][module] if globalref == globalprev: # If the current time coincides with the time of a given gait angle direction, then there is no need to interpolate. localref = globalref else: # Linear interpolation step. interp = (currenttime * singletime - timearray[int(math.floor(currenttime))]) / ( timearray[int(math.ceil(currenttime))] - timearray[int(math.floor(currenttime))]) localref = globalprev + interp * (globalref - globalprev) return localref def run(self): """ Start the demo. This method will block until the demo exits. This method is used if the simulator is run stand-alone. """ self._running = True # Receive Locomotion commands for all the hinges from LTLMoP. # Use these commands as reference angles for simple P-controlled servos. while self._running: self.rungait() def reconfigure(self, name): """ Updates the configuration of robots. """ # Zero out all the joint angles #for module_idx in range(len(self.connM)): # self.cluster[module_idx].set_pos(0) # Load the new robot data from the new file "name".ckbot. print("==========\nReconfiguring: " + name + "\n==========") robotfile = "lib/platforms/ckbot/config/" + name + ".ckbot" x = raw_input( "Type 'OK' when robot is reconfigured (in the command line): ") # Load new data and update the module ID correspondence self.loadRobotData(robotfile) self.getKeyOrders(self.config) self.gait = 0 def run_once(self): """ Run one simulation step -- used for LTLMoP integration. """ self._running = True # Receive Locomotion commands for all the hinges from LTLMoP. # Use these commands as reference angles for simple P-controlled servos. self.rungait()
import math, time, copy, sys sys.path.append("../../../../../Downloads/CKBot/trunk") from ckbot.logical import Cluster """ CKBot Hardware Calibration Script [Sebastian Castro - Cornell University] [Autonomous Systems Laboratory - 2011] """ if (__name__ == '__main__'): # Instantiate a Cluster print "Generating Cluster...\n" c = Cluster() c.populate() # Prompt the user to confirm is the number of modules detected is correct. print "There are %d modules in the current cluster." %len(c) okay = raw_input("Is this OK? Y/N: ") # Quit if the user does not agree with the number of modules detected in the cluster. if (okay.lower()=="n"): pass else: # Continue prompting the user to enter yes(Y) or no(N) if they enter anything else. while(okay.lower()!="y"): okay = raw_input("Please enter Y/N: ") # Loop through all the modules and prompt the user to say what number the module is.
class CKBotRun: """ CKBot Hardware Runtime Class """ def __init__(self,robotfile): """ Initialize the simulator. """ # Initialize the Cluster self.cluster = Cluster() # Load robot data self.loadRobotData(robotfile) self.gait = 0 # Get the module ID correspondence self.getKeyOrders(self.config) # Initialize the time self.starttime = time.time() def loadRobotData(self,filename): """ Loads full robot information from a text file that specifies: 1. Configuration Matrix. 2. Relative rotation and translation from the origin, in CKBot length units. 3. A set of gaits and target gait execution times. """ # Clear all previous information self.config = "none" self.connM = [] self.gaits = [] self.cluster.populate() # Open the text file. data = open(filename,"r") # Go through the text file line by line. reading = "none" for line in data: linesplit = line.split() # If we are currently reading nothing, then we are looking for the next attribute to read. if reading == "none" and linesplit != []: if linesplit[0] == "ConfigName:": reading = "name" elif linesplit[0] == "ConnMatrix:": reading = "matrix" elif linesplit[0] == "ForwardVector:": reading = "fwd_vector" elif linesplit[0] == "Gaits:": reading = "gait" # If we are currently reading something, the continue to do so until finished. # If we are reading the name of the configuration, then it is simply the word that makes the line under "ConfigName:" elif reading == "name": self.config = linesplit[0] reading = "none" # If we are reading the connectivity matrix, then read the matrix row by row until we reach whitespace. elif reading == "matrix": if linesplit == []: reading = "none" else: temprow = [] for num in linesplit: temprow.append(int(num)) self.connM.append(temprow) # Forward vector reading thingy elif reading == "fwd_vector": if linesplit == []: reading = "none" else: sign = linesplit[0] axis = linesplit[1] if sign == "+": coeff = 1 else: coeff = -1 if axis == "x": self.fwdvec = [coeff*1, 0, 0] elif axis == "y": self.fwdvec = [0, coeff*1, 0] elif axis == "z": self.fwdvec = [0, 0, coeff*1] # If we are reading gaits, it is more complicated. We must ensure to read all the gaits specified. # For each gait, we must be able to tell whether the gait is Periodic or Fixed type and # parse it accordingly. elif reading == "gait": # Read the Proportional control gain specified in the text file. if linesplit[0] == "Gain": self.gain = float(linesplit[1]) # Figure out whether the gaits are fixed or periodic if linesplit[0] == "Type": self.gaittype = linesplit[1] if self.gaittype == "Periodic": reading = "periodic_gait" else: reading = "fixed_gait" # If we are reading periodic gaits, we know our gait table is just 3 lines. # The first line is the set of amplitudes (in degrees*100) of each hinge. # The second line is the set of frequencies (in rad/s) of each hinge. # The third line is the set of phase angles (in degrees*100) of each hinge. elif reading == "periodic_gait" and linesplit != []: if linesplit[0] == "Gait": amplitudes = [] frequencies = [] phases = [] reading = "amplitude" elif reading == "amplitude": for elem in linesplit: amplitudes.append( float(elem)*(math.pi/180.0)*(1/100.0) ) reading = "frequency" elif reading == "frequency": for elem in linesplit: frequencies.append( float(elem) ) reading = "phase" elif reading == "phase": for elem in linesplit: phases.append( float(elem)*(math.pi/180.0)*(1/100.0) ) tempgait = [amplitudes] tempgait.append(frequencies) tempgait.append(phases) self.gaits.append(tempgait) reading = "periodic_gait" # If we are reading fixed gaits, the gait table can be an arbitrary number of lines. # For each gait we will read all the steps until we find the last line for the gait # (which the time that the gait should loop in). elif reading == "fixed_gait" and linesplit != []: if linesplit[0] == "Gait": gaitrows = [] gaittime = 0 reading = "fixed_gait_rows" elif reading == "fixed_gait_rows": if len(linesplit)==1: gaittime = [float(linesplit[0])] gaittime.extend(gaitrows) self.gaits.append(gaittime) reading = "fixed_gait" else: temprow = [] for elem in linesplit: temprow.append( float(elem)*(math.pi/180.0)*(1/100.0) ) gaitrows.append(temprow) def getKeyOrders(self,config): """ Get the Key Orders from the KeyOrders.txt file for Module ID correspondence. """ curdir = os.getcwd() if ("platforms" in curdir) and ("ckbot" in curdir): f = open("KeyOrders.txt",'r') else: f = open('lib/platforms/ckbot/KeyOrders.txt','r') self.key_orders = [] for line in f: linesplit = line.split() if linesplit != []: if linesplit[0] == self.config: for idx in range(1,len(linesplit)): self.key_orders.append(int(linesplit[idx])) print self.key_orders def setGait(self,gait): """ Set the gait number for simulation """ self.gait = gait def rungait(self): """ Runs the gait specified by the object variable "self.gait" """ t = time.time() - self.starttime gait = self.gaits[self.gait - 1] # If the gait is set to zero, stop moving all hinges. if self.gait == 0: pass else: #If the gait is of periodic type, read the rows in the following format. #ROW 1: Amplitudes #ROW 2: Frequencies #ROW 3: Phases if self.gaittype == "Periodic": for module_idx in range(len(self.connM)): amplitude = gait[0][module_idx] frequency = gait[1][module_idx] phase = gait[2][module_idx] ref_ang = amplitude*math.sin(frequency*t*0.25 + phase) ref_ang = ref_ang*(0.5*18000.0/math.pi) self.cluster[self.key_orders[module_idx]].set_pos(ref_ang) # If the gait is of fixed type, run the function "gaitangle" to interpolate between # reference hinge angles at the current time. else: for module_idx in range(len(self.connM)): ref_ang = self.gaitangle(gait,t,module_idx) ref_ang = ref_ang*(0.5*18000.0/math.pi) self.cluster[self.key_orders[module_idx]].set_pos(ref_ang) def gaitangle(self,gait,time,module): """ Takes in a gait matrix and returns the reference angle at that point in time. """ nummoves = len(gait)-1 gaittime = gait[0] singletime = float(gaittime)/(nummoves-1) timearray = [] for i in range(0,nummoves): if i==0: timearray.append(0) else: timearray.append(timearray[i-1]+singletime) currenttime = (time%gaittime)/singletime globalref = gait[int(math.ceil(currenttime))+1][module] globalprev = gait[int(math.floor(currenttime))+1][module] if globalref==globalprev: # If the current time coincides with the time of a given gait angle direction, then there is no need to interpolate. localref = globalref else: # Linear interpolation step. interp = (currenttime*singletime - timearray[int(math.floor(currenttime))])/(timearray[int(math.ceil(currenttime))] - timearray[int(math.floor(currenttime))]) localref = globalprev + interp*(globalref-globalprev) return localref def run(self): """ Start the demo. This method will block until the demo exits. This method is used if the simulator is run stand-alone. """ self._running = True # Receive Locomotion commands for all the hinges from LTLMoP. # Use these commands as reference angles for simple P-controlled servos. while self._running: self.rungait() def reconfigure(self, name): """ Updates the configuration of robots. """ # Zero out all the joint angles #for module_idx in range(len(self.connM)): # self.cluster[module_idx].set_pos(0) # Load the new robot data from the new file "name".ckbot. print("==========\nReconfiguring: " + name + "\n==========") robotfile = "lib/platforms/ckbot/config/" + name + ".ckbot" x = raw_input("Type 'OK' when robot is reconfigured (in the command line): ") # Load new data and update the module ID correspondence self.loadRobotData(robotfile) self.getKeyOrders(self.config) self.gait = 0 def run_once(self): """ Run one simulation step -- used for LTLMoP integration. """ self._running = True # Receive Locomotion commands for all the hinges from LTLMoP. # Use these commands as reference angles for simple P-controlled servos. self.rungait()