Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use cubic splines in walking #632

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
Open

Use cubic splines in walking #632

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ba0a920
switch quintic to cubic spline
val-ba Dec 6, 2024
c0369e3
start removing when acceleration is set in walk engine
val-ba Dec 6, 2024
5f7fbf5
Merge branch 'main' into feature/use_cubic_splines_in_walking
Flova Dec 12, 2024
f0d66d4
remove accelration component from spliens in walk engine
val-ba Dec 13, 2024
ff198cb
use cubic splines in dynamic kick
val-ba Dec 13, 2024
9b4838e
Merge branch 'feature/use_cubic_splines_in_walking' of github.com:bit…
val-ba Dec 13, 2024
eeca66d
Merge branch 'main' into feature/use_cubic_splines_in_walking
val-ba Jan 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,17 +107,17 @@ 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);

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());
Expand Down
71 changes: 25 additions & 46 deletions bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -511,23 +508,21 @@ 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);
}
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());
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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());
}
Expand Down Expand Up @@ -686,17 +675,15 @@ 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,
0.0 + (0.0 - support_to_last_.getOrigin().x()) * config_.foot_overshoot_ratio);
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,
Expand Down Expand Up @@ -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);
}

Expand Down
6 changes: 2 additions & 4 deletions bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
39 changes: 14 additions & 25 deletions bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand All @@ -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});
}
}
Expand All @@ -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;
}
Expand All @@ -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;
Expand Down
Loading