bool moveit::core::JointModelGroup::computeIKIndexBijection(const std::vector<std::string> &ik_jnames, std::vector<unsigned int> &joint_bijection) const
{
joint_bijection.clear();
for (std::size_t i = 0 ; i < ik_jnames.size() ; ++i)
{
VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jnames[i]);
if (it == joint_variables_index_map_.end())
{
// skip reported fixed joints
if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED)
continue;
logError("IK solver computes joint values for joint '%s' but group '%s' does not contain such a joint.", ik_jnames[i].c_str(), getName().c_str());
return false;
}
const JointModel *jm = getJointModel(ik_jnames[i]);
for (unsigned int k = 0 ; k < jm->getVariableCount() ; ++k)
joint_bijection.push_back(it->second + k);
}
return true;
}
const planning_models::KinematicModel::JointModel* planning_models::KinematicModel::JointModelGroup::getJointModel(const std::string &name)
{
if(!hasJointModel(name)) return NULL;
return joint_model_map_.find(name)->second;
}
moveit::core::JointModelGroup::JointModelGroup(const std::string& group_name,
const srdf::Model::Group &config,
const std::vector<const JointModel*> &unsorted_group_joints,
const RobotModel* parent_model)
: parent_model_(parent_model)
, name_(group_name)
, common_root_(NULL)
, variable_count_(0)
, is_contiguous_index_list_(true)
, is_chain_(false)
, is_single_dof_(true)
, config_(config)
{
// sort joints in Depth-First order
joint_model_vector_ = unsorted_group_joints;
std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
joint_model_name_vector_.reserve(joint_model_vector_.size());
// figure out active joints, mimic joints, fixed joints
// construct index maps, list of variables
for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
{
joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
joint_model_map_[joint_model_vector_[i]->getName()] = joint_model_vector_[i];
unsigned int vc = joint_model_vector_[i]->getVariableCount();
if (vc > 0)
{
if (vc > 1)
is_single_dof_ = false;
const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
if (joint_model_vector_[i]->getMimic() == NULL)
{
active_joint_model_vector_.push_back(joint_model_vector_[i]);
active_joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
active_joint_model_start_index_.push_back(variable_count_);
active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
}
else
mimic_joints_.push_back(joint_model_vector_[i]);
for (std::size_t j = 0; j < name_order.size(); ++j)
{
variable_names_.push_back(name_order[j]);
variable_names_set_.insert(name_order[j]);
}
int first_index = joint_model_vector_[i]->getFirstVariableIndex();
for (std::size_t j = 0; j < name_order.size(); ++j)
{
variable_index_list_.push_back(first_index + j);
joint_variables_index_map_[name_order[j]] = variable_count_ + j;
}
joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
continuous_joint_model_vector_.push_back(joint_model_vector_[i]);
variable_count_ += vc;
}
else
fixed_joints_.push_back(joint_model_vector_[i]);
}
// now we need to find all the set of joints within this group
// that root distinct subtrees
for (std::size_t i = 0 ; i < active_joint_model_vector_.size() ; ++i)
{
// if we find that an ancestor is also in the group, then the joint is not a root
if (!includesParent(active_joint_model_vector_[i], this))
joint_roots_.push_back(active_joint_model_vector_[i]);
}
// when updating this group within a state, it is useful to know
// if the full state of a group is contiguous within the full state of the robot
if (variable_index_list_.empty())
is_contiguous_index_list_ = false;
else
for (std::size_t i = 1 ; i < variable_index_list_.size() ; ++i)
if (variable_index_list_[i] != variable_index_list_[i - 1] + 1)
{
is_contiguous_index_list_ = false;
break;
}
// when updating/sampling a group state only, only mimic joints that have their parent within the group get updated.
for (std::size_t i = 0 ; i < mimic_joints_.size() ; ++i)
// if the joint we mimic is also in this group, we will need to do updates when sampling
if (hasJointModel(mimic_joints_[i]->getMimic()->getName()))
{
int src = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
int dest = joint_variables_index_map_[mimic_joints_[i]->getName()];
GroupMimicUpdate mu(src, dest, mimic_joints_[i]->getMimicFactor(), mimic_joints_[i]->getMimicOffset());
group_mimic_update_.push_back(mu);
}
// now we need to make another pass for group links (we include the fixed joints here)
std::set<const LinkModel*> group_links_set;
for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
group_links_set.insert(joint_model_vector_[i]->getChildLinkModel());
for (std::set<const LinkModel*>::iterator it = group_links_set.begin(); it != group_links_set.end(); ++it)
link_model_vector_.push_back(*it);
std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
for (std::size_t i = 0 ; i < link_model_vector_.size() ; ++i)
{
link_model_map_[link_model_vector_[i]->getName()] = link_model_vector_[i];
link_model_name_vector_.push_back(link_model_vector_[i]->getName());
if (!link_model_vector_[i]->getShapes().empty())
{
link_model_with_geometry_vector_.push_back(link_model_vector_[i]);
link_model_with_geometry_name_vector_.push_back(link_model_vector_[i]->getName());
}
}
// compute the common root of this group
if (!joint_roots_.empty())
{
common_root_ = joint_roots_[0];
for (std::size_t i = 1 ; i < joint_roots_.size() ; ++i)
common_root_ = parent_model->getCommonRoot(joint_roots_[i], common_root_);
}
// compute updated links
for (std::size_t i = 0 ; i < joint_roots_.size() ; ++i)
{
const std::vector<const LinkModel*> &links = joint_roots_[i]->getDescendantLinkModels();
updated_link_model_set_.insert(links.begin(), links.end());
}
for (std::set<const LinkModel*>::iterator it = updated_link_model_set_.begin(); it != updated_link_model_set_.end(); ++it)
{
updated_link_model_name_set_.insert((*it)->getName());
updated_link_model_vector_.push_back(*it);
if (!(*it)->getShapes().empty())
{
updated_link_model_with_geometry_vector_.push_back(*it);
updated_link_model_with_geometry_set_.insert(*it);
updated_link_model_with_geometry_name_set_.insert((*it)->getName());
}
}
std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
std::sort(updated_link_model_with_geometry_vector_.begin(), updated_link_model_with_geometry_vector_.end(), OrderLinksByIndex());
for (std::size_t i = 0; i < updated_link_model_vector_.size(); ++i)
updated_link_model_name_vector_.push_back(updated_link_model_vector_[i]->getName());
for (std::size_t i = 0; i < updated_link_model_with_geometry_vector_.size(); ++i)
updated_link_model_with_geometry_name_vector_.push_back(updated_link_model_with_geometry_vector_[i]->getName());
// check if this group should actually be a chain
if (joint_roots_.size() == 1 && active_joint_model_vector_.size() > 1)
{
bool chain = true;
// due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
// we should always get to the parent.
for (std::size_t k = joint_model_vector_.size() - 1 ; k > 0 ; --k)
if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
{
chain = false;
break;
}
if (chain)
is_chain_ = true;
}
}
/* ------------------------ JointModelGroup ------------------------ */
planning_models::KinematicModel::JointModelGroup::JointModelGroup(const std::string& group_name,
const std::vector<const JointModel*> &group_joints,
const std::vector<const JointModel*> &fixed_group_joints,
const KinematicModel* parent_model) :
name_(group_name)
{
ROS_DEBUG_STREAM("Group " << group_name);
joint_model_vector_ = group_joints;
fixed_joint_model_vector_ = fixed_group_joints;
joint_model_name_vector_.resize(group_joints.size());
ROS_DEBUG_STREAM("Joints:");
for (unsigned int i = 0 ; i < group_joints.size() ; ++i)
{
ROS_DEBUG_STREAM(group_joints[i]->getName());
joint_model_name_vector_[i] = group_joints[i]->getName();
joint_model_map_[group_joints[i]->getName()] = group_joints[i];
}
ROS_DEBUG_STREAM("Fixed joints:");
for (unsigned int i = 0 ; i < fixed_joint_model_vector_.size() ; ++i)
{
ROS_DEBUG_STREAM(fixed_joint_model_vector_[i]->getName());
}
std::vector<const JointModel*> all_joints = group_joints;
all_joints.insert(all_joints.end(), fixed_group_joints.begin(), fixed_group_joints.end());
//now we need to find all the set of joints within this group
//that root distinct subtrees
std::map<std::string, bool> is_root;
for (unsigned int i = 0 ; i < all_joints.size() ; ++i)
{
bool found = false;
const JointModel *joint = all_joints[i];
//if we find that an ancestor is also in the group, then the joint is not a root
while (joint->getParentLinkModel() != NULL)
{
joint = joint->getParentLinkModel()->getParentJointModel();
if (hasJointModel(joint->name_))
{
found = true;
}
}
if(!found) {
joint_roots_.push_back(all_joints[i]);
is_root[all_joints[i]->getName()] = true;
} else {
is_root[all_joints[i]->getName()] = false;
}
}
//now we need to make another pass for group links
std::set<const LinkModel*> group_links_set;
for(unsigned int i = 0; i < all_joints.size(); i++) {
const JointModel *joint = all_joints[i];
//push children in directly
group_links_set.insert(joint->getChildLinkModel());
while (joint->getParentLinkModel() != NULL)
{
if(is_root.find(joint->getName()) != is_root.end() &&
is_root[joint->getName()]) {
break;
}
group_links_set.insert(joint->getParentLinkModel());
joint = joint->getParentLinkModel()->getParentJointModel();
}
}
ROS_DEBUG("Group links:");
for(std::set<const LinkModel*>::iterator it = group_links_set.begin();
it != group_links_set.end();
it++) {
group_link_model_vector_.push_back(*it);
ROS_DEBUG_STREAM((*it)->getName());
}
//these subtrees are distinct, so we can stack their updated links on top of each other
for (unsigned int i = 0 ; i < joint_roots_.size() ; ++i)
{
std::vector<const LinkModel*> links;
parent_model->getChildLinkModels(joint_roots_[i], links);
updated_link_model_vector_.insert(updated_link_model_vector_.end(), links.begin(), links.end());
}
}
