puma560test.cpp
Go to the documentation of this file.
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     


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14