TestStability.h
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
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 
00080   void scaleConvexHull(double scale);
00081 
00083   geometry_msgs::PolygonStamped getSupportPolygon() const;
00085   visualization_msgs::Marker getCOMMarker() const;
00087   tf::Point getCOM() const {return p_com_;}
00088 
00089 protected:
00090   void initFootPolygon(double scale=1.0);
00091   bool loadFootPolygon();
00092   std::vector<tf::Point> convexHull(const std::vector<tf::Point>& points) const;
00094   bool pointInConvexHull(const tf::Point& point, const std::vector<tf::Point>& polygon) const;
00096   std::vector<tf::Point> foot_support_polygon_right_;
00098   std::vector<tf::Point> foot_support_polygon_left_;
00099 
00100   tf::Point p_com_;
00101   std::vector<tf::Point> support_polygon_;
00102   tf::Transform tf_to_support_;
00103   std::string rfoot_mesh_link_name;
00104 
00105   //Convex Hull scaling factor
00106   double scale_convex_hull_;
00107 };
00108 
00109 } /* namespace hrl_kinematics */
00110 #endif


hrl_kinematics
Author(s): Armin Hornung
autogenerated on Mon Oct 6 2014 00:39:32