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