Python rpy2tr Examples

Example #1

File: matrix-util.py Project: nim65s/sot-core

 def test_rpy_tr(self):
     for rpy, tr in [
         ((0, 0, 0), mod.matrixToTuple(np.identity(4))),
         ((np.pi, 0, 0), ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
         ((0, np.pi, 0), ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
         ((0, 0, np.pi), ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))),
     ]:
         np.testing.assert_allclose(tr, mod.rpy2tr(*rpy), atol=1e-15)

Example #2

File: meta_tasks.py Project: andreadelprete/sot-core

def generic6dReference(p):
    M=eye(4)
    if isinstance(p,(matrix,ndarray)) and p.size == 3: M[0:3,3] = p
    elif isinstance(p,tuple) and len(p) == 3:  M[0:3,3] = p
    elif isinstance(p,(matrix,ndarray)) and p.shape == (4,4): M = p
    elif isinstance(p,(matrix,tuple)) and len(p) == 4 == len(p[0]) == len(p[1]) == len(p[2]) == len(p[3]) : M = matrix(p)
    elif isinstance(p,(matrix,ndarray,tuple)) and len(p) == 6 :
        M = array( rpy2tr(*p[3:7]) )
        M[0:3,3] = p[0:3]
    else: print "Position with other parameters ... todo"
    return M

Example #3

File: meta_tasks.py Project: proyan/sot-core

def generic6dReference(p):
    M=eye(4)
    if isinstance(p,(matrix,ndarray)) and p.size == 3: M[0:3,3] = p
    elif isinstance(p,tuple) and len(p) == 3:  M[0:3,3] = p
    elif isinstance(p,(matrix,ndarray)) and p.shape == (4,4): M = p
    elif isinstance(p,(matrix,tuple)) and len(p) == 4 == len(p[0]) == len(p[1]) == len(p[2]) == len(p[3]) : M = matrix(p)
    elif isinstance(p,(matrix,ndarray,tuple)) and len(p) == 6 :
        M = array( rpy2tr(*p[3:7]) )
        M[0:3,3] = p[0:3]
    else: print "Position with other parameters ... todo"
    return M

Example #4

File: meta_tasks_dyn.py Project: elie-moussy/sot-dyninv

def goto6d(task,position,gain=None):
    M=eye(4)
    if isinstance(position,matrix): position = vectorToTuple(position)
    if( len(position)==3 ): M[0:3,3] = position
    else:
        #print "Position 6D with rotation ... todo" # done?
        M = array( rpy2tr(*position[3:7]) )
        M[0:3,3] = position[0:3]
    task.feature.selec.value = "111111"
    setGain(task.gain,gain)
    task.featureDes.position.value = matrixToTuple(M)
    task.task.resetJacobianDerivative()

Example #5

File: meta_tasks_dyn.py Project: nim65s/sot-dyninv

def goto6d(task, position, gain=None):
    M = eye(4)
    if isinstance(position, matrix): position = vectorToTuple(position)
    if (len(position) == 3): M[0:3, 3] = position
    else:
        #print "Position 6D with rotation ... todo" # done?
        M = array(rpy2tr(*position[3:7]))
        M[0:3, 3] = position[0:3]
    task.feature.selec.value = "111111"
    setGain(task.gain, gain)
    task.featureDes.position.value = matrixToTuple(M)
    task.task.resetJacobianDerivative()

Example #6

File: meta_tasks_dyn.py Project: elie-moussy/sot-dyninv

def gotoNd(task,position,selec,gain=None,resetJacobian=True):
    M=eye(4)
    if isinstance(position,matrix): position = vectorToTuple(position)
    if( len(position)==3 ): M[0:3,3] = position
    else:
        #print "Position 6D with rotation ... todo" # done?
        M = array( rpy2tr(*position[3:7]) )
        M[0:3,3] = position[0:3]
    if isinstance(selec,str):   task.feature.selec.value = selec
    else: task.feature.selec.value = toFlags(selec)
    task.featureDes.position.value = matrixToTuple(M)
    if resetJacobian: task.task.resetJacobianDerivative()
    setGain(task.gain,gain)

