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