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 */
 
#pragma once
 
#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;
 
using SolverAllocatorMapFn = std::map<const JointModelGroup*, SolverAllocatorFn>;
 
using JointModelGroupMap = std::map<std::string, JointModelGroup*>;
 
using JointModelGroupMapConst = std::map<std::string, const JointModelGroup*>;
 
using JointBoundsVector = std::vector<const JointModel::Bounds*>;
 
class JointModelGroup
{
public:
  struct KinematicsSolver
  {
    KinematicsSolver() : default_ik_timeout_(0.5)
    {
    }
 
    operator bool() const
    {
      return allocator_ && !bijection_.empty() && solver_instance_;
    }
 
    void reset()
    {
      solver_instance_.reset();
      bijection_.clear();
    }
 
    SolverAllocatorFn allocator_;
 
    std::vector<unsigned int> bijection_;
 
    kinematics::KinematicsBasePtr solver_instance_;
 
    double default_ik_timeout_;
  };
 
  using KinematicsSolverMap = std::map<const JointModelGroup*, KinematicsSolver>;
 
  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* seed,
                                        const double distance) const
  {
    getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, seed, distance);
  }
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
                                        const std::vector<double>& seed, double distance) const
  {
    values.resize(variable_count_);
    getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &seed[0], distance);
  }
 
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
                                        const std::vector<double>& seed,
                                        const std::map<JointModel::JointType, double>& distance_map) const
  {
    values.resize(variable_count_);
    getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &seed[0], distance_map);
  }
 
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* seed,
                                        const std::vector<double>& distances) const
  {
    getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, seed, distances);
  }
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
                                        const std::vector<double>& seed, const std::vector<double>& distances) const
  {
    values.resize(variable_count_);
    getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &seed[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* seed,
                                        const double distance) const;
 
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
                                        const JointBoundsVector& active_joint_bounds, const double* seed,
                                        const std::map<JointModel::JointType, double>& distance_map) const;
 
  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
                                        const JointBoundsVector& active_joint_bounds, const double* seed,
                                        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_;
  }
 
  unsigned int getActiveVariableCount() const
  {
    return active_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 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);
 
  const std::vector<unsigned int>& getKinematicsSolverJointBijection() const
  {
    return group_kinematics_.first.bijection_;
  }
 
  void printGroupInfo(std::ostream& out = std::cout) const;
 
  bool isValidVelocityMove(const std::vector<double>& from_joint_pose, const std::vector<double>& to_joint_pose,
                           double dt) const;
 
  bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size,
                           double dt) 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_;
 
  unsigned int active_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_;
};
}  // namespace core
}  // namespace moveit