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::set<const LinkModel*>& getUpdatedLinkModelsSet() const
  {
    return updated_link_model_set_;
  }

  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 moveit::core::LinkModel* getOnlyOneEndEffectorTip() 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