state.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2017, Alexander Stumpf, TU Darmstadt
00003 // Based on http://wiki.ros.org/footstep_planner by Johannes Garimort and Armin Hornung
00004 // All rights reserved.
00005 
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00014 //       group, TU Darmstadt nor the names of its contributors may be used to
00015 //       endorse or promote products derived from this software without
00016 //       specific prior written permission.
00017 
00018 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00022 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00023 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00025 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00026 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00027 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 //=================================================================================================
00029 
00030 #ifndef FOOTSTEP_PLANNER_STATE_H__
00031 #define FOOTSTEP_PLANNER_STATE_H__
00032 
00033 #include <geometry_msgs/Pose.h>
00034 #include <geometry_msgs/Vector3.h>
00035 
00036 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
00037 
00038 #include <vigir_footstep_planning_lib/math.h>
00039 
00040 
00041 
00042 namespace vigir_footstep_planning
00043 {
00049 class State
00050 {
00051 public:
00052   State();
00053   State(double x, double y, double z, double roll, double pitch, double yaw, Leg leg);
00054   State(const geometry_msgs::Vector3& position, double roll, double pitch, double yaw, Leg leg);
00055   State(const geometry_msgs::Vector3& position, const geometry_msgs::Vector3& normal, double yaw, Leg leg);
00056   State(const geometry_msgs::Pose& pose, Leg leg);
00057   State(const tf::Transform& t, Leg leg);
00058   State(const msgs::Foot foot);
00059   State(const msgs::Step step);
00060 
00061   ~State();
00062 
00067   bool operator==(const State& s2) const;
00068 
00073   bool operator!=(const State& s2) const;
00074 
00075   void setX(double x) { ivPose.getOrigin().setX(x); }
00076   void setY(double y) { ivPose.getOrigin().setY(y); }
00077   void setZ(double z) { ivPose.getOrigin().setZ(z); }
00078   void setPosition(const geometry_msgs::Point& position);
00079   void setRoll(double roll) { setRPY(roll, ivPitch, ivYaw); }
00080   void setPitch(double pitch) { setRPY(ivRoll, pitch, ivYaw); }
00081   void setYaw(double yaw) { setRPY(ivRoll, ivPitch, yaw); }
00082   void setRPY(double roll, double pitch, double yaw);
00083   void setOrientation(const geometry_msgs::Quaternion& q);
00084   void setNormal(const geometry_msgs::Vector3 &normal);
00085   void setNormal(double x, double y, double z);
00086   void setSwingHeight(double swing_height) { ivSwingHeight = swing_height; }
00087   void setSwayDuration(double sway_duration) { ivSwayDuration = sway_duration; }
00088   void setStepDuration(double step_duration) { ivStepDuration = step_duration; }
00089   void setSwayDistance(double sway_distance) { this->sway_distance = sway_distance; }
00090   void setSwingDistance(double swing_distance) { this->swing_distance = swing_distance; }
00091   void setLeg(Leg leg) { ivLeg = leg; }
00092   void setGroundContactSupport(double ground_contact_support) { ivGroundContactSupport = ground_contact_support; }
00093   void setBodyVelocity(const geometry_msgs::Vector3& body_vel) { this->body_vel = body_vel; }
00094   void setCost(double cost) { this->cost = cost; }
00095   void setRisk(double risk) { this->risk = risk; }
00096 
00097   double getX() const { return ivPose.getOrigin().getX(); }
00098   double getY() const { return ivPose.getOrigin().getY(); }
00099   double getZ() const { return ivPose.getOrigin().getZ(); }
00100   double getRoll() const { return ivRoll; }
00101   double getPitch() const { return ivPitch; }
00102   double getYaw() const { return ivYaw; }
00103   const geometry_msgs::Vector3& getNormal() const { return ivNormal; }
00104   double getNormalX() const { return ivNormal.x; }
00105   double getNormalY() const { return ivNormal.y; }
00106   double getNormalZ() const { return ivNormal.z; }
00107   double getSwingHeight() const { return ivSwingHeight; }
00108   double getSwayDuration() const { return ivSwayDuration; }
00109   double getStepDuration() const { return ivStepDuration; }
00110   double getSwayDistance() const { return sway_distance; }
00111   double getSwingDistance() const { return swing_distance; }
00112   Leg getLeg() const { return ivLeg; }
00113   double getGroundContactSupport() const { return ivGroundContactSupport; }
00114   const geometry_msgs::Vector3& getBodyVelocity() const { return body_vel; }
00115   double getCost() const { return cost; }
00116   double getRisk() const { return risk; }
00117 
00118   const tf::Pose &getPose() const { return ivPose; }
00119   tf::Pose &getPose() { return ivPose; }
00120   void getFoot(msgs::Foot& foot) const;
00121   void getStep(msgs::Step& step) const;
00122 
00123 private:
00124   void recomputeNormal();
00125 
00126   tf::Pose ivPose;
00127 
00129   Leg ivLeg;
00130 
00132   double ivRoll;
00133   double ivPitch;
00134   double ivYaw;
00136   geometry_msgs::Vector3 ivNormal;
00137   double ivSwingHeight;
00138   double ivSwayDuration;
00139   double ivStepDuration;
00140 
00142   double ivGroundContactSupport;
00143 
00144   geometry_msgs::Vector3 body_vel;
00145 
00146   double sway_distance;
00147   double swing_distance;
00148 
00149   double cost;
00150   double risk;
00151 };
00152 }
00153 #endif


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56