1. 下载
git clone https://github.com/orocos/orocos_kinematics_dynamics.git
2.进入orocos_kdl目录,创建build目录,进入build目录
3.cmake ..
4.make
5.sudo make install
------------------------------------正解------------------------------------------
-
// Copyright (C) 2007 Francois Cauwe <francois at cauwe dot org>
-
-
// This library is free software; you can redistribute it and/or
-
// modify it under the terms of the GNU Lesser General Public
-
// License as published by the Free Software Foundation; either
-
// version 2.1 of the License, or (at your option) any later version.
-
-
#include <kdl/chain.hpp>
-
#include <kdl/chainfksolver.hpp>
-
#include <kdl/chainfksolverpos_recursive.hpp>
-
#include <kdl/frames_io.hpp>
-
#include <stdio.h>
-
#include <iostream>
-
-
using namespace KDL;
-
-
-
int main( int argc, char** argv )
-
{
-
//Definition of a kinematic chain & add segments to the chain
-
KDL::Chain chain;
-
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
-
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
-
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
-
chain.addSegment(Segment(Joint(Joint::RotZ)));
-
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
-
chain.addSegment(Segment(Joint(Joint::RotZ)));
-
-
// Create solver based on kinematic chain
-
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
-
-
// Create joint array
-
unsigned int nj = chain.getNrOfJoints();
-
KDL::JntArray jointpositions = JntArray(nj);
-
-
// Assign some values to the joint positions
-
for(unsigned int i=0;i<nj;i++){
-
float myinput;
-
printf ("Enter the position of joint %i: ",i);
-
scanf ("%e",&myinput);
-
jointpositions(i)=(double)myinput;
-
}
-
-
// Create the frame that will contain the results
-
KDL::Frame cartpos;
-
-
// Calculate forward position kinematics
-
bool kinematics_status;
-
kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
-
if(kinematics_status>=0){
-
std::cout << cartpos <<std::endl;
-
printf("%s \n","Succes, thanks KDL!");
-
}else{
-
printf("%s \n","Error: could not calculate forward kinematics :(");
-
}
-
}
-----------------------------------------逆解-------------------------------------------
-
//Creation of the chain:
-
KDL::Chain chain;
-
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
-
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
-
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
-
chain.addSegment(Segment(Joint(Joint::RotZ)));
-
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
-
chain.addSegment(Segment(Joint(Joint::RotZ)));
-
-
//Creation of the solvers:
-
ChainFkSolverPos_recursive fksolver1(chain1);//Forward position solver
-
ChainIkSolverVel_pinv iksolver1v(chain1);//Inverse velocity solver
-
ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
-
-
//Creation of jntarrays:
-
JntArray q(chain.getNrOfJoints());
-
JntArray q_init(chain.getNrOfJoints());
-
-
//Set destination frame
-
Frame F_dest=...;
-
-
int ret = iksolverpos.CartToJnt(q_init,F_dest,q);
阅读(1309) | 评论(0) | 转发(0) |