joint_model_group.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Ioan A. Sucan
*  Copyright (c) 2008-2013, 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_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_
#define MOVEIT_CORE_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 <srdfdom/model.h>
#include <boost/function.hpp>
#include <set>

namespace moveit
{
namespace core
{

class RobotModel;
class JointModelGroup;

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

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


typedef std::map<std::string, JointModelGroup*> JointModelGroupMap;

typedef std::map<std::string, const JointModelGroup*> JointModelGroupMapConst;


typedef std::vector<const JointModel::Bounds*> JointBoundsVector;

class JointModelGroup
{
public:
  
  struct KinematicsSolver
  {
    KinematicsSolver() 
      : default_ik_timeout_(0.5)
      , default_ik_attempts_(2)
    {
    }
    
    operator bool() const
    {
      return allocator_ && !bijection_.empty() && solver_instance_;
    }

    void reset()
    {
      solver_instance_.reset();
      solver_instance_const_.reset();
      bijection_.clear();
    }
    
    SolverAllocatorFn allocator_;
    
    std::vector<unsigned int> bijection_;

    kinematics::KinematicsBaseConstPtr solver_instance_const_;
    
    kinematics::KinematicsBasePtr solver_instance_;

    double default_ik_timeout_;

    unsigned int default_ik_attempts_;
  };
  
  typedef std::map<const JointModelGroup*, KinematicsSolver> KinematicsSolverMap;
  
  JointModelGroup(const std::string& name, const srdf::Model::Group &config,
                  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_;
  }

  const srdf::Model::Group& getConfig() const
  {
    return config_;
  }
  
  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 &link) 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*>& getActiveJointModels() const
  {
    return active_joint_model_vector_;
  }

  const std::vector<std::string>& getActiveJointModelNames() const
  {
    return active_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 std::vector<std::string>& getVariableNames() const
  {
    return variable_names_;
  }

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

  const JointModel* getCommonRoot() const
  {
    return common_root_;
  }
  
  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();
  }
  
  const std::vector<int>& getVariableIndexList() const
  {
    return variable_index_list_;
  }

  int getVariableGroupIndex(const std::string &variable) const;  
  
  const std::vector<std::string>& getDefaultStateNames() const
  {
    return default_states_names_;
  }
  
  void addDefaultState(const std::string &name, const std::map<std::string, double> &default_state);
  
  bool getVariableDefaultPositions(const std::string &name, std::map<std::string, double> &values) const;

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

  void getVariableDefaultPositions(std::vector<double> &values) const
  {
    values.resize(variable_count_);
    getVariableDefaultPositions(&values[0]);
  }

  void getVariableDefaultPositions(double *values) const;

  void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
  {
    getVariableRandomPositions(rng, values, active_joint_models_bounds_);
  }

