00001 #include <chain.hpp> 00002 #include "models.hpp" 00003 #include <frames_io.hpp> 00004 #include <kinfam_io.hpp> 00005 00006 #include <chainfksolverpos_recursive.hpp> 00007 #include <chainidsolver_recursive_newton_euler.hpp> 00008 00009 using namespace KDL; 00010 00011 int main(int argc , char** argv){ 00012 00013 Chain p560=Puma560(); 00014 //Chain p560; 00015 // p560.addSegment(Segment(Joint(Joint::RotX),Frame::Identity(),RigidBodyInertia(1.0,Vector(0.0,1.0,.0),RotationalInertia(1.0,2.0,3.0)))); 00016 // p560.addSegment(Segment(Joint(Joint::RotY),Frame(Rotation::Identity(),Vector(0,2,0)),RigidBodyInertia(1.0,Vector(1.0,0.0,.0),RotationalInertia(1.0,2.0,3,4,5,6)))); 00017 // p560.addSegment(Segment(Joint(Joint::RotZ),Frame(Rotation::Identity(),Vector(2,0,0)),RigidBodyInertia(1.0,Vector(0.0,0.0,1),RotationalInertia(1.0,2.0,3,4,5,6)))); 00018 00019 JntArray q(p560.getNrOfJoints()); 00020 JntArray qdot(p560.getNrOfJoints()); 00021 JntArray qdotdot(p560.getNrOfJoints()); 00022 JntArray tau(p560.getNrOfJoints()); 00023 Wrenches f(p560.getNrOfSegments()); 00024 00025 for(unsigned int i=0;i<p560.getNrOfJoints();i++){ 00026 q(i)=0.0; 00027 qdot(i)=0.0; 00028 qdotdot(i)=0.0; 00029 00030 //if(i<2) 00031 //{ 00032 std::cout << "give q(" << i+1 << ")\n" << std::endl; 00033 std::cin >> q(i); 00034 std::cout << "give qdot(" << i+1 << ")\n" << std::endl; 00035 std::cin >> qdot(i); 00036 std::cout << "give qdotdot(" << i << ")\n" << std::endl; 00037 std::cin >> qdotdot(i); 00038 //} 00039 00040 } 00041 00042 ChainFkSolverPos_recursive fksolver(p560); 00043 Frame T; 00044 ChainIdSolver_RNE idsolver(p560,Vector(0.0,0.0,-9.81)); 00045 00046 //#include <time.h> 00047 //time_t before,after; 00048 //time(&before); 00049 unsigned int k=0; 00050 //for(k=0;k<1e7;k++) 00051 fksolver.JntToCart(q,T); 00052 //time(&after); 00053 //std::cout<<"elapsed time for FK: "<<difftime(after,before)<<" seconds for "<<k<<" iterations"<<std::endl; 00054 //std::cout<<"time per iteration for FK: "<<difftime(after,before)/k<<" seconds."<<std::endl; 00055 //time(&before); 00056 //for(k=0;k<1e7;k++) 00057 idsolver.CartToJnt(q,qdot,qdotdot,f,tau); 00058 //time(&after); 00059 //std::cout<<"elapsed time for ID: "<<difftime(after,before)<<" seconds for "<<k<<" iterations"<<std::endl; 00060 //std::cout<<"time per iteration for ID: "<<difftime(after,before)/k<<" seconds."<<std::endl; 00061 00062 std::cout<<T<<std::endl; 00063 std::cout<<"tau: "<<tau<<std::endl; 00064 00065 00066 } 00067