bool moveit::core::JointModelGroup::canSetStateFromIK(const std::string &tip) const
{
const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
if (!solver || tip.empty())
return false;
const std::vector<std::string> &tip_frames = solver->getTipFrames();
if (tip_frames.empty())
{
logDebug("Group %s has no tip frame(s)", name_.c_str());
return false;
}
// loop through all tip frames supported by the JMG
for (std::size_t i = 0; i < tip_frames.size(); ++i)
{
// remove frame reference, if specified
const std::string &tip_local = tip[0] == '/' ? tip.substr(1) : tip;
const std::string &tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i];
logDebug("joint_model_group.canSetStateFromIK: comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), tip_frame_local.c_str());
// Check if the IK solver's tip is the same as the frame of inquiry
if (tip_local != tip_frame_local)
{
// If not the same, check if this planning group includes the frame of inquiry
if (hasLinkModel(tip_frame_local))
{
const LinkModel *lm = getLinkModel(tip_frame_local);
const LinkTransformMap &fixed_links = lm->getAssociatedFixedTransforms();
// Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
for (LinkTransformMap::const_iterator it = fixed_links.begin() ; it != fixed_links.end() ; ++it)
{
if (it->first->getName() == tip_local)
return true;
}
}
}
else
return true;
}
// Did not find any valid tip frame links to use
return false;
}
bool planning_models::KinematicModel::addModelGroup(const planning_models::KinematicModel::GroupConfig& gc)
{
if(joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end()) {
ROS_WARN_STREAM("Already have a model group named " << gc.name_ <<". Not adding.");
return false;
}
std::vector<const JointModel*> jointv;
std::vector<const JointModel*> fixed_jointv;
if(!gc.tip_link_.empty() && !gc.base_link_.empty()) {
if(!gc.subgroups_.empty()) {
ROS_WARN_STREAM("Ignoring subgroups as tip and base are defined for group " << gc.name_);
}
//if this is not a physical robot link but is the world link
bool base_link_is_world_link = (gc.base_link_ == getRoot()->getParentFrameId() &&
getLinkModel(gc.base_link_) == NULL);
const LinkModel* base_link = NULL;
if(!base_link_is_world_link) {
base_link = getLinkModel(gc.base_link_);
if(base_link == NULL) {
ROS_WARN_STREAM("Group config " << gc.name_ << " has invalid base link " << gc.base_link_);
return false;
}
}
const LinkModel* tip_link = getLinkModel(gc.tip_link_);
if(tip_link == NULL) {
ROS_WARN_STREAM("Group config " << gc.name_ << " has invalid tip link " << gc.tip_link_);
return false;
}
const LinkModel* lm = tip_link;
bool ok = false;
while(true) {
if(lm == NULL) {
if(base_link_is_world_link) {
ok = true;
}
break;
}
if(lm == base_link) {
ok = true;
break;
}
if(lm->getParentJointModel() == NULL) {
break;
}
//only adding non-fixed joint models
const FixedJointModel* fjm = dynamic_cast<const FixedJointModel*>(lm->getParentJointModel());
if(fjm == NULL) {
jointv.push_back(lm->getParentJointModel());
} else {
fixed_jointv.push_back(fjm);
}
lm = lm->getParentJointModel()->getParentLinkModel();
}
if(!ok) {
ROS_WARN_STREAM("For group " << gc.name_
<< " base link " << gc.base_link_
<< " does not appear to be a direct descendent of " << gc.tip_link_);
return false;
}
//need to reverse jointv to get things in right order
std::reverse(jointv.begin(), jointv.end());
} else {
if(!gc.subgroups_.empty()) {
std::set<const JointModel*> joint_set;
for(unsigned int i = 0; i < gc.subgroups_.size(); i++) {
if(joint_model_group_map_.find(gc.subgroups_[i]) == joint_model_group_map_.end()) {
ROS_INFO_STREAM("Subgroup " << gc.subgroups_[i] << " not defined so can't add group " << gc.name_);
return false;
}
const JointModelGroup* jmg = joint_model_group_map_.find(gc.subgroups_[i])->second;
for(unsigned int j = 0; j < jmg->getJointModels().size(); j++) {
joint_set.insert(jmg->getJointModels()[j]);
}
}
for(std::set<const JointModel*>::iterator it = joint_set.begin();
it != joint_set.end();
it++) {
jointv.push_back((*it));
}
}
if(gc.joints_.size() == 0 && gc.subgroups_.empty()) {
ROS_WARN_STREAM("Group " << gc.name_ << " must have tip/base links, subgroups, or one or more joints");
return false;
}
for(unsigned int j = 0; j < gc.joints_.size(); j++) {
const JointModel* joint = getJointModel(gc.joints_[j]);
if(joint == NULL) {
ROS_ERROR_STREAM("Group " << gc.name_ << " has invalid joint " << gc.joints_[j]);
return false;
}
jointv.push_back(joint);
}
}
if(jointv.size() == 0) {
ROS_WARN_STREAM("Group " << gc.name_ << " must have at least one valid joint");
return false;
}
joint_model_group_map_[gc.name_] = new JointModelGroup(gc.name_, jointv, fixed_jointv, this);
joint_model_group_config_map_[gc.name_] = gc;
return true;
}
