kinematic_model.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, 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 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.
*********************************************************************/

#ifndef PLANNING_MODELS_KINEMATIC_MODEL_
#define PLANNING_MODELS_KINEMATIC_MODEL_

#include <geometric_shapes/shapes.h>

#include <urdf/model.h>
#include <tf/LinearMath/Transform.h>
#include <boost/thread/shared_mutex.hpp>

#include <iostream>
#include <vector>
#include <string>
#include <map>
#include <boost/bimap.hpp>

namespace planning_models
{
 
class KinematicModel
{
public: 
  class JointModel;
        
  class LinkModel;

  class AttachedBodyModel;
        
  struct MultiDofConfig
  {
    MultiDofConfig(std::string n) : name(n) {
    }
    
    ~MultiDofConfig() {
    }
    
    std::string name;
    
    std::string type;
    
    std::string parent_frame_id;

    std::string child_frame_id;

    std::map<std::string, std::string> name_equivalents;
  };

  struct GroupConfig
  {
    GroupConfig() { 
    }

    GroupConfig(std::string name, 
                std::string base_link,
                std::string tip_link) 
      : name_(name), base_link_(base_link), tip_link_(tip_link)
    {
    }

    GroupConfig(std::string name,
                std::vector<std::string> joints,
                std::vector<std::string> subgroups) 
      : name_(name)
    {
      joints_ = joints;
      subgroups_ = subgroups;
    }

    std::string name_;
    std::string base_link_;
    std::string tip_link_;
    std::vector<std::string> joints_;
    std::vector<std::string> subgroups_;
  };
  
  class JointModel
  {
    friend class KinematicModel;
  public:
    JointModel(const std::string& name);

    JointModel(const JointModel* joint);

    void initialize(const std::vector<std::string>& local_names,
                    const MultiDofConfig* multi_dof_config = NULL);

    virtual ~JointModel(void);

    typedef boost::bimap< std::string, std::string > js_type;

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

    const LinkModel* getParentLinkModel() const 
    {
      return parent_link_model_;
    }
    
    const LinkModel* getChildLinkModel() const
    {
      return child_link_model_;
    }
    
    const std::string& getParentFrameId() const
    {
      return parent_frame_id_;
    }

    const std::string& getChildFrameId() const
    {
      return child_frame_id_;
    }

    const js_type& getJointStateEquivalents() const
    {
      return joint_state_equivalents_;
    }

    const std::map<unsigned int, std::string>& getComputatationOrderMapIndex() const
    {
      return computation_order_map_index_;
    }
    std::string getEquiv(const std::string& name) const;

    bool getVariableBounds(const std::string& variable, std::pair<double, double>& bounds) const;
   
    bool setVariableBounds(const std::string& variable, double low, double high);

    virtual void getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const;
    
    virtual bool isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const; 

    const std::map<std::string, std::pair<double, double> >& getAllVariableBounds() const {
      return joint_state_bounds_;
    }

    bool hasVariable(const std::string var) const
    {
      return(joint_state_equivalents_.right.find(var) != joint_state_equivalents_.right.end());
    }
   
    virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const = 0;
    
    virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const = 0;

  private:

    std::string name_;
    
    LinkModel *parent_link_model_;
    
    LinkModel *child_link_model_;

    //local names on the left, config names on the right
    js_type joint_state_equivalents_;

    //map for high and low bounds
    std::map<std::string, std::pair<double, double> > joint_state_bounds_;
    
    //correspondance between index into computation array and external name
    std::map<unsigned int, std::string> computation_order_map_index_;
   
    std::string parent_frame_id_;
    
    std::string child_frame_id_;
  };

  class FixedJointModel : public JointModel
  {
  public:
            
    FixedJointModel(const std::string name, const MultiDofConfig* multi_dof_config) :
      JointModel(name)
    {
      std::vector<std::string> local_names;
      initialize(local_names, multi_dof_config);
    }
            
    FixedJointModel(const FixedJointModel* joint): JointModel(joint)
    {
    }

    virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const {
      tf::Transform ident;
      ident.setIdentity();
      return ident;
    }
    
    virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const {
      std::vector<double> ret;
      return ret;
    }

  };

  class PlanarJointModel : public JointModel
  {
  public:
            
    PlanarJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
            
    PlanarJointModel(const PlanarJointModel* joint): JointModel(joint)
    {
    }

    virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;

  };

  class FloatingJointModel : public JointModel
  {
  public:
            
    FloatingJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);

    FloatingJointModel(const FloatingJointModel* joint) : JointModel(joint)
    {
    }

    virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;

    virtual void getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const;

  };

  class PrismaticJointModel : public JointModel
  {
  public:
            
    PrismaticJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
    
    PrismaticJointModel(const PrismaticJointModel* joint) : JointModel(joint){
      axis_ = joint->axis_;
    }
            
    tf::Vector3 axis_;
    
    virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
    
  };
        
  class RevoluteJointModel : public JointModel
  {
  public:
            
    RevoluteJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);

    RevoluteJointModel(const RevoluteJointModel* joint) : JointModel(joint){
      axis_ = joint->axis_;
      continuous_ = joint->continuous_;
    }
                    
    tf::Vector3 axis_;
    bool      continuous_;

    virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
    
    //so we can return true for continuous joints
    virtual bool isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const;
  };
        
  class LinkModel
  {
    friend class KinematicModel;
  public:
    
    LinkModel(const KinematicModel* kinematic_model);
    
    LinkModel(const LinkModel* link_model);

    ~LinkModel(void);

    const std::string& getName() const {
      return name_;
    }
    
    const JointModel* getParentJointModel() const {
      return parent_joint_model_;
    }

    const std::vector<JointModel*>& getChildJointModels() const {
      return child_joint_models_;
    }

    const tf::Transform& getJointOriginTransform() const {
      return joint_origin_transform_;
    }

    const tf::Transform& getCollisionOriginTransform() const {
      return collision_origin_transform_;
    }

    const shapes::Shape* getLinkShape() const {
      return shape_;
    }
    
    const std::vector<AttachedBodyModel*>& getAttachedBodyModels() const {
      return attached_body_models_;
    }

  private:
     
    void clearAttachedBodyModels();
    
    void replaceAttachedBodyModels(std::vector<AttachedBodyModel*>& attached_body_vector);
   
    void clearLinkAttachedBodyModel(const std::string& att_name);

    void addAttachedBodyModel(AttachedBodyModel* attached_body_model);

    std::string name_;
    
    const KinematicModel* kinematic_model_;

    JointModel *parent_joint_model_;
    
    std::vector<JointModel*> child_joint_models_;
    
    tf::Transform joint_origin_transform_;
    
    tf::Transform  collision_origin_transform_;
    
    shapes::Shape *shape_;
    
    std::vector<AttachedBodyModel*> attached_body_models_;              
  };
  
  class AttachedBodyModel
  {
  public:
    
    AttachedBodyModel(const LinkModel *link, 
                      const std::string& id,
                      const std::vector<tf::Transform>& attach_trans,
                      const std::vector<std::string>& touch_links,
                      std::vector<shapes::Shape*>& shapes);

    ~AttachedBodyModel(void);
    
    const std::string& getName() const 
    {
      return id_;
    }
    
    const LinkModel* getAttachedLinkModel() const 
    {
      return attached_link_model_;
    }

    const std::vector<shapes::Shape*>& getShapes() const 
    {
      return shapes_;
    }

    const std::vector<tf::Transform>& getAttachedBodyFixedTransforms() const
    {
      return attach_trans_;
    }

    const std::vector<std::string>& getTouchLinks() const
    {
      return touch_links_;
    }
    
  private:

    const LinkModel *attached_link_model_;
    
    std::vector<shapes::Shape*> shapes_;
    
    std::vector<tf::Transform> attach_trans_;
    
    std::vector<std::string> touch_links_;
    
    std::string id_;
  };

  class JointModelGroup
  {
    friend class KinematicModel;
  public:
            
    JointModelGroup(const std::string& name,
                    const std::vector<const JointModel*>& joint_vector,
                    const std::vector<const JointModel*>& fixed_joint_vector,
                    const KinematicModel* parent_model);
    
    ~JointModelGroup(void);

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

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

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

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

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

    const std::vector<const LinkModel*>& getGroupLinkModels() const
    {
      return group_link_model_vector_;
    }

    std::vector<std::string> getGroupLinkNames() const
    {
      std::vector<std::string> ret_vec;
      for(unsigned int i = 0; i < group_link_model_vector_.size(); i++) {
        ret_vec.push_back(group_link_model_vector_[i]->getName());
      }
      return ret_vec;
    }

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

    std::vector<std::string> getUpdatedLinkModelNames() const
    {
      std::vector<std::string> ret_vec;
      for(unsigned int i = 0; i < updated_link_model_vector_.size(); i++) {
        ret_vec.push_back(updated_link_model_vector_[i]->getName());
      }
      return ret_vec;
    }    

  private:

    bool is_valid_;

    std::string name_;

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

    std::vector<const JointModel*> joint_model_vector_;

    std::vector<const JointModel*> fixed_joint_model_vector_;

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

    std::vector<const JointModel*> joint_roots_;

    std::vector<const LinkModel*> group_link_model_vector_;

    std::vector<const LinkModel*> updated_link_model_vector_;

  };

  KinematicModel(const KinematicModel &source);

  KinematicModel(const urdf::Model &model, 
                 const std::vector<GroupConfig>& group_configs,
                 const std::vector<MultiDofConfig>& multi_dof_configs);
        
  ~KinematicModel(void);

  void copyFrom(const KinematicModel &source);

  const std::string& getName(void) const;

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

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

  void getLinkModelNames(std::vector<std::string> &links) const;

  void getChildLinkModels(const LinkModel* parent, std::vector<const LinkModel*> &links) const;

  void getChildLinkModels(const JointModel* parent, std::vector<const LinkModel*> &links) const;

  void getChildJointModels(const LinkModel* parent, std::vector<const JointModel*> &links) const;

  void getChildJointModels(const JointModel* parent, std::vector<const JointModel*> &links) const;
  
  std::vector<std::string> getChildLinkModelNames(const LinkModel* parent) const;

  std::vector<std::string> getChildJointModelNames(const LinkModel* parent) const;

  std::vector<std::string> getChildJointModelNames(const JointModel* parent) const;

  std::vector<std::string> getChildLinkModelNames(const JointModel* parent) const;

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

  bool hasJointModel(const std::string &name) const;
        
  const std::vector<JointModel*>& getJointModels() const
  {
    return joint_model_vector_;
  }
  const std::vector<LinkModel*>& getLinkModels() const
  {
    return link_model_vector_;
  }

  const std::vector<LinkModel*>& getLinkModelsWithCollisionGeometry() const
  {
    return link_models_with_collision_geometry_vector_;
  }
        
  void getJointModelNames(std::vector<std::string> &joints) const;

  const JointModel* getRoot(void) const;
        
  void exclusiveLock(void) const;
        
  void exclusiveUnlock(void) const;

  void sharedLock(void) const;
  
  void sharedUnlock(void) const;

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

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

  bool addModelGroup(const GroupConfig& group);

  void removeModelGroup(const std::string& group);

  const JointModelGroup* getModelGroup(const std::string& name) const
  {
    if(!hasModelGroup(name)) return NULL;
    return joint_model_group_map_.find(name)->second;
  }

  const std::map<std::string, JointModelGroup*>& getJointModelGroupMap() const
  {
    return joint_model_group_map_;
  }

  const std::map<std::string, GroupConfig>& getJointModelGroupConfigMap() const {
    return joint_model_group_config_map_;
  }

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

  void clearAllAttachedBodyModels();

  void clearLinkAttachedBodyModels(const std::string& link_name);

  void clearLinkAttachedBodyModel(const std::string& link_name, const std::string& att_name);

  void replaceAttachedBodyModels(const std::string& link_name, std::vector<AttachedBodyModel*>& attached_body_vector);

  void addAttachedBodyModel(const std::string& link_name, AttachedBodyModel* att_body_model);

  std::vector<const AttachedBodyModel*> getAttachedBodyModels() const;

  std::string getRobotName() const {
    return model_name_;
  }
        
private:
        
  mutable boost::shared_mutex lock_;

  std::string model_name_;      

  std::map<std::string, LinkModel*> link_model_map_;

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

  std::vector<JointModel*> joint_model_vector_;
        
  std::vector<LinkModel*> link_model_vector_;   
        
  std::vector<LinkModel*> link_models_with_collision_geometry_vector_;

  JointModel *root_;
        
  std::map<std::string, JointModelGroup*> joint_model_group_map_;
  std::map<std::string, GroupConfig> joint_model_group_config_map_;

  void buildGroups(const std::vector<GroupConfig>&);
  JointModel* buildRecursive(LinkModel *parent, const urdf::Link *link, 
                             const std::vector<MultiDofConfig>& multi_dof_configs);
  JointModel* constructJointModel(const urdf::Joint *urdfJointModel,  const urdf::Link *child_link,
                        const std::vector<MultiDofConfig>& multi_dof_configs);
  LinkModel* constructLinkModel(const urdf::Link *urdfLink);
  shapes::Shape* constructShape(const urdf::Geometry *geom);

  JointModel* copyJointModel(const JointModel *joint);
  JointModel* copyRecursive(LinkModel *parent, const LinkModel *link);
        
};

}

#endif