//compute interpolation
Transform3D<> computeinterpolation(Transform3D<> Tin, Transform3D<> Tdes,double delta)
{
//number of steps, considering ti=1, ti-1=0, each step is 1/n
int n=0;
double WS=1.0;
Transform3D<> Tout;
//compute positional difference
Vector3D<> dif=Tdes.P()-Tin.P();
//Compute Wtotal
Rotation3D<> forW =inverse(Tin.R())*Tdes.R();
//Compute W vector
Vector3D<> W=getW(forW);
//Get the number of steps necessary
Vector3D<> trans, rot;
// trans=(1.0/n)*dif;
// rot=(1.0/n)*W;
double step;
do
{n=n+1;
trans=(1.0/n)*dif;
rot=(1.0/n)*W;
step=trans.norm2()/WS+rot.norm2()/pi;
}
while (step>delta);
n=1; //number of steps
EAA<> eaa(rot*n);
Transform3D<> current(Tin.P()+trans*n,Tin.R()*eaa.toRotation3D());
Tout=current;
return Tout;
}
//compute the small iteration
VelocityScrew6D<> computeG(Transform3D<> Tin, Transform3D<> Tdes){
//compute positional difference
Vector3D<> dif=Tdes.P()-Tin.P();
//compute rotation difference
Rotation3D<> rot=Tdes.R()*inverse(Tin.R());
//assign values to deltau
VelocityScrew6D<> deltau(dif(0),dif(1),dif(2),(rot(2,1)-rot(1,2))/2,(rot(0,2)-rot(2,0))/2,(rot(1,0)-rot(0,1))/2);
return(deltau);
}
void calculatePose(Q q){
State tmpState = wc->getDefaultState();
device->setQ(q, tmpState);
Transform3D<double> cPose = device->baseTend(tmpState);
RPY<double> rpy(cPose.R());
pose.x = cPose.P()[0];
pose.y = cPose.P()[1];
pose.z = cPose.P()[2];
pose.R = rpy(0)*180/Pi;
pose.P = rpy(1)*180/Pi;
pose.Y = rpy(2)*180/Pi;
//cout << "POOOOOSE: " << cPose.P() << "\n";
kinematics::Pose msg;
msg.x = cPose.P()[0];
msg.y = cPose.P()[1];
msg.z = cPose.P()[2];
msg.R = rpy(0)*180/Pi;
msg.P = rpy(1)*180/Pi;
msg.Y = rpy(2)*180/Pi;
pose_pub.publish(msg);
}
void transformToText(string pre, Transform3D<double> T){
RPY<double> rpy = RPY<double>(T.R());
ROS_INFO("xyz (%0.2f, %0.2f, %0.2f), RPY (%0.2f, %0.2f, %0.2f)", T.P()[0], T.P()[1], T.P()[2], rpy(0), rpy(1), rpy(2));
}
PSvectorTransform3D::PSvectorTransform3D (const Transform3D& tfrm)
:T(tfrm),bNoRot(tfrm.R().isIdentity())
{}
