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);
}
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)) {
}
}
}
bool KlamptToROS(const Matrix3& kR,geometry_msgs::Quaternion& q)
{
QuaternionRotation kq;
if(!kq.setMatrix(kR)) return false;
q.x = kq.x;
q.y = kq.y;
q.z = kq.z;
q.w = kq.w;
return true;
}
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;
}
