bool checkIdentity(KDL::Frame frame, double tol = 1e-4) { for(int i=0; i < 3; i++ ) { for(int j=0; j < 3; j++ ) { double err; if( i == j ) { err = fabs(frame.M(i,j)-1); } else { err = fabs(frame.M(i,j)); } if( err < tol ) return false; } } for(int i =0; i < 3; i++ ) { double err = fabs(frame.p[i]); if( err < tol ) return false; } return true; }
void leatherman::transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k) { k.p[0] = e(0,3); k.p[1] = e(1,3); k.p[2] = e(2,3); k.M(0,0) = e(0,0); k.M(0,1) = e(0,1); k.M(0,2) = e(0,2); k.M(1,0) = e(1,0); k.M(1,1) = e(1,1); k.M(1,2) = e(1,2); k.M(2,0) = e(2,0); k.M(2,1) = e(2,1); k.M(2,2) = e(2,2); }
void ChainKinematics::getXInv(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const { int i,j; KDL::JntArray qJA(Njnt_); for(i=0; i<Njnt_; i++) qJA(i) = q[i]; KDL::Frame frame; //compute forward kinematics checkSegmentNr(segmentNr); if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) { std::cerr << "ERROR: [ChainKinematics][getXInv] something went wrong during JntToCart! Exiting!" << std::endl; exit(-1); } //get inverse transformation frame = frame.Inverse(); for(i=0; i<3; i++) { t[i] = frame.p(i); for(j=0; j<3; j++) rot[i*3+j] = frame.M(i,j); } }
void ChainKinematics::getX(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const { int i,j; ////DEBUG //std::cout << "++++++++++++++++++++ DEBUG 0" << std::endl; //std::cout << "q = "; //for(i=0; i<Njnt_; i++) //std::cout << q[i] << " " << std::endl; //std::cout << std::endl; KDL::JntArray qJA(Njnt_); for(i=0; i<Njnt_; i++) qJA(i) = q[i]; KDL::Frame frame; //compute forward kinematics checkSegmentNr(segmentNr); if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) { std::cerr << "ERROR: [ChainKinematics][getX] something went wrong during JntToCart! Exiting!" << std::endl; exit(-1); } for(i=0; i<3; i++) { t[i] = frame.p(i); for(j=0; j<3; j++) rot[i*3+j] = frame.M(i,j); } }
void leatherman::transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e) { e(0,3) = k.p[0]; e(1,3) = k.p[1]; e(2,3) = k.p[2]; e(0,0) = k.M(0,0); e(0,1) = k.M(0,1); e(0,2) = k.M(0,2); e(1,0) = k.M(1,0); e(1,1) = k.M(1,1); e(1,2) = k.M(1,2); e(2,0) = k.M(2,0); e(2,1) = k.M(2,1); e(2,2) = k.M(2,2); e(3,0) = 0.0; e(3,1) = 0.0; e(3,2) = 0.0; e(3,3) = 1.0; }
Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p) { Eigen::Matrix4f b = Eigen::Matrix4f::Identity(); for(int i=0; i < 3; i++) { for(int j=0; j<3; j++) { b(i,j) = p.M(i,j); } b(i,3) = p.p(i); } return b; }
TEST(TFKDLConversions, tf_kdl_pose) { tf::Pose t; tf::Quaternion tq; tq[0] = gen_rand(-1.0,1.0); tq[1] = gen_rand(-1.0,1.0); tq[2] = gen_rand(-1.0,1.0); tq[3] = gen_rand(-1.0,1.0); tq.normalize(); t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); t.setRotation(tq); //test tf->kdl KDL::Frame k; PoseTFToKDL(t,k); for(int i=0; i < 3; i++) { ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6); } } //test kdl->tf tf::Pose r; PoseKDLToTF(k,r); for(int i=0; i< 3; i++) { ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6); } } }
void ChainKinematics::getQ(double* q, const double* qInit, const double (&t)[3], const double (&rot)[9]) const { int i,j; KDL::JntArray qInitJA(Njnt_); for(i=0; i<Njnt_; i++) qInitJA(i) = qInit[i]; KDL::Frame frame; for(i=0; i<3; i++) { frame.p(i) = t[i]; for(j=0; j<3; j++) frame.M(i,j) = rot[i*3+j]; } KDL::JntArray qJA; if(iksolverPos_->CartToJnt(qInitJA, frame, qJA) < 0) { std::cerr << "ERROR: [ChainKinematics][getQ] something went wrong during CartToJnt! Exiting!" << std::endl; exit(-1); } for(i=0; i<Njnt_; i++) q[i] = qJA(i); }
bool WamIkKdl::ik(vector<double> goal_in_cartesian, vector<double> currentjoints, vector<double>& goal_in_joints){ //fk solver KDL::ChainFkSolverPos_recursive fksolver(KDL::ChainFkSolverPos_recursive(this->wam63_)); // Create joint array KDL::JntArray setpointJP = KDL::JntArray(this->num_joints_); KDL::JntArray max = KDL::JntArray(this->num_joints_); //The maximum joint positions KDL::JntArray min = KDL::JntArray(this->num_joints_); //The minimium joint positions double minjp[7] = {-2.6,-2.0,-2.8,-0.9,-4.76,-1.6,-3.0}; double maxjp[7] = { 2.6, 2.0, 2.8, 3.2, 1.24, 1.6, 3.0}; for(unsigned int ii=0; ii < this->num_joints_; ii++){ max(ii) = maxjp[ii]; min(ii) = minjp[ii]; } //Create inverse velocity solver KDL::ChainIkSolverVel_pinv_givens iksolverv = KDL::ChainIkSolverVel_pinv_givens(this->wam63_); //Iksolver Position: Maximum 100 iterations, stop at accuracy 1e-6 //ChainIkSolverPos_NR iksolver = ChainIkSolverPos_NR(wam63,fksolver,iksolverv,100,1e-6); KDL::ChainIkSolverPos_NR_JL iksolver = KDL::ChainIkSolverPos_NR_JL(this->wam63_, min, max, fksolver, iksolverv, 2000, 1e-6); //With Joints Limits KDL::Frame cartpos; KDL::JntArray jointpos = KDL::JntArray(this->num_joints_); KDL::JntArray currentJP = KDL::JntArray(this->num_joints_); // Copying position to KDL frame cartpos.p(0) = goal_in_cartesian.at(3); cartpos.p(1) = goal_in_cartesian.at(7); cartpos.p(2) = goal_in_cartesian.at(11); // Copying Rotation to KDL frame cartpos.M(0,0) = goal_in_cartesian.at(0); cartpos.M(0,1) = goal_in_cartesian.at(1); cartpos.M(0,2) = goal_in_cartesian.at(2); cartpos.M(1,0) = goal_in_cartesian.at(4); cartpos.M(1,1) = goal_in_cartesian.at(5); cartpos.M(1,2) = goal_in_cartesian.at(6); cartpos.M(2,0) = goal_in_cartesian.at(8); cartpos.M(2,1) = goal_in_cartesian.at(9); cartpos.M(2,2) = goal_in_cartesian.at(10); for(unsigned int ii=0; ii < this->num_joints_; ii++) currentJP(ii) = currentjoints.at(ii); // Calculate inverse kinematics to go from currentJP to cartpos. The result in jointpos int ret = iksolver.CartToJnt(currentJP, cartpos, jointpos); if (ret >= 0) { std::cout << " Current Joint Position: ["; for(unsigned int ii=0; ii < this->num_joints_; ii++) std::cout << currentJP(ii) << " "; std::cout << "]"<< std::endl; std::cout << "Cartesian Position " << cartpos << std::endl; std::cout << "IK result Joint Position: ["; for(unsigned int ii=0; ii < this->num_joints_; ii++) std::cout << jointpos(ii) << " "; std::cout << "]"<< std::endl; goal_in_joints.clear(); goal_in_joints.resize(this->num_joints_); for(unsigned int ii=0; ii < this->num_joints_; ii++) goal_in_joints[ii] = jointpos(ii); } else { std::cout << "Error: could not calculate inverse kinematics :(" << std::endl; return false; } return true; }