  void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const
  {
    values.resize(variable_count_);
    getVariableRandomPositions(rng, &values[0], active_joint_models_bounds_);
  }

  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values,
                                        const double *near, const double distance) const
  {
    getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distance);
  }
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values,
                                        const std::vector<double> &near, double distance) const
  {
    values.resize(variable_count_);
    getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance);
  }  

  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values,
                                        const std::vector<double> &near, const std::map<JointModel::JointType, double> &distance_map) const
  {
    values.resize(variable_count_);
    getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance_map);
  }

  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, 
                                        const std::vector<double> &near, const std::vector<double> &distances) const
  {
    values.resize(variable_count_);
    getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distances);
  }  
  
  void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const JointBoundsVector &active_joint_bounds) const;
  
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const JointBoundsVector &active_joint_bounds,
                                        const double *near, const double distance) const;
  
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const JointBoundsVector &active_joint_bounds,
                                        const double *near, const std::map<JointModel::JointType, double> &distance_map) const;
  
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const JointBoundsVector &active_joint_bounds,
                                        const double *near, const std::vector<double> &distances) const;
  
  bool enforcePositionBounds(double *state) const
  {
    return enforcePositionBounds(state, active_joint_models_bounds_);
  }

  bool enforcePositionBounds(double *state, const JointBoundsVector &active_joint_bounds) const;
  bool satisfiesPositionBounds(const double *state, double margin = 0.0) const
  {
    return satisfiesPositionBounds(state, active_joint_models_bounds_, margin);
  }
  bool satisfiesPositionBounds(const double *state, const JointBoundsVector &active_joint_bounds, double margin = 0.0) const;

  double getMaximumExtent() const
  {
    return getMaximumExtent(active_joint_models_bounds_);
  }
  double getMaximumExtent(const JointBoundsVector &active_joint_bounds) const;
  
  double distance(const double *state1, const double *state2) const;  
  void interpolate(const double *from, const double *to, double t, double *state) const;
  
  unsigned int getVariableCount() const
  {
    return variable_count_;
  }
  
  void setSubgroupNames(const std::vector<std::string> &subgroups);

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

  void getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const;

  bool isSubgroup(const std::string& group) const
  {
    return subgroup_names_set_.find(group) != subgroup_names_set_.end();
  }  
  
  bool isChain() const
  {
    return is_chain_;
  }

  bool isSingleDOFJoints() const
  {
    return is_single_dof_;
  }
  
  bool isEndEffector() const
  {
    return !end_effector_name_.empty();
  }

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

  void setEndEffectorName(const std::string &name);
  
  void setEndEffectorParent(const std::string &group, const std::string &link);
  
  void attachEndEffector(const std::string &eef_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_;
  }

  bool getEndEffectorTips(std::vector<const LinkModel*> &tips) const;

  bool getEndEffectorTips(std::vector<std::string> &tips) const;

  const JointBoundsVector& getActiveJointModelsBounds() const
  {
    return active_joint_models_bounds_;
  }

  const std::pair<KinematicsSolver, KinematicsSolverMap>& getGroupKinematics() const
  {
    return group_kinematics_;
  }

  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 group_kinematics_.first.solver_instance_const_;
  }

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

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

  bool setRedundantJoints(const std::vector<std::string> &joints)
  {
    if (group_kinematics_.first.solver_instance_)
      return group_kinematics_.first.solver_instance_->setRedundantJoints(joints);
    return false;
  }

  double getDefaultIKTimeout() const
  {
    return group_kinematics_.first.default_ik_timeout_;
  }

  void setDefaultIKTimeout(double ik_timeout);

  unsigned int getDefaultIKAttempts() const
  {
    return group_kinematics_.first.default_ik_attempts_;
  }

  void setDefaultIKAttempts(unsigned int ik_attempts);

  const std::vector<unsigned int>& getKinematicsSolverJointBijection() const
  {
    return group_kinematics_.first.bijection_;
  }

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

protected:

  bool computeIKIndexBijection(const std::vector<std::string> &ik_jnames, std::vector<unsigned int> &joint_bijection) const;
  
  void updateMimicJoints(double *values) const;

  const RobotModel                                          *parent_model_;

  std::string                                                name_;

  std::vector<const JointModel*>                             joint_model_vector_;

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

  std::vector<const JointModel*>                             active_joint_model_vector_;

  std::vector<std::string>                                   active_joint_model_name_vector_;

  std::vector<const JointModel*>                             fixed_joints_;

  std::vector<const JointModel*>                             mimic_joints_;

  std::vector<const JointModel*>                             continuous_joint_model_vector_;

  std::vector<std::string>                                   variable_names_;

  std::set<std::string>                                      variable_names_set_;

  JointModelMapConst                                         joint_model_map_;
  
  std::vector<const JointModel*>                             joint_roots_;

  const JointModel                                          *common_root_;
  
  VariableIndexMap                                           joint_variables_index_map_;

  JointBoundsVector                                          active_joint_models_bounds_;

  std::vector<int>                                           variable_index_list_;
    
  std::vector<int>                                           active_joint_model_start_index_;
  
  std::vector<const LinkModel*>                              link_model_vector_;

  LinkModelMapConst                                          link_model_map_;

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

  std::vector<const LinkModel*>                              link_model_with_geometry_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_;

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

  std::set<std::string>                                      subgroup_names_set_;
  
  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_;

  bool                                                       is_single_dof_;

  struct GroupMimicUpdate
  {
    GroupMimicUpdate(int s, int d, double f, double o) : src(s), dest(d), factor(f), offset(o)
    {
    }
    int src;
    int dest;
    double factor;
    double offset;
  };
  
  std::vector<GroupMimicUpdate>                              group_mimic_update_;

  std::pair<KinematicsSolver, KinematicsSolverMap>           group_kinematics_;
  
  srdf::Model::Group                                         config_;
  
  std::map<std::string, std::map<std::string, double> >      default_states_;

  std::vector<std::string>                                   default_states_names_;
  
};

}
}

#endif