joint_model_group.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, 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 Willow Garage, Inc. 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: Ioan Sucan */

#ifndef MOVEIT_ROBOT_MODEL_JOINT_MODEL_GROUP_
#define MOVEIT_ROBOT_MODEL_JOINT_MODEL_GROUP_

#include <moveit/robot_model/joint_model.h>
#include <moveit/robot_model/link_model.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <boost/function.hpp>
#include <set>

namespace robot_model
{

class RobotModel;
class JointModelGroup;

typedef boost::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;

typedef std::map<const JointModelGroup*, SolverAllocatorFn> SolverAllocatorMapFn;

class JointModelGroup
{
  friend class RobotModel;

public:

  JointModelGroup(const std::string& name, const std::vector<const JointModel*>& joint_vector, const RobotModel *parent_model);

  ~JointModelGroup();

  const RobotModel* getParentModel() const
  {
    return parent_model_;
  }

  const std::string& getName() const
  {
    return name_;
  }

  bool hasJointModel(const std::string &joint) const;

  bool hasLinkModel(const std::string &link) const;

  const JointModel* getJointModel(const std::string &joint) const;

  const LinkModel* getLinkModel(const std::string &joint) const;

  const std::vector<const JointModel*>& getJointModels() const
  {
    return joint_model_vector_;
  }

  const std::vector<std::string>& getJointModelNames() const
  {
    return joint_model_name_vector_;
  }

  const std::vector<const JointModel*>& getFixedJointModels() const
  {
    return fixed_joints_;
  }

  const std::vector<const JointModel*>& getMimicJointModels() const
  {
    return mimic_joints_;
  }

  const std::vector<const JointModel*>& getContinuousJointModels() const
  {
    return continuous_joint_model_vector_const_;
  }

  const std::vector<std::string>& getVariableNames() const
  {
    return active_variable_names_;
  }

  const std::vector<const JointModel*>& getJointRoots() const
  {
    return joint_roots_;
  }

  const std::vector<const LinkModel*>& getLinkModels() const
  {
    return link_model_vector_;
  }

  const std::vector<std::string>& getLinkModelNames() const
  {
    return link_model_name_vector_;
  }

  const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
  {
    return link_model_with_geometry_name_vector_;
  }

  const std::vector<const LinkModel*>& getUpdatedLinkModels() const
  {
    return updated_link_model_vector_;
  }

  const std::vector<std::string>& getUpdatedLinkModelNames() const
  {
    return updated_link_model_name_vector_;
  }

  const std::vector<const LinkModel*>& getUpdatedLinkModelsWithGeometry() const
  {
    return updated_link_model_with_geometry_vector_;
  }

  const std::set<const LinkModel*>& getUpdatedLinkModelsWithGeometrySet() const
  {
    return updated_link_model_with_geometry_set_;
  }

  const std::vector<std::string>& getUpdatedLinkModelsWithGeometryNames() const
  {
    return updated_link_model_with_geometry_name_vector_;
  }

  const std::set<std::string>& getUpdatedLinkModelsWithGeometryNamesSet() const
  {
    return updated_link_model_with_geometry_name_set_;
  }

  bool isLinkUpdated(const std::string &name) const
  {
    return updated_link_model_name_set_.find(name) != updated_link_model_name_set_.end();
  }

  bool isActiveDOF(const std::string &name) const
  {
    return active_variable_names_set_.find(name) != active_variable_names_set_.end();
  }

  const std::map<std::string, unsigned int>& getJointVariablesIndexMap() const
  {
    return joint_variables_index_map_;
  }

  void getKnownDefaultStates(std::vector<std::string> &default_states) const;

  bool getVariableDefaultValues(const std::string &name, std::map<std::string, double> &values) const;

  void getVariableDefaultValues(std::vector<double> &values) const;

  void getVariableDefaultValues(std::map<std::string, double> &values) const;

  void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const;

  void getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const std::vector<double> &near, const std::map<robot_model::JointModel::JointType, double> &distance_map) const;

