state.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_lib/modeling/state.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 State::State()
00006   : ivLeg(NOLEG)
00007   , ivRoll(0.0)
00008   , ivPitch(0.0)
00009   , ivYaw(0.0)
00010   , ivSwingHeight(0.0)
00011   , ivSwayDuration(0.0)
00012   , ivStepDuration(0.0)
00013   , ivGroundContactSupport(0.0)
00014   , body_vel(geometry_msgs::Vector3())
00015   , sway_distance(0.0)
00016   , swing_distance(0.0)
00017   , cost(0.0)
00018   , risk(0.0)
00019 {
00020   ivPose = tf::Pose();
00021 
00022   ivNormal.x = 0.0;
00023   ivNormal.y = 0.0;
00024   ivNormal.z = 1.0;
00025 }
00026 
00027 State::State(double x, double y, double z, double roll, double pitch, double yaw, Leg leg)
00028   : ivLeg(leg)
00029   , ivRoll(roll)
00030   , ivPitch(pitch)
00031   , ivYaw(yaw)
00032   , ivSwingHeight(0.0)
00033   , ivSwayDuration(0.0)
00034   , ivStepDuration(0.0)
00035   , ivGroundContactSupport(1.0)
00036   , body_vel(geometry_msgs::Vector3())
00037   , sway_distance(0.0)
00038   , swing_distance(0.0)
00039   , cost(0.0)
00040   , risk(0.0)
00041 {
00042   ivPose.setOrigin(tf::Vector3(x, y, z));
00043   ivPose.getBasis().setRPY(ivRoll, ivPitch, ivYaw);
00044 
00045   recomputeNormal();
00046 }
00047 
00048 State::State(const geometry_msgs::Vector3& position, double roll, double pitch, double yaw, Leg leg)
00049   : State(position.x, position.y, position.z, roll, pitch, yaw, leg)
00050 {
00051 }
00052 
00053 State::State(const geometry_msgs::Vector3& position, const geometry_msgs::Vector3& normal, double yaw, Leg leg)
00054   : ivLeg(leg)
00055   , ivYaw(yaw)
00056   , ivSwingHeight(0.0)
00057   , ivSwayDuration(0.0)
00058   , ivStepDuration(0.0)
00059   , ivGroundContactSupport(1.0)
00060   , body_vel(geometry_msgs::Vector3())
00061   , sway_distance(0.0)
00062   , swing_distance(0.0)
00063   , cost(0.0)
00064   , risk(0.0)
00065 {
00066   ivPose.setOrigin(tf::Vector3(position.x, position.y, position.z));
00067 
00068   setNormal(normal);
00069 }
00070 
00071 State::State(const geometry_msgs::Pose& pose, Leg leg)
00072   : ivLeg(leg)
00073   , ivSwingHeight(0.0)
00074   , ivSwayDuration(0.0)
00075   , ivStepDuration(0.0)
00076   , ivGroundContactSupport(1.0)
00077   , body_vel(geometry_msgs::Vector3())
00078   , sway_distance(0.0)
00079   , swing_distance(0.0)
00080   , cost(0.0)
00081   , risk(0.0)
00082 {
00083   tf::poseMsgToTF(pose, ivPose);
00084   ivPose.getBasis().getRPY(ivRoll, ivPitch, ivYaw);
00085 
00086   recomputeNormal();
00087 }
00088 
00089 State::State(const tf::Transform& t, Leg leg)
00090   : ivLeg(leg)
00091   , ivSwingHeight(0.0)
00092   , ivSwayDuration(0.0)
00093   , ivStepDuration(0.0)
00094   , ivGroundContactSupport(1.0)
00095   , body_vel(geometry_msgs::Vector3())
00096   , sway_distance(0.0)
00097   , swing_distance(0.0)
00098   , cost(0.0)
00099   , risk(0.0)
00100 {
00101   ivPose = t;
00102   ivPose.getBasis().getRPY(ivRoll, ivPitch, ivYaw);
00103 
00104   recomputeNormal();
00105 }
00106 
00107 State::State(const msgs::Foot foot)
00108   : State(foot.pose, foot.foot_index == msgs::Foot::LEFT ? LEFT : RIGHT)
00109 {
00110 }
00111 
00112 State::State(const msgs::Step step)
00113   : State(step.foot)
00114 {
00115   ivSwingHeight = step.swing_height;
00116   ivSwayDuration = step.sway_duration;
00117   ivStepDuration = step.step_duration;
00118 }
00119 
00120 State::~State()
00121 {}
00122 
00123 bool State::operator==(const State& s2) const
00124 {
00125   return (fabs(getX() - s2.getX()) < FLOAT_CMP_THR &&
00126           fabs(getY() - s2.getY()) < FLOAT_CMP_THR &&
00127           fabs(getZ() - s2.getZ()) < FLOAT_CMP_THR &&
00128           fabs(angles::shortest_angular_distance(ivRoll, s2.ivRoll)) < FLOAT_CMP_THR &&
00129           fabs(angles::shortest_angular_distance(ivPitch, s2.ivPitch)) < FLOAT_CMP_THR &&
00130           fabs(angles::shortest_angular_distance(ivYaw, s2.ivYaw)) < FLOAT_CMP_THR &&
00131           ivLeg == s2.getLeg());
00132 }
00133 
00134 bool State::operator!=(const State& s2) const
00135 {
00136   return not (*this == s2);
00137 }
00138 
00139 void State::setPosition(const geometry_msgs::Point& position)
00140 {
00141   setX(position.x);
00142   setY(position.y);
00143   setZ(position.z);
00144 }
00145 
00146 void State::setOrientation(const geometry_msgs::Quaternion& q)
00147 {
00148   double roll, pitch, yaw;
00149   tf::Quaternion q_tf;
00150   tf::quaternionMsgToTF(q, q_tf);
00151   tf::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);
00152   setRPY(roll, pitch, yaw);
00153 }
00154 
00155 void State::setRPY(double roll, double pitch, double yaw)
00156 {
00157   ivRoll = roll;
00158   ivPitch = pitch;
00159   ivYaw = yaw;
00160 
00161   ivPose.getBasis().setRPY(ivRoll, ivPitch, ivYaw);
00162   recomputeNormal();
00163 }
00164 
00165 void State::setNormal(const geometry_msgs::Vector3 &normal)
00166 {
00167   ivNormal = normal;
00168 
00169   if (ivNormal.z > 0.99)
00170   {
00171     ivNormal.x = 0.0;
00172     ivNormal.y = 0.0;
00173     ivNormal.z = 1.0;
00174     ivRoll = 0.0;
00175     ivPitch = 0.0;
00176   }
00177   else
00178   {
00179     // get roll and pitch
00180     normalToRP(ivNormal, ivYaw, ivRoll, ivPitch);
00181   }
00182 
00183   ivPose.getBasis().setRPY(ivRoll, ivPitch, ivYaw);
00184 }
00185 
00186 void State::setNormal(double x, double y, double z)
00187 {
00188   geometry_msgs::Vector3 n;
00189   n.x = x;
00190   n.y = y;
00191   n.z = z;
00192   setNormal(n);
00193 }
00194 
00195 void State::getStep(msgs::Step &step) const
00196 {
00197   step.step_index = 0;
00198   getFoot(step.foot);
00199   step.sway_duration = ivSwayDuration;
00200   step.step_duration = ivStepDuration;
00201   step.swing_height = ivSwingHeight;
00202   step.cost = cost;
00203   step.risk = risk;
00204 }
00205 
00206 void State::getFoot(msgs::Foot& foot) const
00207 {
00208   foot.foot_index = ivLeg == LEFT ? static_cast<uint8_t>(msgs::Foot::LEFT) : static_cast<uint8_t>(msgs::Foot::RIGHT);
00209   foot.pose.position.x = getX();
00210   foot.pose.position.y = getY();
00211   foot.pose.position.z = getZ();
00212   normalToQuaternion(ivNormal, ivYaw, foot.pose.orientation);
00213 }
00214 
00215 void State::recomputeNormal()
00216 {
00217   if (std::abs(ivRoll) < 0.01 && std::abs(ivPitch) < 0.01)
00218   {
00219     ivNormal.x = 0.0;
00220     ivNormal.y = 0.0;
00221     ivNormal.z = 1.0;
00222   }
00223   else
00224   {
00225     // get normal
00226     RPYToNormal(ivRoll, ivPitch, ivYaw, ivNormal);
00227   }
00228 }
00229 
00230 } // end of namespace


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