00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 import unittest
00024 from PyKDL import *
00025 from math import *
00026
00027 class FramesTestFunctions(unittest.TestCase):
00028
00029 def testVector2(self,v):
00030 self.assertEqual(2*v-v,v)
00031 self.assertEqual(v*2-v,v)
00032 self.assertEqual(v+v+v-2*v,v)
00033 v2=Vector(v)
00034 self.assertEqual(v,v2)
00035 v2+=v
00036 self.assertEqual(2*v,v2)
00037 v2-=v
00038 self.assertEqual(v,v2)
00039 v2.ReverseSign()
00040 self.assertEqual(v,-v2)
00041
00042 def testVector(self):
00043 v=Vector(3,4,5)
00044 self.testVector2(v)
00045 v=Vector.Zero()
00046 self.testVector2(v)
00047
00048 def testTwist2(self,t):
00049 self.assertEqual(2*t-t,t)
00050 self.assertEqual(t*2-t,t)
00051 self.assertEqual(t+t+t-2*t,t)
00052 t2=Twist(t)
00053 self.assertEqual(t,t2)
00054 t2+=t
00055 self.assertEqual(2*t,t2)
00056 t2-=t
00057 self.assertEqual(t,t2)
00058 t.ReverseSign()
00059 self.assertEqual(t,-t2)
00060
00061 def testTwist(self):
00062 t=Twist(Vector(6,3,5),Vector(4,-2,7))
00063 self.testTwist2(t)
00064 t=Twist.Zero()
00065 self.testTwist2(t)
00066 t=Twist(Vector(0,-9,-3),Vector(1,-2,-4))
00067
00068 def testWrench2(self,w):
00069 self.assertEqual(2*w-w,w)
00070 self.assertEqual(w*2-w,w)
00071 self.assertEqual(w+w+w-2*w,w)
00072 w2=Wrench(w)
00073 self.assertEqual(w,w2)
00074 w2+=w
00075 self.assertEqual(2*w,w2)
00076 w2-=w
00077 self.assertEqual(w,w2)
00078 w.ReverseSign()
00079 self.assertEqual(w,-w2)
00080
00081 def testWrench(self):
00082 w=Wrench(Vector(7,-1,3),Vector(2,-3,3))
00083 self.testWrench2(w)
00084 w=Wrench.Zero()
00085 self.testWrench2(w)
00086 w=Wrench(Vector(2,1,4),Vector(5,3,1))
00087 self.testWrench2(w)
00088
00089 def testRotation2(self,v,a,b,c):
00090 w=Wrench(Vector(7,-1,3),Vector(2,-3,3))
00091 t=Twist(Vector(6,3,5),Vector(4,-2,7))
00092 R=Rotation.RPY(a,b,c)
00093
00094 self.assertAlmostEqual(dot(R.UnitX(),R.UnitX()),1.0,15)
00095 self.assertEqual(dot(R.UnitY(),R.UnitY()),1.0)
00096 self.assertEqual(dot(R.UnitZ(),R.UnitZ()),1.0)
00097 self.assertAlmostEqual(dot(R.UnitX(),R.UnitY()),0.0,15)
00098 self.assertAlmostEqual(dot(R.UnitX(),R.UnitZ()),0.0,15)
00099 self.assertEqual(dot(R.UnitY(),R.UnitZ()),0.0)
00100 R2=Rotation(R)
00101 self.assertEqual(R,R2)
00102 self.assertAlmostEqual((R*v).Norm(),v.Norm(),14)
00103 self.assertEqual(R.Inverse(R*v),v)
00104 self.assertEqual(R.Inverse(R*t),t)
00105 self.assertEqual(R.Inverse(R*w),w)
00106 self.assertEqual(R*R.Inverse(v),v)
00107 self.assertEqual(R*Rotation.Identity(),R)
00108 self.assertEqual(Rotation.Identity()*R,R)
00109 self.assertEqual(R*(R*(R*v)),(R*R*R)*v)
00110 self.assertEqual(R*(R*(R*t)),(R*R*R)*t)
00111 self.assertEqual(R*(R*(R*w)),(R*R*R)*w)
00112 self.assertEqual(R*R.Inverse(),Rotation.Identity())
00113 self.assertEqual(R.Inverse()*R,Rotation.Identity())
00114 self.assertEqual(R.Inverse()*v,R.Inverse(v))
00115 (ra,rb,rc)=R.GetRPY()
00116 self.assertEqual(ra,a)
00117 self.assertEqual(rb,b)
00118 self.assertEqual(rc,c)
00119 R = Rotation.EulerZYX(a,b,c)
00120 (ra,rb,rc)=R.GetEulerZYX()
00121 self.assertEqual(ra,a)
00122 self.assertEqual(rb,b)
00123 self.assertEqual(rc,c)
00124 R = Rotation.EulerZYZ(a,b,c)
00125 (ra,rb,rc)=R.GetEulerZYZ()
00126 self.assertEqual(ra,a)
00127 self.assertEqual(rb,b)
00128 self.assertAlmostEqual(rc,c,15)
00129 (angle,v2)= R.GetRotAngle()
00130 R2=Rotation.Rot(v2,angle)
00131 self.assertEqual(R2,R)
00132 R2=Rotation.Rot(v2*1E20,angle)
00133 self.assertEqual(R,R2)
00134 v2=Vector(6,2,4)
00135 self.assertAlmostEqual(v2.Norm(),sqrt(dot(v2,v2)),14)
00136
00137 def testRotation(self):
00138 self.testRotation2(Vector(3,4,5),radians(10),radians(20),radians(30))
00139
00140 def testFrame(self):
00141 v=Vector(3,4,5)
00142 w=Wrench(Vector(7,-1,3),Vector(2,-3,3))
00143 t=Twist(Vector(6,3,5),Vector(4,-2,7))
00144 F = Frame(Rotation.EulerZYX(radians(10),radians(20),radians(-10)),Vector(4,-2,1))
00145 F2=Frame(F)
00146 self.assertEqual(F,F2)
00147 self.assertEqual(F.Inverse(F*v),v)
00148 self.assertEqual(F.Inverse(F*t),t)
00149 self.assertEqual(F.Inverse(F*w),w)
00150 self.assertEqual(F*F.Inverse(v),v)
00151 self.assertEqual(F*F.Inverse(t),t)
00152 self.assertEqual(F*F.Inverse(w),w)
00153 self.assertEqual(F*Frame.Identity(),F)
00154 self.assertEqual(Frame.Identity()*F,F)
00155 self.assertEqual(F*(F*(F*v)),(F*F*F)*v)
00156 self.assertEqual(F*(F*(F*t)),(F*F*F)*t)
00157 self.assertEqual(F*(F*(F*w)),(F*F*F)*w)
00158 self.assertEqual(F*F.Inverse(),Frame.Identity())
00159 self.assertEqual(F.Inverse()*F,Frame.Identity())
00160 self.assertEqual(F.Inverse()*v,F.Inverse(v))
00161
00162
00163 def suite():
00164 suite=unittest.TestSuite()
00165 suite.addTest(FramesTestFunctions('testVector'))
00166 suite.addTest(FramesTestFunctions('testTwist'))
00167 suite.addTest(FramesTestFunctions('testWrench'))
00168 suite.addTest(FramesTestFunctions('testRotation'))
00169 suite.addTest(FramesTestFunctions('testFrame'))
00170 return suite
00171
00172
00173
