44 #include <urdf_model/model.h> 54 LMAKinematicsPlugin::LMAKinematicsPlugin() : active_(false)
60 std::vector<double> jnt_array_vector(
dimension_, 0.0);
68 jnt_array(i) = jnt_array_vector[i];
81 const std::vector<double>& consistency_limits,
87 near[i] = seed_state(i);
90 std::vector<double> consistency_limits_mimic;
95 consistency_limits_mimic.push_back(consistency_limits[i]);
99 consistency_limits_mimic);
113 jnt_array(i) = values[i];
118 const std::vector<double>& consistency_limits,
122 if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
128 const std::string& base_frame,
const std::string& tip_frame,
129 double search_discretization)
131 setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
135 const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.
getURDF();
137 if (!urdf_model || !srdf)
139 ROS_ERROR_NAMED(
"lma",
"URDF and SRDF must be loaded for KDL kinematics solver to work.");
143 robot_model_.reset(
new robot_model::RobotModel(urdf_model, srdf));
145 robot_model::JointModelGroup* joint_model_group =
robot_model_->getJointModelGroup(group_name);
146 if (!joint_model_group)
149 if (!joint_model_group->isChain())
151 ROS_ERROR_NAMED(
"lma",
"Group '%s' is not a chain", group_name.c_str());
154 if (!joint_model_group->isSingleDOFJoints())
156 ROS_ERROR_NAMED(
"lma",
"Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
173 dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
174 for (std::size_t i = 0; i < joint_model_group->getJointModels().size(); ++i)
179 ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
180 const std::vector<moveit_msgs::JointLimits>& jvec =
181 joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
189 if (!joint_model_group->hasLinkModel(
getTipFrame()))
191 ROS_ERROR_NAMED(
"lma",
"Could not find tip name in joint group '%s'", group_name.c_str());
195 fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
207 int max_solver_iterations;
211 lookupParam(
"max_solver_iterations", max_solver_iterations, 500);
213 lookupParam(
"position_only_ik", position_ik,
false);
223 std::vector<unsigned int> redundant_joints_map_index;
225 std::vector<JointMimic> mimic_joints;
226 unsigned int joint_counter = 0;
232 if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
235 mimic_joint.
reset(joint_counter);
237 mimic_joint.
active =
true;
238 mimic_joints.push_back(mimic_joint);
242 if (joint_model_group->hasJointModel(jm->getName()))
244 if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
247 mimic_joint.
reset(joint_counter);
249 mimic_joint.
offset = jm->getMimicOffset();
250 mimic_joint.
multiplier = jm->getMimicFactor();
251 mimic_joints.push_back(mimic_joint);
256 for (std::size_t i = 0; i < mimic_joints.size(); ++i)
258 if (!mimic_joints[i].active)
260 const robot_model::JointModel* joint_model =
261 joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
262 for (std::size_t j = 0; j < mimic_joints.size(); ++j)
264 if (mimic_joints[j].joint_name == joint_model->getName())
266 mimic_joints[i].map_index = mimic_joints[j].map_index;
315 std::vector<unsigned int> redundant_joints_map_index;
316 unsigned int counter = 0;
319 bool is_redundant_joint =
false;
320 for (std::size_t j = 0; j < redundant_joints.size(); ++j)
322 if (i == redundant_joints[j])
324 is_redundant_joint =
true;
329 if (!is_redundant_joint)
334 redundant_joints_map_index.push_back(counter);
339 for (std::size_t i = 0; i < redundant_joints_map_index.size(); ++i)
340 ROS_DEBUG_NAMED(
"lma",
"Redundant joint map index: %d %d", (
int)i, (int)redundant_joints_map_index[i]);
349 for (
unsigned int i = 0; i <
ik_chain_info_.joint_names.size(); i++)
377 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
381 std::vector<double> consistency_limits;
384 consistency_limits, options);
388 double timeout, std::vector<double>& solution,
389 moveit_msgs::MoveItErrorCodes& error_code,
393 std::vector<double> consistency_limits;
395 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
400 double timeout,
const std::vector<double>& consistency_limits,
401 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
405 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
410 double timeout, std::vector<double>& solution,
412 moveit_msgs::MoveItErrorCodes& error_code,
415 std::vector<double> consistency_limits;
416 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
421 double timeout,
const std::vector<double>& consistency_limits,
422 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
423 moveit_msgs::MoveItErrorCodes& error_code,
426 return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
431 double timeout, std::vector<double>& solution,
433 moveit_msgs::MoveItErrorCodes& error_code,
434 const std::vector<double>& consistency_limits,
441 error_code.val = error_code.NO_IK_SOLUTION;
448 << ik_seed_state.size());
449 error_code.val = error_code.NO_IK_SOLUTION;
453 if (!consistency_limits.empty() && consistency_limits.size() !=
dimension_)
456 << consistency_limits.size());
457 error_code.val = error_code.NO_IK_SOLUTION;
466 Eigen::Matrix<double, 6, 1> L;
491 ik_solver_vel.lockRedundantJoints();
500 << ik_pose.position.x <<
" " << ik_pose.position.y <<
" " << ik_pose.position.z
501 <<
" " << ik_pose.orientation.x <<
" " << ik_pose.orientation.y <<
" " 502 << ik_pose.orientation.z <<
" " << ik_pose.orientation.w);
505 jnt_seed_state(i) = ik_seed_state[i];
506 jnt_pos_in = jnt_seed_state;
508 unsigned int counter(0);
517 error_code.val = error_code.TIMED_OUT;
518 ik_solver_vel.unlockRedundantJoints();
521 int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
523 if (!consistency_limits.empty())
529 ROS_DEBUG_NAMED(
"lma",
"Could not find IK solution: does not match consistency limits");
548 solution[j] = jnt_pos_out(j);
549 if (!solution_callback.empty())
550 solution_callback(ik_pose, solution, error_code);
552 error_code.val = error_code.SUCCESS;
554 if (error_code.val == error_code.SUCCESS)
557 ik_solver_vel.unlockRedundantJoints();
561 ROS_DEBUG_NAMED(
"lma",
"An IK that satisifes the constraints and is collision free could not be found");
562 error_code.val = error_code.NO_IK_SOLUTION;
563 ik_solver_vel.unlockRedundantJoints();
568 const std::vector<double>& joint_angles,
569 std::vector<geometry_msgs::Pose>& poses)
const 576 poses.resize(link_names.size());
584 geometry_msgs::PoseStamped pose;
590 jnt_pos_in(i) = joint_angles[i];
596 for (
unsigned int i = 0; i < poses.size(); i++)
605 ROS_ERROR_NAMED(
"lma",
"Could not compute FK for %s", link_names[i].c_str());
std::vector< unsigned int > redundant_joint_indices_
A model of a mimic joint. Mimic joints are typically unactuated joints that are constrained to follow...
bool timedOut(const ros::WallTime &start_time, double duration) const
int getJointIndex(const std::string &name) const
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
#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
unsigned int getNrOfSegments() const
double max_solver_iterations_
moveit_msgs::KinematicSolverInfo fk_chain_info_
bool lock_redundant_joints
int getKDLSegmentIndex(const std::string &name) const
bool return_approximate_solution
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
#define ROS_DEBUG_NAMED(name,...)
const std::string & getName() const
virtual const std::string & getTipFrame() const
int num_possible_redundant_joints_
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
robot_state::RobotStatePtr state_
robot_state::RobotStatePtr state_2_
unsigned int getNrOfJoints() const
Specific implementation of kinematics using Levenberg-Marquardt method available at KDL...
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)
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
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
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
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 getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
std::vector< Segment > segments
std::string joint_name
Name of this joint.
double offset
Offset for this joint value from the joint that it mimics.
#define ROS_ERROR_NAMED(name,...)
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
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.
moveit_msgs::KinematicSolverInfo ik_chain_info_
robot_model::JointModelGroup * joint_model_group_
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
void reset(unsigned int index)
bool active
If true, this joint is an active DOF and not a mimic joint.
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)
std::string robot_description_
bool isRedundantJoint(unsigned int index) const
double multiplier
Multiplier for this joint value from the joint that it mimics.
robot_model::RobotModelPtr robot_model_
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
const urdf::ModelInterfaceSharedPtr & getURDF() const
std::vector< unsigned int > redundant_joints_map_index_