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,
231 bool getPositionFK(
const std::vector<std::string> &link_names,
232 const std::vector<double> &joint_angles,
233 std::vector<geometry_msgs::Pose> &poses)
const;
237 bool initialize(
const std::string &robot_description,
238 const std::string& group_name,
239 const std::string& base_name,
240 const std::string& tip_name,
241 double search_discretization);
254 double harmonize(
const std::vector<double> &ik_seed_state, std::vector<double> &solution)
const;
258 bool getCount(
int &count,
const int &max_count,
const int &min_count)
const;
263 const std::string& group_name,
264 const std::string& base_name,
265 const std::string& tip_name,
266 double search_discretization)
268 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
273 node_handle.
param(
"robot",robot,std::string());
279 if(free_params_.size() > 1)
281 ROS_FATAL(
"Only one free joint paramter supported!");
288 std::string urdf_xml,full_urdf_xml;
289 node_handle.
param(
"urdf_xml",urdf_xml,robot_description);
293 if (!node_handle.
getParam(full_urdf_xml, xml_string))
295 ROS_FATAL_NAMED(
"ikfast",
"Could not load the xml from parameter server: %s", urdf_xml.c_str());
299 node_handle.
param(full_urdf_xml,xml_string,std::string());
304 #if URDFDOM_1_0_0_API 305 urdf::LinkConstSharedPtr link = robot_model.getLink(
getTipFrame());
312 link_names_.push_back(link->name);
313 #if URDFDOM_1_0_0_API 314 urdf::JointSharedPtr joint = link->parent_joint;
320 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
324 joint_names_.push_back(joint->name);
327 if ( joint->type != urdf::Joint::CONTINUOUS )
331 lower = joint->safety->soft_lower_limit;
332 upper = joint->safety->soft_upper_limit;
334 lower = joint->limits->lower;
335 upper = joint->limits->upper;
347 joint_has_limits_vector_.push_back(
true);
348 joint_min_vector_.push_back(lower);
349 joint_max_vector_.push_back(upper);
353 joint_has_limits_vector_.push_back(
false);
354 joint_min_vector_.push_back(-
M_PI);
355 joint_max_vector_.push_back(
M_PI);
360 ROS_WARN_NAMED(
"ikfast",
"no joint corresponding to %s",link->name.c_str());
362 link = link->getParent();
371 std::reverse(link_names_.begin(),link_names_.end());
372 std::reverse(joint_names_.begin(),joint_names_.end());
373 std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
374 std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
375 std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
378 ROS_INFO_STREAM_NAMED(
"ikfast",joint_names_[i] <<
" " << joint_min_vector_[i] <<
" " << joint_max_vector_[i] <<
" " << joint_has_limits_vector_[i]);
390 trans[0] = pose_frame.
p[0];
391 trans[1] = pose_frame.
p[1];
392 trans[2] = pose_frame.
p[2];
417 ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
426 ComputeIk(trans, direction.
data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
433 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
438 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
448 ROS_ERROR_NAMED(
"ikfast",
"IK for this IkParameterizationType not implemented yet.");
452 ROS_ERROR_NAMED(
"ikfast",
"Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
460 solution.resize(num_joints_);
464 std::vector<IkReal> vsolfree( sol.
GetFree().size() );
465 sol.
GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
478 std::vector<double> ss = ik_seed_state;
479 for(
size_t i=0; i< ik_seed_state.size(); ++i)
481 while(ss[i] > 2*
M_PI) {
484 while(ss[i] < 2*
M_PI) {
487 while(solution[i] > 2*
M_PI) {
488 solution[i] -= 2*
M_PI;
490 while(solution[i] < 2*
M_PI) {
491 solution[i] += 2*
M_PI;
493 dist_sqr += fabs(ik_seed_state[i] - solution[i]);
523 double mindist = DBL_MAX;
525 std::vector<double> sol;
531 double dist =
harmonize(ik_seed_state, sol);
534 if(minindex == -1 || dist<mindist){
547 free_params_.clear();
548 for(
int i=0; i<count;++i) free_params_.push_back(array[i]);
555 if(-count >= min_count)
560 else if(count+1 <= max_count)
572 if(1-count <= max_count)
577 else if(count-1 >= min_count)
588 const std::vector<double> &joint_angles,
589 std::vector<geometry_msgs::Pose> &poses)
const 592 ROS_ERROR_NAMED(
"ikfast",
"Can only compute FK for IKTYPE_TRANSFORM_6D!");
597 if(link_names.size() == 0) {
602 if(link_names.size()!=1 || link_names[0]!=
getTipFrame()){
609 IkReal eerot[9],eetrans[3];
610 IkReal
angles[joint_angles.size()];
611 for (
unsigned char i=0; i < joint_angles.size(); i++)
612 angles[i] = joint_angles[i];
617 for(
int i=0; i<3;++i)
618 p_out.
p.
data[i] = eetrans[i];
620 for(
int i=0; i<9;++i)
621 p_out.
M.
data[i] = eerot[i];
630 const std::vector<double> &ik_seed_state,
632 std::vector<double> &solution,
633 moveit_msgs::MoveItErrorCodes &error_code,
637 std::vector<double> consistency_limits;
650 const std::vector<double> &ik_seed_state,
652 const std::vector<double> &consistency_limits,
653 std::vector<double> &solution,
654 moveit_msgs::MoveItErrorCodes &error_code,
669 const std::vector<double> &ik_seed_state,
671 std::vector<double> &solution,
673 moveit_msgs::MoveItErrorCodes &error_code,
676 std::vector<double> consistency_limits;
688 const std::vector<double> &ik_seed_state,
690 const std::vector<double> &consistency_limits,
691 std::vector<double> &solution,
693 moveit_msgs::MoveItErrorCodes &error_code,
702 if(free_params_.size()==0)
707 if(!
getPositionIK(ik_pose, ik_seed_state, solution, error_code))
710 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
715 if( !solution_callback.empty() )
717 solution_callback(ik_pose, solution, error_code);
718 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
740 error_code.val = error_code.NO_IK_SOLUTION;
746 ROS_ERROR_STREAM_NAMED(
"ikfast",
"Seed state must have size " << num_joints_ <<
" instead of size " << ik_seed_state.size());
747 error_code.val = error_code.NO_IK_SOLUTION;
751 if(!consistency_limits.empty() && consistency_limits.size() !=
num_joints_)
753 ROS_ERROR_STREAM_NAMED(
"ikfast",
"Consistency limits be empty or must have size " << num_joints_ <<
" instead of size " << consistency_limits.size());
754 error_code.val = error_code.NO_IK_SOLUTION;
765 std::vector<double> vfree(free_params_.size());
770 double initial_guess = ik_seed_state[free_params_[0]];
771 vfree[0] = initial_guess;
775 int num_positive_increments;
776 int num_negative_increments;
778 if(!consistency_limits.empty())
782 double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
783 double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
797 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);
798 if ((search_mode &
OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
801 double best_costs = -1.0;
802 std::vector<double> best_solution;
803 int nattempts = 0, nvalid = 0;
808 int numsol =
solve(frame,vfree, solutions);
816 for(
int s = 0;
s < numsol; ++
s)
819 std::vector<double> sol;
822 bool obeys_limits =
true;
823 for(
unsigned int i = 0; i < sol.size(); i++)
825 if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
827 obeys_limits =
false;
837 if(!solution_callback.empty())
839 solution_callback(ik_pose, solution, error_code);
843 error_code.val = error_code.SUCCESS;
846 if(error_code.val == error_code.SUCCESS)
849 if (search_mode & OPTIMIZE_MAX_JOINT)
853 for(
unsigned int i = 0; i < solution.size(); i++)
855 double d = fabs(ik_seed_state[i] - solution[i]);
859 if (costs < best_costs || best_costs == -1.0)
862 best_solution = solution;
873 if(!
getCount(counter, num_positive_increments, -num_negative_increments))
876 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
886 if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
888 solution = best_solution;
889 error_code.val = error_code.SUCCESS;
894 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
900 const std::vector<double> &ik_seed_state,
901 std::vector<double> &solution,
902 moveit_msgs::MoveItErrorCodes &error_code,
913 std::vector<double> vfree(free_params_.size());
914 for(std::size_t i = 0; i < free_params_.size(); ++i)
916 int p = free_params_[i];
917 ROS_ERROR(
"%u is %f",p,ik_seed_state[p]);
918 vfree[i] = ik_seed_state[p];
925 int numsol =
solve(frame,vfree,solutions);
931 for(
int s = 0;
s < numsol; ++
s)
933 std::vector<double> sol;
935 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]);
937 bool obeys_limits =
true;
938 for(
unsigned int i = 0; i < sol.size(); i++)
941 if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-
LIMIT_TOLERANCE)) ||
945 obeys_limits =
false;
946 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]);
954 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
964 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
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...
std::vector< bool > joint_has_limits_vector_
IKFAST_API int GetNumJoints()
local point on end effector origin reaches desired 3D global point
IkParameterizationType
The types of inverse kinematics parameterizations supported.
end effector reaches desired 3D rotation
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
URDF_EXPORT bool initString(const std::string &xmlstring)
end effector origin and direction reaches desired 3D translation and direction. Can be thought of as ...
#define ROS_WARN_NAMED(name,...)
bit is set if the ikparameterization contains custom data, this is only used when serializing the ik ...
IKFAST_API int GetNumFreeParameters()
virtual size_t GetNumSolutions() const
returns the number of solutions stored
std::vector< std::string > link_names_
The discrete solutions are returned in this structure.
const std::vector< std::string > & getJointNames() const
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to y...
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)
#define ROS_INFO_STREAM_NAMED(name, args)
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
virtual void Clear()
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
PLUGINLIB_EXPORT_CLASS(nextage_right_arm_ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
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...
2D translation along XY plane
double harmonize(const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
std::vector< std::string > joint_names_
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.
#define ROS_DEBUG_NAMED(name,...)
void fillFreeParams(int count, int *array)
number of parameterizations (does not count IKP_None)
void getClosestSolution(const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
const double LIMIT_TOLERANCE
direction on end effector coordinate system reaches desired direction
virtual const std::string & getTipFrame() const
#define ROS_FATAL_STREAM_NAMED(name, args)
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
IKFAST_API int * GetFreeParameters()
end effector origin reaches desired 3D translation
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
bool getCount(int &count, const int &max_count, const int &min_count) const
std::vector< int > free_params_
const std::vector< std::string > & getLinkNames() const
IKFAST_API int GetIkType()
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 data represents the time-derivate velocity of an IkParameterization ...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
2D translation along XY plane and 1D rotation around Z axis. The offset of the rotation is measured s...
std::vector< double > joint_min_vector_
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
end effector reaches desired 6D transformation
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
int solve(KDL::Frame &pose_frame, const std::vector< double > &vfree, IkSolutionList< IkReal > &solutions) const
Calls the IK solver from IKFast.
SEARCH_MODE
Search modes for searchPositionIK(), see there.
Default implementation of IkSolutionListBase.
bool getParam(const std::string &key, std::string &s) const
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.
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
double search_discretization_
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
void getSolution(const IkSolutionList< IkReal > &solutions, int i, std::vector< double > &solution) const
Gets a specific solution from the set.
direction on end effector coordinate system points to desired 3D position
the mask for the unique ids
ray on end effector coordinate system reaches desired global ray
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z...
std::vector< double > joint_max_vector_
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
#define ROS_WARN_STREAM_NAMED(name, args)