Go to the documentation of this file.00001 #include <vigir_footstep_planning_default_plugins/post_processor/step_dynamics_post_process.h>
00002
00003 #include <tf/tf.h>
00004
00005
00006
00007 namespace vigir_footstep_planning
00008 {
00009 StepDynamicsPostProcess::StepDynamicsPostProcess()
00010 : PostProcessPlugin("step_dynamics_post_processor")
00011 {
00012 }
00013
00014 bool StepDynamicsPostProcess::loadParams(const vigir_generic_params::ParameterSet& params)
00015 {
00016 if (!PostProcessPlugin::loadParams(params))
00017 return false;
00018
00019 params.getParam("swing_height", default_swing_height);
00020 params.getParam("sway_duration", default_sway_duration);
00021 params.getParam("step_duration", default_step_duration);
00022 return true;
00023 }
00024
00025 void StepDynamicsPostProcess::postProcessStepForward(const State& left_foot, const State& right_foot, State& swing_foot) const
00026 {
00027 const State& swing_foot_before = swing_foot.getLeg() == LEFT ? left_foot : right_foot;
00028 const State& stand_foot = swing_foot.getLeg() == LEFT ? right_foot : left_foot;
00029
00030 determineStepAttributes(swing_foot_before, stand_foot, swing_foot);
00031 determineTravelDistance(swing_foot_before, stand_foot, swing_foot);
00032 determineTimings(swing_foot_before, stand_foot, swing_foot);
00033 determineDynamics(swing_foot_before, stand_foot, swing_foot);
00034 }
00035
00036 void StepDynamicsPostProcess::postProcessStepBackward(const State& , const State& , State& ) const
00037 {
00038 ROS_WARN_ONCE("[StepDynamicsPostProcessPlugin] postProcessStepBackward not implemented yet!");
00039 }
00040
00041 void StepDynamicsPostProcess::determineStepAttributes(const State& , const State& , State& swing_foot_after) const
00042 {
00043 swing_foot_after.setSwingHeight(default_swing_height);
00044 }
00045
00046 void StepDynamicsPostProcess::determineTravelDistance(const State& swing_foot_before, const State& stand_foot, State& swing_foot_after) const
00047 {
00048
00049 tf::Transform dsway;
00050 getDeltaStep(stand_foot.getPose(), swing_foot_after.getPose(), dsway);
00051 swing_foot_after.setSwayDistance(sqrt(dsway.getOrigin().x()*dsway.getOrigin().x() + dsway.getOrigin().y()*dsway.getOrigin().y() + dsway.getOrigin().z()*dsway.getOrigin().z()));
00052
00053
00054 tf::Transform dswing;
00055 getDeltaStep(swing_foot_before.getPose(), swing_foot_after.getPose(), dswing);
00056 swing_foot_after.setSwingDistance(sqrt(dswing.getOrigin().x()*dswing.getOrigin().x() + dswing.getOrigin().y()*dswing.getOrigin().y() + dswing.getOrigin().z()*dswing.getOrigin().z()));
00057 }
00058
00059 void StepDynamicsPostProcess::determineTimings(const State& , const State& , State& swing_foot_after) const
00060 {
00061 swing_foot_after.setSwayDuration(default_sway_duration);
00062 swing_foot_after.setStepDuration(default_step_duration);
00063 }
00064
00065 void StepDynamicsPostProcess::determineDynamics(const State& swing_foot_before, const State& , State& swing_foot_after) const
00066 {
00067 if (swing_foot_after.getSwingDistance() > 0.0)
00068 {
00069 geometry_msgs::Vector3 body_vel;
00070
00071
00072
00073
00074 double norm_factor = swing_foot_after.getStepDuration() > 0.0 ? 0.5/swing_foot_after.getStepDuration() : 0.0;
00075
00076 body_vel.x = (swing_foot_after.getX() - swing_foot_before.getX()) * norm_factor;
00077 body_vel.y = (swing_foot_after.getY() - swing_foot_before.getY()) * norm_factor;
00078 body_vel.z = (swing_foot_after.getZ() - swing_foot_before.getZ()) * norm_factor;
00079
00080 swing_foot_after.setBodyVelocity(body_vel);
00081 }
00082 }
00083 }
00084
00085 #include <pluginlib/class_list_macros.h>
00086 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::StepDynamicsPostProcess, vigir_footstep_planning::PostProcessPlugin)