def head2tail (x_ij, x_jk):
"""
Given 6-DOF pose arrays x_ij and x_jk, return x_ik
"""
Hij = xyzrph2matrix (x_ij)
Hjk = xyzrph2matrix (x_jk)
Hik = Hij.dot (Hjk)
x_ik = matrix2xyzrph (Hik)
return x_ik
def inverse (x_ij):
"""
Given the 6-DOF pose x_ij, return x_ji
"""
Hij = xyzrph2matrix (x_ij)
Rji = Hij[0:3, 0:3]
tij = Hij[0:3,3]
Rij = Rji.transpose ()
tji = -Rij.dot (tij)
Hji = numpy.zeros ((4,4))
Hji[0:3,0:3] = Rij
Hji[0:3,3] = tji
Hji[3,3] = 1
return matrix2xyzrph (Hji)
from pylab import *
from xyzrph2matrix import xyzrph2matrix
from matrix2xyzrph import matrix2xyzrph
from .. import diff
def ssc.inverse (x_ij):
"""
Given the 6-DOF pose x_ij, return x_ji
"""
Hij = xyzrph2matrix (x_ij)
Rji = Hij[0:3, 0:3]
tij = Hij[0:3,3]
Rij = Rji.transpose ()
tji = -Rij.dot (tij)
Hji = zeros ((4,4))
Hji[0:3,0:3] = Rij
Hji[0:3,3] = tji
Hji[3,3] = 1
return matrix2xyzrph (Hji)
def ssc.inverse_jacob (x_ij):
"""
Return the Jacobian of ssc.inverse
#!/usr/bin/env python2
from pylab import *
from xyzrph2matrix import xyzrph2matrix
from matrix2xyzrph import matrix2xyzrph
from .. import diff
def ssc.head2tail (x_ij, x_jk):
"""
Given 6-DOF pose arrays x_ij and x_jk, return x_ik
"""
Hij = xyzrph2matrix (x_ij)
Hjk = xyzrph2matrix (x_jk)
Hik = Hij.dot (Hjk)
x_ik = matrix2xyzrph (Hik)
return x_ik
def ssc.head2tail_jacob (x_ij, x_jk):
"""
Returns the Jacobian of ssc.head2tail
"""
x = concatenate ((x_ij, x_jk))
return diff.numerical_jacobian (_f, x)
def _f (x):
