45 #include <urdf_model/model.h> 55 KDLKinematicsPlugin::KDLKinematicsPlugin() : active_(false)
61 std::vector<double> jnt_array_vector(
dimension_, 0.0);
69 jnt_array(i) = jnt_array_vector[i];
82 const std::vector<double>& consistency_limits,
88 near[i] = seed_state(i);
91 std::vector<double> consistency_limits_mimic;
96 consistency_limits_mimic.push_back(consistency_limits[i]);
100 consistency_limits_mimic);
114 jnt_array(i) = values[i];
119 const std::vector<double>& consistency_limits,
123 if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
129 const std::string& base_frame,
const std::string& tip_frame,
130 double search_discretization)
132 setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
136 const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.
getURDF();
138 if (!urdf_model || !srdf)
140 ROS_ERROR_NAMED(
"kdl",
"URDF and SRDF must be loaded for KDL kinematics solver to work.");
144 robot_model_.reset(
new robot_model::RobotModel(urdf_model, srdf));
146 robot_model::JointModelGroup* joint_model_group =
robot_model_->getJointModelGroup(group_name);
147 if (!joint_model_group)
150 if (!joint_model_group->isChain())
152 ROS_ERROR_NAMED(
"kdl",
"Group '%s' is not a chain", group_name.c_str());
155 if (!joint_model_group->isSingleDOFJoints())
157 ROS_ERROR_NAMED(
"kdl",
"Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
174 dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
175 for (std::size_t i = 0; i < joint_model_group->getJointModels().size(); ++i)
180 ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
181 const std::vector<moveit_msgs::JointLimits>& jvec =
182 joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
190 if (!joint_model_group->hasLinkModel(
getTipFrame()))
192 ROS_ERROR_NAMED(
"kdl",
"Could not find tip name in joint group '%s'", group_name.c_str());
196 fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
208 int max_solver_iterations;
212 lookupParam(
"max_solver_iterations", max_solver_iterations, 500);
214 lookupParam(
"position_only_ik", position_ik,
false);
224 std::vector<unsigned int> redundant_joints_map_index;
226 std::vector<JointMimic> mimic_joints;
227 unsigned int joint_counter = 0;
233 if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
236 mimic_joint.
reset(joint_counter);
238 mimic_joint.
active =
true;
239 mimic_joints.push_back(mimic_joint);
243 if (joint_model_group->hasJointModel(jm->getName()))
245 if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
248 mimic_joint.
reset(joint_counter);
250 mimic_joint.
offset = jm->getMimicOffset();
251 mimic_joint.
multiplier = jm->getMimicFactor();
252 mimic_joints.push_back(mimic_joint);
257 for (std::size_t i = 0; i < mimic_joints.size(); ++i)
259 if (!mimic_joints[i].active)
261 const robot_model::JointModel* joint_model =
262 joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
263 for (std::size_t j = 0; j < mimic_joints.size(); ++j)
265 if (mimic_joints[j].joint_name == joint_model->getName())
267 mimic_joints[i].map_index = mimic_joints[j].map_index;
316 std::vector<unsigned int> redundant_joints_map_index;
317 unsigned int counter = 0;
320 bool is_redundant_joint =
false;
321 for (std::size_t j = 0; j < redundant_joints.size(); ++j)
323 if (i == redundant_joints[j])
325 is_redundant_joint =
true;
330 if (!is_redundant_joint)
335 redundant_joints_map_index.push_back(counter);
340 for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
341 ROS_DEBUG_NAMED(
"kdl",
"Redundant joint map index: %d %d", (
int)i, (int)redundant_joints_map_index[i]);
350 for (
unsigned int i = 0; i <
ik_chain_info_.joint_names.size(); i++)
378 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
382 std::vector<double> consistency_limits;
385 consistency_limits, options);
389 double timeout, std::vector<double>& solution,
390 moveit_msgs::MoveItErrorCodes& error_code,
394 std::vector<double> consistency_limits;
396 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
401 double timeout,
const std::vector<double>& consistency_limits,
402 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
406 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
411 double timeout, std::vector<double>& solution,
413 moveit_msgs::MoveItErrorCodes& error_code,
416 std::vector<double> consistency_limits;
417 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
422 double timeout,
const std::vector<double>& consistency_limits,
423 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
424 moveit_msgs::MoveItErrorCodes& error_code,
427 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
432 double timeout, std::vector<double>& solution,
434 moveit_msgs::MoveItErrorCodes& error_code,
435 const std::vector<double>& consistency_limits,
442 error_code.val = error_code.NO_IK_SOLUTION;
449 << ik_seed_state.size());
450 error_code.val = error_code.NO_IK_SOLUTION;
454 if (!consistency_limits.empty() && consistency_limits.size() !=
dimension_)
457 << consistency_limits.size());
458 error_code.val = error_code.NO_IK_SOLUTION;
482 ik_solver_vel.lockRedundantJoints();
491 << ik_pose.position.x <<
" " << ik_pose.position.y <<
" " << ik_pose.position.z
492 <<
" " << ik_pose.orientation.x <<
" " << ik_pose.orientation.y <<
" " 493 << ik_pose.orientation.z <<
" " << ik_pose.orientation.w);
496 jnt_seed_state(i) = ik_seed_state[i];
497 jnt_pos_in = jnt_seed_state;
499 unsigned int counter(0);
508 error_code.val = error_code.TIMED_OUT;
509 ik_solver_vel.unlockRedundantJoints();
512 int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
514 if (!consistency_limits.empty())
520 ROS_DEBUG_NAMED(
"kdl",
"Could not find IK solution: does not match consistency limits");
539 solution[j] = jnt_pos_out(j);
540 if (!solution_callback.empty())
541 solution_callback(ik_pose, solution, error_code);
543 error_code.val = error_code.SUCCESS;
545 if (error_code.val == error_code.SUCCESS)
548 ik_solver_vel.unlockRedundantJoints();
552 ROS_DEBUG_NAMED(
"kdl",
"An IK that satisifes the constraints and is collision free could not be found");
553 error_code.val = error_code.NO_IK_SOLUTION;
554 ik_solver_vel.unlockRedundantJoints();
559 const std::vector<double>& joint_angles,
560 std::vector<geometry_msgs::Pose>& poses)
const 567 poses.resize(link_names.size());
575 geometry_msgs::PoseStamped pose;
581 jnt_pos_in(i) = joint_angles[i];
587 for (
unsigned int i = 0; i < poses.size(); i++)
596 ROS_ERROR_NAMED(
"kdl",
"Could not compute FK for %s", link_names[i].c_str());
robot_state::RobotStatePtr state_
std::vector< unsigned int > redundant_joint_indices_
#define ROS_INFO_NAMED(name,...)
const Segment & getSegment(unsigned int nr) const
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
std::vector< double > values
const srdf::ModelSharedPtr & getSRDF() const
int getKDLSegmentIndex(const std::string &name) const
double max_solver_iterations_
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)
robot_model::JointModelGroup * joint_model_group_
unsigned int getNrOfSegments() const
double multiplier
Multiplier for this joint value from the joint that it mimics.
robot_state::RobotStatePtr state_2_
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
bool timedOut(const ros::WallTime &start_time, double duration) const
int getJointIndex(const std::string &name) const
bool lock_redundant_joints
bool return_approximate_solution
robot_model::RobotModelPtr robot_model_
#define ROS_DEBUG_NAMED(name,...)
const std::string & getName() const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
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.
Specific implementation of kinematics using KDL. This version can be used with any robot...
virtual const std::string & getTipFrame() const
bool active
If true, this joint is an active DOF and not a mimic joint.
moveit_msgs::KinematicSolverInfo fk_chain_info_
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
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)
bool lookupParam(const std::string ¶m, T &val, const T &default_val) const
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
std::string joint_name
Name of this joint.
A model of a mimic joint. Mimic joints are typically unactuated joints that are constrained to follow...
void resize(unsigned int newSize)
std::vector< JointMimic > mimic_joints_
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
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
std::vector< Segment > segments
#define ROS_ERROR_NAMED(name,...)
double offset
Offset for this joint value from the joint that it mimics.
void reset(unsigned int index)
moveit_msgs::KinematicSolverInfo ik_chain_info_
bool isRedundantJoint(unsigned int index) const
std::vector< unsigned int > redundant_joints_map_index_
std::string robot_description_
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
const urdf::ModelInterfaceSharedPtr & getURDF() const
int num_possible_redundant_joints_
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