kinematic_state.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_STATE_
#define PLANNING_MODELS_KINEMATIC_STATE_

#include "kinematic_model.h"

namespace planning_models
{

class KinematicState 
{

public:
  
  class JointState; ;
  
  class LinkState; 
  class AttachedBodyState; 
  class JointGroupState; 
  class JointState
  {
  public: 
    
    JointState(const KinematicModel::JointModel* jm);

    ~JointState()
    {
    }

    bool setJointStateValues(const std::map<std::string, double>& joint_value_map);

    bool setJointStateValues(const std::map<std::string, double>& joint_value_map,
                             std::vector<std::string>& missing_states);
    
    bool setJointStateValues(const std::vector<double>& joint_value_vector);
    
    bool setJointStateValues(const tf::Transform& transform);

    bool allJointStateValuesAreDefined(const std::map<std::string, double>& joint_value_map) const;

    bool getJointValueBounds(const std::string& value_name, double& low, double& high) const;
    
    const std::map<std::string, std::pair<double, double> >& getAllJointValueBounds() const;

    bool areJointStateValuesWithinBounds() const;

    const std::string& getName() const 
    {
      return joint_model_->getName();
    }
    
    unsigned int getDimension() 
    {
      return joint_state_values_.size();
    }

    const std::vector<double>& getJointStateValues() const;

    const std::vector<std::string>& getJointStateNameOrder() const;

    const tf::Transform& getVariableTransform() const 
    {
      return variable_transform_;
    }
    
    const KinematicModel::JointModel* getJointModel() const
    {
      return joint_model_;
    }

    const std::map<std::string, unsigned int>& getJointStateIndexMap() const
    {
      return joint_state_index_map_;
    }

    const std::string& getParentFrameId() const
    {
      return joint_model_->getParentFrameId();
    }

    const std::string& getChildFrameId() const
    {
      return joint_model_->getChildFrameId();
    }

  private:
    
    const KinematicModel::JointModel* joint_model_;
    
    tf::Transform variable_transform_; 
      
    std::map<std::string, unsigned int> joint_state_index_map_;

    std::vector<std::string> joint_state_name_order_;

    std::vector<double> joint_state_values_;
  };

  class LinkState 
  {
  public:
    LinkState(const KinematicModel::LinkModel* lm);

    ~LinkState();

    const std::string& getName() const 
    {
      return link_model_->getName();
    }

    void setParentJointState(const JointState* js) 
    {
      parent_joint_state_ = js;
    }
    
    void setParentLinkState(const LinkState* ls)
    {
      parent_link_state_ = ls;
    }

    void updateGivenGlobalLinkTransform(const tf::Transform& transform)
    {
      global_link_transform_ = transform;
      global_collision_body_transform_.mult(global_link_transform_, link_model_->getCollisionOriginTransform());
      updateAttachedBodies();
    }
    
    void computeTransform(void);
    
    //updates all attached bodies given set link transforms
    void updateAttachedBodies();

    const KinematicModel::LinkModel* getLinkModel() const 
    {
      return link_model_;
    }
    
    const JointState* getParentJointState() const
    {
      return parent_joint_state_;
    }

    const LinkState* getParentLinkState() const
    {
      return parent_link_state_;
    }

    const std::vector<AttachedBodyState*>& getAttachedBodyStateVector() const
    {
      return attached_body_state_vector_;
    }

    const tf::Transform& getGlobalLinkTransform() const 
    {
      return global_link_transform_;
    }

    const tf::Transform& getGlobalCollisionBodyTransform() const
    {
      return global_collision_body_transform_;
    }

  private:
    
    const KinematicModel::LinkModel* link_model_;

    const JointState* parent_joint_state_;

    const LinkState* parent_link_state_;

    std::vector<AttachedBodyState*> attached_body_state_vector_;

    tf::Transform global_link_transform_;
    
    tf::Transform global_collision_body_transform_;    
  };

  class AttachedBodyState 
  {
  public:
    AttachedBodyState(const KinematicModel::AttachedBodyModel* abm, const LinkState* parent_link_state);
    
    ~AttachedBodyState()
    {
    }

    const std::string& getName() const
    {
      return attached_body_model_->getName();
    }

    const std::string& getAttachedLinkName() const
    {
      return attached_body_model_->getAttachedLinkModel()->getName();
    }
    
    const KinematicModel::AttachedBodyModel* getAttachedBodyModel() const
    {
      return attached_body_model_;
    }

    void computeTransform(void);

    const std::vector<tf::Transform>& getGlobalCollisionBodyTransforms() const
    {
      return global_collision_body_transforms_;
    }

  private:
    const KinematicModel::AttachedBodyModel* attached_body_model_;

    const LinkState* parent_link_state_;

    std::vector<tf::Transform> global_collision_body_transforms_;
    
  };

  class JointStateGroup
  {
  public:
            
    JointStateGroup(const KinematicModel::JointModelGroup* jmg, const KinematicState* owner);

