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());
278 ROS_FATAL(
"Only one free joint paramter supported!");
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());
306 urdf::JointSharedPtr joint = link->parent_joint;
309 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
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;
349 ROS_WARN_NAMED(
"ikfast",
"no joint corresponding to %s",link->name.c_str());
351 link = link->getParent();
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){
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,
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;
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++)
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++)
938 obeys_limits =
false;
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 reaches desired 3D translation, manipulator direction makes a specific angle with...
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)
2D translation along XY plane
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,...)
bit is set if the ikparameterization contains custom data, this is only used when serializing the ik ...
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
bool getCount(int &count, const int &max_count, const int &min_count) const
const double LIMIT_TOLERANCE
virtual size_t GetNumSolutions() const
returns the number of solutions stored
void getClosestSolution(const IkSolutionList< IkReal > &solutions, const std::vector< double > &ik_seed_state, std::vector< double > &solution) const
std::vector< double > joint_max_vector_
IkParameterizationType
The types of inverse kinematics parameterizations supported.
The discrete solutions are returned in this structure.
SEARCH_MODE
Search modes for searchPositionIK(), see there.
end effector reaches desired 6D transformation
#define ROS_INFO_STREAM_NAMED(name, args)
number of parameterizations (does not count IKP_None)
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...
std::vector< std::string > joint_names_
end effector reaches desired 3D rotation
std::vector< int > free_params_
local point on end effector origin reaches desired 3D global point
IKFAST_API int GetNumFreeParameters()
direction on end effector coordinate system points to desired 3D position
void fillFreeParams(int count, int *array)
the mask for the unique ids
#define ROS_DEBUG_NAMED(name,...)
bit is set if the data represents the time-derivate velocity of an IkParameterization ...
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
virtual const std::string & getTipFrame() const
#define ROS_FATAL_STREAM_NAMED(name, args)
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
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
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
2D translation along XY plane and 1D rotation around Z axis. The offset of the rotation is measured s...
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
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 and direction reaches desired 3D translation and direction. Can be thought of as ...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to y...
IKFAST_API int * GetFreeParameters()
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
const std::vector< std::string > & getJointNames() const
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to x...
end effector origin reaches desired 3D translation
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
Default implementation of IkSolutionListBase.
ray on end effector coordinate system reaches desired global ray
bool getParam(const std::string &key, std::string &s) const
end effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z...
double search_discretization_
std::vector< std::string > link_names_
#define ROS_FATAL_NAMED(name,...)
#define ROS_ERROR_NAMED(name,...)
std::vector< double > joint_min_vector_
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.
direction on end effector coordinate system reaches desired direction
std::vector< bool > joint_has_limits_vector_
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.
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)