ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
ChainIkSolverPos_NR is very easy to be trapped in local minimums. Use ChainIkSolverPos_LMA instead. However at this time ChainIkSolverPos_LMA hasn't been ported to PyKDL, so you have to use C++ or port it by yourself following the other classes of Orocos KDL.
The following is the C++ implementation of your codes using ChainIkSolverPos_LMA instead of ChainIkSolverPos_NR:
#include <iostream>
#include <frames_io.hpp>
#include <chainiksolverpos_lma.hpp>
#include <chainfksolverpos_recursive.hpp>
using namespace KDL;
using namespace std;
int main(int argc,char* argv[]) {
Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(1.0,0.0,0.0))));
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(1.5,0.0,0.0))));
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.5,0.0,0.0))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1.5))));
Frame pos_goal = Frame(Vector(1.98, 1.50, 1.30));
Frame pos_reached;
JntArray q_init(chain.getNrOfJoints());
JntArray q_out(chain.getNrOfJoints());
//q_init.data.setRandom();
//q_init.data *= M_PI;
Eigen::Matrix<double,6,1> L;
L(0)=1;L(1)=1;L(2)=1;
L(3)=0.01;L(4)=0.01;L(5)=0.01;
ChainFkSolverPos_recursive fwdkin(chain);
ChainIkSolverPos_LMA solver(chain,L);
int retval = solver.CartToJnt(q_init,pos_goal,q_out);
fwdkin.JntToCart(q_out, pos_reached);
cout << "Goal: " << pos_goal.p << endl;
cout << "Reached:" << pos_reached.p << endl;
return 0;
}
And the output:
Goal: [ 1.98, 1.5, 1.3]
Reached:[ 1.98, 1.5, 1.30007]
2 | No.2 Revision |
ChainIkSolverPos_NR is very easy to be trapped in local minimums. Use ChainIkSolverPos_LMA instead. However at this time ChainIkSolverPos_LMA hasn't been ported to PyKDL, so you have to use C++ or port it by yourself following the other classes of Orocos KDL.
The following is the C++ implementation of your codes using ChainIkSolverPos_LMA instead of ChainIkSolverPos_NR:
#include <iostream>
#include <frames_io.hpp>
#include <chainiksolverpos_lma.hpp>
#include <chainfksolverpos_recursive.hpp>
using namespace KDL;
using namespace std;
int main(int argc,char* argv[]) {
Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(1.0,0.0,0.0))));
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(1.5,0.0,0.0))));
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.5,0.0,0.0))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1.5))));
Frame pos_goal = Frame(Vector(1.98, 1.50, 1.30));
Frame pos_reached;
JntArray q_init(chain.getNrOfJoints());
JntArray q_out(chain.getNrOfJoints());
//q_init.data.setRandom();
//q_init.data *= M_PI;
Eigen::Matrix<double,6,1> L;
L(0)=1;L(1)=1;L(2)=1;
L(3)=0.01;L(4)=0.01;L(5)=0.01;
ChainFkSolverPos_recursive fwdkin(chain);
ChainIkSolverPos_LMA solver(chain,L);
int retval = solver.CartToJnt(q_init,pos_goal,q_out);
fwdkin.JntToCart(q_out, pos_reached);
cout << "Goal: " << pos_goal.p << endl;
cout << "Reached:" << pos_reached.p << endl;
return 0;
}
And the output:
Goal: [ 1.98, 1.5, 1.3]
Reached:[ 1.98, 1.5, 1.30007]