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;
}
