diff --git a/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp index b09847137..4ac27db32 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp @@ -107,8 +107,8 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.x()->addPoint(0, flying_foot_pose.translation().x()); flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk, 0); flying_foot_spline_.x()->addPoint(phase_timings_.raise_foot, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0, 0); - flying_foot_spline_.x()->addPoint(phase_timings_.kick, ball_position_.x(), speed_vector.x() * kick_speed_, 0); + flying_foot_spline_.x()->addPoint(phase_timings_.windup, windup_point_.x(), 0); + flying_foot_spline_.x()->addPoint(phase_timings_.kick, ball_position_.x(), speed_vector.x() * kick_speed_); flying_foot_spline_.x()->addPoint(phase_timings_.move_back, 0); flying_foot_spline_.x()->addPoint(phase_timings_.lower_foot, 0); flying_foot_spline_.x()->addPoint(phase_timings_.move_trunk_back, 0); @@ -116,8 +116,8 @@ void KickEngine::calcSplines(const Eigen::Isometry3d &flying_foot_pose, const Ei flying_foot_spline_.y()->addPoint(0, flying_foot_pose.translation().y()); flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk, flying_foot_pose.translation().y()); flying_foot_spline_.y()->addPoint(phase_timings_.raise_foot, kick_foot_sign * params_.foot_distance); - flying_foot_spline_.y()->addPoint(phase_timings_.windup, windup_point_.y(), 0, 0); - flying_foot_spline_.y()->addPoint(phase_timings_.kick, ball_position_.y(), speed_vector.y() * kick_speed_, 0); + flying_foot_spline_.y()->addPoint(phase_timings_.windup, windup_point_.y(), 0); + flying_foot_spline_.y()->addPoint(phase_timings_.kick, ball_position_.y(), speed_vector.y() * kick_speed_); flying_foot_spline_.y()->addPoint(phase_timings_.move_back, kick_foot_sign * params_.foot_distance); flying_foot_spline_.y()->addPoint(phase_timings_.lower_foot, kick_foot_sign * params_.foot_distance); flying_foot_spline_.y()->addPoint(phase_timings_.move_trunk_back, flying_foot_pose.translation().y()); diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp index 331dcc627..946a6a430 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp @@ -468,8 +468,7 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { is_left_support_foot_spline_.addPoint(half_period, is_left_support_foot_); // Flying foot position - foot_spline_.x()->addPoint(0.0, foot_pos_at_foot_change_.x(), foot_pos_vel_at_foot_change_.x(), - foot_pos_acc_at_foot_change_.x()); + foot_spline_.x()->addPoint(0.0, foot_pos_at_foot_change_.x(), foot_pos_vel_at_foot_change_.x()); foot_spline_.x()->addPoint(double_support_length, foot_pos_at_foot_change_.x()); if (kick_step) { foot_spline_.x()->addPoint(double_support_length + single_support_length * config_.kick_phase, @@ -485,8 +484,7 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { support_to_next_.getOrigin().x()); foot_spline_.x()->addPoint(half_period, support_to_next_.getOrigin().x()); - foot_spline_.y()->addPoint(0.0, foot_pos_at_foot_change_.y(), foot_pos_vel_at_foot_change_.y(), - foot_pos_acc_at_foot_change_.y()); + foot_spline_.y()->addPoint(0.0, foot_pos_at_foot_change_.y(), foot_pos_vel_at_foot_change_.y()); foot_spline_.y()->addPoint(double_support_length, foot_pos_at_foot_change_.y()); foot_spline_.y()->addPoint( double_support_length + single_support_length * config_.foot_put_down_phase * config_.foot_overshoot_phase, @@ -496,8 +494,7 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { support_to_next_.getOrigin().y()); foot_spline_.y()->addPoint(half_period, support_to_next_.getOrigin().y()); - foot_spline_.z()->addPoint(0.0, foot_pos_at_foot_change_.z(), foot_pos_vel_at_foot_change_.z(), - foot_pos_acc_at_foot_change_.z()); + foot_spline_.z()->addPoint(0.0, foot_pos_at_foot_change_.z(), foot_pos_vel_at_foot_change_.z()); foot_spline_.z()->addPoint(double_support_length, foot_pos_at_foot_change_.z()); foot_spline_.z()->addPoint(double_support_length + single_support_length * config_.foot_apex_phase - 0.5 * config_.foot_z_pause * single_support_length, @@ -511,15 +508,13 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { // Flying foot orientation foot_spline_.roll()->addPoint(0.0, foot_orientation_pos_at_last_foot_change_.x(), - foot_orientation_vel_at_last_foot_change_.x(), - foot_orientation_acc_at_foot_change_.x()); + foot_orientation_vel_at_last_foot_change_.x()); foot_spline_.roll()->addPoint(double_support_length + 0.1 * single_support_length, 0.0); foot_spline_.roll()->addPoint(double_support_length + single_support_length * config_.foot_put_down_phase, 0.0); foot_spline_.roll()->addPoint(half_period, 0.0); foot_spline_.pitch()->addPoint(0.0, foot_orientation_pos_at_last_foot_change_.y(), - foot_orientation_vel_at_last_foot_change_.y(), - foot_orientation_acc_at_foot_change_.y()); + foot_orientation_vel_at_last_foot_change_.y()); if (!start_movement) { foot_spline_.pitch()->addPoint(double_support_length + single_support_length * config_.foot_apex_phase, config_.foot_apex_pitch); @@ -527,7 +522,7 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { foot_spline_.pitch()->addPoint(half_period, 0.0); foot_spline_.yaw()->addPoint(0.0, foot_orientation_pos_at_last_foot_change_.z(), - foot_orientation_vel_at_last_foot_change_.z(), foot_orientation_acc_at_foot_change_.z()); + foot_orientation_vel_at_last_foot_change_.z()); foot_spline_.yaw()->addPoint(double_support_length, getLastEuler().z()); foot_spline_.yaw()->addPoint(double_support_length + single_support_length * config_.foot_put_down_phase, getNextEuler().z()); @@ -563,19 +558,17 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { // Trunk position if (start_movement || start_step) { - trunk_spline_.x()->addPoint(0.0, 0.0, 0.0, 0.0); + trunk_spline_.x()->addPoint(0.0, 0.0, 0.0); } else { - trunk_spline_.x()->addPoint(0.0, trunk_pos_at_foot_change_.x(), trunk_pos_vel_at_foot_change_.x(), - trunk_pos_acc_at_foot_change_.x()); + trunk_spline_.x()->addPoint(0.0, trunk_pos_at_foot_change_.x(), trunk_pos_vel_at_foot_change_.x()); trunk_spline_.x()->addPoint(half_period + time_shift, trunk_apex_support.x(), trunk_vel_support); } trunk_spline_.x()->addPoint(period + time_shift, trunk_apex_next.x(), trunk_vel_next); if (start_movement) { - trunk_spline_.y()->addPoint(0.0, trunk_point_middle.y(), 0.0, 0.0); + trunk_spline_.y()->addPoint(0.0, trunk_point_middle.y(), 0.0); } else { - trunk_spline_.y()->addPoint(0.0, trunk_pos_at_foot_change_.y(), trunk_pos_vel_at_foot_change_.y(), - trunk_pos_acc_at_foot_change_.y()); + trunk_spline_.y()->addPoint(0.0, trunk_pos_at_foot_change_.y(), trunk_pos_vel_at_foot_change_.y()); } if (start_step || start_movement) { trunk_spline_.y()->addPoint(half_period + time_shift - pause_length, @@ -598,8 +591,7 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { double trunk_height_including_foot_z_movement = config_.trunk_height + std::min(0.0, support_to_next_.getOrigin().z()); // Periodic z movement of trunk is at lowest point at double support center, highest at single support center - trunk_spline_.z()->addPoint(0.0, trunk_pos_at_foot_change_.z(), trunk_pos_vel_at_foot_change_.z(), - trunk_pos_acc_at_foot_change_.z()); + trunk_spline_.z()->addPoint(0.0, trunk_pos_at_foot_change_.z(), trunk_pos_vel_at_foot_change_.z()); trunk_spline_.z()->addPoint(double_support_length / 2, trunk_height_including_foot_z_movement); if (!start_movement) { trunk_spline_.z()->addPoint(double_support_length + single_support_length / 2, @@ -628,20 +620,17 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { // Trunk orientation trunk_spline_.roll()->addPoint(0.0, trunk_orientation_pos_at_last_foot_change_.x(), - trunk_orientation_vel_at_last_foot_change_.x(), - trunk_orientation_acc_at_foot_change_.x()); + trunk_orientation_vel_at_last_foot_change_.x()); trunk_spline_.roll()->addPoint(half_period + time_shift, euler_at_support.x(), axis_vel.x()); trunk_spline_.roll()->addPoint(period + time_shift, euler_at_next.x(), axis_vel.x()); trunk_spline_.pitch()->addPoint(0.0, trunk_orientation_pos_at_last_foot_change_.y(), - trunk_orientation_vel_at_last_foot_change_.y(), - trunk_orientation_acc_at_foot_change_.y()); + trunk_orientation_vel_at_last_foot_change_.y()); trunk_spline_.pitch()->addPoint(half_period + time_shift, euler_at_support.y(), axis_vel.y()); trunk_spline_.pitch()->addPoint(period + time_shift, euler_at_next.y(), axis_vel.y()); trunk_spline_.yaw()->addPoint(0.0, trunk_orientation_pos_at_last_foot_change_.z(), - trunk_orientation_vel_at_last_foot_change_.z(), - trunk_orientation_acc_at_foot_change_.z()); + trunk_orientation_vel_at_last_foot_change_.z()); trunk_spline_.yaw()->addPoint(half_period + time_shift, euler_at_support.z(), axis_vel.z()); trunk_spline_.yaw()->addPoint(period + time_shift, euler_at_next.z(), axis_vel.z()); } @@ -686,8 +675,7 @@ void WalkEngine::buildWalkDisableTrajectories(bool foot_in_idle_position) { is_left_support_foot_spline_.addPoint(half_period, is_left_support_foot_); // Flying foot position - foot_spline_.x()->addPoint(0.0, foot_pos_at_foot_change_.x(), foot_pos_vel_at_foot_change_.x(), - foot_pos_acc_at_foot_change_.x()); + foot_spline_.x()->addPoint(0.0, foot_pos_at_foot_change_.x(), foot_pos_vel_at_foot_change_.x()); foot_spline_.x()->addPoint(double_support_length, foot_pos_at_foot_change_.x()); foot_spline_.x()->addPoint( double_support_length + single_support_length * config_.foot_put_down_phase * config_.foot_overshoot_phase, @@ -695,8 +683,7 @@ void WalkEngine::buildWalkDisableTrajectories(bool foot_in_idle_position) { foot_spline_.x()->addPoint(double_support_length + single_support_length * config_.foot_put_down_phase, 0.0); foot_spline_.x()->addPoint(half_period, 0.0); - foot_spline_.y()->addPoint(0.0, foot_pos_at_foot_change_.y(), foot_pos_vel_at_foot_change_.y(), - foot_pos_acc_at_foot_change_.y()); + foot_spline_.y()->addPoint(0.0, foot_pos_at_foot_change_.y(), foot_pos_vel_at_foot_change_.y()); foot_spline_.y()->addPoint(double_support_length, foot_pos_at_foot_change_.y()); foot_spline_.y()->addPoint( double_support_length + single_support_length * config_.foot_put_down_phase * config_.foot_overshoot_phase, @@ -727,47 +714,39 @@ void WalkEngine::buildWalkDisableTrajectories(bool foot_in_idle_position) { } // Flying foot orientation foot_spline_.roll()->addPoint(0.0, foot_orientation_pos_at_last_foot_change_.x(), - foot_orientation_vel_at_last_foot_change_.x(), - foot_orientation_acc_at_foot_change_.x()); + foot_orientation_vel_at_last_foot_change_.x()); foot_spline_.roll()->addPoint(half_period, 0.0); foot_spline_.pitch()->addPoint(0.0, foot_orientation_pos_at_last_foot_change_.y(), - foot_orientation_vel_at_last_foot_change_.y(), - foot_orientation_acc_at_foot_change_.y()); + foot_orientation_vel_at_last_foot_change_.y()); foot_spline_.pitch()->addPoint(half_period, 0.0); foot_spline_.yaw()->addPoint(0.0, foot_orientation_pos_at_last_foot_change_.z(), - foot_orientation_vel_at_last_foot_change_.z(), foot_orientation_acc_at_foot_change_.z()); + foot_orientation_vel_at_last_foot_change_.z()); foot_spline_.yaw()->addPoint(double_support_length, getLastEuler().z()); foot_spline_.yaw()->addPoint(double_support_length + single_support_length * config_.foot_put_down_phase, 0.0); foot_spline_.yaw()->addPoint(half_period, 0.0); // Trunk position - trunk_spline_.x()->addPoint(0.0, trunk_pos_at_foot_change_.x(), trunk_pos_vel_at_foot_change_.x(), - trunk_pos_acc_at_foot_change_.x()); + trunk_spline_.x()->addPoint(0.0, trunk_pos_at_foot_change_.x(), trunk_pos_vel_at_foot_change_.x()); trunk_spline_.x()->addPoint(half_period, config_.trunk_x_offset); - trunk_spline_.y()->addPoint(0.0, trunk_pos_at_foot_change_.y(), trunk_pos_vel_at_foot_change_.y(), - trunk_pos_acc_at_foot_change_.y()); + trunk_spline_.y()->addPoint(0.0, trunk_pos_at_foot_change_.y(), trunk_pos_vel_at_foot_change_.y()); // move trunk in the center trunk_spline_.y()->addPoint(half_period, -support_sign * 0.5 * config_.foot_distance + config_.trunk_y_offset); - trunk_spline_.z()->addPoint(0.0, trunk_pos_at_foot_change_.z(), trunk_pos_vel_at_foot_change_.z(), - trunk_pos_acc_at_foot_change_.z()); + trunk_spline_.z()->addPoint(0.0, trunk_pos_at_foot_change_.z(), trunk_pos_vel_at_foot_change_.z()); trunk_spline_.z()->addPoint(half_period, config_.trunk_height); // Trunk orientation trunk_spline_.roll()->addPoint(0.0, trunk_orientation_pos_at_last_foot_change_.x(), - trunk_orientation_vel_at_last_foot_change_.x(), - trunk_orientation_acc_at_foot_change_.x()); + trunk_orientation_vel_at_last_foot_change_.x()); trunk_spline_.roll()->addPoint(half_period, 0.0); trunk_spline_.pitch()->addPoint(0.0, trunk_orientation_pos_at_last_foot_change_.y(), - trunk_orientation_vel_at_last_foot_change_.y(), - trunk_orientation_acc_at_foot_change_.y()); + trunk_orientation_vel_at_last_foot_change_.y()); trunk_spline_.pitch()->addPoint(half_period, config_.trunk_pitch); trunk_spline_.yaw()->addPoint(0.0, trunk_orientation_pos_at_last_foot_change_.z(), - trunk_orientation_vel_at_last_foot_change_.z(), - trunk_orientation_acc_at_foot_change_.z()); + trunk_orientation_vel_at_last_foot_change_.z()); trunk_spline_.yaw()->addPoint(half_period, 0.0); } diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp b/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp index b6fe8ffab..4e9ae2b93 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp @@ -25,14 +25,13 @@ class SmoothSpline : public Spline { double time; double position; double velocity; - double acceleration; }; /** * Add a new point with its time, position value, * velocity and acceleration */ - void addPoint(double time, double position, double velocity = 0.0, double acceleration = 0.0); + void addPoint(double time, double position, double velocity = 0.0); /** * Access to points container @@ -68,8 +67,7 @@ class SmoothSpline : public Spline { * Fit a polynom between 0 and t with given * pos, vel and acc initial and final conditions */ - Polynom polynomFit(double t, double pos_1, double vel_1, double acc_1, double pos_2, double vel_2, - double acc_2) const; + Polynom polynomFit(double t, double pos_1, double vel_1, double pos_2, double vel_2) const; }; } // namespace bitbots_splines diff --git a/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp index 682eaa761..b21a17f17 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp @@ -12,8 +12,8 @@ The original files can be found at: namespace bitbots_splines { -void SmoothSpline::addPoint(double time, double position, double velocity, double acceleration) { - points_.push_back({time, position, velocity, acceleration}); +void SmoothSpline::addPoint(double time, double position, double velocity) { + points_.push_back({time, position, velocity}); computeSplines(); } @@ -33,8 +33,7 @@ void SmoothSpline::computeSplines() { double time = points_[i].time - points_[i - 1].time; if (time > 0.00001) { Spline::splines_.push_back( - {polynomFit(time, points_[i - 1].position, points_[i - 1].velocity, points_[i - 1].acceleration, - points_[i].position, points_[i].velocity, points_[i].acceleration), + {polynomFit(time, points_[i - 1].position, points_[i - 1].velocity, points_[i].position, points_[i].velocity), points_[i - 1].time, points_[i].time}); } } @@ -47,50 +46,41 @@ void SmoothSpline::importCallBack() { } double t_begin = Spline::splines_.front().min; - points_.push_back({t_begin, Spline::pos(t_begin), Spline::vel(t_begin), Spline::acc(t_begin)}); + points_.push_back({t_begin, Spline::pos(t_begin), Spline::vel(t_begin)}); for (size_t i = 1; i < size; i++) { double t_1 = Spline::splines_[i - 1].max; double t_2 = Spline::splines_[i].min; double pos_1 = Spline::pos(t_1); double vel_1 = Spline::vel(t_1); - double acc_1 = Spline::acc(t_1); double pos_2 = Spline::pos(t_2); double vel_2 = Spline::vel(t_2); - double acc_2 = Spline::acc(t_2); - if (fabs(t_2 - t_1) < 0.0001 && fabs(pos_2 - pos_1) < 0.0001 && fabs(vel_2 - vel_1) < 0.0001 && - fabs(acc_2 - acc_1) < 0.0001) { - points_.push_back({t_1, pos_1, vel_1, acc_1}); + if (fabs(t_2 - t_1) < 0.0001 && fabs(pos_2 - pos_1) < 0.0001 && fabs(vel_2 - vel_1) < 0.0001) { + points_.push_back({t_1, pos_1, vel_1}); } else { - points_.push_back({t_1, pos_1, vel_1, acc_1}); - points_.push_back({t_2, pos_2, vel_2, acc_2}); + points_.push_back({t_1, pos_1, vel_1}); + points_.push_back({t_2, pos_2, vel_2}); } } double t_end = Spline::splines_.back().max; - points_.push_back({t_end, Spline::pos(t_end), Spline::vel(t_end), Spline::acc(t_end)}); + points_.push_back({t_end, Spline::pos(t_end), Spline::vel(t_end)}); } -Polynom SmoothSpline::polynomFit(double t, double pos_1, double vel_1, double acc_1, double pos_2, double vel_2, - double acc_2) const { +Polynom SmoothSpline::polynomFit(double t, double pos_1, double vel_1, double pos_2, double vel_2) const { if (t <= 0.00001) { throw std::logic_error("SmoothSpline invalid spline interval"); } double t_2 = t * t; double t_3 = t_2 * t; - double t_4 = t_3 * t; - double t_5 = t_4 * t; + Polynom p; - p.getCoefs().resize(6); + p.getCoefs().resize(4); p.getCoefs()[0] = pos_1; p.getCoefs()[1] = vel_1; - p.getCoefs()[2] = acc_1 / 2; - p.getCoefs()[3] = - -(-acc_2 * t_2 + 3 * acc_1 * t_2 + 8 * vel_2 * t + 12 * vel_1 * t - 20 * pos_2 + 20 * pos_1) / (2 * t_3); - p.getCoefs()[4] = - (-2 * acc_2 * t_2 + 3 * acc_1 * t_2 + 14 * vel_2 * t + 16 * vel_1 * t - 30 * pos_2 + 30 * pos_1) / (2 * t_4); - p.getCoefs()[5] = -(-acc_2 * t_2 + acc_1 * t_2 + 6 * vel_2 * t + 6 * vel_1 * t - 12 * pos_2 + 12 * pos_1) / (2 * t_5); + p.getCoefs()[2] = (3 * (pos_2 - pos_1) / (t_2) - (2 * vel_1 + vel_2) / t); + p.getCoefs()[3] = (2 * (pos_1 - pos_2) / (t_3) + (vel_1 + vel_2) / (t_2)); return p; } @@ -103,7 +93,6 @@ std::string SmoothSpline::getDebugString() { output += " Time: " + std::to_string(p.time) + "\n"; output += " Pos: " + std::to_string(p.position) + "\n"; output += " Vel: " + std::to_string(p.velocity) + "\n"; - output += " Acc: " + std::to_string(p.acceleration) + "\n"; i++; } return output;