def makeNameToIndexConverter(robot, autotranslate=True):
""" A closure to easily convert from a string joint name to the robot's
actual DOF index.
Example usage:
#create function for a robot
pose=robot.GetDOFValues()
ind = make_name_to_index_converter(robot)
#Use the function to find an index in a vector of DOF values
pose[ind('LHP')]=pi/4
#This way you don't have to remember the DOF index of a joint to tweak it.
NOTE: Deprecated 3/25/2013, use new naming convention
"""
return _oh.make_name_to_index_converter(robot, autotranslate)
def makeNameToIndexConverter(robot,autotranslate=True):
""" A closure to easily convert from a string joint name to the robot's
actual DOF index.
Example usage:
#create function for a robot
pose=robot.GetDOFValues()
ind = make_name_to_index_converter(robot)
#Use the function to find an index in a vector of DOF values
pose[ind('LHP')]=pi/4
#This way you don't have to remember the DOF index of a joint to tweak it.
NOTE: Deprecated 3/25/2013, use new naming convention
"""
return _oh.make_name_to_index_converter(robot,autotranslate)
def read_text_traj(filename,robot,dt=.01,scale=1.0):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign
convention and scale)
1000 1000 pi/180 pi/180 ... pi/180 (scale of your units wrt openrave
default)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
ind=_oh.make_name_to_index_converter(robot)
f=open(filename,'r')
#Read in header row to find joint names
header=f.readline().rstrip()
#print header.split(' ')
[traj,config]=create_trajectory(robot)
k=0
indices={}
Tindices={}
Tmap={'X':0,'Y':1,'Z':2,'R':3,'P':4,'W':5}
for s in header.split(' '):
j=ind(s)
if j>=0:
indices.setdefault(k,j)
try:
Tindices.setdefault(k,Tmap[s])
except KeyError:
pass
except:
raise
k=k+1
#Read in sign row
signlist=f.readline().rstrip().split(' ')
signs=[]
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with _np.zeros if not used)
offsetlist=f.readline().rstrip().split(' ')
offsets=[float(x) for x in offsetlist]
#Read in scale row (fill with _np.ones if not used)
scalelist=f.readline().rstrip().split(' ')
scales=[float(x) for x in scalelist]
pose=_oh.Pose(robot)
for line in f:
vals=[float(x) for x in line.rstrip().split(' ')]
Tdata=_np.zeros(6)
for i,v in enumerate(vals):
if indices.has_key(i):
pose[indices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
elif Tindices.has_key(i):
Tdata[Tindices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
#TODO: clip joint vals at limits
#TODO: use axis angle form for RPY data?
T=array(_comps.Transform.make_transform(Tdata[3:],Tdata[0:3]))
traj_append(traj,pose.to_waypt(dt,_rave.poseFromMatrix(T)))
return traj
def read_text_traj(filename,robot,dt=.01,scale=1.0):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign
convention and scale)
1000 1000 pi/180 pi/180 ... pi/180 (scale of your units wrt openrave
default)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
ind=_oh.make_name_to_index_converter(robot)
f=open(filename,'r')
#Read in header row to find joint names
header=f.readline().rstrip()
#print header.split(' ')
[traj,config]=create_trajectory(robot)
k=0
indices={}
Tindices={}
Tmap={'X':0,'Y':1,'Z':2,'R':3,'P':4,'W':5}
for s in header.split(' '):
j=ind(s)
if j>=0:
indices.setdefault(k,j)
try:
Tindices.setdefault(k,Tmap[s])
except KeyError:
pass
except:
raise
k=k+1
#Read in sign row
signlist=f.readline().rstrip().split(' ')
signs=[]
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with _np.zeros if not used)
offsetlist=f.readline().rstrip().split(' ')
offsets=[float(x) for x in offsetlist]
#Read in scale row (fill with _np.ones if not used)
scalelist=f.readline().rstrip().split(' ')
scales=[float(x) for x in scalelist]
pose=_oh.Pose(robot)
for line in f:
vals=[float(x) for x in line.rstrip().split(' ')]
Tdata=_np.zeros(6)
for i,v in enumerate(vals):
if indices.has_key(i):
pose[indices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
elif Tindices.has_key(i):
Tdata[Tindices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
#TODO: clip joint vals at limits
#TODO: use axis angle form for RPY data?
T=array(_comps.Transform.make_transform(Tdata[3:],Tdata[0:3]))
traj_append(traj,pose.to_waypt(dt,_rave.poseFromMatrix(T)))
return traj
