puma560test.cpp
Go to the documentation of this file.
1 #include <chain.hpp>
2 #include "models.hpp"
3 #include <frames_io.hpp>
4 #include <kinfam_io.hpp>
5 
8 
9 using namespace KDL;
10 
11 int main(int argc , char** argv){
12 
13  Chain p560=Puma560();
14  //Chain p560;
15 // p560.addSegment(Segment(Joint(Joint::RotX),Frame::Identity(),RigidBodyInertia(1.0,Vector(0.0,1.0,.0),RotationalInertia(1.0,2.0,3.0))));
16 // 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))));
17 // 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))));
18 
19  JntArray q(p560.getNrOfJoints());
20  JntArray qdot(p560.getNrOfJoints());
21  JntArray qdotdot(p560.getNrOfJoints());
22  JntArray tau(p560.getNrOfJoints());
23  Wrenches f(p560.getNrOfSegments());
24 
25  for(unsigned int i=0;i<p560.getNrOfJoints();i++){
26  q(i)=0.0;
27  qdot(i)=0.0;
28  qdotdot(i)=0.0;
29 
30  //if(i<2)
31  //{
32  std::cout << "give q(" << i+1 << ")\n" << std::endl;
33  std::cin >> q(i);
34  std::cout << "give qdot(" << i+1 << ")\n" << std::endl;
35  std::cin >> qdot(i);
36  std::cout << "give qdotdot(" << i << ")\n" << std::endl;
37  std::cin >> qdotdot(i);
38  //}
39 
40  }
41 
42  ChainFkSolverPos_recursive fksolver(p560);
43  Frame T;
44  ChainIdSolver_RNE idsolver(p560,Vector(0.0,0.0,-9.81));
45 
46  //#include <time.h>
47  //time_t before,after;
48  //time(&before);
49  //unsigned int k=0;
50  //for(k=0;k<1e7;k++)
51  fksolver.JntToCart(q,T);
52  //time(&after);
53  //std::cout<<"elapsed time for FK: "<<difftime(after,before)<<" seconds for "<<k<<" iterations"<<std::endl;
54  //std::cout<<"time per iteration for FK: "<<difftime(after,before)/k<<" seconds."<<std::endl;
55  //time(&before);
56  //for(k=0;k<1e7;k++)
57  idsolver.CartToJnt(q,qdot,qdotdot,f,tau);
58  //time(&after);
59  //std::cout<<"elapsed time for ID: "<<difftime(after,before)<<" seconds for "<<k<<" iterations"<<std::endl;
60  //std::cout<<"time per iteration for ID: "<<difftime(after,before)/k<<" seconds."<<std::endl;
61 
62  std::cout<<T<<std::endl;
63  std::cout<<"tau: "<<tau<<std::endl;
64 
65 
66 }
67 
Chain Puma560()
Definition: puma560.cpp:27
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Recursive newton euler inverse dynamics solver.
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
std::vector< Wrench > Wrenches
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
int main(int argc, char **argv)
Definition: puma560test.cpp:11


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44