bool Kinematics::getPositionIK(moveit_msgs::GetPositionIK::Request &request,
moveit_msgs::GetPositionIK::Response &response) {
geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
tf::Stamped<tf::Pose> transform;
tf::Stamped<tf::Pose> transform_root;
tf::poseStampedMsgToTF( pose_msg_in, transform );
//Do the IK
KDL::JntArray jnt_pos_in;
KDL::JntArray jnt_pos_out;
jnt_pos_in.resize(num_joints);
for (unsigned int i=0; i < num_joints; i++) {
// nalt: mod for moveit msgs
int tmp_index = getJointIndex(request.ik_request.robot_state.joint_state.name[i]);
if (tmp_index >=0) {
jnt_pos_in(tmp_index) = request.ik_request.robot_state.joint_state.position[i];
} else {
ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.robot_state.joint_state.name[i].c_str());
}
}
//Convert F to our root_frame
try {
tf_listener.transformPose(root_name, transform, transform_root);
} catch (...) {
ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
KDL::Frame F_dest;
tf::TransformTFToKDL(transform_root, F_dest);
int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out);
if (ik_valid >= 0) {
response.solution.joint_state.name = info.joint_names;
response.solution.joint_state.position.resize(num_joints);
for (unsigned int i=0; i < num_joints; i++) {
response.solution.joint_state.position[i] = jnt_pos_out(i);
ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
}
response.error_code.val = response.error_code.SUCCESS;
return true;
} else {
ROS_DEBUG("An IK solution could not be found");
response.error_code.val = response.error_code.NO_IK_SOLUTION;
return true;
}
}
bool Kinematics::searchPositionIK(kinematics_msgs::GetPositionIK::Request &request,
kinematics_msgs::GetPositionIK::Response &response) {
geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
tf::Stamped<tf::Pose> transform;
tf::Stamped<tf::Pose> transform_root;
tf::poseStampedMsgToTF( pose_msg_in, transform );
ros::WallTime n1 = ros::WallTime::now();
//Do the IK
KDL::JntArray jnt_pos_in;
KDL::JntArray jnt_pos_out;
jnt_pos_in.resize(num_joints);
for (unsigned int i=0; i < num_joints; i++) {
int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]);
if (tmp_index >=0) {
jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
} else {
ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
}
}
//Convert F to our root_frame
try {
tf_listener.transformPose(root_name, transform, transform_root);
} catch (...) {
ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
KDL::Frame F_dest;
tf::TransformTFToKDL(transform_root, F_dest);
for(unsigned int i=0; i < 100; i++) // max_search_iterations_
{
for(unsigned int j=0; j < num_joints; j++) // num_joints_
{
ROS_INFO_STREAM("seed state " << j << " " << jnt_pos_in(j) );
}
int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in,F_dest,jnt_pos_out); // F_dest is pose_desired
ROS_INFO_STREAM("IK returned code " << ik_valid );
if (ik_valid >= 0) {
response.solution.joint_state.name = info.joint_names;
response.solution.joint_state.position.resize(num_joints);
for (unsigned int k=0; k < num_joints; k++) {
response.solution.joint_state.position[k] = jnt_pos_out(k);
ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[k].c_str(),k,jnt_pos_out(k) );
}
response.error_code.val = response.error_code.SUCCESS;
ROS_INFO_STREAM("Solved after " << i+1 << " iterations");
ROS_INFO_STREAM("time: " << (ros::WallTime::now()-n1).toSec() << " sec" );
return true;
}
// no sol, try another seed
jnt_pos_in = getRandomConfiguration();
}
ROS_ERROR("solver: An IK solution could not be found");
response.error_code.val = response.error_code.NO_IK_SOLUTION;
return false;
}
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;
}
