kinematic_state_constraint_evaluator.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #ifndef PLANNING_ENVIRONMENT_UTIL_KINEMATIC_STATE_CONSTRAINT_EVALUATOR_
00038 #define PLANNING_ENVIRONMENT_UTIL_KINEMATIC_STATE_CONSTRAINT_EVALUATOR_
00039 
00040 #include <planning_models/kinematic_model.h>
00041 #include <planning_models/kinematic_state.h>
00042 #include <arm_navigation_msgs/Constraints.h>
00043 #include <arm_navigation_msgs/Shape.h>
00044 #include <geometric_shapes/bodies.h>
00045 #include <geometry_msgs/Pose.h>
00046 #include <iostream>
00047 #include <vector>
00048 
00049 #include <boost/scoped_ptr.hpp>
00050 
00051 namespace planning_environment
00052 {
00053 bool createConstraintRegionFromMsg(const arm_navigation_msgs::Shape &constraint_region_shape, 
00054                                    const geometry_msgs::Pose &constraint_region_pose, 
00055                                    boost::scoped_ptr<bodies::Body> &body);
00056 
00057 class KinematicConstraintEvaluator
00058 {
00059 public:
00060         
00061   KinematicConstraintEvaluator(void)
00062   {
00063   }
00064         
00065   virtual ~KinematicConstraintEvaluator(void)
00066   {
00067   }
00068         
00070   virtual void clear(void) = 0;
00071 
00073   virtual bool decide(const planning_models::KinematicState *state,
00074                       bool verbose=false) const = 0;
00075 
00077   virtual void print(std::ostream &out = std::cout) const
00078   {
00079   }
00080 };
00081     
00082 class JointConstraintEvaluator : public KinematicConstraintEvaluator
00083 {
00084 public:  
00085 
00086   JointConstraintEvaluator(void) : KinematicConstraintEvaluator()
00087   {
00088     m_joint = NULL;
00089   }
00090         
00092   bool use(const arm_navigation_msgs::JointConstraint &jc);
00093 
00095   virtual bool decide(const planning_models::KinematicState  *state,
00096                       bool verbose=false) const;
00097 
00099   virtual void clear(void);
00100 
00102   virtual void print(std::ostream &out = std::cout) const;
00103 
00105   const arm_navigation_msgs::JointConstraint& getConstraintMessage(void) const;
00106 
00107 protected:
00108         
00109   arm_navigation_msgs::JointConstraint         m_jc;
00110   const planning_models::KinematicModel::JointModel *m_joint;    
00111 };
00112     
00113         
00114 class OrientationConstraintEvaluator : public KinematicConstraintEvaluator
00115 {
00116 public:
00117         
00118   OrientationConstraintEvaluator(void) : KinematicConstraintEvaluator()
00119   {
00120   }
00121         
00123   bool use(const arm_navigation_msgs::OrientationConstraint &pc);
00124 
00126   virtual void clear(void);
00127         
00129   virtual bool decide(const planning_models::KinematicState* state, 
00130                       bool verbose=false) const;
00131 
00133   void evaluate(const planning_models::KinematicState* state, double &distAng, bool verbose=false) const;
00134   
00136   bool decide(double dAng, bool verbose=false) const;
00137   
00139   void print(std::ostream &out = std::cout) const;
00140 
00142   const arm_navigation_msgs::OrientationConstraint& getConstraintMessage(void) const;
00143         
00144 protected:
00145         
00146   arm_navigation_msgs::OrientationConstraint  m_oc;
00147   double m_roll, m_pitch, m_yaw;
00148   tf::Matrix3x3 m_rotation_matrix;
00149   boost::scoped_ptr<bodies::Body> m_constraint_region;
00150         
00151 };
00152 
00153 class VisibilityConstraintEvaluator : public KinematicConstraintEvaluator
00154 {
00155 public:
00156         
00157   VisibilityConstraintEvaluator(void) : KinematicConstraintEvaluator()
00158   {
00159   }
00160         
00162   bool use(const arm_navigation_msgs::VisibilityConstraint &vc);
00163 
00165   virtual void clear(void);
00166         
00168   virtual bool decide(const planning_models::KinematicState* state,
00169                       bool verbose=false) const;
00170 
00172   void print(std::ostream &out = std::cout) const;
00173 
00175   const arm_navigation_msgs::VisibilityConstraint& getConstraintMessage(void) const;
00176         
00177 protected:      
00178   arm_navigation_msgs::VisibilityConstraint  m_vc;
00179   tf::Transform m_sensor_offset_pose;
00180 };
00181 
00182 class PositionConstraintEvaluator : public KinematicConstraintEvaluator
00183 {
00184 public:
00185         
00186   PositionConstraintEvaluator(void) : KinematicConstraintEvaluator()
00187   {
00188   }
00189         
00191   bool use(const arm_navigation_msgs::PositionConstraint &pc);
00192 
00194   virtual void clear(void);
00195         
00197   virtual bool decide(const planning_models::KinematicState* state,
00198                       bool verbose=false) const;
00199 
00201   void evaluate(const planning_models::KinematicState* state, double& distPos, bool verbose=false) const;
00202         
00204   bool decide(double dPos, bool verbose=false) const;
00205 
00207   void print(std::ostream &out = std::cout) const;
00208 
00210   const arm_navigation_msgs::PositionConstraint& getConstraintMessage(void) const;
00211         
00212 protected:
00213         
00214   arm_navigation_msgs::PositionConstraint     m_pc;
00215   double                                       m_x, m_y, m_z;
00216   tf::Vector3                                    m_offset;
00217   boost::scoped_ptr<bodies::Body> m_constraint_region;
00218 };
00219         
00220 class KinematicConstraintEvaluatorSet
00221 {
00222 public:
00223         
00224   KinematicConstraintEvaluatorSet(void)
00225   {
00226   }
00227         
00228   ~KinematicConstraintEvaluatorSet(void)
00229   {
00230     clear();
00231   }
00232         
00234   void clear(void);
00235         
00237   bool add(const std::vector<arm_navigation_msgs::JointConstraint> &jc);
00238 
00240   bool add(const std::vector<arm_navigation_msgs::PositionConstraint> &pc);
00241 
00243   bool add(const std::vector<arm_navigation_msgs::OrientationConstraint> &pc);
00244 
00246   bool add(const std::vector<arm_navigation_msgs::VisibilityConstraint> &pc);
00247         
00249   bool decide(const planning_models::KinematicState* state,
00250               bool verbose=false) const;
00251 
00253   void print(std::ostream &out = std::cout) const;
00254         
00256   const std::vector<arm_navigation_msgs::PositionConstraint>& getPositionConstraints(void) const
00257   {
00258     return m_pc;
00259   }
00260 
00262   const std::vector<arm_navigation_msgs::OrientationConstraint>& getOrientationConstraints(void) const
00263   {
00264     return m_oc;
00265   }
00266 
00268   const std::vector<arm_navigation_msgs::JointConstraint>& getJointConstraints(void) const
00269   {
00270     return m_jc;
00271   }
00272         
00273 protected:
00274         
00275   std::vector<KinematicConstraintEvaluator*>         m_kce;
00276   std::vector<arm_navigation_msgs::JointConstraint> m_jc;
00277 
00278   std::vector<arm_navigation_msgs::PositionConstraint>  m_pc;
00279   std::vector<arm_navigation_msgs::OrientationConstraint>  m_oc;
00280   std::vector<arm_navigation_msgs::VisibilityConstraint> m_vc;
00281 };
00282 } // planning_environment
00283 
00284 
00285 #endif


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43