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