38 #include <kdl/chainfksolverpos_recursive.hpp>
39 #include <kdl/chainiksolverpos_lma.hpp>
43 #include <kdl/frames_io.hpp>
44 #include <kdl/kinfam_io.hpp>
56 void LMAKinematicsPlugin::getRandomConfiguration(Eigen::VectorXd& jnt_array)
const
58 state_->setToRandomPositions(joint_model_group_);
59 state_->copyJointGroupPositions(joint_model_group_, &jnt_array[0]);
62 void LMAKinematicsPlugin::getRandomConfiguration(
const Eigen::VectorXd& seed_state,
63 const std::vector<double>& consistency_limits,
64 Eigen::VectorXd& jnt_array)
const
66 joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), &jnt_array[0],
67 &seed_state[0], consistency_limits);
70 bool LMAKinematicsPlugin::checkConsistency(
const Eigen::VectorXd& seed_state,
71 const std::vector<double>& consistency_limits,
72 const Eigen::VectorXd& solution)
const
74 for (std::size_t i = 0; i < dimension_; ++i)
75 if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
81 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
82 double search_discretization)
84 storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
85 joint_model_group_ = robot_model_->getJointModelGroup(group_name);
86 if (!joint_model_group_)
89 if (!joint_model_group_->isChain())
91 ROS_ERROR_NAMED(
"lma",
"Group '%s' is not a chain", group_name.c_str());
94 if (!joint_model_group_->isSingleDOFJoints())
96 ROS_ERROR_NAMED(
"lma",
"Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
107 if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
117 joints_.push_back(jm);
118 joint_names_.push_back(jm->getName());
121 dimension_ = joints_.size();
124 lookupParam(
"max_solver_iterations", max_solver_iterations_, 500);
125 lookupParam(
"epsilon", epsilon_, 1e-5);
126 lookupParam(
"orientation_vs_position", orientation_vs_position_weight_, 0.01);
129 lookupParam(
"position_only_ik", position_ik,
false);
131 orientation_vs_position_weight_ = 0.0;
132 if (orientation_vs_position_weight_ == 0.0)
136 state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
138 fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(kdl_chain_);
150 bool LMAKinematicsPlugin::getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
151 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
154 std::vector<double> consistency_limits;
157 return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code,
161 bool LMAKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
162 double timeout, std::vector<double>& solution,
163 moveit_msgs::MoveItErrorCodes& error_code,
166 std::vector<double> consistency_limits;
168 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
172 bool LMAKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
173 double timeout,
const std::vector<double>& consistency_limits,
174 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
177 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution,
IKCallbackFn(), error_code,
181 bool LMAKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
182 double timeout, std::vector<double>& solution,
184 moveit_msgs::MoveItErrorCodes& error_code,
187 std::vector<double> consistency_limits;
188 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
192 void LMAKinematicsPlugin::harmonize(Eigen::VectorXd& values)
const
195 for (
auto* jm : joints_)
196 jm->harmonizePosition(&
values[i++]);
199 bool LMAKinematicsPlugin::obeysLimits(
const Eigen::VectorXd& values)
const
202 for (
const auto& jm : joints_)
203 if (!jm->satisfiesPositionBounds(&values[i++]))
208 bool LMAKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
209 double timeout,
const std::vector<double>& consistency_limits,
210 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
211 moveit_msgs::MoveItErrorCodes& error_code,
218 error_code.val = error_code.NO_IK_SOLUTION;
222 if (ik_seed_state.size() != dimension_)
225 "Seed state must have size " << dimension_ <<
" instead of size " << ik_seed_state.size());
226 error_code.val = error_code.NO_IK_SOLUTION;
230 if (!consistency_limits.empty() && consistency_limits.size() != dimension_)
232 ROS_ERROR_STREAM_NAMED(
"lma",
"Consistency limits be empty or must have size " << dimension_ <<
" instead of size "
233 << consistency_limits.size());
234 error_code.val = error_code.NO_IK_SOLUTION;
238 Eigen::Matrix<double, 6, 1> cartesian_weights;
239 cartesian_weights(0) = 1;
240 cartesian_weights(1) = 1;
241 cartesian_weights(2) = 1;
242 cartesian_weights(3) = orientation_vs_position_weight_;
243 cartesian_weights(4) = orientation_vs_position_weight_;
244 cartesian_weights(5) = orientation_vs_position_weight_;
246 KDL::JntArray jnt_seed_state(dimension_);
247 KDL::JntArray jnt_pos_in(dimension_);
248 KDL::JntArray jnt_pos_out(dimension_);
249 jnt_seed_state.data = Eigen::Map<const Eigen::VectorXd>(ik_seed_state.data(), ik_seed_state.size());
250 jnt_pos_in = jnt_seed_state;
252 KDL::ChainIkSolverPos_LMA ik_solver_pos(kdl_chain_, cartesian_weights, epsilon_, max_solver_iterations_);
253 solution.resize(dimension_);
255 KDL::Frame pose_desired;
259 << ik_pose.position.x <<
" " << ik_pose.position.y <<
" " << ik_pose.position.z
260 <<
" " << ik_pose.orientation.x <<
" " << ik_pose.orientation.y <<
" "
261 << ik_pose.orientation.z <<
" " << ik_pose.orientation.w);
262 unsigned int attempt = 0;
268 if (!consistency_limits.empty())
269 getRandomConfiguration(jnt_seed_state.data, consistency_limits, jnt_pos_in.data);
271 getRandomConfiguration(jnt_pos_in.data);
275 int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
278 harmonize(jnt_pos_out.data);
279 if (!consistency_limits.empty() && !checkConsistency(jnt_seed_state.data, consistency_limits, jnt_pos_out.data))
281 if (!obeysLimits(jnt_pos_out.data))
284 Eigen::Map<Eigen::VectorXd>(solution.data(), solution.size()) = jnt_pos_out.data;
285 if (!solution_callback.empty())
287 solution_callback(ik_pose, solution, error_code);
288 if (error_code.val != error_code.SUCCESS)
293 error_code.val = error_code.SUCCESS;
295 <<
"s and " << attempt <<
" attempts");
298 }
while (!timedOut(start_time, timeout));
301 <<
"s and " << attempt <<
" attempts");
302 error_code.val = error_code.TIMED_OUT;
306 bool LMAKinematicsPlugin::getPositionFK(
const std::vector<std::string>& link_names,
307 const std::vector<double>& joint_angles,
308 std::vector<geometry_msgs::Pose>& poses)
const
315 poses.resize(link_names.size());
316 if (joint_angles.size() != dimension_)
318 ROS_ERROR_NAMED(
"lma",
"Joint angles vector must have size: %d", dimension_);
323 KDL::JntArray jnt_pos_in(dimension_);
324 jnt_pos_in.data = Eigen::Map<const Eigen::VectorXd>(joint_angles.data(), joint_angles.size());
327 for (
unsigned int i = 0; i < poses.size(); i++)
329 if (fk_solver_->JntToCart(jnt_pos_in, p_out) >= 0)
335 ROS_ERROR_NAMED(
"lma",
"Could not compute FK for %s", link_names[i].c_str());
342 const std::vector<std::string>& LMAKinematicsPlugin::getJointNames()
const
347 const std::vector<std::string>& LMAKinematicsPlugin::getLinkNames()
const
349 return getTipFrames();