void TestRotationDiff() { cout<<"Testing rotation angle differentiation..."<<endl; RotationAngleFunction rf; QuaternionRotation r; int numTests=100; for(int i=0;i<numTests;i++) { SampleSphere(One,rf.z); RandRotation(r); r.getMatrix(rf.R); Real t=0; cout<<"Initial matrix angle: "<<rf(t)<<endl; AngleAxisRotation aa; r.getAngleAxis(aa); cout<<"Difference between matrix angle and rotation angle: "<<Acos(Clamp(rf.z.dot(aa.axis),-1,1))<<endl; if(!TestDeriv(&rf,t,0.001,0.001,1e-2)) { } } cout<<"Testing moment rotation differentiation..."<<endl; RotationAxisFunction f; for(int i=0;i<numTests;i++) { SampleSphere(One,f.a); RandRotation(r); r.getMatrix(f.A); RandRotation(r); r.getMatrix(f.B); Real t=Rand(-3,3); if(!TestDeriv(&f,t,0.001,0.001,1e-2)) { } } }
void SampleTransform(const IKGoal& goal,RigidTransform& T) { Assert(goal.posConstraint==IKGoal::PosFixed); switch(goal.rotConstraint) { case IKGoal::RotFixed: goal.GetFixedGoalTransform(T); break; case IKGoal::RotTwoAxis: FatalError("Can't have 2 axis terms rotation!"); break; case IKGoal::RotAxis: { Real theta = Rand()*TwoPi; goal.GetEdgeGoalTransform(theta,T); } break; case IKGoal::RotNone: { QuaternionRotation q; RandRotation(q); q.getMatrix(T.R); T.t = goal.endPosition - T.R*goal.localPosition; } break; } }
void SO3CSpace::Sample(Config& x) { QuaternionRotation q = RandRotation(); Matrix3 R; q.getMatrix(R); SetRotation(R,x); }
bool ROSToKlampt(const geometry_msgs::Quaternion& q,Matrix3& kR) { QuaternionRotation kq; kq.x = q.x; kq.y = q.y; kq.z = q.z; kq.w = q.w; kq.getMatrix(kR); return true; }