82 #include <urdf_model/model.h> 97 URKinematicsPlugin::URKinematicsPlugin():active_(false) {}
101 std::vector<double> jnt_array_vector(
dimension_, 0.0);
109 jnt_array(i) = jnt_array_vector[i];
122 const std::vector<double> &consistency_limits,
124 bool lock_redundancy)
const 129 near[i] = seed_state(i);
132 std::vector<double> consistency_limits_mimic;
137 consistency_limits_mimic.push_back(consistency_limits[i]);
140 joint_model_group_->getVariableRandomPositionsNearBy(
state_->getRandomNumberGenerator(), values, near, consistency_limits_mimic);
154 jnt_array(i) = values[i];
159 const std::vector<double> &consistency_limits,
163 if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
169 const std::string& group_name,
170 const std::string& base_frame,
171 const std::string& tip_frame,
172 double search_discretization)
174 setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
178 const urdf::ModelInterfaceSharedPtr &urdf_model = rdf_loader.
getURDF();
180 if (!urdf_model || !srdf)
182 ROS_ERROR_NAMED(
"kdl",
"URDF and SRDF must be loaded for KDL kinematics solver to work.");
186 robot_model_.reset(
new robot_model::RobotModel(urdf_model, srdf));
188 robot_model::JointModelGroup* joint_model_group =
robot_model_->getJointModelGroup(group_name);
189 if (!joint_model_group)
192 if(!joint_model_group->isChain())
194 ROS_ERROR_NAMED(
"kdl",
"Group '%s' is not a chain", group_name.c_str());
197 if(!joint_model_group->isSingleDOFJoints())
199 ROS_ERROR_NAMED(
"kdl",
"Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
216 dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
217 for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
221 ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
222 const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
230 if(!joint_model_group->hasLinkModel(
getTipFrame()))
232 ROS_ERROR_NAMED(
"kdl",
"Could not find tip name in joint group '%s'", group_name.c_str());
236 fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
248 int max_solver_iterations;
252 lookupParam(
"max_solver_iterations", max_solver_iterations, 500);
254 lookupParam(group_name+
"/position_only_ik", position_ik,
false);
262 bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
263 std::vector<unsigned int> redundant_joints_map_index;
265 std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
266 unsigned int joint_counter = 0;
272 if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
275 mimic_joint.
reset(joint_counter);
277 mimic_joint.
active =
true;
278 mimic_joints.push_back(mimic_joint);
282 if (joint_model_group->hasJointModel(jm->getName()))
284 if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
287 mimic_joint.
reset(joint_counter);
289 mimic_joint.
offset = jm->getMimicOffset();
290 mimic_joint.
multiplier = jm->getMimicFactor();
291 mimic_joints.push_back(mimic_joint);
296 for (std::size_t i = 0; i < mimic_joints.size(); ++i)
298 if(!mimic_joints[i].active)
300 const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
301 for(std::size_t j=0; j < mimic_joints.size(); ++j)
303 if(mimic_joints[j].joint_name == joint_model->getName())
305 mimic_joints[i].map_index = mimic_joints[j].map_index;
345 for(
int i=1; i<6; i++) {
347 if(cur_ur_joint_ind < 0) {
349 "Kin chain provided in model doesn't contain standard UR joint '%s'.",
353 if(cur_ur_joint_ind != last_ur_joint_ind + 1) {
355 "Kin chain provided in model doesn't have proper serial joint order: '%s'.",
359 last_ur_joint_ind = cur_ur_joint_ind;
394 std::vector<unsigned int> redundant_joints_map_index;
395 unsigned int counter = 0;
398 bool is_redundant_joint =
false;
399 for(std::size_t j=0; j < redundant_joints.size(); ++j)
401 if(i == redundant_joints[j])
403 is_redundant_joint =
true;
408 if(!is_redundant_joint)
413 redundant_joints_map_index.push_back(counter);
418 for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
419 ROS_DEBUG_NAMED(
"kdl",
"Redundant joint map index: %d %d", (
int) i, (int) redundant_joints_map_index[i]);
428 for (
unsigned int i=0; i <
ik_chain_info_.joint_names.size(); i++) {
453 const std::vector<double> &ik_seed_state,
454 std::vector<double> &solution,
455 moveit_msgs::MoveItErrorCodes &error_code,
459 std::vector<double> consistency_limits;
472 const std::vector<double> &ik_seed_state,
474 std::vector<double> &solution,
475 moveit_msgs::MoveItErrorCodes &error_code,
479 std::vector<double> consistency_limits;
492 const std::vector<double> &ik_seed_state,
494 const std::vector<double> &consistency_limits,
495 std::vector<double> &solution,
496 moveit_msgs::MoveItErrorCodes &error_code,
511 const std::vector<double> &ik_seed_state,
513 std::vector<double> &solution,
515 moveit_msgs::MoveItErrorCodes &error_code,
518 std::vector<double> consistency_limits;
530 const std::vector<double> &ik_seed_state,
532 const std::vector<double> &consistency_limits,
533 std::vector<double> &solution,
535 moveit_msgs::MoveItErrorCodes &error_code,
550 {
return l.second < r.second; }
553 const std::vector<double> &ik_seed_state,
555 std::vector<double> &solution,
557 moveit_msgs::MoveItErrorCodes &error_code,
558 const std::vector<double> &consistency_limits,
564 error_code.val = error_code.NO_IK_SOLUTION;
570 error_code.val = error_code.NO_IK_SOLUTION;
574 if(!consistency_limits.empty() && consistency_limits.size() !=
dimension_) {
576 error_code.val = error_code.NO_IK_SOLUTION;
582 jnt_seed_state(i) = ik_seed_state[i];
584 solution.
resize(dimension_);
596 double homo_ik_pose[4][4];
597 double q_ik_sols[8][6];
603 error_code.val = error_code.TIMED_OUT;
609 for(uint32_t i=0; i<jnt_pos_base.
rows(); i++)
610 jnt_pos_base(i) = jnt_pos_test(i);
611 for(uint32_t i=0; i<jnt_pos_tip.
rows(); i++)
613 for(uint32_t i=0; i<jnt_seed_state.
rows(); i++)
614 solution[i] = jnt_pos_test(i);
616 if(fk_solver_base.
JntToCart(jnt_pos_base, pose_base) < 0) {
621 if(fk_solver_tip.
JntToCart(jnt_pos_tip, pose_tip) < 0) {
630 kdl_ik_pose_ur_chain = pose_base.
Inverse() * kdl_ik_pose * pose_tip.
Inverse();
632 kdl_ik_pose_ur_chain.
Make4x4((
double*) homo_ik_pose);
635 for(
int i=0; i<3; i++) homo_ik_pose[i][3] *= 1000;
640 num_sols =
inverse((
double*) homo_ik_pose, (
double*) q_ik_sols,
644 uint16_t num_valid_sols;
645 std::vector< std::vector<double> > q_ik_valid_sols;
646 for(uint16_t i=0; i<num_sols; i++)
649 std::vector< double > valid_solution;
650 valid_solution.assign(6,0.0);
652 for(uint16_t j=0; j<6; j++)
656 valid_solution[j] = q_ik_sols[i][j];
662 valid_solution[j] = q_ik_sols[i][j]-2*
M_PI;
668 valid_solution[j] = q_ik_sols[i][j]+2*
M_PI;
681 q_ik_valid_sols.push_back(valid_solution);
687 std::vector<idx_double> weighted_diffs;
688 for(uint16_t i=0; i<q_ik_valid_sols.size(); i++) {
689 double cur_weighted_diff = 0;
690 for(uint16_t j=0; j<6; j++) {
694 cur_weighted_diff = std::numeric_limits<double>::infinity();
699 weighted_diffs.push_back(
idx_double(i, cur_weighted_diff));
702 std::sort(weighted_diffs.begin(), weighted_diffs.end(),
comparator);
706 printf(
" q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", ik_seed_state[1], ik_seed_state[2], ik_seed_state[3], ik_seed_state[4], ik_seed_state[5], ik_seed_state[6]);
707 for(uint16_t i=0; i<weighted_diffs.size(); i++) {
708 int cur_idx = weighted_diffs[i].first;
709 printf(
"diff %f, i %d, q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", weighted_diffs[i].second, cur_idx, q_ik_valid_sols[cur_idx][0], q_ik_valid_sols[cur_idx][1], q_ik_valid_sols[cur_idx][2], q_ik_valid_sols[cur_idx][3], q_ik_valid_sols[cur_idx][4], q_ik_valid_sols[cur_idx][5]);
714 for(uint16_t i=0; i<weighted_diffs.size(); i++) {
715 if(weighted_diffs[i].second == std::numeric_limits<double>::infinity()) {
721 int cur_idx = weighted_diffs[i].first;
722 solution = q_ik_valid_sols[cur_idx];
725 if(!solution_callback.empty())
726 solution_callback(ik_pose, solution, error_code);
728 error_code.val = error_code.SUCCESS;
730 if(error_code.val == error_code.SUCCESS) {
732 std::vector<std::string> fk_link_names;
734 std::vector<geometry_msgs::Pose> fk_poses;
738 printf(
"FK(solution) - pose \n");
739 for(
int i=0; i<4; i++) {
740 for(
int j=0; j<4; j++)
741 printf(
"%1.6f ", kdl_fk_pose(i, j)-kdl_ik_pose(i, j));
751 ROS_DEBUG_NAMED(
"kdl",
"Will not pertubate redundant joints to find solution");
755 if(dimension_ == 6) {
756 ROS_DEBUG_NAMED(
"kdl",
"No other joints to pertubate, cannot find solution");
761 if(!consistency_limits.empty()) {
768 ROS_DEBUG_NAMED(
"kdl",
"An IK that satisifes the constraints and is collision free could not be found");
769 error_code.val = error_code.NO_IK_SOLUTION;
774 const std::vector<double> &joint_angles,
775 std::vector<geometry_msgs::Pose> &poses)
const 783 poses.resize(link_names.size());
791 geometry_msgs::PoseStamped pose;
797 jnt_pos_in(i) = joint_angles[i];
803 for(
unsigned int i=0; i < poses.size(); i++)
812 ROS_ERROR_NAMED(
"kdl",
"Could not compute FK for %s",link_names[i].c_str());
std::vector< unsigned int > redundant_joint_indices_
#define ROS_INFO_NAMED(name,...)
bool isRedundantJoint(unsigned int index) const
const Segment & getSegment(unsigned int nr) const
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
robot_state::RobotStatePtr state_2_
int getKDLSegmentIndex(const std::string &name) const
#define ROS_ERROR_STREAM_NAMED(name, args)
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
unsigned int rows() const
virtual const std::string & getBaseFrame() const
std::vector< double > values
const srdf::ModelSharedPtr & getSRDF() const
KDL::Chain kdl_tip_chain_
std::vector< double > ik_weights_
bool comparator(const idx_double &l, const idx_double &r)
unsigned int getNrOfSegments() const
KDL::Chain kdl_base_chain_
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
std::vector< std::string > ur_joint_names_
virtual 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
double max_solver_iterations_
robot_model::RobotModelPtr robot_model_
robot_model::JointModelGroup * joint_model_group_
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
bool checkConsistency(const KDL::JntArray &seed_state, const std::vector< double > &consistency_limit, const KDL::JntArray &solution) const
Check whether the solution lies within the consistency limit of the seed state.
bool lock_redundant_joints
std::vector< unsigned int > redundant_joints_map_index_
bool timedOut(const ros::WallTime &start_time, double duration) const
#define ROS_DEBUG_NAMED(name,...)
const std::string & getName() const
Specific implementation of kinematics using KDL. This version can be used with any robot...
int inverse(const double *T, double *q_sols, double q6_des=0.0)
virtual const std::string & getTipFrame() const
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
unsigned int getNrOfJoints() 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)
std::vector< kdl_kinematics_plugin::JointMimic > mimic_joints_
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
bool lookupParam(const std::string ¶m, T &val, const T &default_val) const
moveit_msgs::KinematicSolverInfo fk_chain_info_
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
std::vector< std::string > ur_link_names_
void resize(unsigned int newSize)
void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
int num_possible_redundant_joints_
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
robot_state::RobotStatePtr state_
virtual 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
std::vector< Segment > segments
#define ROS_ERROR_NAMED(name,...)
std::pair< int, double > idx_double
void reset(unsigned int index)
moveit_msgs::KinematicSolverInfo ik_chain_info_
int getJointIndex(const std::string &name) const
std::string robot_description_
virtual 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)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
const urdf::ModelInterfaceSharedPtr & getURDF() const