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 <LinearMath/btTransform.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;
  };
  
  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;

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

    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 btTransform computeTransform(const std::vector<double>& joint_values) const = 0;
    
    virtual std::vector<double> computeJointStateValues(const btTransform& 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 btTransform computeTransform(const std::vector<double>& joint_values) const {
      btTransform ident;
      ident.setIdentity();
      return ident;
    }
    
    virtual std::vector<double> computeJointStateValues(const btTransform& 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 btTransform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const btTransform& transform) const;

  };

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

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

    virtual btTransform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const btTransform& transform) 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_;
    }
            
    btVector3 axis_;
    
    virtual btTransform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const btTransform& 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_;
    }
                    
    btVector3 axis_;
    bool      continuous_;

    virtual btTransform computeTransform(const std::vector<double>& joint_values) const;
    
    virtual std::vector<double> computeJointStateValues(const btTransform& transform) 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 btTransform& getJointOriginTransform() const {
      return joint_origin_transform_;
    }

    const btTransform& 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_;
    
    btTransform joint_origin_transform_;
    
    btTransform  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<btTransform>& 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<btTransform>& 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<btTransform> attach_trans_;
    
    std::vector<std::string> touch_links_;
    
    std::string id_;
  };

  class JointModelGroup
  {
    friend class KinematicModel;
  public:
            
    JointModelGroup(
                    const std::string& groupName, 
                    const std::vector<const JointModel*> &groupJoints);
    ~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<std::string>& getJointModelNames() const
    {
      return joint_model_name_vector_;
    }

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

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

    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::vector<const LinkModel*> updated_link_model_vector_;
            
  };

  KinematicModel(const KinematicModel &source);

  KinematicModel(const urdf::Model &model, 
                 const std::map< std::string, std::vector<std::string> > &groups, 
                 const std::vector<MultiDofConfig>& multi_dof_configs);
        
  ~KinematicModel(void);

  void copyFrom(const KinematicModel &source);

  void defaultState(void);
        
  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;
  
  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_;
  }
        
  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;

  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_;
  }

  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);
        
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_;   
        
  JointModel *root_;
        
  std::map<std::string, JointModelGroup*> joint_model_group_map_;

  void buildGroups(const std::map< std::string, std::vector<std::string> > &groups);
  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

---

planning_models
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Fri Jan 11 09:14:25 2013