  void getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const std::vector<double> &near, const std::vector<double> &distances) const;

  unsigned int getVariableCount() const
  {
    return variable_count_;
  }

  const std::vector<std::string>& getSubgroupNames() const
  {
    return subgroup_names_;
  }

  bool isSubgroup(const std::string& group) const;

  bool isChain() const
  {
    return is_chain_;
  }

  bool isEndEffector() const
  {
    return is_end_effector_;
  }

  const std::string& getEndEffectorName() const
  {
    return end_effector_name_;
  }

  const std::pair<std::string, std::string>& getEndEffectorParentGroup() const
  {
    return end_effector_parent_;
  }

  const std::vector<std::string>& getAttachedEndEffectorNames() const
  {
    return attached_end_effector_names_;
  }

  double getMaximumExtent(void) const;

  std::vector<moveit_msgs::JointLimits> getVariableDefaultLimits() const;

  std::vector<moveit_msgs::JointLimits> getVariableLimits() const;

  void setVariableLimits(const std::vector<moveit_msgs::JointLimits>& jlim);

  const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& getSolverAllocators() const
  {
    return solver_allocators_;
  }

  void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map = SolverAllocatorMapFn())
  {
    setSolverAllocators(std::make_pair(solver, solver_map));
  }

  void setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn> &solvers);

  const kinematics::KinematicsBaseConstPtr& getSolverInstance() const
  {
    return solver_instance_const_;
  }

  const kinematics::KinematicsBasePtr& getSolverInstance()
  {
    return solver_instance_;
  }

  bool canSetStateFromIK(const std::string &tip) const;

  bool setRedundantJoints(const std::vector<unsigned int> &joints)
  {
    if(solver_instance_)
      return (solver_instance_->setRedundantJoints(joints));
    return false;
  }

  double getDefaultIKTimeout() const
  {
    return default_ik_timeout_;
  }

  void setDefaultIKTimeout(double ik_timeout);

  unsigned int getDefaultIKAttempts() const
  {
    return default_ik_attempts_;
  }

  void setDefaultIKAttempts(unsigned int ik_attempts)
  {
    default_ik_attempts_ = ik_attempts;
  }

  const std::vector<unsigned int>& getKinematicsSolverJointBijection() const
  {
    return ik_joint_bijection_;
  }

  void printGroupInfo(std::ostream &out = std::cout) const;

protected:

  const RobotModel                                 *parent_model_;

  std::string                                           name_;

  std::vector<std::string>                              joint_model_name_vector_;

  std::vector<const JointModel*>                        joint_model_vector_;

  std::map<std::string, const JointModel*>              joint_model_map_;

  std::vector<const JointModel*>                        joint_roots_;

  std::map<std::string, unsigned int>                   joint_variables_index_map_;

  std::vector<const JointModel*>                        fixed_joints_;

  std::vector<const JointModel*>                        mimic_joints_;

  std::vector<const JointModel*>                        continuous_joint_model_vector_const_;

  std::vector<std::string>                              active_variable_names_;

  std::set<std::string>                                 active_variable_names_set_;

  std::vector<const LinkModel*>                         link_model_vector_;

  std::vector<std::string>                              link_model_name_vector_;

  std::vector<std::string>                              link_model_with_geometry_name_vector_;

  std::vector<const LinkModel*>                         updated_link_model_vector_;

  std::set<const LinkModel*>                            updated_link_model_set_;

  std::vector<std::string>                              updated_link_model_name_vector_;

  std::set<std::string>                                 updated_link_model_name_set_;

  std::vector<const LinkModel*>                         updated_link_model_with_geometry_vector_;

  std::set<const LinkModel*>                            updated_link_model_with_geometry_set_;

  std::vector<std::string>                              updated_link_model_with_geometry_name_vector_;

  std::set<std::string>                                 updated_link_model_with_geometry_name_set_;

  unsigned int                                          variable_count_;

  std::vector<std::string>                              subgroup_names_;

  bool                                                  is_end_effector_;

  std::vector<std::string>                              attached_end_effector_names_;

  std::pair<std::string, std::string>                   end_effector_parent_;

  std::string                                           end_effector_name_;

  bool                                                  is_chain_;

  std::pair<SolverAllocatorFn, SolverAllocatorMapFn>    solver_allocators_;

  kinematics::KinematicsBaseConstPtr                    solver_instance_const_;

  kinematics::KinematicsBasePtr                         solver_instance_;

  std::vector<unsigned int>                             ik_joint_bijection_;

  double                                                default_ik_timeout_;

  unsigned int                                          default_ik_attempts_;

  std::map<std::string, std::map<std::string, double> > default_states_;
};

}

#endif