def testCurveToTransformation(self):
rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez())
for r in rvs:
bsp = bsplines.BSplinePose(4,r)
# Build a random, valid transformation.
T1 = bsp.curveValueToTransformation(numpy.random.random(6))
p = bsp.transformationToCurveValue(T1)
T2 = bsp.curveValueToTransformation(p)
self.assertMatricesEqual(T1, T2, 1e-9,"Checking the invertiblity of the transformation to curve values:")
def testAngularVelocity(self):
bsp = bsplines.BSplinePose(4,sm.EulerAnglesZYX())
# Create two
e1 = numpy.array([0,0,0,0,0,0])
T_n_0 = bsp.curveValueToTransformation(e1)
e2 = numpy.array([0,0,0,math.pi*0.5,0,0])
T_n_1 = bsp.curveValueToTransformation(e2)
# Initialize the curve.
bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1)
def testInversePose2(self):
rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez())
for r in rvs:
bsp = bsplines.BSplinePose(4,r)
# Create two random transformations.
T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6))
T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6))
# Initialize the curve.
bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1)
for t in numpy.arange(0.0,1.0,0.1):
T = bsp.transformation(t)
invT,J,C = bsp.inverseTransformationAndJacobian(t)
one = numpy.dot(T,invT)
self.assertMatricesEqual(one,numpy.eye(4),1e-14,"T * inv(T)")
def testAngularVelocityBodyFrameJacobian(self):
r = sm.EulerAnglesZYX();
for order in range(2,7):
bsp = bsplines.BSplinePose(order,r)
T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6))
T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6))
# Initialize the curve.
bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1)
for t in numpy.linspace(bsp.t_min(), bsp.t_max(), 4):
oJI = bsp.angularVelocityBodyFrameAndJacobian(t);
#print "TJI: %s" % (TJI)
je = nd.Jacobian(lambda c: bsp.setLocalCoefficientVector(t,c) or bsp.angularVelocityBodyFrame(t))
estJ = je(bsp.localCoefficientVector(t))
J = oJI[1]
self.assertMatricesEqual(J, estJ, 1e-9,"omega Jacobian")
def testInverseOrientationJacobian(self):
rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez())
for r in rvs:
for order in range(2,7):
bsp = bsplines.BSplinePose(order,r)
T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6))
T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6))
# Initialize the curve.
bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1)
for t in numpy.linspace(bsp.t_min(), bsp.t_max(), 4):
# Create a random homogeneous vector
v = numpy.random.random(3)
CJI = bsp.inverseOrientationAndJacobian(t);
#print "TJI: %s" % (TJI)
je = nd.Jacobian(lambda c: bsp.setLocalCoefficientVector(t,c) or numpy.dot(bsp.inverseOrientation(t), v))
estJ = je(bsp.localCoefficientVector(t))
JT = CJI[1]
J = numpy.dot(sm.crossMx(numpy.dot(CJI[0],v)), JT)
self.assertMatricesEqual(J, estJ, 1e-8,"C_n_0")