10 : PostProcessPlugin(
"step_dynamics_post_processor")
16 if (!PostProcessPlugin::loadParams(params))
27 const State& swing_foot_before = swing_foot.getLeg() == LEFT ? left_foot : right_foot;
28 const State& stand_foot = swing_foot.getLeg() == LEFT ? right_foot : left_foot;
38 ROS_WARN_ONCE(
"[StepDynamicsPostProcessPlugin] postProcessStepBackward not implemented yet!");
50 getDeltaStep(stand_foot.getPose(), swing_foot_after.getPose(), dsway);
51 swing_foot_after.setSwayDistance(sqrt(dsway.getOrigin().x()*dsway.getOrigin().x() + dsway.getOrigin().y()*dsway.getOrigin().y() + dsway.getOrigin().z()*dsway.getOrigin().z()));
55 getDeltaStep(swing_foot_before.getPose(), swing_foot_after.getPose(), dswing);
56 swing_foot_after.setSwingDistance(sqrt(dswing.getOrigin().x()*dswing.getOrigin().x() + dswing.getOrigin().y()*dswing.getOrigin().y() + dswing.getOrigin().z()*dswing.getOrigin().z()));
67 if (swing_foot_after.getSwingDistance() > 0.0)
69 geometry_msgs::Vector3 body_vel;
74 double norm_factor = swing_foot_after.getStepDuration() > 0.0 ? 0.5/swing_foot_after.getStepDuration() : 0.0;
76 body_vel.x = (swing_foot_after.getX() - swing_foot_before.getX()) * norm_factor;
77 body_vel.y = (swing_foot_after.getY() - swing_foot_before.getY()) * norm_factor;
78 body_vel.z = (swing_foot_after.getZ() - swing_foot_before.getZ()) * norm_factor;
80 swing_foot_after.setBodyVelocity(body_vel);
85 #include <pluginlib/class_list_macros.h>
#define ROS_WARN_ONCE(...)