61 #define IKFAST_NO_MAIN // Don't include main() from IKFast 148 const std::vector<double> &ik_seed_state,
149 std::vector<double> &solution,
150 moveit_msgs::MoveItErrorCodes &error_code,
162 const std::vector<double> &ik_seed_state,
164 std::vector<double> &solution,
165 moveit_msgs::MoveItErrorCodes &error_code,
178 const std::vector<double> &ik_seed_state,
180 const std::vector<double> &consistency_limits,
181 std::vector<double> &solution,
182 moveit_msgs::MoveItErrorCodes &error_code,
194 const std::vector<double> &ik_seed_state,
196 std::vector<double> &solution,
198 moveit_msgs::MoveItErrorCodes &error_code,
212 const std::vector<double> &ik_seed_state,
214 const std::vector<double> &consistency_limits,
215 std::vector<double> &solution,
217 moveit_msgs::MoveItErrorCodes &error_code,
228 bool getPositionFK(
const std::vector<std::string> &link_names,
229 const std::vector<double> &joint_angles,
230 std::vector<geometry_msgs::Pose> &poses)
const;
234 bool initialize(
const std::string &robot_description,
235 const std::string& group_name,
236 const std::string& base_name,
237 const std::string& tip_name,
238 double search_discretization);
251 double harmonize(
const std::vector<double> &ik_seed_state, std::vector<double> &solution)
const;
255 bool getCount(
int &count,
const int &max_count,
const int &min_count)
const;
260 const std::string& group_name,
261 const std::string& base_name,
262 const std::string& tip_name,
263 double search_discretization)
265 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
270 node_handle.
param(
"robot",robot,std::string());
276 if(free_params_.size() > 1)
285 std::string urdf_xml,full_urdf_xml;
286 node_handle.
param(
"urdf_xml",urdf_xml,robot_description);
290 if (!node_handle.
getParam(full_urdf_xml, xml_string))
292 ROS_FATAL_NAMED(
"ikfast",
"Could not load the xml from parameter server: %s", urdf_xml.c_str());
296 node_handle.
param(full_urdf_xml,xml_string,std::string());
301 urdf::LinkConstSharedPtr link = robot_model.getLink(
getTipFrame());
305 link_names_.push_back(link->name);
306 urdf::JointSharedPtr joint = link->parent_joint;
309 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
313 joint_names_.push_back(joint->name);
316 if ( joint->type != urdf::Joint::CONTINUOUS )
320 lower = joint->safety->soft_lower_limit;
321 upper = joint->safety->soft_upper_limit;
323 lower = joint->limits->lower;
324 upper = joint->limits->upper;
336 joint_has_limits_vector_.push_back(
true);
337 joint_min_vector_.push_back(lower);
338 joint_max_vector_.push_back(upper);
342 joint_has_limits_vector_.push_back(
false);
343 joint_min_vector_.push_back(-
M_PI);
344 joint_max_vector_.push_back(
M_PI);
349 ROS_WARN_NAMED(
"ikfast",
"no joint corresponding to %s",link->name.c_str());
351 link = link->getParent();
360 std::reverse(link_names_.begin(),link_names_.end());
361 std::reverse(joint_names_.begin(),joint_names_.end());
362 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
363 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
364 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
367 ROS_DEBUG_STREAM_NAMED(
"ikfast",joint_names_[i] <<
" " << joint_min_vector_[i] <<
" " << joint_max_vector_[i] <<
" " << joint_has_limits_vector_[i]);
379 trans[0] = pose_frame.
p[0];
380 trans[1] = pose_frame.
p[1];
381 trans[2] = pose_frame.
p[2];
406 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
415 ComputeIk(trans, direction.
data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
422 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
427 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
437 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
441 ROS_ERROR_NAMED(
"ikfast",
"Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
449 solution.resize(num_joints_);
453 std::vector<IkReal> vsolfree( sol.
GetFree().size() );
454 sol.
GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
467 std::vector<double> ss = ik_seed_state;
468 for(
size_t i=0; i< ik_seed_state.size(); ++i)
470 while(ss[i] > 2*
M_PI) {
473 while(ss[i] < 2*
M_PI) {
476 while(solution[i] > 2*
M_PI) {
477 solution[i] -= 2*
M_PI;
479 while(solution[i] < 2*
M_PI) {
480 solution[i] += 2*
M_PI;
482 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
512 double mindist = DBL_MAX;
514 std::vector<double> sol;
520 double dist =
harmonize(ik_seed_state, sol);
523 if(minindex == -1 || dist<mindist){
536 free_params_.clear();
537 for(
int i=0; i<count;++i) free_params_.push_back(array[i]);
544 if(-count >= min_count)
549 else if(count+1 <= max_count)
561 if(1-count <= max_count)
566 else if(count-1 >= min_count)
577 const std::vector<double> &joint_angles,
578 std::vector<geometry_msgs::Pose> &poses)
const 585 ROS_ERROR_NAMED(
"ikfast",
"Can only compute FK for Transform6D IK type!");
590 if(link_names.size() == 0) {
595 if(link_names.size()!=1 || link_names[0]!=
getTipFrame()){
602 IkReal eerot[9],eetrans[3];
603 IkReal
angles[joint_angles.size()];
604 for (
unsigned char i=0; i < joint_angles.size(); i++)
605 angles[i] = joint_angles[i];
610 for(
int i=0; i<3;++i)
611 p_out.
p.
data[i] = eetrans[i];
613 for(
int i=0; i<9;++i)
614 p_out.
M.
data[i] = eerot[i];
623 const std::vector<double> &ik_seed_state,
625 std::vector<double> &solution,
626 moveit_msgs::MoveItErrorCodes &error_code,
630 std::vector<double> consistency_limits;
643 const std::vector<double> &ik_seed_state,
645 const std::vector<double> &consistency_limits,
646 std::vector<double> &solution,
647 moveit_msgs::MoveItErrorCodes &error_code,
662 const std::vector<double> &ik_seed_state,
664 std::vector<double> &solution,
666 moveit_msgs::MoveItErrorCodes &error_code,
669 std::vector<double> consistency_limits;
681 const std::vector<double> &ik_seed_state,
683 const std::vector<double> &consistency_limits,
684 std::vector<double> &solution,
686 moveit_msgs::MoveItErrorCodes &error_code,
695 if(free_params_.size()==0)
700 if(!
getPositionIK(ik_pose, ik_seed_state, solution, error_code))
703 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
708 if( !solution_callback.empty() )
710 solution_callback(ik_pose, solution, error_code);
711 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
733 error_code.val = error_code.NO_IK_SOLUTION;
739 ROS_ERROR_STREAM_NAMED(
"ikfast",
"Seed state must have size " << num_joints_ <<
" instead of size " << ik_seed_state.size());
740 error_code.val = error_code.NO_IK_SOLUTION;
744 if(!consistency_limits.empty() && consistency_limits.size() !=
num_joints_)
746 ROS_ERROR_STREAM_NAMED(
"ikfast",
"Consistency limits be empty or must have size " << num_joints_ <<
" instead of size " << consistency_limits.size());
747 error_code.val = error_code.NO_IK_SOLUTION;
758 std::vector<double> vfree(free_params_.size());
763 double initial_guess = ik_seed_state[free_params_[0]];
764 vfree[0] = initial_guess;
768 int num_positive_increments;
769 int num_negative_increments;
771 if(!consistency_limits.empty())
775 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
776 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
790 ROS_DEBUG_STREAM_NAMED(
"ikfast",
"Free param is " << free_params_[0] <<
" initial guess is " << initial_guess <<
", # positive increments: " << num_positive_increments <<
", # negative increments: " << num_negative_increments);
791 if ((search_mode &
OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
794 double best_costs = -1.0;
795 std::vector<double> best_solution;
796 int nattempts = 0, nvalid = 0;
801 int numsol =
solve(frame,vfree, solutions);
809 for(
int s = 0;
s < numsol; ++
s)
812 std::vector<double> sol;
815 bool obeys_limits =
true;
816 for(
unsigned int i = 0; i < sol.size(); i++)
818 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
820 obeys_limits =
false;
830 if(!solution_callback.empty())
832 solution_callback(ik_pose, solution, error_code);
836 error_code.val = error_code.SUCCESS;
839 if(error_code.val == error_code.SUCCESS)
842 if (search_mode & OPTIMIZE_MAX_JOINT)
846 for(
unsigned int i = 0; i < solution.size(); i++)
848 double d = fabs(ik_seed_state[i] - solution[i]);
852 if (costs < best_costs || best_costs == -1.0)
855 best_solution = solution;
866 if(!
getCount(counter, num_positive_increments, -num_negative_increments))
869 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
879 if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
881 solution = best_solution;
882 error_code.val = error_code.SUCCESS;
887 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
893 const std::vector<double> &ik_seed_state,
894 std::vector<double> &solution,
895 moveit_msgs::MoveItErrorCodes &error_code,
906 std::vector<double> vfree(free_params_.size());
907 for(std::size_t i = 0; i < free_params_.size(); ++i)
909 int p = free_params_[i];
910 ROS_ERROR(
"%u is %f",p,ik_seed_state[p]);
911 vfree[i] = ik_seed_state[p];
918 int numsol =
solve(frame,vfree,solutions);
924 for(
int s = 0;
s < numsol; ++
s)
926 std::vector<double> sol;
928 ROS_DEBUG_NAMED(
"ikfast",
"Sol %d: %e %e %e %e %e %e",
s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]);
930 bool obeys_limits =
true;
931 for(
unsigned int i = 0; i < sol.size(); i++)
934 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-
LIMIT_TOLERANCE)) ||
938 obeys_limits =
false;
939 ROS_DEBUG_STREAM_NAMED(
"ikfast",
"Not in limits! " << i <<
" value " << sol[i] <<
" has limit: " << joint_has_limits_vector_[i] <<
" being " << joint_min_vector_[i] <<
" to " << joint_max_vector_[i]);
947 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
957 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
end effector origin and direction reaches desired 3D translation and direction. Can be thought of as ...
bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_name, double search_discretization)
const std::vector< std::string > & getLinkNames() const
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
URDF_EXPORT bool initString(const std::string &xmlstring)
#define ROS_WARN_NAMED(name,...)
bool getCount(int &count, const int &max_count, const int &min_count) const
virtual size_t GetNumSolutions() const
returns the number of solutions stored
the mask for the unique ids
void getClosestSolution(const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
ray on end effector coordinate system reaches desired global ray
IkParameterizationType
The types of inverse kinematics parameterizations supported.
std::vector< int > free_params_
The discrete solutions are returned in this structure.
local point on end effector origin reaches desired 3D global point
std::vector< std::string > link_names_
#define ROS_INFO_STREAM_NAMED(name, args)
2D translation along XY plane and 1D rotation around Z axis. The offset of the rotation is measured s...
2D translation along XY plane
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
Given a desired pose of the end-effector, search for the joint angles required to reach it...
IKFAST_API int GetNumFreeParameters()
void fillFreeParams(int count, int *array)
number of parameterizations (does not count IKP_None)
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
#define ROS_DEBUG_NAMED(name,...)
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
const double LIMIT_TOLERANCE
end effector reaches desired 6D transformation
virtual const std::string & getTipFrame() const
end effector reaches desired 3D rotation
#define ROS_FATAL_STREAM_NAMED(name, args)
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
std::vector< bool > joint_has_limits_vector_
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
IKFAST_API int * GetFreeParameters()
direction on end effector coordinate system points to desired 3D position
double harmonize(const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
virtual void setValues(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
def xml_string(rootXml, addHeader=True)
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to x...
bit is set if the ikparameterization contains custom data, this is only used when serializing the ik ...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
direction on end effector coordinate system reaches desired direction
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
SEARCH_MODE
Search modes for searchPositionIK(), see there.
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to y...
const std::vector< std::string > & getJointNames() const
bit is set if the data represents the time-derivate velocity of an IkParameterization ...
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
Default implementation of IkSolutionListBase.
bool getParam(const std::string &key, std::string &s) const
double search_discretization_
#define ROS_FATAL_NAMED(name,...)
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z...
#define ROS_ERROR_NAMED(name,...)
IKFAST_API int GetIkType()
IKFAST_API int GetNumJoints()
bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
Given a desired pose of the end-effector, compute the joint angles to reach it.
int solve(KDL::Frame &pose_frame, const std::vector< double > &vfree, IkSolutionList< IkReal > &solutions) const
Calls the IK solver from IKFast.
end effector origin reaches desired 3D translation
std::vector< double > joint_min_vector_
std::vector< std::string > joint_names_
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
Given a set of joint angles and a set of links, compute their pose.
std::vector< double > joint_max_vector_
void getSolution(const IkSolutionList< IkReal > &solutions, int i, std::vector< double > &solution) const
Gets a specific solution from the set.
#define ROS_WARN_STREAM_NAMED(name, args)
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...