dynamics_reachability.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_default_plugins/robot_model/dynamics_reachability.h>
00002 
00003 
00004 
00005 namespace vigir_footstep_planning
00006 {
00007 DynamicsReachability::DynamicsReachability()
00008   : ReachabilityPlugin("dynamics_reachability")
00009 {
00010 }
00011 
00012 bool DynamicsReachability::loadParams(const ParameterSet& params)
00013 {
00014   if (!ReachabilityPlugin::loadParams(params))
00015     return false;
00016 
00017   params.getParam("dynamics/body/max_vel", max_body_vel_sq, 0.0);
00018   max_body_vel_sq *= max_body_vel_sq;
00019   params.getParam("dynamics/body/max_acc", max_body_acc_sq, 0.0);
00020   max_body_acc_sq *= max_body_acc_sq;
00021   return true;
00022 }
00023 
00024 bool DynamicsReachability::isReachable(const State& current, const State& next) const
00025 {
00026   if (max_body_vel_sq > 0.0)
00027   {
00028     const geometry_msgs::Vector3& v = next.getBodyVelocity();
00029     if ((v.x*v.x + v.y*v.y + v.z*v.z) > max_body_vel_sq)
00030       return false;
00031   }
00032 
00033   if (max_body_acc_sq > 0.0 && next.getStepDuration() > 0.0)
00034   {
00035     const geometry_msgs::Vector3& v0 = current.getBodyVelocity();
00036     const geometry_msgs::Vector3& v1 = next.getBodyVelocity();
00037 
00038     geometry_msgs::Vector3 acc;
00039     acc.x = (v1.x-v0.x)/next.getStepDuration();
00040     acc.y = (v1.y-v0.y)/next.getStepDuration();
00041     acc.z = (v1.z-v0.z)/next.getStepDuration();
00042 
00043     if ((acc.x*acc.x + acc.y*acc.y + acc.z*acc.z) > max_body_acc_sq)
00044       return false;
00045   }
00046 
00047   return true;
00048 }
00049 }
00050 
00051 #include <pluginlib/class_list_macros.h>
00052 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::DynamicsReachability, vigir_footstep_planning::ReachabilityPlugin)


vigir_footstep_planning_default_plugins
Author(s): Alexander Stumpf
autogenerated on Thu Jun 6 2019 18:38:06