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