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());
279 ROS_FATAL(
"Only one free joint paramter supported!");
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_);
307 urdf::JointSharedPtr joint = link->parent_joint;
310 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
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;
350 ROS_WARN_NAMED(
"ikfast",
"no joint corresponding to %s",link->name.c_str());
352 link = link->getParent();
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){
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,
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;
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++)
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++)
896 obeys_limits =
false;
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 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 ...
bool getCount(int &count, const int &max_count, const int &min_count) const
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.
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_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
PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase)
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...
const double LIMIT_TOLERANCE
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)