59 #define IKFAST_NO_MAIN // Don't include main() from IKFast 146 const std::vector<double> &ik_seed_state,
147 std::vector<double> &solution,
148 moveit_msgs::MoveItErrorCodes &error_code,
160 const std::vector<double> &ik_seed_state,
162 std::vector<double> &solution,
163 moveit_msgs::MoveItErrorCodes &error_code,
176 const std::vector<double> &ik_seed_state,
178 const std::vector<double> &consistency_limits,
179 std::vector<double> &solution,
180 moveit_msgs::MoveItErrorCodes &error_code,
192 const std::vector<double> &ik_seed_state,
194 std::vector<double> &solution,
196 moveit_msgs::MoveItErrorCodes &error_code,
210 const std::vector<double> &ik_seed_state,
212 const std::vector<double> &consistency_limits,
213 std::vector<double> &solution,
215 moveit_msgs::MoveItErrorCodes &error_code,
229 bool getPositionFK(
const std::vector<std::string> &link_names,
230 const std::vector<double> &joint_angles,
231 std::vector<geometry_msgs::Pose> &poses)
const;
235 bool initialize(
const std::string &robot_description,
236 const std::string& group_name,
237 const std::string& base_name,
238 const std::string& tip_name,
239 double search_discretization);
252 double harmonize(
const std::vector<double> &ik_seed_state, std::vector<double> &solution)
const;
256 bool getCount(
int &count,
const int &max_count,
const int &min_count)
const;
261 const std::string& group_name,
262 const std::string& base_name,
263 const std::string& tip_name,
264 double search_discretization)
266 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
271 node_handle.
param(
"robot",robot,std::string());
277 if(free_params_.size() > 1)
286 std::string urdf_xml,full_urdf_xml;
287 node_handle.
param(
"urdf_xml",urdf_xml,robot_description);
291 if (!node_handle.
getParam(full_urdf_xml, xml_string))
293 ROS_FATAL_NAMED(
"ikfast",
"Could not load the xml from parameter server: %s", urdf_xml.c_str());
297 node_handle.
param(full_urdf_xml,xml_string,std::string());
302 urdf::LinkConstSharedPtr link = robot_model.getLink(
tip_frame_);
306 link_names_.push_back(link->name);
307 urdf::JointSharedPtr joint = link->parent_joint;
310 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
314 joint_names_.push_back(joint->name);
317 if ( joint->type != urdf::Joint::CONTINUOUS )
321 lower = joint->safety->soft_lower_limit;
322 upper = joint->safety->soft_upper_limit;
324 lower = joint->limits->lower;
325 upper = joint->limits->upper;
337 joint_has_limits_vector_.push_back(
true);
338 joint_min_vector_.push_back(lower);
339 joint_max_vector_.push_back(upper);
343 joint_has_limits_vector_.push_back(
false);
344 joint_min_vector_.push_back(-M_PI);
345 joint_max_vector_.push_back(M_PI);
350 ROS_WARN_NAMED(
"ikfast",
"no joint corresponding to %s",link->name.c_str());
352 link = link->getParent();
361 std::reverse(link_names_.begin(),link_names_.end());
362 std::reverse(joint_names_.begin(),joint_names_.end());
363 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
364 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
365 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
368 ROS_INFO_STREAM_NAMED(
"ikfast",joint_names_[i] <<
" " << joint_min_vector_[i] <<
" " << joint_max_vector_[i] <<
" " << joint_has_limits_vector_[i]);
380 trans[0] = pose_frame.
p[0];
381 trans[1] = pose_frame.
p[1];
382 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.");
438 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
442 ROS_ERROR_NAMED(
"ikfast",
"Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
450 solution.resize(num_joints_);
454 std::vector<IkReal> vsolfree( sol.
GetFree().size() );
455 sol.
GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
468 std::vector<double> ss = ik_seed_state;
469 for(
size_t i=0; i< ik_seed_state.size(); ++i)
471 while(ss[i] > 2*
M_PI) {
474 while(ss[i] < 2*
M_PI) {
477 while(solution[i] > 2*
M_PI) {
478 solution[i] -= 2*
M_PI;
480 while(solution[i] < 2*
M_PI) {
481 solution[i] += 2*
M_PI;
483 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
513 double mindist = DBL_MAX;
515 std::vector<double> sol;
521 double dist =
harmonize(ik_seed_state, sol);
524 if(minindex == -1 || dist<mindist){
537 free_params_.clear();
538 for(
int i=0; i<count;++i) free_params_.push_back(array[i]);
545 if(-count >= min_count)
550 else if(count+1 <= max_count)
562 if(1-count <= max_count)
567 else if(count-1 >= min_count)
578 const std::vector<double> &joint_angles,
579 std::vector<geometry_msgs::Pose> &poses)
const 581 #ifndef IKTYPE_TRANSFORM_6D 582 ROS_ERROR_NAMED(
"ikfast",
"Can only compute FK for IKTYPE_TRANSFORM_6D!");
587 if(link_names.size() == 0) {
592 if(link_names.size()!=1 || link_names[0]!=
tip_frame_){
599 IkReal eerot[9],eetrans[3];
600 IkReal
angles[joint_angles.size()];
601 for (
unsigned char i=0; i < joint_angles.size(); i++)
602 angles[i] = joint_angles[i];
607 for(
int i=0; i<3;++i)
608 p_out.
p.
data[i] = eetrans[i];
610 for(
int i=0; i<9;++i)
611 p_out.
M.
data[i] = eerot[i];
620 const std::vector<double> &ik_seed_state,
622 std::vector<double> &solution,
623 moveit_msgs::MoveItErrorCodes &error_code,
627 std::vector<double> consistency_limits;
640 const std::vector<double> &ik_seed_state,
642 const std::vector<double> &consistency_limits,
643 std::vector<double> &solution,
644 moveit_msgs::MoveItErrorCodes &error_code,
659 const std::vector<double> &ik_seed_state,
661 std::vector<double> &solution,
663 moveit_msgs::MoveItErrorCodes &error_code,
666 std::vector<double> consistency_limits;
678 const std::vector<double> &ik_seed_state,
680 const std::vector<double> &consistency_limits,
681 std::vector<double> &solution,
683 moveit_msgs::MoveItErrorCodes &error_code,
689 if(free_params_.size()==0)
694 if(!
getPositionIK(ik_pose, ik_seed_state, solution, error_code))
697 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
702 if( !solution_callback.empty() )
704 solution_callback(ik_pose, solution, error_code);
705 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
727 error_code.val = error_code.NO_IK_SOLUTION;
733 ROS_ERROR_STREAM_NAMED(
"ikfast",
"Seed state must have size " << num_joints_ <<
" instead of size " << ik_seed_state.size());
734 error_code.val = error_code.NO_IK_SOLUTION;
738 if(!consistency_limits.empty() && consistency_limits.size() !=
num_joints_)
740 ROS_ERROR_STREAM_NAMED(
"ikfast",
"Consistency limits be empty or must have size " << num_joints_ <<
" instead of size " << consistency_limits.size());
741 error_code.val = error_code.NO_IK_SOLUTION;
752 std::vector<double> vfree(free_params_.size());
757 double initial_guess = ik_seed_state[free_params_[0]];
758 vfree[0] = initial_guess;
762 int num_positive_increments;
763 int num_negative_increments;
765 if(!consistency_limits.empty())
769 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
770 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
784 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);
789 int numsol =
solve(frame,vfree, solutions);
797 for(
int s = 0;
s < numsol; ++
s)
799 std::vector<double> sol;
802 bool obeys_limits =
true;
803 for(
unsigned int i = 0; i < sol.size(); i++)
805 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
807 obeys_limits =
false;
817 if(!solution_callback.empty())
819 solution_callback(ik_pose, solution, error_code);
823 error_code.val = error_code.SUCCESS;
826 if(error_code.val == error_code.SUCCESS)
834 if(!
getCount(counter, num_positive_increments, num_negative_increments))
836 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
841 ROS_DEBUG_STREAM_NAMED(
"ikfast",
"Attempt " << counter <<
" with 0th free joint having value " << vfree[0]);
845 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
851 const std::vector<double> &ik_seed_state,
852 std::vector<double> &solution,
853 moveit_msgs::MoveItErrorCodes &error_code,
864 std::vector<double> vfree(free_params_.size());
865 for(std::size_t i = 0; i < free_params_.size(); ++i)
867 int p = free_params_[i];
868 ROS_ERROR(
"%u is %f",p,ik_seed_state[p]);
869 vfree[i] = ik_seed_state[p];
876 int numsol =
solve(frame,vfree,solutions);
882 for(
int s = 0;
s < numsol; ++
s)
884 std::vector<double> sol;
886 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]);
888 bool obeys_limits =
true;
889 for(
unsigned int i = 0; i < sol.size(); i++)
892 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-
LIMIT_TOLERANCE)) ||
896 obeys_limits =
false;
897 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]);
905 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
915 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.
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
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,...)
end effector reaches desired 6D transformation
end effector reaches desired 3D rotation
#define ROS_FATAL_STREAM_NAMED(name, args)
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
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()
const double LIMIT_TOLERANCE
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...