Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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')
Exemplo n.º 4
0
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')