Example #7

File: meta_tasks_dyn.py Project: nim65s/sot-dyninv

def gotoNd(task, position, selec, gain=None, resetJacobian=True):
    M = eye(4)
    if isinstance(position, matrix): position = vectorToTuple(position)
    if (len(position) == 3): M[0:3, 3] = position
    else:
        #print "Position 6D with rotation ... todo" # done?
        M = array(rpy2tr(*position[3:7]))
        M[0:3, 3] = position[0:3]
    if isinstance(selec, str): task.feature.selec.value = selec
    else: task.feature.selec.value = toFlags(selec)
    task.featureDes.position.value = matrixToTuple(M)
    if resetJacobian: task.task.resetJacobianDerivative()
    setGain(task.gain, gain)

Example #8

File: meta_task_dyn_passing_point.py Project: nim65s/sot-dyninv

def goto6dPP(task, position, velocity, duration, current):
    if isinstance(position, matrix): position = vectorToTuple(position)
    if (len(position) == 3):
        ''' If only position specification '''
        M = eye(4)
        M[0:3, 3] = position
    else:
        ''' If there is position and orientation '''
        M = array(rpy2tr(*position[3:7]))
        M[0:3, 3] = position[0:3]
    task.feature.selec.value = "111111"
    task.task.controlSelec.value = "111111"
    task.featureDes.position.value = matrixToTuple(M)
    task.task.duration.value = duration
    task.task.initialTime.value = current
    task.task.velocityDes.value = velocity
    task.task.resetJacobianDerivative()

Example #9

File: meta_task_dyn_passing_point.py Project: elie-moussy/sot-dyninv

def goto6dPP(task, position, velocity, duration, current):
    if isinstance(position,matrix): position = vectorToTuple(position)
    if( len(position)==3 ):
        ''' If only position specification '''
        M=eye(4)
        M[0:3,3] = position
    else:
        ''' If there is position and orientation '''
        M = array( rpy2tr(*position[3:7]) )
        M[0:3,3] = position[0:3]
    task.feature.selec.value = "111111"
    task.task.controlSelec.value = "111111"
    task.featureDes.position.value = matrixToTuple(M)
    task.task.duration.value = duration
    task.task.initialTime.value = current
    task.task.velocityDes.value = velocity
    task.task.resetJacobianDerivative()

Example #10

File: meta_task_dyn_passing_point.py Project: nim65s/sot-dyninv

def gotoNdPP(task, position, velocity, selec, duration, current):
    if isinstance(position, matrix): position = vectorToTuple(position)
    if (len(position) == 3):
        M = eye(4)
        M[0:3, 3] = position
    else:
        M = array(rpy2tr(*position[3:7]))
        M[0:3, 3] = position[0:3]
    if isinstance(selec, str):
        task.feature.selec.value = selec
        task.task.controlSelec.value = selec
    else:
        task.feature.selec.value = toFlags(selec)
        task.task.controlSelec.value = toFlags(selec)
    task.featureDes.position.value = matrixToTuple(M)
    task.task.duration.value = duration
    task.task.initialTime.value = current
    task.task.velocityDes.value = velocity
    task.task.resetJacobianDerivative()

Example #11

File: meta_task_dyn_passing_point.py Project: elie-moussy/sot-dyninv

def gotoNdPP(task, position, velocity, selec, duration, current):
    if isinstance(position,matrix): position = vectorToTuple(position)
    if( len(position)==3 ):
        M=eye(4)
        M[0:3,3] = position
    else:
        M = array( rpy2tr(*position[3:7]) )
        M[0:3,3] = position[0:3]
    if isinstance(selec,str):  
        task.feature.selec.value = selec
        task.task.controlSelec.value = selec
    else:
        task.feature.selec.value = toFlags(selec)
        task.task.controlSelec.value = toFlags(selec)
    task.featureDes.position.value = matrixToTuple(M)
    task.task.duration.value = duration
    task.task.initialTime.value = current
    task.task.velocityDes.value = velocity
    task.task.resetJacobianDerivative()