def dofmap_huboread_from_oh(robot):
#Get all the DOF's..
dofmap={}
for d in xrange(robot.GetDOF()):
n = robot.GetJointFromDOFIndex(d).GetName()
hname=mapping.ha_from_oh(n)
if hname:
dofmap[hubo_read_trajectory_map[hname]]=d
#print d,n,hname
return dofmap
def dofmap_huboread_from_oh(robot):
#Get all the DOF's..
hr_from_oh_map={}
for d in xrange(robot.GetDOF()):
n = robot.GetJointFromDOFIndex(d).GetName()
hname=mapping.ha_from_oh(n)
if hname:
hr_from_oh_map[d]=hubo_read_trajectory_map[hname]
#print d,n,hname
return hr_from_oh_map
def write_youngbum_traj(traj,robot,dt,filename='exported.traj',dofs=None,oldnames=False):
""" Create a text trajectory in youngbum's style, assuming no offsets or
scaling, and openHubo default sign convention.
"""
config=robot.GetConfigurationSpecification()
f=open(filename,'w')
namelist=[]
signlist=[]
scalelist=[]
offsetlist=[]
if dofs is None:
dofs=range(robot.GetDOF())
for d in dofs:
name=robot.GetJointFromDOFIndex(d).GetName()
if oldnames:
namelist.append(mapping.ha_from_oh(name))
else:
namelist.append(name)
#TODO make this an argument?
signlist.append('+')
offsetlist.append(0.0)
scalelist.append(1.0)
#Find overall trajectory properties
T=traj.GetDuration()
with open(filename,'w') as f:
f.write(' '.join(namelist)+'\n')
f.write(' '.join(signlist)+'\n')
f.write(' '.join(['{}'.format(x) for x in offsetlist])+'\n')
f.write(' '.join(['{}'.format(x) for x in scalelist])+'\n')
for t in _np.arange(0,T,dt):
waypt=traj.Sample(t)
vals=config.ExtractJointValues(waypt,robot,dofs)
f.write(' '.join(['{}'.format(x) for x in vals])+'\n')
def write_youngbum_traj(traj,robot,dt,filename='exported.traj',dofs=None,oldnames=False):
""" Create a text trajectory in youngbum's style, assuming no offsets or
scaling, and openHubo default sign convention.
"""
config=robot.GetConfigurationSpecification()
f=open(filename,'w')
namelist=[]
signlist=[]
scalelist=[]
offsetlist=[]
if dofs is None:
dofs=range(robot.GetDOF())
for d in dofs:
name=robot.GetJointFromDOFIndex(d).GetName()
if oldnames:
namelist.append(mapping.ha_from_oh(name))
else:
namelist.append(name)
#TODO make this an argument?
signlist.append('+')
offsetlist.append(0.0)
scalelist.append(1.0)
#Find overall trajectory properties
T=traj.GetDuration()
with open(filename,'w') as f:
f.write(' '.join(namelist)+'\n')
f.write(' '.join(signlist)+'\n')
f.write(' '.join(['{}'.format(x) for x in offsetlist])+'\n')
f.write(' '.join(['{}'.format(x) for x in scalelist])+'\n')
for t in _np.arange(0,T,dt):
waypt=traj.Sample(t)
vals=config.ExtractJointValues(waypt,robot,dofs)
f.write(' '.join(['{}'.format(x) for x in vals])+'\n')
