44 #include <kdl/chainfksolverpos_recursive.hpp>
45 #include <kdl/frames_io.hpp>
46 #include <kdl/kinfam_io.hpp>
58 void KDLKinematicsPlugin::getRandomConfiguration(Eigen::VectorXd& jnt_array)
const
60 state_->setToRandomPositions(joint_model_group_);
61 state_->copyJointGroupPositions(joint_model_group_, &jnt_array[0]);
64 void KDLKinematicsPlugin::getRandomConfiguration(
const Eigen::VectorXd& seed_state,
65 const std::vector<double>& consistency_limits,
66 Eigen::VectorXd& jnt_array)
const
68 joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), &jnt_array[0],
69 &seed_state[0], consistency_limits);
72 bool KDLKinematicsPlugin::checkConsistency(
const Eigen::VectorXd& seed_state,
73 const std::vector<double>& consistency_limits,
74 const Eigen::VectorXd& solution)
const
76 for (std::size_t i = 0; i < dimension_; ++i)
77 if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
82 void KDLKinematicsPlugin::getJointWeights()
84 const std::vector<std::string>& active_names = joint_model_group_->getActiveJointModelNames();
85 std::vector<std::string> names;
86 std::vector<double> weights;
87 if (lookupParam(
"joint_weights/weights", weights, weights))
89 if (!lookupParam(
"joint_weights/names", names, names) || names.size() != weights.size())
91 ROS_ERROR_NAMED(
"kdl",
"Expecting list parameter joint_weights/names of same size as list joint_weights/weights");
96 else if (lookupParam(
"joint_weights", weights, weights))
98 std::size_t num_active = active_names.size();
99 if (weights.size() == num_active)
101 joint_weights_ = weights;
104 else if (!weights.empty())
106 ROS_ERROR_NAMED(
"kdl",
"Expecting parameter joint_weights to list weights for all active joints (%zu) in order",
114 joint_weights_ = std::vector<double>(active_names.size(), 1.0);
119 assert(names.size() == weights.size());
120 for (
size_t i = 0; i != names.size(); ++i)
122 auto it = std::find(active_names.begin(), active_names.end(), names[i]);
123 if (it == active_names.cend())
124 ROS_WARN_NAMED(
"kdl",
"Joint '%s' is not an active joint in group '%s'", names[i].c_str(),
125 joint_model_group_->getName().c_str());
126 else if (weights[i] < 0.0)
127 ROS_WARN_NAMED(
"kdl",
"Negative weight %f for joint '%s' will be ignored", weights[i], names[i].c_str());
129 joint_weights_[it - active_names.begin()] = weights[i];
132 "kdl",
"Joint weights for group '"
133 << getGroupName() <<
"': \n"
134 << Eigen::Map<const Eigen::VectorXd>(joint_weights_.data(), joint_weights_.size()).transpose());
138 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
139 double search_discretization)
141 storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
142 joint_model_group_ = robot_model_->getJointModelGroup(group_name);
143 if (!joint_model_group_)
146 if (!joint_model_group_->isChain())
148 ROS_ERROR_NAMED(
"kdl",
"Group '%s' is not a chain", group_name.c_str());
151 if (!joint_model_group_->isSingleDOFJoints())
153 ROS_ERROR_NAMED(
"kdl",
"Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
164 if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
170 dimension_ = joint_model_group_->getActiveJointModels().size() + joint_model_group_->getMimicJointModels().size();
171 for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i)
176 solver_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
177 const std::vector<moveit_msgs::JointLimits>& jvec =
178 joint_model_group_->getJointModels()[i]->getVariableBoundsMsg();
179 solver_info_.limits.insert(solver_info_.limits.end(), jvec.begin(), jvec.end());
183 if (!joint_model_group_->hasLinkModel(getTipFrame()))
185 ROS_ERROR_NAMED(
"kdl",
"Could not find tip name in joint group '%s'", group_name.c_str());
188 solver_info_.link_names.push_back(getTipFrame());
190 joint_min_.resize(solver_info_.limits.size());
191 joint_max_.resize(solver_info_.limits.size());
193 for (
unsigned int i = 0; i < solver_info_.limits.size(); i++)
195 joint_min_(i) = solver_info_.limits[i].min_position;
196 joint_max_(i) = solver_info_.limits[i].max_position;
200 lookupParam(
"max_solver_iterations", max_solver_iterations_, 500);
201 lookupParam(
"epsilon", epsilon_, 1e-5);
202 lookupParam(
"orientation_vs_position", orientation_vs_position_weight_, 1.0);
205 lookupParam(
"position_only_ik", position_ik,
false);
207 orientation_vs_position_weight_ = 0.0;
208 if (orientation_vs_position_weight_ == 0.0)
214 unsigned int joint_counter = 0;
215 for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
222 JointMimic mimic_joint;
223 mimic_joint.reset(joint_counter);
224 mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().
getName();
225 mimic_joint.active =
true;
226 mimic_joints_.push_back(mimic_joint);
230 if (joint_model_group_->hasJointModel(jm->
getName()))
234 JointMimic mimic_joint;
235 mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
238 mimic_joints_.push_back(mimic_joint);
243 for (JointMimic& mimic_joint : mimic_joints_)
245 if (!mimic_joint.active)
248 joint_model_group_->getJointModel(mimic_joint.joint_name)->
getMimic();
249 for (JointMimic& mimic_joint_recal : mimic_joints_)
251 if (mimic_joint_recal.joint_name == joint_model->
getName())
253 mimic_joint.map_index = mimic_joint_recal.map_index;
260 state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
262 fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(kdl_chain_);
274 bool KDLKinematicsPlugin::getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
275 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
278 std::vector<double> consistency_limits;
281 return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code,
285 bool KDLKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
286 double timeout, std::vector<double>& solution,
287 moveit_msgs::MoveItErrorCodes& error_code,
290 std::vector<double> consistency_limits;
292 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
296 bool KDLKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
297 double timeout,
const std::vector<double>& consistency_limits,
298 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
301 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution,
IKCallbackFn(), error_code,
305 bool KDLKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
306 double timeout, std::vector<double>& solution,
308 moveit_msgs::MoveItErrorCodes& error_code,
311 std::vector<double> consistency_limits;
312 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
316 bool KDLKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
317 double timeout,
const std::vector<double>& consistency_limits,
318 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
319 moveit_msgs::MoveItErrorCodes& error_code,
326 error_code.val = error_code.NO_IK_SOLUTION;
330 if (ik_seed_state.size() != dimension_)
333 "Seed state must have size " << dimension_ <<
" instead of size " << ik_seed_state.size());
334 error_code.val = error_code.NO_IK_SOLUTION;
339 std::vector<double> consistency_limits_mimic;
340 if (!consistency_limits.empty())
342 if (consistency_limits.size() != dimension_)
345 << dimension_ <<
" instead of size " << consistency_limits.size());
346 error_code.val = error_code.NO_IK_SOLUTION;
350 for (std::size_t i = 0; i < dimension_; ++i)
352 if (mimic_joints_[i].active)
353 consistency_limits_mimic.push_back(consistency_limits[i]);
356 Eigen::Matrix<double, 6, 1> cartesian_weights;
357 cartesian_weights.topRows<3>().setConstant(1.0);
358 cartesian_weights.bottomRows<3>().setConstant(orientation_vs_position_weight_);
360 KDL::JntArray jnt_seed_state(dimension_);
361 KDL::JntArray jnt_pos_in(dimension_);
362 KDL::JntArray jnt_pos_out(dimension_);
363 jnt_seed_state.data = Eigen::Map<const Eigen::VectorXd>(ik_seed_state.data(), ik_seed_state.size());
364 jnt_pos_in = jnt_seed_state;
367 solution.resize(dimension_);
369 KDL::Frame pose_desired;
373 << ik_pose.position.x <<
" " << ik_pose.position.y <<
" " << ik_pose.position.z
374 <<
" " << ik_pose.orientation.x <<
" " << ik_pose.orientation.y <<
" "
375 << ik_pose.orientation.z <<
" " << ik_pose.orientation.w);
377 unsigned int attempt = 0;
383 if (!consistency_limits_mimic.empty())
384 getRandomConfiguration(jnt_seed_state.data, consistency_limits_mimic, jnt_pos_in.data);
386 getRandomConfiguration(jnt_pos_in.data);
391 CartToJnt(ik_solver_vel, jnt_pos_in, pose_desired, jnt_pos_out, max_solver_iterations_,
392 Eigen::Map<const Eigen::VectorXd>(joint_weights_.data(), joint_weights_.size()), cartesian_weights);
395 if (!consistency_limits_mimic.empty() &&
396 !checkConsistency(jnt_seed_state.data, consistency_limits_mimic, jnt_pos_out.data))
399 Eigen::Map<Eigen::VectorXd>(solution.data(), solution.size()) = jnt_pos_out.data;
400 if (!solution_callback.empty())
402 solution_callback(ik_pose, solution, error_code);
403 if (error_code.val != error_code.SUCCESS)
408 error_code.val = error_code.SUCCESS;
410 <<
"s and " << attempt <<
" attempts");
413 }
while (!timedOut(start_time, timeout));
416 <<
"s and " << attempt <<
" attempts");
417 error_code.val = error_code.TIMED_OUT;
423 const KDL::Frame& p_in, KDL::JntArray& q_out,
const unsigned int max_iter,
424 const Eigen::VectorXd& joint_weights,
const Twist& cartesian_weights)
const
426 double last_delta_twist_norm = DBL_MAX;
427 double step_size = 1.0;
429 KDL::Twist delta_twist;
430 KDL::JntArray delta_q(q_out.rows()), q_backup(q_out.rows());
431 Eigen::ArrayXd extra_joint_weights(joint_weights.rows());
432 extra_joint_weights.setOnes();
438 bool success =
false;
439 for (i = 0; i < max_iter; ++i)
441 fk_solver_->JntToCart(q_out,
f);
442 delta_twist = diff(
f, p_in);
446 const double position_error = delta_twist.vel.Norm();
447 const double orientation_error = ik_solver.
isPositionOnly() ? 0 : delta_twist.rot.Norm();
448 const double delta_twist_norm = std::max(position_error, orientation_error);
449 if (delta_twist_norm <= epsilon_)
455 if (delta_twist_norm >= last_delta_twist_norm)
458 double old_step_size = step_size;
459 step_size *= std::min(0.2, last_delta_twist_norm / delta_twist_norm);
460 KDL::Multiply(delta_q, step_size / old_step_size, delta_q);
461 ROS_DEBUG_NAMED(
"kdl",
" error increased: %f -> %f, scale: %f", last_delta_twist_norm, delta_twist_norm,
469 last_delta_twist_norm = delta_twist_norm;
471 ik_solver.
CartToJnt(q_out, delta_twist, delta_q, extra_joint_weights * joint_weights.array(), cartesian_weights);
474 clipToJointLimits(q_out, delta_q, extra_joint_weights);
476 const double delta_q_norm = delta_q.data.lpNorm<1>();
477 ROS_DEBUG_NAMED(
"kdl",
"[%3d] pos err: %f rot err: %f delta_q: %f", i, position_error, orientation_error,
479 if (delta_q_norm < epsilon_)
481 if (step_size < epsilon_)
484 last_delta_twist_norm = DBL_MAX;
485 delta_q.data.setRandom();
486 delta_q.data *= std::min(0.1, delta_twist_norm);
487 clipToJointLimits(q_out, delta_q, extra_joint_weights);
488 extra_joint_weights.setOnes();
491 KDL::Add(q_out, delta_q, q_out);
497 int result = (i == max_iter) ? -3 : (success ? 0 : -2);
503 void KDLKinematicsPlugin::clipToJointLimits(
const KDL::JntArray& q, KDL::JntArray& q_delta,
504 Eigen::ArrayXd& weighting)
const
507 for (std::size_t i = 0; i < q.rows(); ++i)
509 const double delta_max = joint_max_(i) - q(i);
510 const double delta_min = joint_min_(i) - q(i);
511 if (q_delta(i) > delta_max)
512 q_delta(i) = delta_max;
513 else if (q_delta(i) < delta_min)
514 q_delta(i) = delta_min;
518 weighting[mimic_joints_[i].map_index] = 0.01;
522 bool KDLKinematicsPlugin::getPositionFK(
const std::vector<std::string>& link_names,
523 const std::vector<double>& joint_angles,
524 std::vector<geometry_msgs::Pose>& poses)
const
531 poses.resize(link_names.size());
532 if (joint_angles.size() != dimension_)
534 ROS_ERROR_NAMED(
"kdl",
"Joint angles vector must have size: %d", dimension_);
539 KDL::JntArray jnt_pos_in(dimension_);
540 jnt_pos_in.data = Eigen::Map<const Eigen::VectorXd>(joint_angles.data(), joint_angles.size());
543 for (
unsigned int i = 0; i < poses.size(); i++)
545 if (fk_solver_->JntToCart(jnt_pos_in, p_out) >= 0)
551 ROS_ERROR_NAMED(
"kdl",
"Could not compute FK for %s", link_names[i].c_str());
558 const std::vector<std::string>& KDLKinematicsPlugin::getJointNames()
const
560 return solver_info_.joint_names;
563 const std::vector<std::string>& KDLKinematicsPlugin::getLinkNames()
const
565 return solver_info_.link_names;