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)