def record(self,time=None): if not time: self.data[self.count,0]=self.env.GetSimulationTime() else: self.data[self.count,0]=time for k in range(len(self.bodies)): #COM is coincident with body frame due to ODE restrictions com=self.bodies[k].GetGlobalCOM() c0=1+k*3 c1=1+3*(k+1) self.data[self.count,c0:c1]=(com) self.com[self.count,:]=_oh.find(self.robot) self.count+=1
def load_mapping(self,filename,path=None): with open(_oh.find(filename,path),'r') as f: line=f.readline() #Strip header self.affine_signs=[] self.affine_offsets=[] for k in range(6): line=f.readline() datalist=re.split(',| |\t',line.rstrip()) self.affine_signs.append(int(datalist[4])) self.affine_offsets.append(int(datalist[5])) for line in f: datalist=re.split(',| |\t',line.rstrip()) #print datalist j=self.robot.GetJoint(datalist[1]) if j: print j dof=j.GetDOFIndex() self.joint_map[dof]=int(datalist[0]) #Note that this corresponds to the IU index... self.joint_signs[dof]=int(datalist[4]) self.joint_offsets[dof]=float(datalist[5])
def load_from_file(self,filename,mapfile=None,path=None): if mapfile: self.load_mapping(mapfile,path) with open(_oh.find(filename,path)) as f: """ Format of q_path file: version string runtime timestep val0,val1...val57 """ line = f.readline() datalist=re.split(',| |\t',line)[:-1] if len(datalist)>1: #Assume header is omitted print "Hubo Version Missing" version="HuboDefault" else: version=datalist[0] #line 2 has the total time print version line = f.readline() datalist=re.split(',| |\t',line)[:-1] if len(datalist)>1: #Assume header is omitted print "Total Time Missing" #total_time=0 else: #total_time=float(datalist[0]) #line 3 has the step _np.size line = f.readline() datalist=re.split(',| |\t',line)[:-1] if len(datalist)>1: print "Time Step is Missing, assuming default..." timestep=.05 else: timestep=datalist[0] line = f.readline() datalist=re.split(',| |\t',line)[:-1] srcdata=[] count=0 for line in f: #Split configuration into a list, throwing out endline characters and strip length configlist=re.split(',| |\t',line)[:-1] #Convert to float values for the current pose data=[float(x) for x in configlist[1:]] if not(len(data) == int(configlist[0])): print "Incorrect data formatting on line P{}".format(count) srcdata.append(data) count+=1 #Convert to neat numpy array self.srcdata=array(srcdata) self.timestep=timestep #FIXME: Ugly hack to get env without knowing which one is really what we want #retime = True if _oh.check_physics(_rave.RaveGetEnvironments()[0]) else False #print retime return self.to_openrave(retime=False)
def load_parameters(self,paramfile): filename=_oh.find(paramfile) with open(filename,'r') as file_read: #line 1 stringer_width=0 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser if not line: print "stringer width Missing" else: stringer_width=float(line[0:counter-1])/2 print "stringer_width "+str(stringer_width) #line 2 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser stringer_height=0 if not line: print "stringer height Missing" else: stringer_height=float(line[0:counter-1])/2 print "stringer_height "+str(stringer_height) #line 3 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser stringer_thickness=0 if not line: print "stringer thickness Missing" else: stringer_thickness=float(line[0:counter-1])/2 print "stringer_thickness "+str(stringer_thickness) #line 4 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser rung_width=0 if not line: print "rung width Missing" else: rung_width=float(line[0:counter-1])/2 print "rung_width "+str(rung_width) #line 5 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser rung_height=0 if not line: print "rung height Missing" else: rung_height=float(line[0:counter-1])/2 print "rung_height "+str(rung_height) #line 6 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser rung_thickness=0 if not line: print "rung thickness Missing" else: rung_thickness=float(line[0:counter-1])/2 #line 7 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser ladder_angle=0 if not line: print "ladder angle Missing" else: ladder_angle=float(line[0:counter-1]) #line 8 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser rung_space=0 if not line: print "rung space Missing" else: rung_space=float(line[0:counter-1]) #line 9 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser stringer_radius=0 if not line: print "stringer_radius Missing" else: print line[0:counter-1] stringer_radius=float(line[0:counter-1]) print 'stringer radius is ' +str(stringer_radius) #line 10 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser stringer_nface=0 if not line: print "stringer_nface Missing" else: stringer_nface=float(line[0:counter-1]) #line 11 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser rung_radius=0 if not line: print "rung_radius Missing" else: rung_radius=float(line[0:counter-1]) #line 12 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser rung_nface=0 if not line: print "rung_nface Missing" else: rung_nface=float(line[0:counter-1]) #line 13 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser rung_shape=0 if not line: print "rung shape Missing" else: rung_shape=float(line[0:counter-1]) #line 14 line = file_read.readline() counter=0; length =len(line) for parser in range(0, length): if line[parser]=='\t': counter=parser stringer_shape=0 if not line: print "stringer shape Missing" else: stringer_shape=float(line[0:counter-1]) rungs=0 if stringer_shape==1: rungs=int(2*stringer_height/rung_space) else: rungs=int(stringer_nface/rung_space) self.parameters.setdefault('rungs',rungs) self.parameters.setdefault('stringer_width',stringer_width) self.parameters.setdefault('stringer_height',stringer_height) self.parameters.setdefault('stringer_radius',stringer_radius) self.parameters.setdefault('stringer_thickness',stringer_thickness) self.parameters.setdefault('stringer_nface',stringer_nface) self.parameters.setdefault('stringer_shape',stringer_shape) self.parameters.setdefault('rung_width',rung_width) self.parameters.setdefault('rung_height',rung_height) self.parameters.setdefault('rung_thickness',rung_thickness) self.parameters.setdefault('rung_space',rung_space) self.parameters.setdefault('rung_nface',rung_nface) self.parameters.setdefault('rung_radius',rung_radius) self.parameters.setdefault('rung_shape',rung_shape) self.parameters.setdefault('ladder_angle',ladder_angle) print 'Parsing done' paramname=paramfile.split('/')[-1] name_tokens=paramname.split('.') if len(name_tokens)>1: outname='.'.join(name_tokens[:-1]) else: outname=name_tokens[-1] self.scenefile=outname+'.env.xml' self.name=outname+'.kinbody.xml'