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