<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, <NAME>, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: <NAME> */

#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_model/robot_model.h>
#include <algorithm>

namespace robot_model
{
namespace
{

bool orderLinksByIndex(const LinkModel *a, const LinkModel *b)
{
  return a->getTreeIndex() < b->getTreeIndex();
}

bool orderJointsByIndex(const JointModel *a, const JointModel *b)
{
  return a->getTreeIndex() < b->getTreeIndex();
}

bool includesParent(const JointModel *joint, const JointModelGroup *group)
{
  bool found = false;
  // 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 (group->hasJointModel(joint->getName()) && joint->getVariableCount() > 0 && joint->getMimic() == NULL)
    {
      found = true;
      break;
    }
    else
      if (joint->getMimic() != NULL)
      {
        const JointModel *mjoint = joint->getMimic();
        if (group->hasJointModel(mjoint->getName()) && mjoint->getVariableCount() > 0 && mjoint->getMimic() == NULL)
          found = true;
        else
          if (includesParent(mjoint, group))
            found = true;
        if (found)
          break;
      }
  }
  return found;
}

}
}

robot_model::JointModelGroup::JointModelGroup(const std::string& group_name,
                                                  const std::vector<const JointModel*> &unsorted_group_joints,
                                                  const RobotModel* parent_model) :
  parent_model_(parent_model), name_(group_name),
  variable_count_(0), is_end_effector_(false), is_chain_(false),
  default_ik_timeout_(0.5), default_ik_attempts_(2)
{
  // sort joints in Depth-First order
  std::vector<const JointModel*> group_joints = unsorted_group_joints;
  std::sort(group_joints.begin(), group_joints.end(), &orderJointsByIndex);
  
  for (std::size_t i = 0 ; i < group_joints.size() ; ++i)
  {
    joint_model_map_[group_joints[i]->getName()] = group_joints[i];
    unsigned int vc = group_joints[i]->getVariableCount();
    if (vc > 0)
    {
      if (group_joints[i]->getMimic() == NULL)
      {
        joint_model_vector_.push_back(group_joints[i]);
        variable_count_ += vc;
      }
      else
        mimic_joints_.push_back(group_joints[i]);

      if (group_joints[i]->getType() == JointModel::REVOLUTE && static_cast<const RevoluteJointModel*>(group_joints[i])->isContinuous())
        continuous_joint_model_vector_const_.push_back(group_joints[i]);
    }
    else
      fixed_joints_.push_back(group_joints[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 < joint_model_vector_.size() ; ++i)
  {
    joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
    bool found = false;
    const JointModel *joint = joint_model_vector_[i];
    // if we find that an ancestor is also in the group, then the joint is not a root
    if (!includesParent(joint, this))
      joint_roots_.push_back(joint_model_vector_[i]);
  }
  
  // compute joint_variables_index_map_
  unsigned int vector_index_counter = 0;
  for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
  {
    const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
    for (std::size_t j = 0; j < name_order.size(); ++j)
    {
      joint_variables_index_map_[name_order[j]] = vector_index_counter + j;
      active_variable_names_.push_back(name_order[j]);
      active_variable_names_set_.insert(name_order[j]);
    }
    joint_variables_index_map_[joint_model_vector_[i]->getName()] = vector_index_counter;
    vector_index_counter += name_order.size();
  }

  for (std::size_t i = 0 ; i < mimic_joints_.size() ; ++i)
  {
    const std::vector<std::string>& name_order = mimic_joints_[i]->getVariableNames();
    const std::vector<std::string>& mim_name_order = mimic_joints_[i]->getMimic()->getVariableNames();
    for (std::size_t j = 0; j < name_order.size(); ++j)
      joint_variables_index_map_[name_order[j]] = joint_variables_index_map_[mim_name_order[j]];
    joint_variables_index_map_[mimic_joints_[i]->getName()] = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
  }

  // 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 < group_joints.size() ; ++i)
    group_links_set.insert(group_joints[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_name_vector_.push_back(link_model_vector_[i]->getName());

  // compute updated links
  std::set<const LinkModel*> u_links;
  for (std::size_t i = 0 ; i < joint_roots_.size() ; ++i)
  {
    std::vector<const LinkModel*> links;
    parent_model->getChildLinkModels(joint_roots_[i], links);
    u_links.insert(links.begin(), links.end());
  }
  for (std::set<const LinkModel*>::iterator it = u_links.begin(); it != u_links.end(); ++it)
  {
    updated_link_model_vector_.push_back(*it);
    updated_link_model_set_.insert(*it);
    updated_link_model_name_set_.insert((*it)->getName());
    if ((*it)->getShape())
    {
      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 && !is_chain_)
  {
    bool chain = true;
    // this would be the leaf of the chain, due to our sorting
    for (std::size_t k = group_joints.size() - 1 ; k > 0 ; --k)
      if (!group_joints[k]->getParentLinkModel() || group_joints[k]->getParentLinkModel()->getParentJointModel() != group_joints[k-1])
      {
        chain = false;
        break;<commit_msg>minor bugfix<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, <NAME>, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the <NAME> nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: <NAME> */

#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_model/robot_model.h>
#include <algorithm>

namespace robot_model
{
namespace
{

bool orderLinksByIndex(const LinkModel *a, const LinkModel *b)
{
  return a->getTreeIndex() < b->getTreeIndex();
}

bool orderJointsByIndex(const JointModel *a, const JointModel *b)
{
  return a->getTreeIndex() < b->getTreeIndex();
}

bool includesParent(const JointModel *joint, const JointModelGroup *group)
{
  bool found = false;
  // 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 (group->hasJointModel(joint->getName()) && joint->getVariableCount() > 0 && joint->getMimic() == NULL)
    {
      found = true;
      break;
    }
    else
      if (joint->getMimic() != NULL)
      {
        const JointModel *mjoint = joint->getMimic();
        if (group->hasJointModel(mjoint->getName()) && mjoint->getVariableCount() > 0 && mjoint->getMimic() == NULL)
          found = true;
        else
          if (includesParent(mjoint, group))
            found = true;
        if (found)
          break;
      }
  }
  return found;
}

}
}

robot_model::JointModelGroup::JointModelGroup(const std::string& group_name,
                                                  const std::vector<const JointModel*> &unsorted_group_joints,
                                                  const RobotModel* parent_model) :
  parent_model_(parent_model), name_(group_name),
  variable_count_(0), is_end_effector_(false), is_chain_(false),
  default_ik_timeout_(0.5), default_ik_attempts_(2)
{
  // sort joints in Depth-First order
  std::vector<const JointModel*> group_joints = unsorted_group_joints;
  std::sort(group_joints.begin(), group_joints.end(), &orderJointsByIndex);
  
  for (std::size_t i = 0 ; i < group_joints.size() ; ++i)
  {
    joint_model_map_[group_joints[i]->getName()] = group_joints[i];
    unsigned int vc = group_joints[i]->getVariableCount();
    if (vc > 0)
    {
      if (group_joints[i]->getMimic() == NULL)
      {
        joint_model_vector_.push_back(group_joints[i]);
        variable_count_ += vc;
      }
      else
        mimic_joints_.push_back(group_joints[i]);

      if (group_joints[i]->getType() == JointModel::REVOLUTE && static_cast<const RevoluteJointModel*>(group_joints[i])->isContinuous())
        continuous_joint_model_vector_const_.push_back(group_joints[i]);
    }
    else
      fixed_joints_.push_back(group_joints[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 < joint_model_vector_.size() ; ++i)
  {
    joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
    bool found = false;
    const JointModel *joint = joint_model_vector_[i];
    // if we find that an ancestor is also in the group, then the joint is not a root
    if (!includesParent(joint, this))
      joint_roots_.push_back(joint_model_vector_[i]);
  }
  
  // compute joint_variables_index_map_
  unsigned int vector_index_counter = 0;
  for (std::size_t i = 0 ; i < joint_model_vector_.size() ; ++i)
  {
    const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
    for (std::size_t j = 0; j < name_order.size(); ++j)
    {
      joint_variables_index_map_[name_order[j]] = vector_index_counter + j;
      active_variable_names_.push_back(name_order[j]);
      active_variable_names_set_.insert(name_order[j]);
    }
    joint_variables_index_map_[joint_model_vector_[i]->getName()] = vector_index_counter;
    vector_index_counter += name_order.size();
  }

  for (std::size_t i = 0 ; i < mimic_joints_.size() ; ++i)
  {
    const std::vector<std::string>& name_order = mimic_joints_[i]->getVariableNames();
    const std::vector<std::string>& mim_name_order = mimic_joints_[i]->getMimic()->getVariableNames();
    for (std::size_t j = 0; j < name_order.size(); ++j)
      joint_variables_index_map_[name_order[j]] = joint_variables_index_map_[mim_name_order[j]];
    joint_variables_index_map_[mimic_joints_[i]->getName()] = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
  }

  // 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 < group_joints.size() ; ++i)
    group_links_set.insert(group_joints[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_name_vector_.push_back(link_model_vector_[i]->getName());

  // compute updated links
  std::set<const LinkModel*> u_links;
  for (std::size_t i = 0 ; i < joint_roots_.size() ; ++i)
  {
    std::vector<const LinkModel*> links;
    parent_model->getChildLinkModels(joint_roots_[i], links);
    u_links.insert(links.begin(), links.end());
  }
  for (std::set<const LinkModel*>::iterator it = u_links.begin(); it != u_links.end(); ++it)
  {
    updated_link_model_vector_.push_back(*it);
    updated_link_model_set_.insert(*it);
    updated_link_model_name_set_.insert((*it)->getName());
    if ((*it)->getShape())
    {
      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 && joint_model_vector_.size() > 1 && !is_chain_)
  {
    bool chain = true;
    // this would be the leaf of the chain, due to our sorting
    for (std::size_t k = group_joints.size() - 1 ; k > 0 ; --k)
      if (!group_joints[k]->getParentLinkModel() || group_joints[k]->getParentLinkModel()->getParentJointModel() != group_joints[k-1])
      {
        chain = false;
        break;
{'fraction_non_alphanumeric': 0.13606480394458792, 'fraction_numerical': 0.0032284573843625263, 'mean_word_length': 4.1409173204586605, 'pattern_counts': {'":': 0, '<': 62, '<?xml version=': 0, '>': 129, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1363866', 'n_tokens_mistral': 5501, 'n_tokens_neox': 5285, 'n_words': 1299}