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