hrl_kinematics: hrl_kinematics: TestStability.h Source File

00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/hrl_kinematics/include/hrl_kinematics/TestStability.h $
00002 // SVN $Id: TestStability.h 3069 2012-08-21 14:26:59Z hornunga@informatik.uni-freiburg.de $
00003 
00004 /*
00005  * hrl_kinematics - a kinematics library for humanoid robots based on KDL
00006  *
00007  * Copyright 2011-2012 Armin Hornung, University of Freiburg
00008  * License: BSD
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions are met:
00012  *
00013  *     * Redistributions of source code must retain the above copyright
00014  *       notice, this list of conditions and the following disclaimer.
00015  *     * Redistributions in binary form must reproduce the above copyright
00016  *       notice, this list of conditions and the following disclaimer in the
00017  *       documentation and/or other materials provided with the distribution.
00018  *     * Neither the name of the University of Freiburg nor the names of its
00019  *       contributors may be used to endorse or promote products derived from
00020  *       this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 
00036 #ifndef HRL_KINEMATICS_TESTSTABILITY_H_
00037 #define HRL_KINEMATICS_TESTSTABILITY_H_
00038 
00039 #include <hrl_kinematics/Kinematics.h>
00040 
00041 #include <geometric_shapes/shape_operations.h>
00042 #include <geometry_msgs/PolygonStamped.h>
00043 
00044 #include <vector>
00045 
00046 namespace hrl_kinematics {
00047 
00054 class TestStability : public Kinematics{
00055 public:
00056   TestStability();
00057   virtual ~TestStability();
00058 
00067   bool isPoseStable(const std::map<std::string, double>& joint_positions, FootSupport support_mode);
00068 
00078   bool isPoseStable(const std::map<std::string, double>& joint_positions, FootSupport support_mode, const tf::Vector3& normal_vector);
00079 
00081   geometry_msgs::PolygonStamped getSupportPolygon() const;
00083   visualization_msgs::Marker getCOMMarker() const;
00085   tf::Point getCOM() const {return p_com_;}
00086 
00087 protected:
00088   void initFootPolygon();
00089   bool loadFootPolygon();
00090   std::vector<tf::Point> convexHull(const std::vector<tf::Point>& points) const;
00092   bool pointInConvexHull(const tf::Point& point, const std::vector<tf::Point>& polygon) const;
00094   std::vector<tf::Point> foot_support_polygon_right_;
00096   std::vector<tf::Point> foot_support_polygon_left_;
00097 
00098   tf::Point p_com_;
00099   std::vector<tf::Point> support_polygon_;
00100   tf::Transform tf_to_support_;
00101   std::string rfoot_mesh_link_name;
00102 };
00103 
00104 } /* namespace hrl_kinematics */
00105 #endif