42 #include <rclcpp/logger.hpp>
43 #include <rclcpp/logging.hpp>
61 bool includesParent(
const JointModel* joint,
const JointModelGroup* group)
65 while (joint->getParentLinkModel() !=
nullptr)
67 joint = joint->getParentLinkModel()->getParentJointModel();
68 if (group->hasJointModel(joint->getName()) && joint->getVariableCount() > 0 && joint->getMimic() ==
nullptr)
73 else if (joint->getMimic() !=
nullptr)
75 const JointModel* mjoint = joint->getMimic();
76 if (group->hasJointModel(mjoint->getName()) && mjoint->getVariableCount() > 0 && mjoint->getMimic() ==
nullptr)
80 else if (includesParent(mjoint, group))
92 bool jointPrecedes(
const JointModel* a,
const JointModel* b)
94 if (!a->getParentLinkModel())
96 const JointModel* p = a->getParentLinkModel()->getParentJointModel();
103 p = p->getParentLinkModel() ? p->getParentLinkModel()->getParentJointModel() :
nullptr;
116 const std::vector<const JointModel*>& unsorted_group_joints,
118 : parent_model_(parent_model)
120 , common_root_(nullptr)
122 , active_variable_count_(0)
123 , is_contiguous_index_list_(true)
125 , is_single_dof_(true)
139 unsigned int vc = joint_model->getVariableCount();
144 const std::vector<std::string>& name_order = joint_model->getVariableNames();
146 if (joint_model->getMimic() ==
nullptr)
156 for (
const std::string&
name : name_order)
162 int first_index = joint_model->getFirstVariableIndex();
163 for (std::size_t j = 0; j < name_order.size(); ++j)
185 if (!includesParent(active_joint_model,
this))
221 std::set<const LinkModel*> group_links_set;
223 group_links_set.insert(joint_model->getChildLinkModel());
224 for (
const LinkModel* group_link : group_links_set)
234 !link_model->getParentLinkModel()->getShapes().empty())
240 if (!link_model->getShapes().empty())
265 if (!updated_link_model->getShapes().empty())
274 OrderLinksByIndex());
331 RCLCPP_ERROR(
getLogger(),
"Link '%s' not found in group '%s'",
name.c_str(),
name_.c_str());
342 RCLCPP_ERROR(
getLogger(),
"Joint '%s' not found in group '%s'",
name.c_str(),
name_.c_str());
355 *active_joint_bounds[i]);
369 *active_joint_bounds[i],
378 const std::map<JointModel::JointType, double>& distance_map)
const
384 std::map<JointModel::JointType, double>::const_iterator iter =
386 if (iter != distance_map.end())
395 *active_joint_bounds[i],
404 const std::vector<double>& distances)
const
409 throw Exception(
"When sampling random values nearby for group '" +
name_ +
411 ", but it is of size " + std::to_string(distances.size()));
416 *active_joint_bounds[i],
430 *active_joint_bounds[i], margin))
443 *active_joint_bounds[i]))
453 double max_distance = 0.0;
492 values[mimic_update.dest] = values[mimic_update.src] * mimic_update.factor + mimic_update.offset;
503 std::map<std::string, std::map<std::string, double> >::const_iterator it =
default_states_.find(
name);
544 std::vector<const LinkModel*> tip_links;
550 for (
const LinkModel* link_model : tip_links)
551 tips.push_back(link_model->getName());
563 RCLCPP_ERROR(
getLogger(),
"Unable to find joint model group for eef");
571 RCLCPP_ERROR(
getLogger(),
"Unable to find end effector link for eef");
575 const auto insert_it = std::lower_bound(tips.cbegin(), tips.cend(), eef_link);
576 if (insert_it == tips.end() || eef_link != *insert_it)
577 tips.insert(insert_it, eef_link);
584 std::vector<const LinkModel*> tips;
586 if (tips.size() == 1)
590 else if (tips.size() > 1)
592 RCLCPP_ERROR(
getLogger(),
"More than one end effector tip found for joint model group, so cannot return only one");
596 RCLCPP_ERROR(
getLogger(),
"No end effector tips found in joint model group");
606 RCLCPP_ERROR(
getLogger(),
"Variable '%s' is not part of group '%s'", variable.c_str(),
name_.c_str());
617 for (std::pair<const JointModelGroup* const, KinematicsSolver>& it :
group_kinematics_.second)
618 it.second.default_ik_timeout_ = ik_timeout;
622 std::vector<size_t>& joint_bijection)
const
624 joint_bijection.clear();
625 for (
const std::string& joint_name : joint_names)
634 "Looking for variables for joint '%s', "
635 "but group '%s' does not contain such a joint.",
636 joint_name.c_str(),
getName().c_str());
641 joint_bijection.push_back(it->second + k);
663 for (
const std::pair<const JointModelGroup* const, SolverAllocatorFn>& it : solvers.second)
665 if (it.first->getSolverInstance())
684 if (!solver || tip.empty())
687 const std::vector<std::string>& tip_frames = solver->getTipFrames();
689 if (tip_frames.empty())
691 RCLCPP_WARN(
getLogger(),
"Group %s has no tip frame(s)",
name_.c_str());
696 for (
const std::string& tip_frame : tip_frames)
699 const std::string& tip_local = tip[0] ==
'/' ? tip.substr(1) : tip;
700 const std::string& tip_frame_local = tip_frame[0] ==
'/' ? tip_frame.substr(1) : tip_frame;
701 RCLCPP_DEBUG(
getLogger(),
"comparing input tip: %s to this groups tip: %s ", tip_local.c_str(),
702 tip_frame_local.c_str());
705 if (tip_local != tip_frame_local)
713 for (
const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
715 if (fixed_link.first->getName() == tip_local)
731 out <<
" * Joints:\n";
733 out <<
" '" << joint_model->getName() <<
"' (" << joint_model->getTypeName() <<
")\n";
734 out <<
" * Variables:\n";
739 out <<
" '" << variable_name <<
"', index "
741 << local_idx <<
" in group state";
747 out <<
" * Variables Index List:\n ";
749 out << variable_index <<
' ';
752 out <<
"(contiguous)";
756 out <<
"(non-contiguous)";
761 out <<
" * Kinematics solver bijection:\n";
769 out <<
" * Compound kinematics solver:\n";
770 for (
const std::pair<const JointModelGroup* const, KinematicsSolver>& it :
group_kinematics_.second)
772 out <<
" " << it.first->getName() <<
':';
773 for (
unsigned int index : it.second.bijection_)
781 out <<
" * Local Mimic Updates:\n";
784 out <<
" [" << mimic_update.dest <<
"] = " << mimic_update.factor <<
" * [" << mimic_update.src <<
"] + "
785 << mimic_update.offset <<
'\n';
792 const std::vector<double>& to_joint_pose,
double dt)
const
795 if (from_joint_pose.size() != to_joint_pose.size())
797 RCLCPP_ERROR(
getLogger(),
"To and from joint poses are of different sizes.");
801 return isValidVelocityMove(&from_joint_pose[0], &to_joint_pose[0], from_joint_pose.size(), dt);
805 std::size_t array_size,
double dt)
const
810 for (std::size_t i = 0; i < array_size; ++i)
812 double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]);
813 const std::vector<moveit::core::VariableBounds>* var_bounds = bounds[bij[i]];
815 if (var_bounds->size() != 1)
818 RCLCPP_ERROR(
getLogger(),
"Attempting to check velocity bounds for waypoint move with joints that have multiple "
822 const double max_velocity = (*var_bounds)[0].max_velocity_;
824 double max_dtheta = dt * max_velocity;
825 if (dtheta > max_dtheta)
827 RCLCPP_DEBUG(
getLogger(),
"Not valid velocity move because of joint %lu", i);
This may be thrown if unrecoverable errors occur.
std::set< const LinkModel * > updated_link_model_with_geometry_set_
The list of downstream link models in the order they should be updated (may include links that are no...
std::vector< int > variable_index_list_
The list of index values this group includes, with respect to a full robot state; this includes mimic...
std::string name_
Name of group.
std::vector< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
bool is_contiguous_index_list_
True if the state of this group is contiguous within the full robot state; this also means that the i...
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this group contains.
bool getEndEffectorTips(std::vector< const LinkModel * > &tips) const
Get the unique set of end effector tips included in a particular joint model group as defined by the ...
std::vector< const JointModel * > active_joint_model_vector_
Active joint instances in the order they appear in the group state.
std::vector< const JointModel * > joint_roots_
The list of active joint models that are roots in this group.
void setDefaultIKTimeout(double ik_timeout)
Set the default IK timeout.
std::vector< const LinkModel * > link_model_vector_
The links that are on the direct lineage between joints and joint_roots_, as well as the children of ...
std::vector< const LinkModel * > updated_link_model_with_geometry_vector_
The list of downstream link models in the order they should be updated (may include links that are no...
const std::string & getName() const
Get the name of the joint group.
std::map< std::string, std::map< std::string, double > > default_states_
The set of default states specified for this group in the SRDF.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
std::vector< std::string > updated_link_model_name_vector_
The list of downstream link names in the order they should be updated (may include links that are not...
double distance(const double *state1, const double *state2) const
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *near, const double distance) const
Compute random values for the state of the joint group.
void attachEndEffector(const std::string &eef_name)
Notify this group that there is an end-effector attached to it.
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
std::vector< std::string > variable_names_
The names of the DOF that make up this group (this is just a sequence of joint variable names; not ne...
bool computeJointVariableIndices(const std::vector< std::string > &joint_names, std::vector< size_t > &joint_bijection) const
Computes the indices of joint variables given a vector of joint names to look up.
std::vector< std::string > default_states_names_
The names of the default states specified for this group in the SRDF.
const JointModel * common_root_
The joint that is a common root for all joints in this group (not necessarily part of this group)
void setEndEffectorName(const std::string &name)
Set the name of the end-effector, and remember this group is indeed an end-effector.
LinkModelMapConst link_model_map_
A map from link names to their instances.
std::vector< std::string > attached_end_effector_names_
If an end-effector is attached to this group, the name of that end-effector is stored in this variabl...
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel * > &joint_vector, const RobotModel *parent_model)
const std::pair< std::string, std::string > & getEndEffectorParentGroup() const
Get the name of the group this end-effector attaches to (first) and the name of the link in that grou...
JointModelMapConst joint_model_map_
A map from joint names to their instances. This includes all joints in the group.
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
std::vector< const LinkModel * > updated_link_model_vector_
The list of downstream link models in the order they should be updated (may include links that are no...
std::set< std::string > variable_names_set_
The names of the DOF that make up this group (this is just a sequence of joint variable names; not ne...
bool enforcePositionBounds(double *state) const
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
std::vector< int > active_joint_model_start_index_
For each active joint model in this group, hold the index at which the corresponding joint state star...
std::set< std::string > updated_link_model_with_geometry_name_set_
The list of downstream link names in the order they should be updated (may include links that are not...
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Throw an exception if the link is not part of this group.
void interpolate(const double *from, const double *to, double t, double *state) const
const moveit::core::LinkModel * getOnlyOneEndEffectorTip() const
Get one end effector tip, throwing an error if there ends up being more in the joint model group This...
std::vector< std::string > subgroup_names_
The set of labelled subgroups that are included in this group.
std::set< const LinkModel * > updated_link_model_set_
The list of downstream link models in the order they should be updated (may include links that are no...
bool isValidVelocityMove(const std::vector< double > &from_joint_pose, const std::vector< double > &to_joint_pose, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits.
unsigned int active_variable_count_
The number of variables necessary to describe the active joints in this group of joints.
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints....
std::vector< GroupMimicUpdate > group_mimic_update_
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
unsigned int variable_count_
The number of variables necessary to describe this group of joints.
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
void addDefaultState(const std::string &name, const std::map< std::string, double > &default_state)
std::pair< KinematicsSolver, KinematicsSolverMap > group_kinematics_
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
bool canSetStateFromIK(const std::string &tip) const
const std::vector< size_t > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
std::vector< const LinkModel * > link_model_with_geometry_vector_
std::set< std::string > updated_link_model_name_set_
The list of downstream link names in the order they should be updated (may include links that are not...
std::vector< const JointModel * > mimic_joints_
Joints that mimic other joints.
std::vector< std::string > link_model_name_vector_
The names of the links in this group.
double getMaximumExtent() const
VariableIndexMap joint_variables_index_map_
The group includes all the joint variables that make up the joints the group consists of....
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure.
std::vector< std::string > updated_link_model_with_geometry_name_vector_
The list of downstream link names in the order they should be updated (may include links that are not...
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
std::pair< std::string, std::string > end_effector_parent_
First: name of the group that is parent to this end-effector group; Second: the link this in the pare...
std::vector< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
const RobotModel * parent_model_
Owner model.
std::vector< std::string > joint_model_name_vector_
Names of joints in the order they appear in the group state.
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
void setEndEffectorParent(const std::string &group, const std::string &link)
If this group is an end-effector, specify the parent group (e.g., the arm holding the eef) and the li...
std::set< std::string > subgroup_names_set_
The set of labelled subgroups that are included in this group.
std::vector< std::string > link_model_with_geometry_name_vector_
The names of the links in this group that also have geometry.
std::vector< const JointModel * > fixed_joints_
The joints that have no DOF (fixed)
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
std::string end_effector_name_
The name of the end effector, if this group is an end-effector.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
size_t getLocalVariableIndex(const std::string &variable) const
Get the index of the variable within this joint.
size_t getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
const std::string & getName() const
Get the name of the joint.
const std::vector< const LinkModel * > & getDescendantLinkModels() const
Get all the link models that descend from this joint, in the kinematic tree.
const JointModel * getMimic() const
Get the joint this one is mimicking.
A link from the robot. Contains the constant transform applied to the link and its geometry.
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms. The returned transforms are ...
bool isContinuous() const
Check if this joint wraps around.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a specific variable. Throw an exception of variable is not found.
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
const JointModel * getJointOfVariable(int variable_index) const
const LinkModel * getLinkModel(const std::string &link, bool *has_link=nullptr) const
Get a link by its name. Output error and return nullptr when the link is missing.
const JointModel * getCommonRoot(const JointModel *a, const JointModel *b) const
Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument...
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
std::vector< const JointModel::Bounds * > JointBoundsVector
Main namespace for MoveIt.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
double distance(const urdf::Pose &transform)
FilterFn chain(const std::vector< FilterFn > &filter_functions)
std::vector< size_t > bijection_
The mapping between the order of the joints in the group and the order of the joints in the kinematic...
double default_ik_timeout_
SolverAllocatorFn allocator_
Function type that allocates a kinematics solver for a particular group.
kinematics::KinematicsBasePtr solver_instance_