Pose3D Attachment::worldPose() const
{
AssertM(m_entity.hasComponent<Transform3DComponent>(), "Needs Transform");
const auto & transform = m_entity.component<Transform3DComponent>().transform();
Pose3D pose = m_desc.pose;
pose.setPosition(
pose.position() + glm::vec3(m_desc.voxel) + glm::vec3(0.5f));
return Pose3D::fromTransformedPose(transform, pose);
}
int main(void)
{
Pose3D pz;
pz.setAxisAngle(0,1,0, M_PI/3);
pz.setPosition(0,2,0);
Pose3D pz2;
pz2.setAxisAngle(0,1,0, M_PI/2 - M_PI/3);
pz2.setPosition(0,1,0);
pz.multiplyPose(pz2);
cout << pz;
NEWMAT::Matrix m = pz.asMatrix();
cout << m;
Position p;
p.x = 0;
p.y = 0;
p.z = 1;
pz.applyToPosition(p);
printf("After rotate: %f %f %f\n", p.x, p.y, p.z);
cout << endl;
for (int ind = 0; ind < 2; ind++)
{
bool caching;
if (ind == 0) {
caching = true;
std::cout << "Caching Mode"<<std::endl;
}
else {
caching = false;
std::cout << std::endl<<std::endl<<std::endl<<"Not Caching Mode" <<std::endl;
}
double dx,dy,dz,dyaw,dp,dr;
std::cout <<"Creating TransformReference" << std::endl;
TransformReference mTR(caching);
dx = dy= dz = 0;
dyaw = dp = dr = 0.1;
uint64_t atime;
timeval temp_time_struct;
gettimeofday(&temp_time_struct,NULL);
atime = temp_time_struct.tv_sec * 1000000000ULL + (uint64_t)temp_time_struct.tv_usec * 1000ULL;
std::cout <<"Setting values" << std::endl;
//Fill in some transforms
// mTR.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
mTR.setWithDH("10","2",1.0,1.0,1.0,dyaw,atime);
//mTR.setWithEulers("2","3",1-1,1,1,dyaw,dp,dr,atime-1000);
mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime-100);
mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime-50);
mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime-1000);
mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime+500);
mTR.setWithEulers("2","3",1+100,1,1,dyaw,dp,dr,atime+1000);
mTR.setWithEulers("2","3",1,1,1,dyaw,dp,dr,atime+1100);
mTR.setWithEulers("3","5",dx,dy,dz,dyaw,dp,dr,atime);
mTR.setWithEulers("5","1",dx,dy,dz,dyaw,dp,dr,atime);
mTR.setWithEulers("6","5",dx,dy,dz,dyaw,dp,dr,atime);
mTR.setWithEulers("6","5",dx,dy,dz,dyaw,dp,dr,atime);
mTR.setWithEulers("7","6",1,1,1,dyaw,dp,dr,atime);
mTR.setWithDH("8","7",1.0,1.0,1.0,dyaw,atime);
//mTR.setWithEulers("8","7",1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
std::cout <<"Trying some tests" << std::endl;
//Demonstrate InvalidFrame LookupException
try
{
std::cout<< mTR.viewChain("10","9");
}
catch (TransformReference::LookupException &ex)
{
std::cout << "Caught " << ex.what()<<std::endl;
}
// See the list of transforms to get between the frames
std::cout<<"Viewing (10,8):"<<std::endl;
std::cout << mTR.viewChain("10","8");
//See the resultant transform
std::cout <<"Calling getMatrix(10,8)"<<std::endl;
// NEWMAT::Matrix mat = mTR.getMatrix(1,1);
NEWMAT::Matrix mat = mTR.getMatrix("10","8",atime);
std::cout << "Result of getMatrix(10,8,atime):" << std::endl << mat<< std::endl;
TFPoint mPoint;
mPoint.x = 1;
mPoint.y = 1;
mPoint.z = 1;
mPoint.frame = "10";
mPoint.time = atime;
TFPoint nPoint = mPoint;
std::cout <<"Point 1,1,1 goes like this:" <<std::endl;
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
mPoint = mTR.transformPoint("2", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
mPoint = mTR.transformPoint("3", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
mPoint = mTR.transformPoint("5", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
mPoint = mTR.transformPoint("6", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
mPoint = mTR.transformPoint("7", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
mPoint = mTR.transformPoint("8", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
mPoint = mTR.transformPoint("10", mPoint);
std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
//Break the graph, making it loop and demonstrate catching MaxDepthException
mTR.setWithEulers("6","7",dx,dy,dz,dyaw,dp,dr,atime);
try {
std::cout<<mTR.viewChain("10","8");
}
catch (TransformReference::MaxDepthException &ex)
{
std::cout <<"caught loop in graph"<<ex.what()<<std::endl;
}
//Break the graph, making it disconnected, and demonstrate catching ConnectivityException
mTR.setWithEulers("6","0",dx,dy,dz,dyaw,dp,dr,atime);
try {
std::cout<<mTR.viewChain("10","8");
}
catch (TransformReference::ConnectivityException &ex)
{
std::cout <<"caught unconnected frame"<<ex.what()<<std::endl;
}
//Testing clearing the history with parent change
mTR.setWithEulers("7","5",1,1,1,dyaw,dp,dr,atime);
//todo display this somehow
std::cout << " Testing accessors" <<std::endl;
double x,y,z,yaw,pitch,roll;
x = 30.0;
y = 10.0;
z = 0.0;
yaw = 0.1;
pitch = 0.0;
roll = 0.0;
mTR.setWithEulers("2","1",x,y,z,yaw,pitch,roll,atime);
libTF::TFPose2D in, out;
in.x = 0.0;
in.y = 0.0;
in.yaw = 0.0;
in.frame = "2";
in.time = atime;
out = mTR.transformPose2D("1",in);
printf("%.3f %.3f %.3f\n",
out.x, out.y, out.yaw*180.0/M_PI);
try
{
out = mTR.transformPose2D("0",in);
std::cout << "failed to throw" << std::endl;
}
catch (TransformReference::LookupException &ex)
{
std::cout << "transformPose2D(0,in): Caught " << ex.what()<<std::endl;
}
catch (TransformReference::ConnectivityException &ex)
{
std::cout <<"caught unconnected frame, used to be lookup exception: "<<ex.what()<<std::endl;
}
libTF::TFEulerYPR ypr_in;
ypr_in.yaw = 0;
ypr_in.pitch = 0;
ypr_in.roll = 0;
ypr_in.time = atime;
ypr_in.frame = "1";
std::cout <<"YPR in:"<< ypr_in.yaw<<","<<ypr_in.pitch <<"," <<ypr_in.roll<<std::endl;
libTF::TFEulerYPR ypr_out = mTR.transformEulerYPR("2", ypr_in);
std::cout <<"YPR out:"<< ypr_out.yaw<<","<<ypr_out.pitch <<"," <<ypr_out.roll<<std::endl;
}
std::cout <<"Congratulations! You reached the end of the test program without errors." <<std::endl;
return 0;
};
