bool KDLRobotModel::computeIKSearch(const std::vector<double> &pose, const std::vector<double> &start, std::vector<double> &solution, double timeout) { //pose: {x,y,z,r,p,y} or {x,y,z,qx,qy,qz,qw} KDL::Frame frame_des; frame_des.p.x(pose[0]); frame_des.p.y(pose[1]); frame_des.p.z(pose[2]); // RPY if(pose.size() == 6) frame_des.M = KDL::Rotation::RPY(pose[3],pose[4],pose[5]); // quaternion else frame_des.M = KDL::Rotation::Quaternion(pose[3],pose[4],pose[5],pose[6]); // transform into kinematics frame frame_des = T_planning_to_kinematics_ * frame_des; // seed configuration for(size_t i = 0; i < start.size(); i++) jnt_pos_in_(i) = angles::normalize_angle(start[i]); // must be normalized for CartToJntSearch double initial_guess = jnt_pos_in_(free_angle_); double search_discretization_angle = 0.02; ros::Time start_time = ros::Time::now(); double loop_time = 0; int count = 0; int num_positive_increments = (int)((min_limits_[free_angle_]-initial_guess)/search_discretization_angle); int num_negative_increments = (int)((initial_guess-min_limits_[free_angle_])/search_discretization_angle); while(loop_time < timeout) { if(ik_solver_->CartToJnt(jnt_pos_in_, frame_des, jnt_pos_out_) >= 0) { solution.resize(start.size()); for(size_t i = 0; i < solution.size(); ++i) solution[i] = angles::normalize_angle(jnt_pos_out_(i)); return true; } if(!getCount(count,num_positive_increments,-num_negative_increments)) return false; jnt_pos_in_(free_angle_) = initial_guess + search_discretization_angle * count; ROS_DEBUG("%d, %f",count,jnt_pos_in_(free_angle_)); loop_time = (ros::Time::now()-start_time).toSec(); } if(loop_time >= timeout) { ROS_DEBUG("IK Timed out in %f seconds",timeout); return false; } else { ROS_DEBUG("No IK solution was found"); return false; } return false; }
bool KDLRobotModel::computeFastIK(const std::vector<double> &pose, const std::vector<double> &start, std::vector<double> &solution) { //pose: {x,y,z,r,p,y} or {x,y,z,qx,qy,qz,qw} KDL::Frame frame_des; frame_des.p.x(pose[0]); frame_des.p.y(pose[1]); frame_des.p.z(pose[2]); // RPY if(pose.size() == 6) frame_des.M = KDL::Rotation::RPY(pose[3],pose[4],pose[5]); // quaternion else frame_des.M = KDL::Rotation::Quaternion(pose[3],pose[4],pose[5],pose[6]); // transform into kinematics frame frame_des = T_planning_to_kinematics_ * frame_des; // seed configuration for(size_t i = 0; i < start.size(); i++) jnt_pos_in_(i) = angles::normalize_angle(start[i]); // must be normalized for CartToJntSearch if(ik_solver_->CartToJnt(jnt_pos_in_, frame_des, jnt_pos_out_) < 0) return false; solution.resize(start.size()); for(size_t i = 0; i < solution.size(); ++i) solution[i] = angles::normalize_angle(jnt_pos_out_(i)); return true; }
bool KDLRobotModel::computeFK(const std::vector<double> &angles, std::string name, KDL::Frame &f) { for(size_t i = 0; i < angles.size(); ++i) jnt_pos_in_(i) = angles::normalize_angle(angles[i]); KDL::Frame f1; if(fk_solver_->JntToCart(jnt_pos_in_, f1, link_map_[name]) < 0) { ROS_ERROR("JntToCart returned < 0."); return false; } f = T_kinematics_to_planning_ * f1; /* KDL::Frame f1; for(std::map<std::string, int>::const_iterator iter = link_map_.begin(); iter != link_map_.end(); ++iter) { if(fk_solver_->JntToCart(jnt_pos_in_, f1, iter->second) < 0) { ROS_ERROR("JntToCart returned < 0."); return false; } f = T_kinematics_to_planning_ * f1; leatherman::printKDLFrame(f,iter->first); } */ return true; }
bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names, const std::vector<double> &joint_angles, std::vector<geometry_msgs::Pose> &poses) const { ros::WallTime n1 = ros::WallTime::now(); if(!active_) { ROS_ERROR("kinematics not active"); return false; } if(poses.size() != link_names.size()) { ROS_ERROR("Poses vector must have size: %zu",link_names.size()); return false; } if(joint_angles.size() != dimension_) { ROS_ERROR("Joint angles vector must have size: %d",dimension_); return false; } KDL::Frame p_out; geometry_msgs::PoseStamped pose; tf::Stamped<tf::Pose> tf_pose; for(unsigned int i=0; i < dimension_; i++) { jnt_pos_in_(i) = joint_angles[i]; } bool valid = true; for(unsigned int i=0; i < poses.size(); i++) { ROS_DEBUG("End effector index: %d",getKDLSegmentIndex(link_names[i])); if(fk_solver_->JntToCart(jnt_pos_in_,p_out,getKDLSegmentIndex(link_names[i])) >=0) { tf::poseKDLToMsg(p_out,poses[i]); } else { ROS_ERROR("Could not compute FK for %s",link_names[i].c_str()); valid = false; } } return valid; }
bool KDLRobotModel::computePlanningLinkFK(const std::vector<double> &angles, std::vector<double> &pose) { KDL::Frame f, f1; pose.resize(6,0); for(size_t i = 0; i < angles.size(); ++i) jnt_pos_in_(i) = angles::normalize_angle(angles[i]); if(fk_solver_->JntToCart(jnt_pos_in_, f1, link_map_[planning_link_]) < 0) { ROS_ERROR("JntToCart returned < 0."); return false; } f = T_kinematics_to_planning_ * f1; pose[0] = f.p[0]; pose[1] = f.p[1]; pose[2] = f.p[2]; f.M.GetRPY(pose[3], pose[4], pose[5]); return true; }