    //JointGroupState(const JointGroupState* jgs)

    ~JointStateGroup(void)
    {
    }

    const KinematicModel::JointModelGroup* getJointModelGroup() {
      return joint_model_group_;
    }

    const std::string& getName() const 
    {
      return joint_model_group_->getName();
    }

    unsigned int getDimension() const
    {
      return dimension_;
    }

    bool setKinematicState(const std::vector<double>& joint_state_values);
    
    bool setKinematicState(const std::map<std::string, double>& joint_state_map);

    void updateKinematicLinks();        

    bool hasJointState(const std::string &joint) const;

    bool updatesLinkState(const std::string& joint) const;

    JointState* getJointState(const std::string &joint) const;

    void getKinematicStateValues(std::vector<double>& joint_state_values) const;
    
    void getKinematicStateValues(std::map<std::string, double>& joint_state_values) const;
   
    void setKinematicStateToDefault();

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

    const std::map<std::string, unsigned int>& getKinematicStateIndexMap() const
    {
      return kinematic_state_index_map_;
    }
    
    const std::vector<std::string>& getJointNames() const
    {
      return joint_names_;
    }

    const std::vector<JointState*>& getJointStateVector() const
    {
      return joint_state_vector_;
    }
            
  private:
    
    boost::shared_ptr<KinematicModel> kinematic_model_;     

    const KinematicModel::JointModelGroup* joint_model_group_;
    
    unsigned int dimension_;
    std::map<std::string, unsigned int> kinematic_state_index_map_;

    std::vector<std::string> joint_names_;

    std::vector<JointState*> joint_state_vector_;

    std::map<std::string, JointState*> joint_state_map_;

    std::vector<JointState*> joint_roots_;

    std::vector<LinkState*> updated_links_;         
  };

  KinematicState(const KinematicModel* kinematic_model);
  
  KinematicState(const KinematicState& state);

  ~KinematicState(void);

  bool setKinematicState(const std::vector<double>& joint_state_values);
  
  bool setKinematicState(const std::map<std::string, double>& joint_state_map);

  bool setKinematicState(const std::map<std::string, double>& joint_state_map,
                         std::vector<std::string>& missing_links);

  void getKinematicStateValues(std::vector<double>& joint_state_values) const;

  void getKinematicStateValues(std::map<std::string, double>& joint_state_values) const;

  void updateKinematicLinks();

  bool updateKinematicStateWithLinkAt(const std::string& link_name, const tf::Transform& transform);

  const KinematicModel* getKinematicModel() const 
  {
    return kinematic_model_;
  } 

  unsigned int getDimension() const {
    return dimension_;
  }
  
  std::vector<LinkState*> getChildLinkStates(const std::string& link_name) const;

  void setKinematicStateToDefault();

  bool areJointsWithinBounds(const std::vector<std::string>& joints) const;

  bool isJointWithinBounds(const std::string& joint) const;

  const JointStateGroup* getJointStateGroup(const std::string &name) const;

  JointStateGroup* getJointStateGroup(const std::string &name);
        
  bool hasJointStateGroup(const std::string &name) const;

  bool hasJointState(const std::string &joint) const;
  
  bool hasLinkState(const std::string& joint) const;

  JointState* getJointState(const std::string &joint) const;

  LinkState* getLinkState(const std::string &link) const;

  AttachedBodyState* getAttachedBodyState(const std::string &attached) const;

  const std::vector<JointState*>& getJointStateVector() const 
  {
    return joint_state_vector_;
  }

  std::vector<JointState*>& getJointStateVector()
  {
    return joint_state_vector_;
  }

  const std::vector<LinkState*>& getLinkStateVector() const 
  {
    return link_state_vector_;
  } 
  
  const std::vector<const AttachedBodyState*>& getAttachedBodyStateVector() const
  {
    return attached_body_state_vector_;
  }
  
  const std::map<std::string, JointStateGroup*>& getJointStateGroupMap() const
  {
    return joint_state_group_map_;
  }

  void getJointStateGroupNames(std::vector<std::string>& names) const;
                
  const std::map<std::string, unsigned int> getKinematicStateIndexMap() const
  {
    return kinematic_state_index_map_;
  }

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

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

  void printTransform(const std::string &st, const tf::Transform &t, std::ostream &out = std::cout) const;

  const tf::Transform& getRootTransform() const;
  
private:

  void setLinkStatesParents();

  const KinematicModel* kinematic_model_;

  unsigned int dimension_;
  std::map<std::string, unsigned int> kinematic_state_index_map_;

  std::vector<JointState*> joint_state_vector_;
  std::map<std::string, JointState*> joint_state_map_;

  std::vector<LinkState*> link_state_vector_;
  std::map<std::string, LinkState*> link_state_map_;

  //vector of bodies, owned by states
  std::vector<const AttachedBodyState*> attached_body_state_vector_;
  
  std::map<std::string, JointStateGroup*> joint_state_group_map_;
};

}

#endif