From f59489d92cfd77bb230807c5b750ccd391a70eef Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Mon, 9 Aug 2021 16:53:06 +0200
Subject: [PATCH] Massive formatting of cpp files
---
src/Estimator.cpp | 796 +++++++++++++++-----------------
src/Filter.cpp | 56 +--
src/FootTrajectoryGenerator.cpp | 358 ++++++++------
src/FootstepPlanner.cpp | 459 +++++++++---------
src/Gait.cpp | 410 ++++++++--------
src/InvKin.cpp | 191 ++++----
src/Joystick.cpp | 35 +-
src/MPC.cpp | 26 +-
src/Params.cpp | 274 ++++++-----
src/QPWBC.cpp | 158 +++----
src/StatePlanner.cpp | 96 ++--
src/main.cpp | 211 +++++----
src/st_to_cc.cpp | 736 +++++++++++++----------------
13 files changed, 1804 insertions(+), 2002 deletions(-)
diff --git a/src/Estimator.cpp b/src/Estimator.cpp
index 7bab040f..1757eb65 100644
--- a/src/Estimator.cpp
+++ b/src/Estimator.cpp
@@ -5,41 +5,35 @@
////////////////////////////////////
ComplementaryFilter::ComplementaryFilter()
- : x_(Vector3::Zero())
- , dx_(Vector3::Zero())
- , HP_x_(Vector3::Zero())
- , LP_x_(Vector3::Zero())
- , alpha_(Vector3::Zero())
- , filt_x_(Vector3::Zero())
-{
+ : x_(Vector3::Zero()),
+ dx_(Vector3::Zero()),
+ HP_x_(Vector3::Zero()),
+ LP_x_(Vector3::Zero()),
+ alpha_(Vector3::Zero()),
+ filt_x_(Vector3::Zero()) {}
+
+void ComplementaryFilter::initialize(double dt, Vector3 HP_x, Vector3 LP_x) {
+ dt_ = dt;
+ HP_x_ = HP_x;
+ LP_x_ = LP_x;
}
+Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector3 const& alpha) {
+ // For logging
+ x_ = x;
+ dx_ = dx;
+ alpha_ = alpha;
-void ComplementaryFilter::initialize(double dt, Vector3 HP_x, Vector3 LP_x)
-{
- dt_ = dt;
- HP_x_ = HP_x;
- LP_x_ = LP_x;
-}
-
-
-Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector3 const& alpha)
-{
- // For logging
- x_ = x;
- dx_ = dx;
- alpha_ = alpha;
-
- // Process high pass filter
- HP_x_ = alpha.cwiseProduct(HP_x_ + dx_ * dt_);
+ // Process high pass filter
+ HP_x_ = alpha.cwiseProduct(HP_x_ + dx_ * dt_);
- // Process low pass filter
- LP_x_ = alpha.cwiseProduct(LP_x_) + (Vector3::Ones() - alpha).cwiseProduct(x_);
+ // Process low pass filter
+ LP_x_ = alpha.cwiseProduct(LP_x_) + (Vector3::Ones() - alpha).cwiseProduct(x_);
- // Add both to get the filtered output
- filt_x_ = HP_x_ + LP_x_;
+ // Add both to get the filtered output
+ filt_x_ = HP_x_ + LP_x_;
- return filt_x_;
+ return filt_x_;
}
/////////////////////////
@@ -47,426 +41,398 @@ Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector
/////////////////////////
Estimator::Estimator()
- : dt_wbc(0.0)
- , alpha_v_(0.0)
- , alpha_secu_(0.0)
- , offset_yaw_IMU_(0.0)
- , perfect_estimator(false)
- , N_SIMULATION(0)
- , k_log_(0)
- , IMU_lin_acc_(Vector3::Zero())
- , IMU_ang_vel_(Vector3::Zero())
- , IMU_RPY_(Vector3::Zero())
- , IMU_ang_pos_(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0))
- , actuators_pos_(Vector12::Zero())
- , actuators_vel_(Vector12::Zero())
- , q_FK_(Vector19::Zero())
- , v_FK_(Vector18::Zero())
- , feet_status_(MatrixN::Zero(1, 4))
- , feet_goals_(MatrixN::Zero(3, 4))
- , FK_lin_vel_(Vector3::Zero())
- , FK_xyz_(Vector3::Zero())
- , b_filt_lin_vel_(Vector3::Zero())
- , xyz_mean_feet_(Vector3::Zero())
- , k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero())
- , q_filt_(Vector19::Zero())
- , v_filt_(Vector18::Zero())
- , v_secu_(Vector12::Zero())
- , q_filt_dyn_(VectorN::Zero(19, 1))
- , v_filt_dyn_(VectorN::Zero(18, 1))
- , v_secu_dyn_(VectorN::Zero(12, 1))
- , q_up_(VectorN::Zero(18))
- , v_ref_(VectorN::Zero(6))
- , h_v_(VectorN::Zero(6))
- , oRh_(Matrix3::Identity())
- , oTh_(Vector3::Zero())
- , yaw_estim_(0.0)
- , N_queue_(0)
- , v_filt_bis_(Vector3::Zero())
- , h_v_bis_(Vector3::Zero())
-{
+ : dt_wbc(0.0),
+ alpha_v_(0.0),
+ alpha_secu_(0.0),
+ offset_yaw_IMU_(0.0),
+ perfect_estimator(false),
+ N_SIMULATION(0),
+ k_log_(0),
+ IMU_lin_acc_(Vector3::Zero()),
+ IMU_ang_vel_(Vector3::Zero()),
+ IMU_RPY_(Vector3::Zero()),
+ IMU_ang_pos_(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0)),
+ actuators_pos_(Vector12::Zero()),
+ actuators_vel_(Vector12::Zero()),
+ q_FK_(Vector19::Zero()),
+ v_FK_(Vector18::Zero()),
+ feet_status_(MatrixN::Zero(1, 4)),
+ feet_goals_(MatrixN::Zero(3, 4)),
+ FK_lin_vel_(Vector3::Zero()),
+ FK_xyz_(Vector3::Zero()),
+ b_filt_lin_vel_(Vector3::Zero()),
+ xyz_mean_feet_(Vector3::Zero()),
+ k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero()),
+ q_filt_(Vector19::Zero()),
+ v_filt_(Vector18::Zero()),
+ v_secu_(Vector12::Zero()),
+ q_filt_dyn_(VectorN::Zero(19, 1)),
+ v_filt_dyn_(VectorN::Zero(18, 1)),
+ v_secu_dyn_(VectorN::Zero(12, 1)),
+ q_up_(VectorN::Zero(18)),
+ v_ref_(VectorN::Zero(6)),
+ h_v_(VectorN::Zero(6)),
+ oRh_(Matrix3::Identity()),
+ oTh_(Vector3::Zero()),
+ yaw_estim_(0.0),
+ N_queue_(0),
+ v_filt_bis_(Vector3::Zero()),
+ h_v_bis_(Vector3::Zero()) {}
+
+void Estimator::initialize(Params& params) {
+ dt_wbc = params.dt_wbc;
+ N_SIMULATION = params.N_SIMULATION;
+ perfect_estimator = params.perfect_estimator;
+
+ // Filtering estimated linear velocity
+ double fc = params.fc_v_esti; // Cut frequency
+ double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
+ alpha_v_ = -y + std::sqrt(y * y + 2 * y);
+
+ N_queue_ = static_cast<int>(std::round(1.0 / (dt_wbc * 3.0)));
+ vx_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ vy_queue_.resize(N_queue_, 0.0); // List full of 0.0
+ vz_queue_.resize(N_queue_, 0.0); // List full of 0.0
+
+ // Filtering velocities used for security checks
+ fc = 6.0; // Cut frequency
+ y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
+ alpha_secu_ = -y + std::sqrt(y * y + 2 * y);
+
+ FK_xyz_(2, 0) = params.h_ref;
+
+ filter_xyz_vel_.initialize(dt_wbc, Vector3::Zero(), Vector3::Zero());
+ filter_xyz_pos_.initialize(dt_wbc, Vector3::Zero(), FK_xyz_);
+
+ _1Mi_ = pinocchio::SE3(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0), Vector3(0.1163, 0.0, 0.02));
+
+ q_security_ = (Vector3(M_PI * 0.4, M_PI * 80 / 180, M_PI)).replicate<4, 1>();
+
+ q_FK_(6, 0) = 1.0; // Last term of the quaternion
+ q_filt_(6, 0) = 1.0; // Last term of the quaternion
+ q_filt_dyn_(6, 0) = 1.0; // Last term of the quaternion
+
+ q_up_(2, 0) = params.h_ref; // Reference height
+ q_up_.tail(12) = Vector12(params.q_init.data()); // Actuator initial positions
+
+ // Path to the robot URDF (TODO: Automatic path)
+ const std::string filename =
+ std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
+
+ // Build model from urdf
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_for_xyz_, false);
+
+ // Construct data from model
+ data_ = pinocchio::Data(model_);
+ data_for_xyz_ = pinocchio::Data(model_for_xyz_);
+
+ // Update all the quantities of the model
+ pinocchio::computeAllTerms(model_, data_, q_filt_, v_filt_);
+ pinocchio::computeAllTerms(model_for_xyz_, data_for_xyz_, q_filt_, v_filt_);
}
+void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
+ Vector3 const& baseOrientation) {
+ // Linear acceleration of the trunk (base frame)
+ IMU_lin_acc_ = baseLinearAcceleration;
-void Estimator::initialize(Params& params)
-{
- dt_wbc = params.dt_wbc;
- N_SIMULATION = params.N_SIMULATION;
- perfect_estimator = params.perfect_estimator;
-
- // Filtering estimated linear velocity
- double fc = params.fc_v_esti; // Cut frequency
- double y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
- alpha_v_ = -y + std::sqrt(y * y + 2 * y);
-
- N_queue_ = static_cast<int>(std::round(1.0/(dt_wbc * 3.0)));
- vx_queue_.resize(N_queue_, 0.0); // List full of 0.0
- vy_queue_.resize(N_queue_, 0.0); // List full of 0.0
- vz_queue_.resize(N_queue_, 0.0); // List full of 0.0
-
- // Filtering velocities used for security checks
- fc = 6.0; // Cut frequency
- y = 1 - std::cos(2 * M_PI * fc * dt_wbc);
- alpha_secu_ = -y + std::sqrt(y * y + 2 * y);
-
- FK_xyz_(2, 0) = params.h_ref;
+ // Angular velocity of the trunk (base frame)
+ IMU_ang_vel_ = baseAngularVelocity;
- filter_xyz_vel_.initialize(dt_wbc, Vector3::Zero(), Vector3::Zero());
- filter_xyz_pos_.initialize(dt_wbc, Vector3::Zero(), FK_xyz_);
+ // Angular position of the trunk (local frame)
+ IMU_RPY_ = baseOrientation;
- _1Mi_ = pinocchio::SE3(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0), Vector3(0.1163, 0.0, 0.02));
+ if (k_log_ <= 1) {
+ offset_yaw_IMU_ = IMU_RPY_(2, 0);
+ }
+ IMU_RPY_(2, 0) -= offset_yaw_IMU_; // Remove initial offset of IMU
- q_security_ = (Vector3(M_PI * 0.4, M_PI * 80/180, M_PI)).replicate<4, 1>();
-
- q_FK_(6, 0) = 1.0; // Last term of the quaternion
- q_filt_(6, 0) = 1.0; // Last term of the quaternion
- q_filt_dyn_(6, 0) = 1.0; // Last term of the quaternion
-
- q_up_(2, 0) = params.h_ref; // Reference height
- q_up_.tail(12) = Vector12(params.q_init.data()); // Actuator initial positions
-
- // Path to the robot URDF (TODO: Automatic path)
- const std::string filename = std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
-
- // Build model from urdf
- pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
- pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_for_xyz_, false);
-
- // Construct data from model
- data_ = pinocchio::Data(model_);
- data_for_xyz_ = pinocchio::Data(model_for_xyz_);
-
- // Update all the quantities of the model
- pinocchio::computeAllTerms(model_, data_ , q_filt_, v_filt_);
- pinocchio::computeAllTerms(model_for_xyz_, data_for_xyz_, q_filt_, v_filt_);
+ IMU_ang_pos_ =
+ pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0), IMU_RPY_(1, 0), IMU_RPY_(2, 0)));
+ // Above could be commented since IMU_ang_pos yaw is not used anywhere and instead
+ // replace by: IMU_ang_pos_ = baseOrientation_
}
-
-void Estimator::get_data_IMU(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity, Vector3 const& baseOrientation)
-{
- // Linear acceleration of the trunk (base frame)
- IMU_lin_acc_ = baseLinearAcceleration;
-
- // Angular velocity of the trunk (base frame)
- IMU_ang_vel_ = baseAngularVelocity;
-
- // Angular position of the trunk (local frame)
- IMU_RPY_ = baseOrientation;
-
- if(k_log_ <= 1) {
- offset_yaw_IMU_ = IMU_RPY_(2, 0);
- }
- IMU_RPY_(2, 0) -= offset_yaw_IMU_; // Remove initial offset of IMU
-
- IMU_ang_pos_ = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMU_RPY_(0, 0),
- IMU_RPY_(1, 0),
- IMU_RPY_(2, 0)));
- // Above could be commented since IMU_ang_pos yaw is not used anywhere and instead
- // replace by: IMU_ang_pos_ = baseOrientation_
+void Estimator::get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes) {
+ actuators_pos_ = q_mes;
+ actuators_vel_ = v_mes;
}
-
-void Estimator::get_data_joints(Vector12 const& q_mes, Vector12 const& v_mes)
-{
- actuators_pos_ = q_mes;
- actuators_vel_ = v_mes;
-}
-
-
-void Estimator::get_data_FK(Eigen::Matrix<double, 1, 4> const& feet_status)
-{
- // Update estimator FK model
- q_FK_.tail(12) = actuators_pos_; // Position of actuators
- v_FK_.tail(12) = actuators_vel_; // Velocity of actuators
- // Position and orientation of the base remain at 0
- // Linear and angular velocities of the base remain at 0
-
- // Update model used for the forward kinematics
- q_FK_.block(3, 0, 4, 1) << 0.0, 0.0, 0.0, 1.0;
- pinocchio::forwardKinematics(model_, data_, q_FK_, v_FK_);
- // pin.updateFramePlacements(self.model, self.data)
-
- // Update model used for the forward geometry
- q_FK_.block(3, 0, 4, 1) = IMU_ang_pos_.coeffs();
- pinocchio::forwardKinematics(model_for_xyz_, data_for_xyz_, q_FK_);
-
- // Get estimated velocity from updated model
- int cpt = 0;
- Vector3 vel_est = Vector3::Zero();
- Vector3 xyz_est = Vector3::Zero();
- for (int j = 0; j < 4; j++) {
- // Consider only feet in contact + Security margin after the contact switch
- if(feet_status(0, j) == 1.0 && k_since_contact_[j] >= 16)
- {
- // Estimated velocity of the base using the considered foot
- Vector3 vel_estimated_baseframe = BaseVelocityFromKinAndIMU(feet_indexes_[j]);
-
- // Estimated position of the base using the considered foot
- pinocchio::updateFramePlacement(model_for_xyz_, data_for_xyz_, feet_indexes_[j]);
- Vector3 xyz_estimated = -data_for_xyz_.oMf[feet_indexes_[j]].translation();
-
- // Logging
- // self.log_v_est[:, i, self.k_log] = vel_estimated_baseframe[0:3, 0]
- // self.log_h_est[i, self.k_log] = xyz_estimated[2]
-
- // Increment counter and add estimated quantities to the storage variables
- cpt++;
- vel_est += vel_estimated_baseframe; // Linear velocity
- xyz_est += xyz_estimated; // Position
-
- double r_foot = 0.0155; // 31mm of diameter on meshlab
- if(j <= 1) {
- vel_est(0, 0) += r_foot * (actuators_vel_(1+3*j, 0) - actuators_vel_(2+3*j, 0));
- }
- else {
- vel_est(0, 0) += r_foot * (actuators_vel_(1+3*j, 0) + actuators_vel_(2+3*j, 0));
- }
- }
+void Estimator::get_data_FK(Eigen::Matrix<double, 1, 4> const& feet_status) {
+ // Update estimator FK model
+ q_FK_.tail(12) = actuators_pos_; // Position of actuators
+ v_FK_.tail(12) = actuators_vel_; // Velocity of actuators
+ // Position and orientation of the base remain at 0
+ // Linear and angular velocities of the base remain at 0
+
+ // Update model used for the forward kinematics
+ q_FK_.block(3, 0, 4, 1) << 0.0, 0.0, 0.0, 1.0;
+ pinocchio::forwardKinematics(model_, data_, q_FK_, v_FK_);
+ // pin.updateFramePlacements(self.model, self.data)
+
+ // Update model used for the forward geometry
+ q_FK_.block(3, 0, 4, 1) = IMU_ang_pos_.coeffs();
+ pinocchio::forwardKinematics(model_for_xyz_, data_for_xyz_, q_FK_);
+
+ // Get estimated velocity from updated model
+ int cpt = 0;
+ Vector3 vel_est = Vector3::Zero();
+ Vector3 xyz_est = Vector3::Zero();
+ for (int j = 0; j < 4; j++) {
+ // Consider only feet in contact + Security margin after the contact switch
+ if (feet_status(0, j) == 1.0 && k_since_contact_[j] >= 16) {
+ // Estimated velocity of the base using the considered foot
+ Vector3 vel_estimated_baseframe = BaseVelocityFromKinAndIMU(feet_indexes_[j]);
+
+ // Estimated position of the base using the considered foot
+ pinocchio::updateFramePlacement(model_for_xyz_, data_for_xyz_, feet_indexes_[j]);
+ Vector3 xyz_estimated = -data_for_xyz_.oMf[feet_indexes_[j]].translation();
+
+ // Logging
+ // self.log_v_est[:, i, self.k_log] = vel_estimated_baseframe[0:3, 0]
+ // self.log_h_est[i, self.k_log] = xyz_estimated[2]
+
+ // Increment counter and add estimated quantities to the storage variables
+ cpt++;
+ vel_est += vel_estimated_baseframe; // Linear velocity
+ xyz_est += xyz_estimated; // Position
+
+ double r_foot = 0.0155; // 31mm of diameter on meshlab
+ if (j <= 1) {
+ vel_est(0, 0) += r_foot * (actuators_vel_(1 + 3 * j, 0) - actuators_vel_(2 + 3 * j, 0));
+ } else {
+ vel_est(0, 0) += r_foot * (actuators_vel_(1 + 3 * j, 0) + actuators_vel_(2 + 3 * j, 0));
+ }
}
+ }
- // If at least one foot is in contact, we do the average of feet results
- if(cpt > 0)
- {
- FK_lin_vel_ = vel_est / cpt;
- FK_xyz_ = xyz_est / cpt;
- }
+ // If at least one foot is in contact, we do the average of feet results
+ if (cpt > 0) {
+ FK_lin_vel_ = vel_est / cpt;
+ FK_xyz_ = xyz_est / cpt;
+ }
}
+void Estimator::get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals) {
+ int cpt = 0;
+ Vector3 xyz_feet = Vector3::Zero();
-void Estimator::get_xyz_feet(Eigen::Matrix<double, 1, 4> const& feet_status, Matrix34 const& goals)
-{
- int cpt = 0;
- Vector3 xyz_feet = Vector3::Zero();
-
- // Consider only feet in contact
- for (int j = 0; j < 4; j++) {
- if(feet_status(0, j) == 1.0) {
- cpt++;
- xyz_feet += goals.col(j);
- }
+ // Consider only feet in contact
+ for (int j = 0; j < 4; j++) {
+ if (feet_status(0, j) == 1.0) {
+ cpt++;
+ xyz_feet += goals.col(j);
}
+ }
- // If at least one foot is in contact, we do the average of feet results
- if(cpt > 0) {
- xyz_mean_feet_ = xyz_feet / cpt;
- }
+ // If at least one foot is in contact, we do the average of feet results
+ if (cpt > 0) {
+ xyz_mean_feet_ = xyz_feet / cpt;
+ }
}
-
-Vector3 Estimator::BaseVelocityFromKinAndIMU(int contactFrameId)
-{
- Vector3 frameVelocity = pinocchio::getFrameVelocity(model_, data_, contactFrameId, pinocchio::LOCAL).linear();
- pinocchio::updateFramePlacement(model_, data_, contactFrameId);
-
- // Angular velocity of the base wrt the world in the base frame (Gyroscope)
- Vector3 _1w01 = IMU_ang_vel_;
- // Linear velocity of the foot wrt the base in the foot frame
- Vector3 _Fv1F = frameVelocity;
- // Level arm between the base and the foot
- Vector3 _1F = data_.oMf[contactFrameId].translation();
- // Orientation of the foot wrt the base
- Matrix3 _1RF = data_.oMf[contactFrameId].rotation();
- // Linear velocity of the base wrt world in the base frame
- Vector3 _1v01 = _1F.cross(_1w01) - _1RF * _Fv1F;
-
- // IMU and base frames have the same orientation
- // _iv0i = _1v01 + self.cross3(self._1Mi.translation.ravel(), _1w01.ravel())
-
- return _1v01;
+Vector3 Estimator::BaseVelocityFromKinAndIMU(int contactFrameId) {
+ Vector3 frameVelocity = pinocchio::getFrameVelocity(model_, data_, contactFrameId, pinocchio::LOCAL).linear();
+ pinocchio::updateFramePlacement(model_, data_, contactFrameId);
+
+ // Angular velocity of the base wrt the world in the base frame (Gyroscope)
+ Vector3 _1w01 = IMU_ang_vel_;
+ // Linear velocity of the foot wrt the base in the foot frame
+ Vector3 _Fv1F = frameVelocity;
+ // Level arm between the base and the foot
+ Vector3 _1F = data_.oMf[contactFrameId].translation();
+ // Orientation of the foot wrt the base
+ Matrix3 _1RF = data_.oMf[contactFrameId].rotation();
+ // Linear velocity of the base wrt world in the base frame
+ Vector3 _1v01 = _1F.cross(_1w01) - _1RF * _Fv1F;
+
+ // IMU and base frames have the same orientation
+ // _iv0i = _1v01 + self.cross3(self._1Mi.translation.ravel(), _1w01.ravel())
+
+ return _1v01;
}
-
void Estimator::run_filter(MatrixN const& gait, MatrixN const& goals, VectorN const& baseLinearAcceleration,
VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q_mes,
- VectorN const& v_mes, VectorN const& dummyPos, VectorN const& b_baseVel)
-{
- feet_status_ = gait.block(0, 0, 1, 4);
- feet_goals_ = goals;
-
- int remaining_steps = 1; // Remaining MPC steps for the current gait phase
- while((gait.block(0, 0, 1, 4)).isApprox(gait.row(remaining_steps))) {
- remaining_steps++;
- }
-
- // Update IMU data
- get_data_IMU(baseLinearAcceleration, baseAngularVelocity, baseOrientation);
-
- // Angular position of the trunk
- Vector4 filt_ang_pos = IMU_ang_pos_.coeffs();
-
- // Angular velocity of the trunk
- Vector3 filt_ang_vel = IMU_ang_vel_;
-
- // Update joints data
- get_data_joints(q_mes, v_mes);
-
- // Update nb of iterations since contact
- k_since_contact_ += feet_status_; // Increment feet in stance phase
- k_since_contact_ = k_since_contact_.cwiseProduct(feet_status_); // Reset feet in swing phase
-
- // Update forward kinematics data
- get_data_FK(feet_status_);
-
- // Update forward geometry data
- get_xyz_feet(feet_status_, goals);
-
- // Tune alpha depending on the state of the gait (close to contact switch or not)
- double a = std::ceil(k_since_contact_.maxCoeff() * 0.1) - 1;
- double b = static_cast<double>(remaining_steps);
- const double n = 1; // Nb of steps of margin around contact switch
-
- const double v_max = 1.00; // Maximum alpha value
- const double v_min = 0.97; // Minimum alpha value
- double c = ((a + b) - 2 * n) * 0.5;
- double alpha = 0.0;
- if(a <= (n-1) || b <= n) { // If we are close from contact switch
- alpha = v_max; // Only trust IMU data
- }
- else {
- alpha = v_min + (v_max - v_min) * std::abs(c - (a - n)) / c;
- //self.alpha = 0.997
- }
-
-
- // Use cascade of complementary filters
-
- // Rotation matrix to go from base frame to world frame
- Matrix3 oRb = IMU_ang_pos_.toRotationMatrix();
-
- // Get FK estimated velocity at IMU location (base frame)
- Vector3 cross_product = (_1Mi_.translation()).cross(IMU_ang_vel_);
- Vector3 i_FK_lin_vel = FK_lin_vel_ + cross_product;
-
- // Get FK estimated velocity at IMU location (world frame)
- Vector3 oi_FK_lin_vel = oRb * i_FK_lin_vel;
-
- // Integration of IMU acc at IMU location (world frame)
- Vector3 oi_filt_lin_vel = filter_xyz_vel_.compute(oi_FK_lin_vel, oRb * IMU_lin_acc_,
- alpha * Vector3::Ones());
-
- // Filtered estimated velocity at IMU location (base frame)
- Vector3 i_filt_lin_vel = oRb.transpose() * oi_filt_lin_vel;
-
- // Filtered estimated velocity at center base (base frame)
- b_filt_lin_vel_ = i_filt_lin_vel - cross_product;
-
- // Filtered estimated velocity at center base (world frame)
- Vector3 ob_filt_lin_vel = oRb * b_filt_lin_vel_;
-
- // Position of the center of the base from FGeometry and filtered velocity (world frame)
- Vector3 filt_lin_pos = filter_xyz_pos_.compute(FK_xyz_ + xyz_mean_feet_, ob_filt_lin_vel,
- Vector3(0.995, 0.995, 0.9));
-
- // Output filtered position vector (19 x 1)
- q_filt_.head(3) = filt_lin_pos;
- if(perfect_estimator) { // Base height directly from PyBullet
- q_filt_(2, 0) = dummyPos(2, 0) - 0.0155; // Minus feet radius
- }
- q_filt_.block(3, 0, 4, 1) = filt_ang_pos;
- q_filt_.tail(12) = actuators_pos_; // Actuators pos are already directly from PyBullet
-
- // Output filtered velocity vector (18 x 1)
- if(perfect_estimator) { // Linear velocities directly from PyBullet
- v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_baseVel;
- }
- else {
- v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_filt_lin_vel_;
- }
- v_filt_.block(3, 0, 3, 1) = filt_ang_vel; // Angular velocities are already directly from PyBullet
- v_filt_.tail(12) = actuators_vel_; // Actuators velocities are already directly from PyBullet
-
- vx_queue_.pop_back();
- vy_queue_.pop_back();
- vz_queue_.pop_back();
- vx_queue_.push_front(b_filt_lin_vel_(0));
- vy_queue_.push_front(b_filt_lin_vel_(1));
- vz_queue_.push_front(b_filt_lin_vel_(2));
- v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
- v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
- v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
-
- //////
-
- // Update model used for the forward kinematics
- /*pin.forwardKinematics(self.model, self.data, q_up__filt, self.v_filt)
- pin.updateFramePlacements(self.model, self.data)
-
- z_min = 100
- for i in (np.where(feet_status == 1))[0]: // Consider only feet in contact
- // Estimated position of the base using the considered foot
- framePlacement = pin.updateFramePlacement(self.model, self.data, self.indexes[i])
- z_min = np.min((framePlacement.translation[2], z_min))
- q_up__filt[2, 0] -= z_min*/
-
- //////
-
- // Output filtered actuators velocity for security checks
- v_secu_ = (1 - alpha_secu_) * actuators_vel_ + alpha_secu_ * v_secu_;
-
- // Copy data to dynamic sized matrices since Python converters for big sized fixed matrices do not exist
- // TODO: Find a way to cast a fixed size eigen matrix as dynamic size to remove the need for those variables
- q_filt_dyn_ = q_filt_;
- v_filt_dyn_ = v_filt_;
- v_secu_dyn_ = v_secu_;
-
- // Increment iteration counter
- k_log_++;
+ VectorN const& v_mes, VectorN const& dummyPos, VectorN const& b_baseVel) {
+ feet_status_ = gait.block(0, 0, 1, 4);
+ feet_goals_ = goals;
+
+ int remaining_steps = 1; // Remaining MPC steps for the current gait phase
+ while ((gait.block(0, 0, 1, 4)).isApprox(gait.row(remaining_steps))) {
+ remaining_steps++;
+ }
+
+ // Update IMU data
+ get_data_IMU(baseLinearAcceleration, baseAngularVelocity, baseOrientation);
+
+ // Angular position of the trunk
+ Vector4 filt_ang_pos = IMU_ang_pos_.coeffs();
+
+ // Angular velocity of the trunk
+ Vector3 filt_ang_vel = IMU_ang_vel_;
+
+ // Update joints data
+ get_data_joints(q_mes, v_mes);
+
+ // Update nb of iterations since contact
+ k_since_contact_ += feet_status_; // Increment feet in stance phase
+ k_since_contact_ = k_since_contact_.cwiseProduct(feet_status_); // Reset feet in swing phase
+
+ // Update forward kinematics data
+ get_data_FK(feet_status_);
+
+ // Update forward geometry data
+ get_xyz_feet(feet_status_, goals);
+
+ // Tune alpha depending on the state of the gait (close to contact switch or not)
+ double a = std::ceil(k_since_contact_.maxCoeff() * 0.1) - 1;
+ double b = static_cast<double>(remaining_steps);
+ const double n = 1; // Nb of steps of margin around contact switch
+
+ const double v_max = 1.00; // Maximum alpha value
+ const double v_min = 0.97; // Minimum alpha value
+ double c = ((a + b) - 2 * n) * 0.5;
+ double alpha = 0.0;
+ if (a <= (n - 1) || b <= n) { // If we are close from contact switch
+ alpha = v_max; // Only trust IMU data
+ } else {
+ alpha = v_min + (v_max - v_min) * std::abs(c - (a - n)) / c;
+ // self.alpha = 0.997
+ }
+
+ // Use cascade of complementary filters
+
+ // Rotation matrix to go from base frame to world frame
+ Matrix3 oRb = IMU_ang_pos_.toRotationMatrix();
+
+ // Get FK estimated velocity at IMU location (base frame)
+ Vector3 cross_product = (_1Mi_.translation()).cross(IMU_ang_vel_);
+ Vector3 i_FK_lin_vel = FK_lin_vel_ + cross_product;
+
+ // Get FK estimated velocity at IMU location (world frame)
+ Vector3 oi_FK_lin_vel = oRb * i_FK_lin_vel;
+
+ // Integration of IMU acc at IMU location (world frame)
+ Vector3 oi_filt_lin_vel = filter_xyz_vel_.compute(oi_FK_lin_vel, oRb * IMU_lin_acc_, alpha * Vector3::Ones());
+
+ // Filtered estimated velocity at IMU location (base frame)
+ Vector3 i_filt_lin_vel = oRb.transpose() * oi_filt_lin_vel;
+
+ // Filtered estimated velocity at center base (base frame)
+ b_filt_lin_vel_ = i_filt_lin_vel - cross_product;
+
+ // Filtered estimated velocity at center base (world frame)
+ Vector3 ob_filt_lin_vel = oRb * b_filt_lin_vel_;
+
+ // Position of the center of the base from FGeometry and filtered velocity (world frame)
+ Vector3 filt_lin_pos =
+ filter_xyz_pos_.compute(FK_xyz_ + xyz_mean_feet_, ob_filt_lin_vel, Vector3(0.995, 0.995, 0.9));
+
+ // Output filtered position vector (19 x 1)
+ q_filt_.head(3) = filt_lin_pos;
+ if (perfect_estimator) { // Base height directly from PyBullet
+ q_filt_(2, 0) = dummyPos(2, 0) - 0.0155; // Minus feet radius
+ }
+ q_filt_.block(3, 0, 4, 1) = filt_ang_pos;
+ q_filt_.tail(12) = actuators_pos_; // Actuators pos are already directly from PyBullet
+
+ // Output filtered velocity vector (18 x 1)
+ if (perfect_estimator) { // Linear velocities directly from PyBullet
+ v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_baseVel;
+ } else {
+ v_filt_.head(3) = (1 - alpha_v_) * v_filt_.head(3) + alpha_v_ * b_filt_lin_vel_;
+ }
+ v_filt_.block(3, 0, 3, 1) = filt_ang_vel; // Angular velocities are already directly from PyBullet
+ v_filt_.tail(12) = actuators_vel_; // Actuators velocities are already directly from PyBullet
+
+ vx_queue_.pop_back();
+ vy_queue_.pop_back();
+ vz_queue_.pop_back();
+ vx_queue_.push_front(b_filt_lin_vel_(0));
+ vy_queue_.push_front(b_filt_lin_vel_(1));
+ vz_queue_.push_front(b_filt_lin_vel_(2));
+ v_filt_bis_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.0) / N_queue_;
+ v_filt_bis_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.0) / N_queue_;
+ v_filt_bis_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.0) / N_queue_;
+
+ //////
+
+ // Update model used for the forward kinematics
+ /*pin.forwardKinematics(self.model, self.data, q_up__filt, self.v_filt)
+ pin.updateFramePlacements(self.model, self.data)
+
+ z_min = 100
+ for i in (np.where(feet_status == 1))[0]: // Consider only feet in contact
+ // Estimated position of the base using the considered foot
+ framePlacement = pin.updateFramePlacement(self.model, self.data, self.indexes[i])
+ z_min = np.min((framePlacement.translation[2], z_min))
+ q_up__filt[2, 0] -= z_min*/
+
+ //////
+
+ // Output filtered actuators velocity for security checks
+ v_secu_ = (1 - alpha_secu_) * actuators_vel_ + alpha_secu_ * v_secu_;
+
+ // Copy data to dynamic sized matrices since Python converters for big sized fixed matrices do not exist
+ // TODO: Find a way to cast a fixed size eigen matrix as dynamic size to remove the need for those variables
+ q_filt_dyn_ = q_filt_;
+ v_filt_dyn_ = v_filt_;
+ v_secu_dyn_ = v_secu_;
+
+ // Increment iteration counter
+ k_log_++;
}
-int Estimator::security_check(VectorN const& tau_ff)
-{
- if(((q_filt_.tail(12).cwiseAbs()).array() > q_security_.array()).any()) { // Test position limits
- return 1;
- } else if (((v_secu_.cwiseAbs()).array() > 50.0).any()) { // Test velocity limits
- return 2;
- } else if (((tau_ff.cwiseAbs()).array() > 8.0).any()) { // Test feedforward torques limits
- return 3;
- }
- return 0;
+int Estimator::security_check(VectorN const& tau_ff) {
+ if (((q_filt_.tail(12).cwiseAbs()).array() > q_security_.array()).any()) { // Test position limits
+ return 1;
+ } else if (((v_secu_.cwiseAbs()).array() > 50.0).any()) { // Test velocity limits
+ return 2;
+ } else if (((tau_ff.cwiseAbs()).array() > 8.0).any()) { // Test feedforward torques limits
+ return 3;
+ }
+ return 0;
}
-void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait)
-{
- // TODO: Joystick velocity given in base frame and not in horizontal frame (case of non flat ground)
+void Estimator::updateState(VectorN const& joystick_v_ref, Gait& gait) {
+ // TODO: Joystick velocity given in base frame and not in horizontal frame (case of non flat ground)
- // Update reference velocity vector
- v_ref_.head(3) = joystick_v_ref.head(3);
- v_ref_.tail(3) = joystick_v_ref.tail(3);
+ // Update reference velocity vector
+ v_ref_.head(3) = joystick_v_ref.head(3);
+ v_ref_.tail(3) = joystick_v_ref.tail(3);
- // Update position and velocity state vectors
- if (!gait.getIsStatic())
- {
- // Integration to get evolution of perfect x, y and yaw
- Matrix2 Ryaw;
- Ryaw << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
+ // Update position and velocity state vectors
+ if (!gait.getIsStatic()) {
+ // Integration to get evolution of perfect x, y and yaw
+ Matrix2 Ryaw;
+ Ryaw << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
- q_up_.head(2) = q_up_.head(2) + Ryaw * v_ref_.head(2) * dt_wbc;
+ q_up_.head(2) = q_up_.head(2) + Ryaw * v_ref_.head(2) * dt_wbc;
- // Mix perfect x and y with height measurement
- q_up_[2] = q_filt_dyn_[2];
+ // Mix perfect x and y with height measurement
+ q_up_[2] = q_filt_dyn_[2];
- // Mix perfect yaw with pitch and roll measurements
- yaw_estim_ += v_ref_[5] * dt_wbc;
- q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
+ // Mix perfect yaw with pitch and roll measurements
+ yaw_estim_ += v_ref_[5] * dt_wbc;
+ q_up_.block(3, 0, 3, 1) << IMU_RPY_[0], IMU_RPY_[1], yaw_estim_;
- // Actuators measurements
- q_up_.tail(12) = q_filt_dyn_.tail(12);
+ // Actuators measurements
+ q_up_.tail(12) = q_filt_dyn_.tail(12);
- // Velocities are the one estimated by the estimator
- Matrix3 hRb = pinocchio::rpy::rpyToMatrix(IMU_RPY_[0], IMU_RPY_[1], 0.0);
-
- h_v_.head(3) = hRb * v_filt_dyn_.block(0, 0, 3, 1);
- h_v_.tail(3) = hRb * v_filt_dyn_.block(3, 0, 3, 1);
- h_v_bis_ = hRb * v_filt_bis_;
- }
- else
- {
- // TODO: Adapt static mode to new version of the code
- }
+ // Velocities are the one estimated by the estimator
+ Matrix3 hRb = pinocchio::rpy::rpyToMatrix(IMU_RPY_[0], IMU_RPY_[1], 0.0);
- // Transformation matrices between world and horizontal frames
- oRh_ = Matrix3::Identity();
- oRh_.block(0, 0, 2, 2) << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
- oTh_ << q_up_[0], q_up_[1], 0.0;
+ h_v_.head(3) = hRb * v_filt_dyn_.block(0, 0, 3, 1);
+ h_v_.tail(3) = hRb * v_filt_dyn_.block(3, 0, 3, 1);
+ h_v_bis_ = hRb * v_filt_bis_;
+ } else {
+ // TODO: Adapt static mode to new version of the code
+ }
+ // Transformation matrices between world and horizontal frames
+ oRh_ = Matrix3::Identity();
+ oRh_.block(0, 0, 2, 2) << cos(yaw_estim_), -sin(yaw_estim_), sin(yaw_estim_), cos(yaw_estim_);
+ oTh_ << q_up_[0], q_up_[1], 0.0;
}
diff --git a/src/Filter.cpp b/src/Filter.cpp
index 6dba205d..49575558 100644
--- a/src/Filter.cpp
+++ b/src/Filter.cpp
@@ -1,51 +1,44 @@
#include "qrw/Filter.hpp"
Filter::Filter()
- : b_(Vector1::Zero())
- , a_(Vector2::Zero())
- , x_(Vector6::Zero())
- , y_(VectorN::Zero(6, 1))
- , accum_(Vector6::Zero())
- , init_(false)
-{
+ : b_(Vector1::Zero()),
+ a_(Vector2::Zero()),
+ x_(Vector6::Zero()),
+ y_(VectorN::Zero(6, 1)),
+ accum_(Vector6::Zero()),
+ init_(false) {
// Empty
}
-void Filter::initialize(Params& params)
-{
+void Filter::initialize(Params& params) {
const double fc = 15.0;
double alpha = (2 * M_PI * params.dt_wbc * fc) / (2 * M_PI * params.dt_wbc * fc + 1.0);
-
+
b_ << alpha;
a_ << 1.0, -(1.0 - alpha);
x_queue_.resize(b_.rows(), Vector6::Zero());
- y_queue_.resize(a_.rows()-1, Vector6::Zero());
+ y_queue_.resize(a_.rows() - 1, Vector6::Zero());
}
-VectorN Filter::filter(Vector6 const& x, bool check_modulo)
-{
+VectorN Filter::filter(Vector6 const& x, bool check_modulo) {
// Retrieve measurement
x_ = x;
// Handle modulo for orientation
- if (check_modulo)
- {
+ if (check_modulo) {
// Handle 2 pi modulo for roll, pitch and yaw
// Should happen sometimes for yaw but now for roll and pitch except
// if the robot rolls over
- for (int i = 3; i < 6; i++)
- {
- if (std::abs(x_(i, 0) - y_(i, 0)) > 1.5 * M_PI)
- {
+ for (int i = 3; i < 6; i++) {
+ if (std::abs(x_(i, 0) - y_(i, 0)) > 1.5 * M_PI) {
handle_modulo(i, x_(i, 0) - y_(i, 0) > 0);
}
}
}
// Initialisation of the value in the queues to the first measurement
- if (!init_)
- {
+ if (!init_) {
init_ = true;
std::fill(x_queue_.begin(), x_queue_.end(), x_.head(6));
std::fill(y_queue_.begin(), y_queue_.end(), x_.head(6));
@@ -57,15 +50,13 @@ VectorN Filter::filter(Vector6 const& x, bool check_modulo)
// Compute result (y/x = b/a for the transfert function)
accum_ = Vector6::Zero();
- for (int i = 0; i < b_.rows(); i++)
- {
+ for (int i = 0; i < b_.rows(); i++) {
accum_ += b_[i] * x_queue_[i];
}
- for (int i = 1; i < a_.rows(); i++)
- {
- accum_ -= a_[i] * y_queue_[i-1];
+ for (int i = 1; i < a_.rows(); i++) {
+ accum_ -= a_[i] * y_queue_[i - 1];
}
-
+
// Store result in y queue for recursion
y_queue_.pop_back();
y_queue_.push_front(accum_ / a_[0]);
@@ -77,15 +68,12 @@ VectorN Filter::filter(Vector6 const& x, bool check_modulo)
return y_;
}
-void Filter::handle_modulo(int a, bool dir)
-{
+void Filter::handle_modulo(int a, bool dir) {
// Add or remove 2 PI to all elements in the queues
- for (int i = 0; i < b_.rows(); i++)
- {
+ for (int i = 0; i < b_.rows(); i++) {
(x_queue_[i])(a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
}
- for (int i = 1; i < a_.rows(); i++)
- {
- (y_queue_[i-1])(a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
+ for (int i = 1; i < a_.rows(); i++) {
+ (y_queue_[i - 1])(a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
}
}
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index 33fb3528..cc6322b5 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -3,168 +3,228 @@
// Trajectory generator functions (output reference pos, vel and acc of feet in swing phase)
FootTrajectoryGenerator::FootTrajectoryGenerator()
- : gait_(NULL)
- , dt_wbc(0.0)
- , k_mpc(0)
- , maxHeight_(0.0)
- , lockTime_(0.0)
- , feet()
- , t0s(Vector4::Zero())
- , t_swing(Vector4::Zero())
- , targetFootstep_(Matrix34::Zero())
- , Ax(Matrix64::Zero())
- , Ay(Matrix64::Zero())
- , position_(Matrix34::Zero())
- , velocity_(Matrix34::Zero())
- , acceleration_(Matrix34::Zero())
- , position_base_(Matrix34::Zero())
- , velocity_base_(Matrix34::Zero())
- , acceleration_base_(Matrix34::Zero())
-{
+ : gait_(NULL),
+ dt_wbc(0.0),
+ k_mpc(0),
+ maxHeight_(0.0),
+ lockTime_(0.0),
+ feet(),
+ t0s(Vector4::Zero()),
+ t_swing(Vector4::Zero()),
+ targetFootstep_(Matrix34::Zero()),
+ Ax(Matrix64::Zero()),
+ Ay(Matrix64::Zero()),
+ position_(Matrix34::Zero()),
+ velocity_(Matrix34::Zero()),
+ acceleration_(Matrix34::Zero()),
+ position_base_(Matrix34::Zero()),
+ velocity_base_(Matrix34::Zero()),
+ acceleration_base_(Matrix34::Zero()) {}
+
+void FootTrajectoryGenerator::initialize(Params ¶ms, Gait &gaitIn) {
+ dt_wbc = params.dt_wbc;
+ k_mpc = (int)std::round(params.dt_mpc / params.dt_wbc);
+ maxHeight_ = params.max_height;
+ lockTime_ = params.lock_time;
+ vertTime_ = params.vert_time;
+ targetFootstep_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(),
+ params.footsteps_init.size());
+ position_ = targetFootstep_;
+ position_base_ = targetFootstep_;
+ gait_ = &gaitIn;
}
-void FootTrajectoryGenerator::initialize(Params& params, Gait& gaitIn)
-{
- dt_wbc = params.dt_wbc;
- k_mpc = (int)std::round(params.dt_mpc / params.dt_wbc);
- maxHeight_ = params.max_height;
- lockTime_ = params.lock_time;
- vertTime_ = params.vert_time;
- targetFootstep_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(), params.footsteps_init.size());
- position_ = targetFootstep_;
- position_base_ = targetFootstep_;
- gait_ = &gaitIn;
+void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &targetFootstep) {
+ double ddx0 = acceleration_(0, j);
+ double ddy0 = acceleration_(1, j);
+ double dx0 = velocity_(0, j);
+ double dy0 = velocity_(1, j);
+ double x0 = position_(0, j);
+ double y0 = position_(1, j);
+
+ double t = t0s[j] - vertTime_;
+ double d = t_swing[j] - 2 * vertTime_;
+ double dt = dt_wbc;
+
+ if (t < d - lockTime_) {
+ // compute polynoms coefficients for x and y
+ Ax(0, j) = (ddx0 * std::pow(t, 2) - 2 * ddx0 * t * d - 6 * dx0 * t + ddx0 * std::pow(d, 2) + 6 * dx0 * d +
+ 12 * x0 - 12 * targetFootstep[0]) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(1, j) =
+ (30 * t * targetFootstep[0] - 30 * t * x0 - 30 * d * x0 + 30 * d * targetFootstep[0] -
+ 2 * std::pow(t, 3) * ddx0 - 3 * std::pow(d, 3) * ddx0 + 14 * std::pow(t, 2) * dx0 -
+ 16 * std::pow(d, 2) * dx0 + 2 * t * d * dx0 + 4 * t * std::pow(d, 2) * ddx0 + std::pow(t, 2) * d * ddx0) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(2, j) = (std::pow(t, 4) * ddx0 + 3 * std::pow(d, 4) * ddx0 - 8 * std::pow(t, 3) * dx0 +
+ 12 * std::pow(d, 3) * dx0 + 20 * std::pow(t, 2) * x0 - 20 * std::pow(t, 2) * targetFootstep[0] +
+ 20 * std::pow(d, 2) * x0 - 20 * std::pow(d, 2) * targetFootstep[0] + 80 * t * d * x0 -
+ 80 * t * d * targetFootstep[0] + 4 * std::pow(t, 3) * d * ddx0 + 28 * t * std::pow(d, 2) * dx0 -
+ 32 * std::pow(t, 2) * d * dx0 - 8 * std::pow(t, 2) * std::pow(d, 2) * ddx0) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(3, j) = -(std::pow(d, 5) * ddx0 + 4 * t * std::pow(d, 4) * ddx0 + 3 * std::pow(t, 4) * d * ddx0 +
+ 36 * t * std::pow(d, 3) * dx0 - 24 * std::pow(t, 3) * d * dx0 + 60 * t * std::pow(d, 2) * x0 +
+ 60 * std::pow(t, 2) * d * x0 - 60 * t * std::pow(d, 2) * targetFootstep[0] -
+ 60 * std::pow(t, 2) * d * targetFootstep[0] - 8 * std::pow(t, 2) * std::pow(d, 3) * ddx0 -
+ 12 * std::pow(t, 2) * std::pow(d, 2) * dx0) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(4, j) = -(2 * std::pow(d, 5) * dx0 - 2 * t * std::pow(d, 5) * ddx0 - 10 * t * std::pow(d, 4) * dx0 +
+ std::pow(t, 2) * std::pow(d, 4) * ddx0 + 4 * std::pow(t, 3) * std::pow(d, 3) * ddx0 -
+ 3 * std::pow(t, 4) * std::pow(d, 2) * ddx0 - 16 * std::pow(t, 2) * std::pow(d, 3) * dx0 +
+ 24 * std::pow(t, 3) * std::pow(d, 2) * dx0 - 60 * std::pow(t, 2) * std::pow(d, 2) * x0 +
+ 60 * std::pow(t, 2) * std::pow(d, 2) * targetFootstep[0]) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ax(5, j) = (2 * targetFootstep[0] * std::pow(t, 5) - ddx0 * std::pow(t, 4) * std::pow(d, 3) -
+ 10 * targetFootstep[0] * std::pow(t, 4) * d + 2 * ddx0 * std::pow(t, 3) * std::pow(d, 4) +
+ 8 * dx0 * std::pow(t, 3) * std::pow(d, 3) + 20 * targetFootstep[0] * std::pow(t, 3) * std::pow(d, 2) -
+ ddx0 * std::pow(t, 2) * std::pow(d, 5) - 10 * dx0 * std::pow(t, 2) * std::pow(d, 4) -
+ 20 * x0 * std::pow(t, 2) * std::pow(d, 3) + 2 * dx0 * t * std::pow(d, 5) +
+ 10 * x0 * t * std::pow(d, 4) - 2 * x0 * std::pow(d, 5)) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+
+ Ay(0, j) = (ddy0 * std::pow(t, 2) - 2 * ddy0 * t * d - 6 * dy0 * t + ddy0 * std::pow(d, 2) + 6 * dy0 * d +
+ 12 * y0 - 12 * targetFootstep[1]) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(1, j) =
+ (30 * t * targetFootstep[1] - 30 * t * y0 - 30 * d * y0 + 30 * d * targetFootstep[1] -
+ 2 * std::pow(t, 3) * ddy0 - 3 * std::pow(d, 3) * ddy0 + 14 * std::pow(t, 2) * dy0 -
+ 16 * std::pow(d, 2) * dy0 + 2 * t * d * dy0 + 4 * t * std::pow(d, 2) * ddy0 + std::pow(t, 2) * d * ddy0) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(2, j) = (std::pow(t, 4) * ddy0 + 3 * std::pow(d, 4) * ddy0 - 8 * std::pow(t, 3) * dy0 +
+ 12 * std::pow(d, 3) * dy0 + 20 * std::pow(t, 2) * y0 - 20 * std::pow(t, 2) * targetFootstep[1] +
+ 20 * std::pow(d, 2) * y0 - 20 * std::pow(d, 2) * targetFootstep[1] + 80 * t * d * y0 -
+ 80 * t * d * targetFootstep[1] + 4 * std::pow(t, 3) * d * ddy0 + 28 * t * std::pow(d, 2) * dy0 -
+ 32 * std::pow(t, 2) * d * dy0 - 8 * std::pow(t, 2) * std::pow(d, 2) * ddy0) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(3, j) = -(std::pow(d, 5) * ddy0 + 4 * t * std::pow(d, 4) * ddy0 + 3 * std::pow(t, 4) * d * ddy0 +
+ 36 * t * std::pow(d, 3) * dy0 - 24 * std::pow(t, 3) * d * dy0 + 60 * t * std::pow(d, 2) * y0 +
+ 60 * std::pow(t, 2) * d * y0 - 60 * t * std::pow(d, 2) * targetFootstep[1] -
+ 60 * std::pow(t, 2) * d * targetFootstep[1] - 8 * std::pow(t, 2) * std::pow(d, 3) * ddy0 -
+ 12 * std::pow(t, 2) * std::pow(d, 2) * dy0) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(4, j) = -(2 * std::pow(d, 5) * dy0 - 2 * t * std::pow(d, 5) * ddy0 - 10 * t * std::pow(d, 4) * dy0 +
+ std::pow(t, 2) * std::pow(d, 4) * ddy0 + 4 * std::pow(t, 3) * std::pow(d, 3) * ddy0 -
+ 3 * std::pow(t, 4) * std::pow(d, 2) * ddy0 - 16 * std::pow(t, 2) * std::pow(d, 3) * dy0 +
+ 24 * std::pow(t, 3) * std::pow(d, 2) * dy0 - 60 * std::pow(t, 2) * std::pow(d, 2) * y0 +
+ 60 * std::pow(t, 2) * std::pow(d, 2) * targetFootstep[1]) /
+ (2 * std::pow((t - d), 2) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+ Ay(5, j) = (2 * targetFootstep[1] * std::pow(t, 5) - ddy0 * std::pow(t, 4) * std::pow(d, 3) -
+ 10 * targetFootstep[1] * std::pow(t, 4) * d + 2 * ddy0 * std::pow(t, 3) * std::pow(d, 4) +
+ 8 * dy0 * std::pow(t, 3) * std::pow(d, 3) + 20 * targetFootstep[1] * std::pow(t, 3) * std::pow(d, 2) -
+ ddy0 * std::pow(t, 2) * std::pow(d, 5) - 10 * dy0 * std::pow(t, 2) * std::pow(d, 4) -
+ 20 * y0 * std::pow(t, 2) * std::pow(d, 3) + 2 * dy0 * t * std::pow(d, 5) +
+ 10 * y0 * t * std::pow(d, 4) - 2 * y0 * std::pow(d, 5)) /
+ (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) *
+ (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
+
+ targetFootstep_(0, j) = targetFootstep[0];
+ targetFootstep_(1, j) = targetFootstep[1];
+ }
+
+ // Coefficients for z (deterministic)
+ double Tz = t_swing[j];
+ Vector4 Az;
+ Az(0, j) = -maxHeight_ / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
+ Az(1, j) = (3 * Tz * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
+ Az(2, j) = -(3 * std::pow(Tz, 2) * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
+ Az(3, j) = (std::pow(Tz, 3) * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
+
+ // Get the next point
+ double ev = t + dt;
+ double evz = t0s[j] + dt;
+
+ if (t < 0.0 || t > d) // Just vertical motion
+ {
+ position_(0, j) = x0;
+ position_(1, j) = y0;
+ velocity_(0, j) = 0.0;
+ velocity_(1, j) = 0.0;
+ acceleration_(0, j) = 0.0;
+ acceleration_(1, j) = 0.0;
+ } else {
+ position_(0, j) = Ax(5, j) + Ax(4, j) * ev + Ax(3, j) * std::pow(ev, 2) + Ax(2, j) * std::pow(ev, 3) +
+ Ax(1, j) * std::pow(ev, 4) + Ax(0, j) * std::pow(ev, 5);
+ position_(1, j) = Ay(5, j) + Ay(4, j) * ev + Ay(3, j) * std::pow(ev, 2) + Ay(2, j) * std::pow(ev, 3) +
+ Ay(1, j) * std::pow(ev, 4) + Ay(0, j) * std::pow(ev, 5);
+ velocity_(0, j) = Ax(4, j) + 2 * Ax(3, j) * ev + 3 * Ax(2, j) * std::pow(ev, 2) + 4 * Ax(1, j) * std::pow(ev, 3) +
+ 5 * Ax(0, j) * std::pow(ev, 4);
+ velocity_(1, j) = Ay(4, j) + 2 * Ay(3, j) * ev + 3 * Ay(2, j) * std::pow(ev, 2) + 4 * Ay(1, j) * std::pow(ev, 3) +
+ 5 * Ay(0, j) * std::pow(ev, 4);
+ acceleration_(0, j) =
+ 2 * Ax(3, j) + 3 * 2 * Ax(2, j) * ev + 4 * 3 * Ax(1, j) * std::pow(ev, 2) + 5 * 4 * Ax(0, j) * std::pow(ev, 3);
+ acceleration_(1, j) =
+ 2 * Ay(3, j) + 3 * 2 * Ay(2, j) * ev + 4 * 3 * Ay(1, j) * std::pow(ev, 2) + 5 * 4 * Ay(0, j) * std::pow(ev, 3);
+ }
+ velocity_(2, j) = 3 * Az(3, j) * std::pow(evz, 2) + 4 * Az(2, j) * std::pow(evz, 3) +
+ 5 * Az(1, j) * std::pow(evz, 4) + 6 * Az(0, j) * std::pow(evz, 5);
+ acceleration_(2, j) = 2 * 3 * Az(3, j) * evz + 3 * 4 * Az(2, j) * std::pow(evz, 2) +
+ 4 * 5 * Az(1, j) * std::pow(evz, 3) + 5 * 6 * Az(0, j) * std::pow(evz, 4);
+ position_(2, j) = Az(3, j) * std::pow(evz, 3) + Az(2, j) * std::pow(evz, 4) + Az(1, j) * std::pow(evz, 5) +
+ Az(0, j) * std::pow(evz, 6);
}
-
-void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const& targetFootstep)
-{
- double ddx0 = acceleration_(0, j);
- double ddy0 = acceleration_(1, j);
- double dx0 = velocity_(0, j);
- double dy0 = velocity_(1, j);
- double x0 = position_(0, j);
- double y0 = position_(1, j);
-
- double t = t0s[j] - vertTime_;
- double d = t_swing[j] - 2 * vertTime_;
- double dt = dt_wbc;
-
- if (t < d - lockTime_)
- {
- // compute polynoms coefficients for x and y
- Ax(0, j) = (ddx0 * std::pow(t, 2) - 2 * ddx0 * t * d - 6 * dx0 * t + ddx0 * std::pow(d, 2) + 6 * dx0 * d + 12 * x0 - 12 * targetFootstep[0]) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ax(1, j) = (30 * t * targetFootstep[0] - 30 * t * x0 - 30 * d * x0 + 30 * d * targetFootstep[0] - 2 * std::pow(t, 3) * ddx0 - 3 * std::pow(d, 3) * ddx0 + 14 * std::pow(t, 2) * dx0 - 16 * std::pow(d, 2) * dx0 + 2 * t * d * dx0 + 4 * t * std::pow(d, 2) * ddx0 + std::pow(t, 2) * d * ddx0) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ax(2, j) = (std::pow(t, 4) * ddx0 + 3 * std::pow(d, 4) * ddx0 - 8 * std::pow(t, 3) * dx0 + 12 * std::pow(d, 3) * dx0 + 20 * std::pow(t, 2) * x0 - 20 * std::pow(t, 2) * targetFootstep[0] + 20 * std::pow(d, 2) * x0 - 20 * std::pow(d, 2) * targetFootstep[0] + 80 * t * d * x0 - 80 * t * d * targetFootstep[0] + 4 * std::pow(t, 3) * d * ddx0 + 28 * t * std::pow(d, 2) * dx0 - 32 * std::pow(t, 2) * d * dx0 - 8 * std::pow(t, 2) * std::pow(d, 2) * ddx0) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ax(3, j) = -(std::pow(d, 5) * ddx0 + 4 * t * std::pow(d, 4) * ddx0 + 3 * std::pow(t, 4) * d * ddx0 + 36 * t * std::pow(d, 3) * dx0 - 24 * std::pow(t, 3) * d * dx0 + 60 * t * std::pow(d, 2) * x0 + 60 * std::pow(t, 2) * d * x0 - 60 * t * std::pow(d, 2) * targetFootstep[0] - 60 * std::pow(t, 2) * d * targetFootstep[0] - 8 * std::pow(t, 2) * std::pow(d, 3) * ddx0 - 12 * std::pow(t, 2) * std::pow(d, 2) * dx0) / (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ax(4, j) = -(2 * std::pow(d, 5) * dx0 - 2 * t * std::pow(d, 5) * ddx0 - 10 * t * std::pow(d, 4) * dx0 + std::pow(t, 2) * std::pow(d, 4) * ddx0 + 4 * std::pow(t, 3) * std::pow(d, 3) * ddx0 - 3 * std::pow(t, 4) * std::pow(d, 2) * ddx0 - 16 * std::pow(t, 2) * std::pow(d, 3) * dx0 + 24 * std::pow(t, 3) * std::pow(d, 2) * dx0 - 60 * std::pow(t, 2) * std::pow(d, 2) * x0 + 60 * std::pow(t, 2) * std::pow(d, 2) * targetFootstep[0]) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ax(5, j) = (2 * targetFootstep[0] * std::pow(t, 5) - ddx0 * std::pow(t, 4) * std::pow(d, 3) - 10 * targetFootstep[0] * std::pow(t, 4) * d + 2 * ddx0 * std::pow(t, 3) * std::pow(d, 4) + 8 * dx0 * std::pow(t, 3) * std::pow(d, 3) + 20 * targetFootstep[0] * std::pow(t, 3) * std::pow(d, 2) - ddx0 * std::pow(t, 2) * std::pow(d, 5) - 10 * dx0 * std::pow(t, 2) * std::pow(d, 4) - 20 * x0 * std::pow(t, 2) * std::pow(d, 3) + 2 * dx0 * t * std::pow(d, 5) + 10 * x0 * t * std::pow(d, 4) - 2 * x0 * std::pow(d, 5)) / (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
-
- Ay(0, j) = (ddy0 * std::pow(t, 2) - 2 * ddy0 * t * d - 6 * dy0 * t + ddy0 * std::pow(d, 2) + 6 * dy0 * d + 12 * y0 - 12 * targetFootstep[1]) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ay(1, j) = (30 * t * targetFootstep[1] - 30 * t * y0 - 30 * d * y0 + 30 * d * targetFootstep[1] - 2 * std::pow(t, 3) * ddy0 - 3 * std::pow(d, 3) * ddy0 + 14 * std::pow(t, 2) * dy0 - 16 * std::pow(d, 2) * dy0 + 2 * t * d * dy0 + 4 * t * std::pow(d, 2) * ddy0 + std::pow(t, 2) * d * ddy0) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ay(2, j) = (std::pow(t, 4) * ddy0 + 3 * std::pow(d, 4) * ddy0 - 8 * std::pow(t, 3) * dy0 + 12 * std::pow(d, 3) * dy0 + 20 * std::pow(t, 2) * y0 - 20 * std::pow(t, 2) * targetFootstep[1] + 20 * std::pow(d, 2) * y0 - 20 * std::pow(d, 2) * targetFootstep[1] + 80 * t * d * y0 - 80 * t * d * targetFootstep[1] + 4 * std::pow(t, 3) * d * ddy0 + 28 * t * std::pow(d, 2) * dy0 - 32 * std::pow(t, 2) * d * dy0 - 8 * std::pow(t, 2) * std::pow(d, 2) * ddy0) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ay(3, j) = -(std::pow(d, 5) * ddy0 + 4 * t * std::pow(d, 4) * ddy0 + 3 * std::pow(t, 4) * d * ddy0 + 36 * t * std::pow(d, 3) * dy0 - 24 * std::pow(t, 3) * d * dy0 + 60 * t * std::pow(d, 2) * y0 + 60 * std::pow(t, 2) * d * y0 - 60 * t * std::pow(d, 2) * targetFootstep[1] - 60 * std::pow(t, 2) * d * targetFootstep[1] - 8 * std::pow(t, 2) * std::pow(d, 3) * ddy0 - 12 * std::pow(t, 2) * std::pow(d, 2) * dy0) / (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ay(4, j) = -(2 * std::pow(d, 5) * dy0 - 2 * t * std::pow(d, 5) * ddy0 - 10 * t * std::pow(d, 4) * dy0 + std::pow(t, 2) * std::pow(d, 4) * ddy0 + 4 * std::pow(t, 3) * std::pow(d, 3) * ddy0 - 3 * std::pow(t, 4) * std::pow(d, 2) * ddy0 - 16 * std::pow(t, 2) * std::pow(d, 3) * dy0 + 24 * std::pow(t, 3) * std::pow(d, 2) * dy0 - 60 * std::pow(t, 2) * std::pow(d, 2) * y0 + 60 * std::pow(t, 2) * std::pow(d, 2) * targetFootstep[1]) / (2 * std::pow((t - d), 2) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
- Ay(5, j) = (2 * targetFootstep[1] * std::pow(t, 5) - ddy0 * std::pow(t, 4) * std::pow(d, 3) - 10 * targetFootstep[1] * std::pow(t, 4) * d + 2 * ddy0 * std::pow(t, 3) * std::pow(d, 4) + 8 * dy0 * std::pow(t, 3) * std::pow(d, 3) + 20 * targetFootstep[1] * std::pow(t, 3) * std::pow(d, 2) - ddy0 * std::pow(t, 2) * std::pow(d, 5) - 10 * dy0 * std::pow(t, 2) * std::pow(d, 4) - 20 * y0 * std::pow(t, 2) * std::pow(d, 3) + 2 * dy0 * t * std::pow(d, 5) + 10 * y0 * t * std::pow(d, 4) - 2 * y0 * std::pow(d, 5)) / (2 * (std::pow(t, 2) - 2 * t * d + std::pow(d, 2)) * (std::pow(t, 3) - 3 * std::pow(t, 2) * d + 3 * t * std::pow(d, 2) - std::pow(d, 3)));
-
- targetFootstep_(0, j) = targetFootstep[0];
- targetFootstep_(1, j) = targetFootstep[1];
- }
-
- // Coefficients for z (deterministic)
- double Tz = t_swing[j];
- Vector4 Az;
- Az(0, j) = -maxHeight_ / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
- Az(1, j) = (3 * Tz * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
- Az(2, j) = -(3 * std::pow(Tz, 2) * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
- Az(3, j) = (std::pow(Tz, 3) * maxHeight_) / (std::pow((Tz / 2), 3) * std::pow((Tz - Tz / 2), 3));
-
- // Get the next point
- double ev = t + dt;
- double evz = t0s[j] + dt;
-
- if (t < 0.0 || t > d) // Just vertical motion
- {
- position_(0, j) = x0;
- position_(1, j) = y0;
- velocity_(0, j) = 0.0;
- velocity_(1, j) = 0.0;
- acceleration_(0, j) = 0.0;
- acceleration_(1, j) = 0.0;
+void FootTrajectoryGenerator::update(int k, MatrixN const &targetFootstep) {
+ if ((k % k_mpc) == 0) {
+ // Indexes of feet in swing phase
+ feet.clear();
+ for (int i = 0; i < 4; i++) {
+ if (gait_->getCurrentGait()(0, i) == 0) feet.push_back(i);
}
- else
- {
- position_(0, j) = Ax(5, j) + Ax(4, j) * ev + Ax(3, j) * std::pow(ev, 2) + Ax(2, j) * std::pow(ev, 3) + Ax(1, j) * std::pow(ev, 4) + Ax(0, j) * std::pow(ev, 5);
- position_(1, j) = Ay(5, j) + Ay(4, j) * ev + Ay(3, j) * std::pow(ev, 2) + Ay(2, j) * std::pow(ev, 3) + Ay(1, j) * std::pow(ev, 4) + Ay(0, j) * std::pow(ev, 5);
- velocity_(0, j) = Ax(4, j) + 2 * Ax(3, j) * ev + 3 * Ax(2, j) * std::pow(ev, 2) + 4 * Ax(1, j) * std::pow(ev, 3) + 5 * Ax(0, j) * std::pow(ev, 4);
- velocity_(1, j) = Ay(4, j) + 2 * Ay(3, j) * ev + 3 * Ay(2, j) * std::pow(ev, 2) + 4 * Ay(1, j) * std::pow(ev, 3) + 5 * Ay(0, j) * std::pow(ev, 4);
- acceleration_(0, j) = 2 * Ax(3, j) + 3 * 2 * Ax(2, j) * ev + 4 * 3 * Ax(1, j) * std::pow(ev, 2) + 5 * 4 * Ax(0, j) * std::pow(ev, 3);
- acceleration_(1, j) = 2 * Ay(3, j) + 3 * 2 * Ay(2, j) * ev + 4 * 3 * Ay(1, j) * std::pow(ev, 2) + 5 * 4 * Ay(0, j) * std::pow(ev, 3);
- }
- velocity_(2, j) = 3 * Az(3, j) * std::pow(evz, 2) + 4 * Az(2, j) * std::pow(evz, 3) + 5 * Az(1, j) * std::pow(evz, 4) + 6 * Az(0, j) * std::pow(evz, 5);
- acceleration_(2, j) = 2 * 3 * Az(3, j) * evz + 3 * 4 * Az(2, j) * std::pow(evz, 2) + 4 * 5 * Az(1, j) * std::pow(evz, 3) + 5 * 6 * Az(0, j) * std::pow(evz, 4);
- position_(2, j) = Az(3, j) * std::pow(evz, 3) + Az(2, j) * std::pow(evz, 4) + Az(1, j) * std::pow(evz, 5) + Az(0, j) * std::pow(evz, 6);
-}
-
-void FootTrajectoryGenerator::update(int k, MatrixN const& targetFootstep)
-{
- if ((k % k_mpc) == 0)
- {
- // Indexes of feet in swing phase
- feet.clear();
- for (int i = 0; i < 4; i++)
- {
- if (gait_->getCurrentGait()(0, i) == 0)
- feet.push_back(i);
- }
- // If no foot in swing phase
- if (feet.size() == 0)
- return;
-
- // For each foot in swing phase get remaining duration of the swing phase
- for (int j = 0; j < (int)feet.size(); j++)
- {
- int i = feet[j];
- t_swing[i] = gait_->getPhaseDuration(0, feet[j], 0.0); // 0.0 for swing phase
- double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
- t0s[i] = std::max(0.0, value);
- }
+ // If no foot in swing phase
+ if (feet.size() == 0) return;
+
+ // For each foot in swing phase get remaining duration of the swing phase
+ for (int j = 0; j < (int)feet.size(); j++) {
+ int i = feet[j];
+ t_swing[i] = gait_->getPhaseDuration(0, feet[j], 0.0); // 0.0 for swing phase
+ double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
+ t0s[i] = std::max(0.0, value);
}
- else
- {
- // If no foot in swing phase
- if (feet.size() == 0)
- return;
-
- // Increment of one time step for feet in swing phase
- for (int i = 0; i < (int)feet.size(); i++)
- {
- double value = t0s[feet[i]] + dt_wbc;
- t0s[feet[i]] = std::max(0.0, value);
- }
+ } else {
+ // If no foot in swing phase
+ if (feet.size() == 0) return;
+
+ // Increment of one time step for feet in swing phase
+ for (int i = 0; i < (int)feet.size(); i++) {
+ double value = t0s[feet[i]] + dt_wbc;
+ t0s[feet[i]] = std::max(0.0, value);
}
+ }
- for (int i = 0; i < (int)feet.size(); i++)
- {
- updateFootPosition(feet[i], targetFootstep.col(feet[i]));
- }
- return;
+ for (int i = 0; i < (int)feet.size(); i++) {
+ updateFootPosition(feet[i], targetFootstep.col(feet[i]));
+ }
+ return;
}
-Eigen::MatrixXd FootTrajectoryGenerator::getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &T) {
-
- position_base_ = R * (position_ - T.replicate<1, 4>()); // Value saved because it is used to get velocity and acceleration
- return position_base_;
+Eigen::MatrixXd FootTrajectoryGenerator::getFootPositionBaseFrame(const Eigen::Matrix<double, 3, 3> &R,
+ const Eigen::Matrix<double, 3, 1> &T) {
+ position_base_ =
+ R * (position_ - T.replicate<1, 4>()); // Value saved because it is used to get velocity and acceleration
+ return position_base_;
}
-Eigen::MatrixXd FootTrajectoryGenerator::getFootVelocityBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &v_ref,
+Eigen::MatrixXd FootTrajectoryGenerator::getFootVelocityBaseFrame(const Eigen::Matrix<double, 3, 3> &R,
+ const Eigen::Matrix<double, 3, 1> &v_ref,
const Eigen::Matrix<double, 3, 1> &w_ref) {
-
- velocity_base_ = R * velocity_ - v_ref.replicate<1, 4>() + position_base_.colwise().cross(w_ref); // Value saved because it is used to get acceleration
- return velocity_base_;
+ velocity_base_ = R * velocity_ - v_ref.replicate<1, 4>() +
+ position_base_.colwise().cross(w_ref); // Value saved because it is used to get acceleration
+ return velocity_base_;
}
-Eigen::MatrixXd FootTrajectoryGenerator::getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3> &R, const Eigen::Matrix<double, 3, 1> &w_ref) {
-
- return R * acceleration_ - (position_base_.colwise().cross(w_ref)).colwise().cross(w_ref) + 2 * velocity_base_.colwise().cross(w_ref);
+Eigen::MatrixXd FootTrajectoryGenerator::getFootAccelerationBaseFrame(const Eigen::Matrix<double, 3, 3> &R,
+ const Eigen::Matrix<double, 3, 1> &w_ref) {
+ return R * acceleration_ - (position_base_.colwise().cross(w_ref)).colwise().cross(w_ref) +
+ 2 * velocity_base_.colwise().cross(w_ref);
}
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index 6030d8c3..e1e10f8d 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -1,292 +1,261 @@
#include "qrw/FootstepPlanner.hpp"
FootstepPlanner::FootstepPlanner()
- : gait_(NULL)
- , g(9.81)
- , L(0.155)
- , nextFootstep_(Matrix34::Zero())
- , footsteps_()
- , Rz(MatrixN::Zero(3, 3))
- , dt_cum()
- , yaws()
- , dx()
- , dy()
- , q_dxdy(Vector3::Zero())
- , RPY_(Vector3::Zero())
- , pos_feet_(Matrix34::Zero())
- , q_FK_(Vector19::Zero())
-{
- // Empty
+ : gait_(NULL),
+ g(9.81),
+ L(0.155),
+ nextFootstep_(Matrix34::Zero()),
+ footsteps_(),
+ Rz(MatrixN::Zero(3, 3)),
+ dt_cum(),
+ yaws(),
+ dx(),
+ dy(),
+ q_dxdy(Vector3::Zero()),
+ RPY_(Vector3::Zero()),
+ pos_feet_(Matrix34::Zero()),
+ q_FK_(Vector19::Zero()) {
+ // Empty
}
-void FootstepPlanner::initialize(Params& params, Gait& gaitIn)
-{
- params_ = ¶ms;
- dt = params.dt_mpc;
- dt_wbc = params.dt_wbc;
- T_mpc = params.T_mpc;
- h_ref = params.h_ref;
- n_steps = (int)std::lround(params.T_mpc / params.dt_mpc);
- footsteps_under_shoulders_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_under_shoulders.data(), params.footsteps_under_shoulders.size());
- currentFootstep_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(), params.footsteps_init.size());
- gait_ = &gaitIn;
- targetFootstep_ = currentFootstep_;
- o_targetFootstep_ = currentFootstep_;
- dt_cum = VectorN::Zero(params.N_gait);
- yaws = VectorN::Zero(params.N_gait);
- dx = VectorN::Zero(params.N_gait);
- dy = VectorN::Zero(params.N_gait);
- for (int i = 0; i < params.N_gait; i++)
- {
- footsteps_.push_back(Matrix34::Zero());
- }
- Rz(2, 2) = 1.0;
-
- // Path to the robot URDF (TODO: Automatic path)
- const std::string filename = std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
-
- // Build model from urdf (base is not free flyer)
- pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
-
- // Construct data from model
- data_ = pinocchio::Data(model_);
+void FootstepPlanner::initialize(Params& params, Gait& gaitIn) {
+ params_ = ¶ms;
+ dt = params.dt_mpc;
+ dt_wbc = params.dt_wbc;
+ T_mpc = params.T_mpc;
+ h_ref = params.h_ref;
+ n_steps = (int)std::lround(params.T_mpc / params.dt_mpc);
+ footsteps_under_shoulders_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_under_shoulders.data(),
+ params.footsteps_under_shoulders.size());
+ currentFootstep_ << Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(params.footsteps_init.data(),
+ params.footsteps_init.size());
+ gait_ = &gaitIn;
+ targetFootstep_ = currentFootstep_;
+ o_targetFootstep_ = currentFootstep_;
+ dt_cum = VectorN::Zero(params.N_gait);
+ yaws = VectorN::Zero(params.N_gait);
+ dx = VectorN::Zero(params.N_gait);
+ dy = VectorN::Zero(params.N_gait);
+ for (int i = 0; i < params.N_gait; i++) {
+ footsteps_.push_back(Matrix34::Zero());
+ }
+ Rz(2, 2) = 1.0;
+
+ // Path to the robot URDF (TODO: Automatic path)
+ const std::string filename =
+ std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
+
+ // Build model from urdf (base is not free flyer)
+ pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
+
+ // Construct data from model
+ data_ = pinocchio::Data(model_);
+
+ // Update all the quantities of the model
+ VectorN q_tmp = VectorN::Zero(model_.nq);
+ q_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
+ pinocchio::computeAllTerms(model_, data_, q_tmp, VectorN::Zero(model_.nv));
+
+ // Get feet frame IDs
+ foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT")); // from long uint to int
+ foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
+ foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
+ foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
+}
- // Update all the quantities of the model
- VectorN q_tmp = VectorN::Zero(model_.nq);
- q_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
- pinocchio::computeAllTerms(model_, data_ , q_tmp, VectorN::Zero(model_.nv));
+MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v,
+ Vector6 const& b_vref) {
+ if (q.rows() != 18) {
+ throw std::runtime_error("q should be a vector of size 18 (pos+RPY+mot)");
+ }
+
+ // Update location of feet in stance phase (for those which just entered stance phase)
+ if (refresh && gait_->isNewPhase()) {
+ updateNewContact(q);
+ }
+
+ // Feet in contact with the ground are moving in base frame (they don't move in world frame)
+ double rotation_yaw = dt_wbc * b_vref(5); // Rotation along Z for the last time step
+ double c = std::cos(rotation_yaw);
+ double s = std::sin(rotation_yaw);
+ Rz.topLeftCorner<2, 2>() << c, s, -s, c;
+ Vector2 dpos = dt_wbc * b_vref.head(2); // Displacement along X and Y for the last time step
+ for (int j = 0; j < 4; j++) {
+ if (gait_->getCurrentGaitCoeff(0, j) == 1.0) {
+ currentFootstep_.block(0, j, 2, 1) = Rz * (currentFootstep_.block(0, j, 2, 1) - dpos);
+ }
+ }
- // Get feet frame IDs
- foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT")); // from long uint to int
- foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
- foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
- foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
+ // Compute location of footsteps
+ return computeTargetFootstep(k, q.head(6), b_v, b_vref);
}
-MatrixN FootstepPlanner::updateFootsteps(bool refresh, int k, VectorN const& q, Vector6 const& b_v, Vector6 const& b_vref)
-{
- if (q.rows() != 18)
- {
- throw std::runtime_error("q should be a vector of size 18 (pos+RPY+mot)");
- }
+void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_vref) {
+ for (uint i = 0; i < footsteps_.size(); i++) {
+ footsteps_[i] = Matrix34::Zero();
+ }
+ MatrixN gait = gait_->getCurrentGait();
- // Update location of feet in stance phase (for those which just entered stance phase)
- if (refresh && gait_->isNewPhase())
- {
- updateNewContact(q);
+ // Set current position of feet for feet in stance phase
+ for (int j = 0; j < 4; j++) {
+ if (gait(0, j) == 1.0) {
+ footsteps_[0].col(j) = currentFootstep_.col(j);
}
-
- // Feet in contact with the ground are moving in base frame (they don't move in world frame)
- double rotation_yaw = dt_wbc * b_vref(5); // Rotation along Z for the last time step
- double c = std::cos(rotation_yaw);
- double s = std::sin(rotation_yaw);
- Rz.topLeftCorner<2, 2>() << c, s, -s, c;
- Vector2 dpos = dt_wbc * b_vref.head(2); // Displacement along X and Y for the last time step
- for (int j = 0; j < 4; j++)
- {
- if (gait_->getCurrentGaitCoeff(0, j) == 1.0)
- {
- currentFootstep_.block(0, j, 2, 1) = Rz * (currentFootstep_.block(0, j, 2, 1) - dpos);
- }
+ }
+
+ // Cumulative time by adding the terms in the first column (remaining number of timesteps)
+ // Get future yaw yaws compared to current position
+ dt_cum(0) = dt_wbc * k;
+ yaws(0) = b_vref(5) * dt_cum(0);
+ for (uint j = 1; j < footsteps_.size(); j++) {
+ dt_cum(j) = gait.row(j).isZero() ? dt_cum(j - 1) : dt_cum(j - 1) + dt;
+ yaws(j) = b_vref(5) * dt_cum(j);
+ }
+
+ // Displacement following the reference velocity compared to current position
+ if (b_vref(5, 0) != 0) {
+ for (uint j = 0; j < footsteps_.size(); j++) {
+ dx(j) = (b_vref(0) * std::sin(b_vref(5) * dt_cum(j)) + b_vref(1) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) /
+ b_vref(5);
+ dy(j) = (b_vref(1) * std::sin(b_vref(5) * dt_cum(j)) - b_vref(0) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) /
+ b_vref(5);
}
-
- // Compute location of footsteps
- return computeTargetFootstep(k, q.head(6), b_v, b_vref);
-}
-
-void FootstepPlanner::computeFootsteps(int k, Vector6 const& b_v, Vector6 const& b_vref)
-{
- for (uint i = 0; i < footsteps_.size(); i++)
- {
- footsteps_[i] = Matrix34::Zero();
+ } else {
+ for (uint j = 0; j < footsteps_.size(); j++) {
+ dx(j) = b_vref(0) * dt_cum(j);
+ dy(j) = b_vref(1) * dt_cum(j);
}
- MatrixN gait = gait_->getCurrentGait();
-
- // Set current position of feet for feet in stance phase
- for (int j = 0; j < 4; j++)
- {
- if (gait(0, j) == 1.0)
- {
- footsteps_[0].col(j) = currentFootstep_.col(j);
- }
+ }
+
+ // Update the footstep matrix depending on the different phases of the gait (swing & stance)
+ int i = 1;
+ while (!gait.row(i).isZero()) {
+ // Feet that were in stance phase and are still in stance phase do not move
+ for (int j = 0; j < 4; j++) {
+ if (gait(i - 1, j) * gait(i, j) > 0) {
+ footsteps_[i].col(j) = footsteps_[i - 1].col(j);
+ }
}
- // Cumulative time by adding the terms in the first column (remaining number of timesteps)
- // Get future yaw yaws compared to current position
- dt_cum(0) = dt_wbc * k;
- yaws(0) = b_vref(5) * dt_cum(0);
- for (uint j = 1; j < footsteps_.size(); j++)
- {
- dt_cum(j) = gait.row(j).isZero() ? dt_cum(j - 1) : dt_cum(j - 1) + dt;
- yaws(j) = b_vref(5) * dt_cum(j);
- }
+ // Feet that were in swing phase and are now in stance phase need to be updated
+ for (int j = 0; j < 4; j++) {
+ if ((1 - gait(i - 1, j)) * gait(i, j) > 0) {
+ // Offset to the future position
+ q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
- // Displacement following the reference velocity compared to current position
- if (b_vref(5, 0) != 0)
- {
- for (uint j = 0; j < footsteps_.size(); j++)
- {
- dx(j) = (b_vref(0) * std::sin(b_vref(5) * dt_cum(j)) + b_vref(1) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) / b_vref(5);
- dy(j) = (b_vref(1) * std::sin(b_vref(5) * dt_cum(j)) - b_vref(0) * (std::cos(b_vref(5) * dt_cum(j)) - 1.0)) / b_vref(5);
- }
- }
- else
- {
- for (uint j = 0; j < footsteps_.size(); j++)
- {
- dx(j) = b_vref(0) * dt_cum(j);
- dy(j) = b_vref(1) * dt_cum(j);
- }
- }
+ // Get future desired position of footsteps
+ computeNextFootstep(i, j, b_v, b_vref);
- // Update the footstep matrix depending on the different phases of the gait (swing & stance)
- int i = 1;
- while (!gait.row(i).isZero())
- {
- // Feet that were in stance phase and are still in stance phase do not move
- for (int j = 0; j < 4; j++)
- {
- if (gait(i - 1, j) * gait(i, j) > 0)
- {
- footsteps_[i].col(j) = footsteps_[i - 1].col(j);
- }
- }
-
- // Feet that were in swing phase and are now in stance phase need to be updated
- for (int j = 0; j < 4; j++)
- {
- if ((1 - gait(i - 1, j)) * gait(i, j) > 0)
- {
- // Offset to the future position
- q_dxdy << dx(i - 1, 0), dy(i - 1, 0), 0.0;
-
- // Get future desired position of footsteps
- computeNextFootstep(i, j, b_v, b_vref);
-
- // Get desired position of footstep compared to current position
- double c = std::cos(yaws(i - 1));
- double s = std::sin(yaws(i - 1));
- Rz.topLeftCorner<2, 2>() << c, -s, s, c;
-
- footsteps_[i].col(j) = (Rz * nextFootstep_.col(j) + q_dxdy).transpose();
- }
- }
- i++;
+ // Get desired position of footstep compared to current position
+ double c = std::cos(yaws(i - 1));
+ double s = std::sin(yaws(i - 1));
+ Rz.topLeftCorner<2, 2>() << c, -s, s, c;
+
+ footsteps_[i].col(j) = (Rz * nextFootstep_.col(j) + q_dxdy).transpose();
+ }
}
+ i++;
+ }
}
-void FootstepPlanner::computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref)
-{
- nextFootstep_ = Matrix34::Zero();
+void FootstepPlanner::computeNextFootstep(int i, int j, Vector6 const& b_v, Vector6 const& b_vref) {
+ nextFootstep_ = Matrix34::Zero();
- double t_stance = gait_->getPhaseDuration(i, j, 1.0); // 1.0 for stance phase
+ double t_stance = gait_->getPhaseDuration(i, j, 1.0); // 1.0 for stance phase
- // Add symmetry term
- nextFootstep_.col(j) = t_stance * 0.5 * b_v.head(3);
+ // Add symmetry term
+ nextFootstep_.col(j) = t_stance * 0.5 * b_v.head(3);
- // Add feedback term
- nextFootstep_.col(j) += params_->k_feedback * (b_v.head(3) - b_vref.head(3));
+ // Add feedback term
+ nextFootstep_.col(j) += params_->k_feedback * (b_v.head(3) - b_vref.head(3));
- // Add centrifugal term
- Vector3 cross;
- cross << b_v(1) * b_vref(5) - b_v(2) * b_vref(4), b_v(2) * b_vref(3) - b_v(0) * b_vref(5), 0.0;
- nextFootstep_.col(j) += 0.5 * std::sqrt(h_ref / g) * cross;
+ // Add centrifugal term
+ Vector3 cross;
+ cross << b_v(1) * b_vref(5) - b_v(2) * b_vref(4), b_v(2) * b_vref(3) - b_v(0) * b_vref(5), 0.0;
+ nextFootstep_.col(j) += 0.5 * std::sqrt(h_ref / g) * cross;
- // Legs have a limited length so the deviation has to be limited
- nextFootstep_(0, j) = std::min(nextFootstep_(0, j), L);
- nextFootstep_(0, j) = std::max(nextFootstep_(0, j), -L);
- nextFootstep_(1, j) = std::min(nextFootstep_(1, j), L);
- nextFootstep_(1, j) = std::max(nextFootstep_(1, j), -L);
+ // Legs have a limited length so the deviation has to be limited
+ nextFootstep_(0, j) = std::min(nextFootstep_(0, j), L);
+ nextFootstep_(0, j) = std::max(nextFootstep_(0, j), -L);
+ nextFootstep_(1, j) = std::min(nextFootstep_(1, j), L);
+ nextFootstep_(1, j) = std::max(nextFootstep_(1, j), -L);
- // Add shoulders
- nextFootstep_.col(j) += footsteps_under_shoulders_.col(j);
+ // Add shoulders
+ nextFootstep_.col(j) += footsteps_under_shoulders_.col(j);
- // Remove Z component (working on flat ground)
- nextFootstep_.row(2) = Vector4::Zero().transpose();
+ // Remove Z component (working on flat ground)
+ nextFootstep_.row(2) = Vector4::Zero().transpose();
}
-void FootstepPlanner::updateTargetFootsteps()
-{
- for (int i = 0; i < 4; i++)
- {
- int index = 0;
- while (footsteps_[index](0, i) == 0.0)
- {
- index++;
- }
- targetFootstep_.col(i) << footsteps_[index](0, i), footsteps_[index](1, i), 0.0;
+void FootstepPlanner::updateTargetFootsteps() {
+ for (int i = 0; i < 4; i++) {
+ int index = 0;
+ while (footsteps_[index](0, i) == 0.0) {
+ index++;
}
+ targetFootstep_.col(i) << footsteps_[index](0, i), footsteps_[index](1, i), 0.0;
+ }
}
-MatrixN FootstepPlanner::computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref)
-{
- // Compute the desired location of footsteps over the prediction horizon
- computeFootsteps(k, b_v, b_vref);
-
- // Update desired location of footsteps on the ground
- updateTargetFootsteps();
-
- // Get o_targetFootstep_ in world frame from targetFootstep_ in horizontal frame
- RPY_ = q.tail(3);
- double c = std::cos(RPY_(2));
- double s = std::sin(RPY_(2));
- Rz.topLeftCorner<2, 2>() << c, -s, s, c;
- for (int i = 0; i < 4; i++)
- {
- o_targetFootstep_.block(0, i, 2, 1) = Rz.topLeftCorner<2, 2>() * targetFootstep_.block(0, i, 2, 1) + q.head(2);
- }
+MatrixN FootstepPlanner::computeTargetFootstep(int k, Vector6 const& q, Vector6 const& b_v, Vector6 const& b_vref) {
+ // Compute the desired location of footsteps over the prediction horizon
+ computeFootsteps(k, b_v, b_vref);
- return o_targetFootstep_;
+ // Update desired location of footsteps on the ground
+ updateTargetFootsteps();
+
+ // Get o_targetFootstep_ in world frame from targetFootstep_ in horizontal frame
+ RPY_ = q.tail(3);
+ double c = std::cos(RPY_(2));
+ double s = std::sin(RPY_(2));
+ Rz.topLeftCorner<2, 2>() << c, -s, s, c;
+ for (int i = 0; i < 4; i++) {
+ o_targetFootstep_.block(0, i, 2, 1) = Rz.topLeftCorner<2, 2>() * targetFootstep_.block(0, i, 2, 1) + q.head(2);
+ }
+
+ return o_targetFootstep_;
}
-void FootstepPlanner::updateNewContact(Vector18 const& q)
-{
- // Remove translation and yaw rotation to get position in local frame
- q_FK_.head(2) = Vector2::Zero();
- q_FK_(2, 0) = q(2, 0);
- q_FK_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), 0.0)).coeffs();
- q_FK_.tail(12) = q.tail(12);
-
- // Update model and data of the robot
- pinocchio::forwardKinematics(model_, data_, q_FK_);
- pinocchio::updateFramePlacements(model_, data_);
-
- // Get data required by IK with Pinocchio
- for (int i = 0; i < 4; i++)
- {
- pos_feet_.col(i) = data_.oMf[foot_ids_[i]].translation();
- }
+void FootstepPlanner::updateNewContact(Vector18 const& q) {
+ // Remove translation and yaw rotation to get position in local frame
+ q_FK_.head(2) = Vector2::Zero();
+ q_FK_(2, 0) = q(2, 0);
+ q_FK_.block(3, 0, 4, 1) = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(q(3, 0), q(4, 0), 0.0)).coeffs();
+ q_FK_.tail(12) = q.tail(12);
+
+ // Update model and data of the robot
+ pinocchio::forwardKinematics(model_, data_, q_FK_);
+ pinocchio::updateFramePlacements(model_, data_);
+
+ // Get data required by IK with Pinocchio
+ for (int i = 0; i < 4; i++) {
+ pos_feet_.col(i) = data_.oMf[foot_ids_[i]].translation();
+ }
- /*std::cout << "q: " << q.transpose() << std::endl;
- */
+ /*std::cout << "q: " << q.transpose() << std::endl; */
- // TODO: Put pos_feet_ into currentFootsteps_
- // std::cout << "--- pos_feet_: " << std::endl << pos_feet_ << std::endl;
- // std::cout << "--- footsteps_:" << std::endl << footsteps_[1] << std::endl;
+ // TODO: Put pos_feet_ into currentFootsteps_
+ // std::cout << "--- pos_feet_: " << std::endl << pos_feet_ << std::endl;
+ // std::cout << "--- footsteps_:" << std::endl << footsteps_[1] << std::endl;
- // Refresh position with estimated position if foot is in stance phase
- for (int i = 0; i < 4; i++)
- {
- if (gait_->getCurrentGaitCoeff(0, i) == 1.0)
- {
- currentFootstep_.col(i) = (footsteps_[1]).col(i);
- }
+ // Refresh position with estimated position if foot is in stance phase
+ for (int i = 0; i < 4; i++) {
+ if (gait_->getCurrentGaitCoeff(0, i) == 1.0) {
+ currentFootstep_.col(i) = (footsteps_[1]).col(i);
}
+ }
}
MatrixN FootstepPlanner::getFootsteps() { return vectorToMatrix(footsteps_); }
MatrixN FootstepPlanner::getTargetFootsteps() { return targetFootstep_; }
MatrixN FootstepPlanner::getRz() { return Rz; }
-MatrixN FootstepPlanner::vectorToMatrix(std::vector<Matrix34> const& array)
-{
- MatrixN M = MatrixN::Zero(array.size(), 12);
- for (uint i = 0; i < array.size(); i++)
- {
- for (int j = 0; j < 4; j++)
- {
- M.row(i).segment<3>(3 * j) = array[i].col(j);
- }
+MatrixN FootstepPlanner::vectorToMatrix(std::vector<Matrix34> const& array) {
+ MatrixN M = MatrixN::Zero(array.size(), 12);
+ for (uint i = 0; i < array.size(); i++) {
+ for (int j = 0; j < 4; j++) {
+ M.row(i).segment<3>(3 * j) = array[i].col(j);
}
- return M;
+ }
+ return M;
}
diff --git a/src/Gait.cpp b/src/Gait.cpp
index 146478e1..92f9737c 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -1,266 +1,238 @@
#include "qrw/Gait.hpp"
Gait::Gait()
- : pastGait_()
- , currentGait_()
- , desiredGait_()
- , dt_(0.0)
- , T_gait_(0.0)
- , T_mpc_(0.0)
- , remainingTime_(0.0)
- , newPhase_(false)
- , is_static_(true)
- , switch_to_gait_(0)
- , q_static_(VectorN::Zero(19))
-{
- // Empty
+ : pastGait_(),
+ currentGait_(),
+ desiredGait_(),
+ dt_(0.0),
+ T_gait_(0.0),
+ T_mpc_(0.0),
+ remainingTime_(0.0),
+ newPhase_(false),
+ is_static_(true),
+ switch_to_gait_(0),
+ q_static_(VectorN::Zero(19)) {
+ // Empty
}
+void Gait::initialize(Params& params) {
+ dt_ = params.dt_mpc;
+ T_gait_ = params.T_gait;
+ T_mpc_ = params.T_mpc;
+ n_steps_ = (int)std::lround(params.T_mpc / params.dt_mpc);
-void Gait::initialize(Params& params)
-{
- dt_ = params.dt_mpc;
- T_gait_ = params.T_gait;
- T_mpc_ = params.T_mpc;
- n_steps_ = (int)std::lround(params.T_mpc / params.dt_mpc);
+ pastGait_ = MatrixN::Zero(params.N_gait, 4);
+ currentGait_ = MatrixN::Zero(params.N_gait, 4);
+ desiredGait_ = MatrixN::Zero(params.N_gait, 4);
- pastGait_ = MatrixN::Zero(params.N_gait, 4);
- currentGait_ = MatrixN::Zero(params.N_gait, 4);
- desiredGait_ = MatrixN::Zero(params.N_gait, 4);
+ if ((n_steps_ > params.N_gait) || ((int)std::lround(params.T_gait / params.dt_mpc) > params.N_gait))
+ throw std::invalid_argument(
+ "Sizes of matrices are too small for considered durations. Increase N_gait in config file.");
- if((n_steps_ > params.N_gait) || ((int)std::lround(params.T_gait / params.dt_mpc) > params.N_gait))
- throw std::invalid_argument("Sizes of matrices are too small for considered durations. Increase N_gait in config file.");
-
- create_trot();
- create_gait_f();
+ create_trot();
+ create_gait_f();
}
-
-void Gait::create_walk()
-{
- // Number of timesteps in 1/4th period of gait
- int N = (int)std::lround(0.25 * T_gait_ / dt_);
-
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
-
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 0.0, 1.0, 1.0, 1.0;
- desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 0.0, 1.0, 1.0;
- desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 1.0, 0.0, 1.0;
- desiredGait_.block(2*N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1.0, 1.0, 1.0, 0.0;
- desiredGait_.block(3*N, 0, N, 4) = sequence.colwise().replicate(N);
+void Gait::create_walk() {
+ // Number of timesteps in 1/4th period of gait
+ int N = (int)std::lround(0.25 * T_gait_ / dt_);
+
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 0.0, 1.0, 1.0, 1.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1.0, 0.0, 1.0, 1.0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1.0, 1.0, 0.0, 1.0;
+ desiredGait_.block(2 * N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1.0, 1.0, 1.0, 0.0;
+ desiredGait_.block(3 * N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_trot()
-{
- // Number of timesteps in a half period of gait
- int N = (int)std::lround(0.5 * T_gait_ / dt_);
+void Gait::create_trot() {
+ // Number of timesteps in a half period of gait
+ int N = (int)std::lround(0.5 * T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 0.0, 0.0, 1.0;
- desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 1.0, 0.0;
- desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1.0, 0.0, 0.0, 1.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0.0, 1.0, 1.0, 0.0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_pacing()
-{
- // Number of timesteps in a half period of gait
- int N = (int)std::lround(0.5 * T_gait_ / dt_);
+void Gait::create_pacing() {
+ // Number of timesteps in a half period of gait
+ int N = (int)std::lround(0.5 * T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 0.0, 1.0, 0.0;
- desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 1.0, 0.0, 1.0;
- desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1.0, 0.0, 1.0, 0.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0.0, 1.0, 0.0, 1.0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_bounding()
-{
- // Number of timesteps in a half period of gait
- int N = (int)std::lround(0.5 * T_gait_ / dt_);
+void Gait::create_bounding() {
+ // Number of timesteps in a half period of gait
+ int N = (int)std::lround(0.5 * T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 1.0, 0.0, 0.0;
- desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 0.0, 0.0, 1.0, 1.0;
- desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1.0, 1.0, 0.0, 0.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 0.0, 0.0, 1.0, 1.0;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_static()
-{
- // Number of timesteps in a period of gait
- int N = (int)std::lround(T_gait_ / dt_);
+void Gait::create_static() {
+ // Number of timesteps in a period of gait
+ int N = (int)std::lround(T_gait_ / dt_);
- desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
+ desiredGait_ = MatrixN::Zero(currentGait_.rows(), 4);
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 1.0, 1.0, 1.0, 1.0;
- desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 1.0, 1.0, 1.0, 1.0;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
}
-void Gait::create_gait_f()
-{
- int i = 0;
-
- // Fill currrent gait matrix
- for (int j = 0; j < n_steps_; j++)
+void Gait::create_gait_f() {
+ int i = 0;
+
+ // Fill currrent gait matrix
+ for (int j = 0; j < n_steps_; j++) {
+ currentGait_.row(j) = desiredGait_.row(i);
+ i++;
+ if (desiredGait_.row(i).isZero()) {
+ i = 0;
+ } // Loop back if T_mpc_ longer than gait duration
+ }
+
+ // Get index of first empty line
+ int index = 1;
+ while (!desiredGait_.row(index).isZero()) {
+ index++;
+ }
+
+ // Age desired gait to take into account what has been put in the current gait matrix
+ for (int k = 0; k < i; k++) {
+ for (int m = 0; m < index - 1; m++) // TODO: Find optimized circular shift function
{
- currentGait_.row(j) = desiredGait_.row(i);
- i++;
- if (desiredGait_.row(i).isZero())
- {
- i = 0;
- } // Loop back if T_mpc_ longer than gait duration
- }
-
- // Get index of first empty line
- int index = 1;
- while (!desiredGait_.row(index).isZero())
- {
- index++;
- }
-
- // Age desired gait to take into account what has been put in the current gait matrix
- for (int k = 0; k < i; k++)
- {
- for (int m = 0; m < index-1; m++) // TODO: Find optimized circular shift function
- {
- desiredGait_.row(m).swap(desiredGait_.row(m+1));
- }
+ desiredGait_.row(m).swap(desiredGait_.row(m + 1));
}
+ }
}
-double Gait::getPhaseDuration(int i, int j, double value)
-{
- double t_phase = 1;
- int a = i;
-
- // Looking for the end of the swing/stance phase in currentGait_
- while ((!currentGait_.row(i + 1).isZero()) && (currentGait_(i + 1, j) == value))
- {
- i++;
- t_phase++;
- }
- // If we reach the end of currentGait_ we continue looking for the end of the swing/stance phase in desiredGait_
- if (currentGait_.row(i + 1).isZero())
- {
- int k = 0;
- while ((!desiredGait_.row(k).isZero()) && (desiredGait_(k, j) == value))
- {
- k++;
- t_phase++;
- }
- }
- // We suppose that we found the end of the swing/stance phase either in currentGait_ or desiredGait_
-
- remainingTime_ = t_phase;
-
- // Looking for the beginning of the swing/stance phase in currentGait_
- while ((a > 0) && (currentGait_(a - 1, j) == value))
- {
- a--;
- t_phase++;
+double Gait::getPhaseDuration(int i, int j, double value) {
+ double t_phase = 1;
+ int a = i;
+
+ // Looking for the end of the swing/stance phase in currentGait_
+ while ((!currentGait_.row(i + 1).isZero()) && (currentGait_(i + 1, j) == value)) {
+ i++;
+ t_phase++;
+ }
+ // If we reach the end of currentGait_ we continue looking for the end of the swing/stance phase in desiredGait_
+ if (currentGait_.row(i + 1).isZero()) {
+ int k = 0;
+ while ((!desiredGait_.row(k).isZero()) && (desiredGait_(k, j) == value)) {
+ k++;
+ t_phase++;
}
- // If we reach the end of currentGait_ we continue looking for the beginning of the swing/stance phase in pastGait_
- if (a == 0)
- {
- while ((!pastGait_.row(a).isZero()) && (pastGait_(a, j) == value))
- {
- a++;
- t_phase++;
- }
+ }
+ // We suppose that we found the end of the swing/stance phase either in currentGait_ or desiredGait_
+
+ remainingTime_ = t_phase;
+
+ // Looking for the beginning of the swing/stance phase in currentGait_
+ while ((a > 0) && (currentGait_(a - 1, j) == value)) {
+ a--;
+ t_phase++;
+ }
+ // If we reach the end of currentGait_ we continue looking for the beginning of the swing/stance phase in pastGait_
+ if (a == 0) {
+ while ((!pastGait_.row(a).isZero()) && (pastGait_(a, j) == value)) {
+ a++;
+ t_phase++;
}
- // We suppose that we found the beginning of the swing/stance phase either in currentGait_ or pastGait_
+ }
+ // We suppose that we found the beginning of the swing/stance phase either in currentGait_ or pastGait_
- return t_phase * dt_; // Take into account time step value
+ return t_phase * dt_; // Take into account time step value
}
-void Gait::updateGait(int const k,
- int const k_mpc,
- int const joystickCode)
-{
- changeGait(k, k_mpc, joystickCode);
- if (k % k_mpc == 0 && k > 0)
- rollGait();
+void Gait::updateGait(int const k, int const k_mpc, int const joystickCode) {
+ changeGait(k, k_mpc, joystickCode);
+ if (k % k_mpc == 0 && k > 0) rollGait();
}
-bool Gait::changeGait(int const k, int const k_mpc, int const code)
-{
- if (code != 0 && switch_to_gait_ == 0)
- {
- switch_to_gait_ = code;
- }
- is_static_ = false;
- if (switch_to_gait_ != 0 && std::remainder(static_cast<double>(k - k_mpc), (k_mpc * T_gait_ * 0.5) / dt_) == 0.0)
- {
- switch(switch_to_gait_) {
- case 1 : create_pacing(); break;
- case 2 : create_bounding(); break;
- case 3 : create_trot(); break;
- case 4 : create_static(); break;
- }
- switch_to_gait_ = 0;
+bool Gait::changeGait(int const k, int const k_mpc, int const code) {
+ if (code != 0 && switch_to_gait_ == 0) {
+ switch_to_gait_ = code;
+ }
+ is_static_ = false;
+ if (switch_to_gait_ != 0 && std::remainder(static_cast<double>(k - k_mpc), (k_mpc * T_gait_ * 0.5) / dt_) == 0.0) {
+ switch (switch_to_gait_) {
+ case 1:
+ create_pacing();
+ break;
+ case 2:
+ create_bounding();
+ break;
+ case 3:
+ create_trot();
+ break;
+ case 4:
+ create_static();
+ break;
}
+ switch_to_gait_ = 0;
+ }
- return is_static_;
+ return is_static_;
}
-void Gait::rollGait()
-{
- // Transfer current gait into past gait
- for (int m = n_steps_; m > 0; m--) // TODO: Find optimized circular shift function
- {
- pastGait_.row(m).swap(pastGait_.row(m-1));
- }
- pastGait_.row(0) = currentGait_.row(0);
-
-
- // Entering new contact phase, store positions of feet that are now in contact
- if (!currentGait_.row(0).isApprox(currentGait_.row(1)))
- {
- newPhase_ = true;
- }
- else
- {
- newPhase_ = false;
- }
-
- // Age current gait
- int index = 1;
- while (!currentGait_.row(index).isZero())
- {
- currentGait_.row(index-1).swap(currentGait_.row(index));
- index++;
- }
-
- // Insert a new line from desired gait into current gait
- currentGait_.row(index-1) = desiredGait_.row(0);
-
- // Age desired gait
- index = 1;
- while (!desiredGait_.row(index).isZero())
- {
- desiredGait_.row(index-1).swap(desiredGait_.row(index));
- index++;
- }
-
+void Gait::rollGait() {
+ // Transfer current gait into past gait
+ for (int m = n_steps_; m > 0; m--) // TODO: Find optimized circular shift function
+ {
+ pastGait_.row(m).swap(pastGait_.row(m - 1));
+ }
+ pastGait_.row(0) = currentGait_.row(0);
+
+ // Entering new contact phase, store positions of feet that are now in contact
+ if (!currentGait_.row(0).isApprox(currentGait_.row(1))) {
+ newPhase_ = true;
+ } else {
+ newPhase_ = false;
+ }
+
+ // Age current gait
+ int index = 1;
+ while (!currentGait_.row(index).isZero()) {
+ currentGait_.row(index - 1).swap(currentGait_.row(index));
+ index++;
+ }
+
+ // Insert a new line from desired gait into current gait
+ currentGait_.row(index - 1) = desiredGait_.row(0);
+
+ // Age desired gait
+ index = 1;
+ while (!desiredGait_.row(index).isZero()) {
+ desiredGait_.row(index - 1).swap(desiredGait_.row(index));
+ index++;
+ }
}
-bool Gait::setGait(MatrixN const& gaitMatrix)
-{
- std::cout << "Gait matrix received by setGait:" << std::endl;
- std::cout << gaitMatrix << std::endl;
+bool Gait::setGait(MatrixN const& gaitMatrix) {
+ std::cout << "Gait matrix received by setGait:" << std::endl;
+ std::cout << gaitMatrix << std::endl;
- // Todo: Check if the matrix is a static gait (only ones)
- return false;
+ // Todo: Check if the matrix is a static gait (only ones)
+ return false;
}
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 69962406..91ab49b8 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -1,113 +1,110 @@
#include "qrw/InvKin.hpp"
InvKin::InvKin()
- : invJ(Matrix12::Zero())
- , acc(Matrix112::Zero())
- , x_err(Matrix112::Zero())
- , dx_r(Matrix112::Zero())
- , pfeet_err(Matrix43::Zero())
- , vfeet_ref(Matrix43::Zero())
- , afeet(Matrix43::Zero())
- , posf_(Matrix43::Zero())
- , vf_(Matrix43::Zero())
- , wf_(Matrix43::Zero())
- , af_(Matrix43::Zero())
- , Jf_(Matrix12::Zero())
- , Jf_tmp_(Eigen::Matrix<double, 6, 12>::Zero())
- , ddq_cmd_(Vector12::Zero())
- , dq_cmd_(Vector12::Zero())
- , q_cmd_(Vector12::Zero())
- , q_step_(Vector12::Zero())
-{}
+ : invJ(Matrix12::Zero()),
+ acc(Matrix112::Zero()),
+ x_err(Matrix112::Zero()),
+ dx_r(Matrix112::Zero()),
+ pfeet_err(Matrix43::Zero()),
+ vfeet_ref(Matrix43::Zero()),
+ afeet(Matrix43::Zero()),
+ posf_(Matrix43::Zero()),
+ vf_(Matrix43::Zero()),
+ wf_(Matrix43::Zero()),
+ af_(Matrix43::Zero()),
+ Jf_(Matrix12::Zero()),
+ Jf_tmp_(Eigen::Matrix<double, 6, 12>::Zero()),
+ ddq_cmd_(Vector12::Zero()),
+ dq_cmd_(Vector12::Zero()),
+ q_cmd_(Vector12::Zero()),
+ q_step_(Vector12::Zero()) {}
void InvKin::initialize(Params& params) {
+ // Params store parameters
+ params_ = ¶ms;
- // Params store parameters
- params_ = ¶ms;
+ // Path to the robot URDF (TODO: Automatic path)
+ const std::string filename =
+ std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
- // Path to the robot URDF (TODO: Automatic path)
- const std::string filename = std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
+ // Build model from urdf (base is not free flyer)
+ pinocchio::urdf::buildModel(filename, model_, false);
- // Build model from urdf (base is not free flyer)
- pinocchio::urdf::buildModel(filename, model_, false);
+ // Construct data from model
+ data_ = pinocchio::Data(model_);
- // Construct data from model
- data_ = pinocchio::Data(model_);
-
- // Update all the quantities of the model
- pinocchio::computeAllTerms(model_, data_ , VectorN::Zero(model_.nq), VectorN::Zero(model_.nv));
-
- // Get feet frame IDs
- foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT")); // from long uint to int
- foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
- foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
- foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
+ // Update all the quantities of the model
+ pinocchio::computeAllTerms(model_, data_, VectorN::Zero(model_.nq), VectorN::Zero(model_.nv));
+ // Get feet frame IDs
+ foot_ids_[0] = static_cast<int>(model_.getFrameId("FL_FOOT")); // from long uint to int
+ foot_ids_[1] = static_cast<int>(model_.getFrameId("FR_FOOT"));
+ foot_ids_[2] = static_cast<int>(model_.getFrameId("HL_FOOT"));
+ foot_ids_[3] = static_cast<int>(model_.getFrameId("HR_FOOT"));
}
-void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals,
- Matrix43 const& vgoals, Matrix43 const& agoals) {
-
- // Process feet
- for (int i = 0; i < 4; i++) {
- pfeet_err.row(i) = pgoals.row(i) - posf_.row(i);
- vfeet_ref.row(i) = vgoals.row(i);
-
- afeet.row(i) = + params_->Kp_flyingfeet * pfeet_err.row(i) - params_->Kd_flyingfeet * (vf_.row(i)-vgoals.row(i)) + agoals.row(i);
- if (contacts(0, i) == 1.0) {
- afeet.row(i).setZero(); // Set to 0.0 to disable position/velocity control of feet in contact
- }
- afeet.row(i) -= af_.row(i) + (wf_.row(i)).cross(vf_.row(i)); // Drift
- }
-
- // Store data and invert the Jacobian
- for (int i = 0; i < 4; i++) {
- acc.block(0, 3*i, 1, 3) = afeet.row(i);
- x_err.block(0, 3*i, 1, 3) = pfeet_err.row(i);
- dx_r.block(0, 3*i, 1, 3) = vfeet_ref.row(i);
- invJ.block(3*i, 3*i, 3, 3) = Jf_.block(3*i, 3*i, 3, 3).inverse();
+void InvKin::refreshAndCompute(Matrix14 const& contacts, Matrix43 const& pgoals, Matrix43 const& vgoals,
+ Matrix43 const& agoals) {
+ // Process feet
+ for (int i = 0; i < 4; i++) {
+ pfeet_err.row(i) = pgoals.row(i) - posf_.row(i);
+ vfeet_ref.row(i) = vgoals.row(i);
+
+ afeet.row(i) = +params_->Kp_flyingfeet * pfeet_err.row(i) - params_->Kd_flyingfeet * (vf_.row(i) - vgoals.row(i)) +
+ agoals.row(i);
+ if (contacts(0, i) == 1.0) {
+ afeet.row(i).setZero(); // Set to 0.0 to disable position/velocity control of feet in contact
}
-
- // Once Jacobian has been inverted we can get command accelerations, velocities and positions
- ddq_cmd_ = invJ * acc.transpose();
- dq_cmd_ = invJ * dx_r.transpose();
- q_step_ = invJ * x_err.transpose(); // Not a position but a step in position
-
- /*
- std::cout << "J" << std::endl << Jf << std::endl;
- std::cout << "invJ" << std::endl << invJ << std::endl;
- std::cout << "acc" << std::endl << acc << std::endl;
- std::cout << "q_step" << std::endl << q_step << std::endl;
- std::cout << "dq_cmd" << std::endl << dq_cmd << std::endl;
- */
+ afeet.row(i) -= af_.row(i) + (wf_.row(i)).cross(vf_.row(i)); // Drift
+ }
+
+ // Store data and invert the Jacobian
+ for (int i = 0; i < 4; i++) {
+ acc.block(0, 3 * i, 1, 3) = afeet.row(i);
+ x_err.block(0, 3 * i, 1, 3) = pfeet_err.row(i);
+ dx_r.block(0, 3 * i, 1, 3) = vfeet_ref.row(i);
+ invJ.block(3 * i, 3 * i, 3, 3) = Jf_.block(3 * i, 3 * i, 3, 3).inverse();
+ }
+
+ // Once Jacobian has been inverted we can get command accelerations, velocities and positions
+ ddq_cmd_ = invJ * acc.transpose();
+ dq_cmd_ = invJ * dx_r.transpose();
+ q_step_ = invJ * x_err.transpose(); // Not a position but a step in position
+
+ /*
+ std::cout << "J" << std::endl << Jf << std::endl;
+ std::cout << "invJ" << std::endl << invJ << std::endl;
+ std::cout << "acc" << std::endl << acc << std::endl;
+ std::cout << "q_step" << std::endl << q_step << std::endl;
+ std::cout << "dq_cmd" << std::endl << dq_cmd << std::endl;
+ */
}
-void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals)
-{
- // Update model and data of the robot
- pinocchio::forwardKinematics(model_, data_, q, dq, VectorN::Zero(model_.nv));
- pinocchio::computeJointJacobians(model_, data_);
- pinocchio::updateFramePlacements(model_, data_);
-
- // Get data required by IK with Pinocchio
- for (int i = 0; i < 4; i++) {
- int idx = foot_ids_[i];
- posf_.row(i) = data_.oMf[idx].translation();
- pinocchio::Motion nu = pinocchio::getFrameVelocity(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED);
- vf_.row(i) = nu.linear();
- wf_.row(i) = nu.angular();
- af_.row(i) = pinocchio::getFrameAcceleration(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED).linear();
- Jf_tmp_.setZero(); // Fill with 0s because getFrameJacobian only acts on the coeffs it changes so the
- // other coeffs keep their previous value instead of being set to 0
- pinocchio::getFrameJacobian(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED, Jf_tmp_);
- Jf_.block(3 * i, 0, 3, 12) = Jf_tmp_.block(0, 0, 3, 12);
- }
-
- // IK output for accelerations of actuators (stored in ddq_cmd_)
- // IK output for velocities of actuators (stored in dq_cmd_)
- refreshAndCompute(contacts, pgoals, vgoals, agoals);
-
- // IK output for positions of actuators
- q_cmd_ = q + q_step_;
-
+void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
+ MatrixN const& vgoals, MatrixN const& agoals) {
+ // Update model and data of the robot
+ pinocchio::forwardKinematics(model_, data_, q, dq, VectorN::Zero(model_.nv));
+ pinocchio::computeJointJacobians(model_, data_);
+ pinocchio::updateFramePlacements(model_, data_);
+
+ // Get data required by IK with Pinocchio
+ for (int i = 0; i < 4; i++) {
+ int idx = foot_ids_[i];
+ posf_.row(i) = data_.oMf[idx].translation();
+ pinocchio::Motion nu = pinocchio::getFrameVelocity(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED);
+ vf_.row(i) = nu.linear();
+ wf_.row(i) = nu.angular();
+ af_.row(i) = pinocchio::getFrameAcceleration(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED).linear();
+ Jf_tmp_.setZero(); // Fill with 0s because getFrameJacobian only acts on the coeffs it changes so the
+ // other coeffs keep their previous value instead of being set to 0
+ pinocchio::getFrameJacobian(model_, data_, idx, pinocchio::LOCAL_WORLD_ALIGNED, Jf_tmp_);
+ Jf_.block(3 * i, 0, 3, 12) = Jf_tmp_.block(0, 0, 3, 12);
+ }
+
+ // IK output for accelerations of actuators (stored in ddq_cmd_)
+ // IK output for velocities of actuators (stored in dq_cmd_)
+ refreshAndCompute(contacts, pgoals, vgoals, agoals);
+
+ // IK output for positions of actuators
+ q_cmd_ = q + q_step_;
}
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 8ec7893c..92edaf95 100644
--- a/src/Joystick.cpp
+++ b/src/Joystick.cpp
@@ -1,25 +1,18 @@
#include "qrw/Joystick.hpp"
-Joystick::Joystick()
- : A3_(Vector6::Zero())
- , A2_(Vector6::Zero())
- , v_ref_(Vector6::Zero())
-{}
+Joystick::Joystick() : A3_(Vector6::Zero()), A2_(Vector6::Zero()), v_ref_(Vector6::Zero()) {}
-VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch)
-{
- int i = 1;
- while ((i < k_switch.rows()) && k_switch[i] <= k)
- {
- i++;
- }
- if (i != k_switch.rows())
- {
- double ev = k - k_switch[i-1];
- double t1 = k_switch[i] - k_switch[i-1];
- A3_ = 2 * (v_switch.col(i-1) - v_switch.col(i)) / pow(t1, 3);
- A2_ = (-3.0/2.0) * t1 * A3_;
- v_ref_ = v_switch.col(i-1) + A2_ * pow(ev, 2) + A3_ * pow(ev, 3);
- }
- return v_ref_;
+VectorN Joystick::handle_v_switch(double k, VectorN const& k_switch, MatrixN const& v_switch) {
+ int i = 1;
+ while ((i < k_switch.rows()) && k_switch[i] <= k) {
+ i++;
+ }
+ if (i != k_switch.rows()) {
+ double ev = k - k_switch[i - 1];
+ double t1 = k_switch[i] - k_switch[i - 1];
+ A3_ = 2 * (v_switch.col(i - 1) - v_switch.col(i)) / pow(t1, 3);
+ A2_ = (-3.0 / 2.0) * t1 * A3_;
+ v_ref_ = v_switch.col(i - 1) + A2_ * pow(ev, 2) + A3_ * pow(ev, 3);
+ }
+ return v_ref_;
}
diff --git a/src/MPC.cpp b/src/MPC.cpp
index a37b6b47..109c5552 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -1,7 +1,6 @@
#include "qrw/MPC.hpp"
-MPC::MPC(Params& params) {
-
+MPC::MPC(Params ¶ms) {
params_ = ¶ms;
dt = params_->dt_mpc;
@@ -21,7 +20,7 @@ MPC::MPC(Params& params) {
mu = 0.9f;
cpt_ML = 0;
cpt_P = 0;
- offset_CoM(2, 0) = - 0.03; // Approximation of the vertical offset between center of base and CoM
+ offset_CoM(2, 0) = -0.03; // Approximation of the vertical offset between center of base and CoM
// Predefined matrices
footholds << 0.19, 0.19, -0.19, -0.19, 0.15005, -0.15005, 0.15005, -0.15005, 0.0, 0.0, 0.0, 0.0;
@@ -37,7 +36,7 @@ MPC::MPC(Params& params) {
osqp_set_default_settings(settings);
}
-MPC::MPC() { }
+MPC::MPC() {}
int MPC::create_matrices() {
// Create the constraint matrices
@@ -145,7 +144,7 @@ int MPC::create_ML() {
int nst = cpt_ML; // number of non zero elements
int ncc = st_to_cc_size(nst, r_ML, c_ML); // number of CC values
// int m = 12 * n_steps * 2 + 20 * n_steps; // number of rows
- int n = 12 * n_steps * 2; // number of columns
+ int n = 12 * n_steps * 2; // number of columns
/*int i_min = i4vec_min(nst, r_ML);
int i_max = i4vec_max(nst, r_ML);
@@ -343,7 +342,7 @@ int MPC::create_weight_matrices() {
int nst = cpt_P; // number of non zero elements
int ncc = st_to_cc_size(nst, r_P, c_P); // number of CC values
// int m = 12 * n_steps * 2; // number of rows
- int n = 12 * n_steps * 2; // number of columns
+ int n = 12 * n_steps * 2; // number of columns
// Get the CC indices.
icc = (int *)malloc(ncc * sizeof(int));
@@ -411,7 +410,7 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
// Get skew-symetric matrix for each foothold
// Eigen::Map<Eigen::Matrix<double, 3, 4>> fsteps_tmp((fsteps.block(j, 1, 1, 12)).data(), 3, 4);
- footholds_tmp = fsteps.row(j); // block(j, 1, 1, 12);
+ footholds_tmp = fsteps.row(j); // block(j, 1, 1, 12);
// footholds = footholds_tmp.reshaped(3, 4);
Eigen::Map<Eigen::MatrixXd> footholds_bis(footholds_tmp.data(), 3, 4);
@@ -497,7 +496,7 @@ int MPC::call_solver(int k) {
save_dns_matrix(v_NK_low, 12 * n_steps * 2 + 20 * n_steps, "l");
save_dns_matrix(v_NK_up, 12 * n_steps * 2 + 20 * n_steps, "u");*/
- //settings->rho = 0.1f;
+ // settings->rho = 0.1f;
settings->sigma = (c_float)1e-6;
// settings->max_iter = 4000;
settings->eps_abs = (c_float)1e-6;
@@ -524,9 +523,9 @@ int MPC::call_solver(int k) {
// osqp_warm_start_x(workspce, &v_warmxf[0]);
}
- //char t_char[1] = {'M'};
- //my_print_csc_matrix(ML, t_char);
- //std::cout << v_NK_low[1] << " <= A x <= " << v_NK_up[1] << std::endl;
+ // char t_char[1] = {'M'};
+ // my_print_csc_matrix(ML, t_char);
+ // std::cout << v_NK_low[1] << " <= A x <= " << v_NK_up[1] << std::endl;
// Run the solver to solve the QP problem
osqp_solve(workspce);
@@ -541,8 +540,8 @@ int MPC::retrieve_result() {
// Retrieve the "contact forces" part of the solution of the QP problem
for (int i = 0; i < (n_steps); i++) {
for (int k = 0; k < 12; k++) {
- x_f_applied(k, i) = (workspce->solution->x)[k + 12*i] + xref(k, 1+i);
- x_f_applied(k + 12, i) = (workspce->solution->x)[12 * (n_steps+i) + k];
+ x_f_applied(k, i) = (workspce->solution->x)[k + 12 * i] + xref(k, 1 + i);
+ x_f_applied(k + 12, i) = (workspce->solution->x)[12 * (n_steps + i) + k];
}
}
for (int k = 0; k < 12; k++) {
@@ -629,7 +628,6 @@ int MPC::construct_S() {
}
int MPC::construct_gait(Eigen::MatrixXd fsteps_in) {
-
int k = 0;
while (!fsteps_in.row(k).isZero()) {
for (int i = 0; i < 4; i++) {
diff --git a/src/Params.cpp b/src/Params.cpp
index 78152349..6aff9e69 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -3,182 +3,180 @@
using namespace yaml_control_interface;
Params::Params()
- : interface("")
- , SIMULATION(false)
- , LOGGING(false)
- , PLOTTING(false)
- , envID(0)
- , use_flat_plane(false)
- , predefined_vel(false)
- , velID(0)
- , N_SIMULATION(0)
- , enable_pyb_GUI(false)
- , enable_corba_viewer(false)
- , enable_multiprocessing(false)
- , perfect_estimator(false)
-
- , q_init(12, 0.0) // Fill with zeros, will be filled with values later
- , dt_wbc(0.0)
- , N_gait(0)
- , dt_mpc(0.0)
- , T_gait(0.0)
- , T_mpc(0.0)
- , type_MPC(0)
- , kf_enabled(false)
- , Kp_main(0.0)
- , Kd_main(0.0)
- , Kff_main(0.0)
-
- , fc_v_esti(0.0)
-
- , k_feedback(0.0)
-
- , max_height(0.0)
- , lock_time(0.0)
- , vert_time(0.0)
-
- , osqp_w_states(12, 0.0) // Fill with zeros, will be filled with values later
- , osqp_w_forces(3, 0.0) // Fill with zeros, will be filled with values later
- , osqp_Nz_lim(0.0)
-
- , Kp_flyingfeet(0.0)
- , Kd_flyingfeet(0.0)
-
- , Q1(0.0)
- , Q2(0.0)
- , Fz_max(0.0)
- , Fz_min(0.0)
-
- , mass(0.0)
- , I_mat(9, 0.0) // Fill with zeros, will be filled with values later
- , h_ref(0.0)
- , shoulders(12, 0.0) // Fill with zeros, will be filled with values later
- , footsteps_init(12, 0.0) // Fill with zeros, will be filled with values later
- , footsteps_under_shoulders(12, 0.0) // Fill with zeros, will be filled with values later
-{
- initialize(CONFIG_SOLO12_YAML);
+ : interface(""),
+ SIMULATION(false),
+ LOGGING(false),
+ PLOTTING(false),
+ envID(0),
+ use_flat_plane(false),
+ predefined_vel(false),
+ velID(0),
+ N_SIMULATION(0),
+ enable_pyb_GUI(false),
+ enable_corba_viewer(false),
+ enable_multiprocessing(false),
+ perfect_estimator(false),
+
+ q_init(12, 0.0), // Fill with zeros, will be filled with values later
+ dt_wbc(0.0),
+ N_gait(0),
+ dt_mpc(0.0),
+ T_gait(0.0),
+ T_mpc(0.0),
+ type_MPC(0),
+ kf_enabled(false),
+ Kp_main(0.0),
+ Kd_main(0.0),
+ Kff_main(0.0),
+
+ fc_v_esti(0.0),
+
+ k_feedback(0.0),
+
+ max_height(0.0),
+ lock_time(0.0),
+ vert_time(0.0),
+
+ osqp_w_states(12, 0.0), // Fill with zeros, will be filled with values later
+ osqp_w_forces(3, 0.0), // Fill with zeros, will be filled with values later
+ osqp_Nz_lim(0.0),
+
+ Kp_flyingfeet(0.0),
+ Kd_flyingfeet(0.0),
+
+ Q1(0.0),
+ Q2(0.0),
+ Fz_max(0.0),
+ Fz_min(0.0),
+
+ mass(0.0),
+ I_mat(9, 0.0), // Fill with zeros, will be filled with values later
+ h_ref(0.0),
+ shoulders(12, 0.0), // Fill with zeros, will be filled with values later
+ footsteps_init(12, 0.0), // Fill with zeros, will be filled with values later
+ footsteps_under_shoulders(12, 0.0) // Fill with zeros, will be filled with values later
+{
+ initialize(CONFIG_SOLO12_YAML);
}
-void Params::initialize(const std::string& file_path)
-{
- // Load YAML file
- assert_file_exists(file_path);
- YAML::Node param = YAML::LoadFile(file_path);
+void Params::initialize(const std::string& file_path) {
+ // Load YAML file
+ assert_file_exists(file_path);
+ YAML::Node param = YAML::LoadFile(file_path);
- // Check if YAML node is detected and retrieve it
- assert_yaml_parsing(param, file_path, "robot");
- const YAML::Node& robot_node = param["robot"];
+ // Check if YAML node is detected and retrieve it
+ assert_yaml_parsing(param, file_path, "robot");
+ const YAML::Node& robot_node = param["robot"];
- // Retrieve robot parameters
- assert_yaml_parsing(robot_node, "robot", "interface");
- interface = robot_node["interface"].as<std::string>();
+ // Retrieve robot parameters
+ assert_yaml_parsing(robot_node, "robot", "interface");
+ interface = robot_node["interface"].as<std::string>();
- assert_yaml_parsing(robot_node, "robot", "SIMULATION");
- SIMULATION = robot_node["SIMULATION"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "SIMULATION");
+ SIMULATION = robot_node["SIMULATION"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "LOGGING");
- LOGGING = robot_node["LOGGING"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "LOGGING");
+ LOGGING = robot_node["LOGGING"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "PLOTTING");
- PLOTTING = robot_node["PLOTTING"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "PLOTTING");
+ PLOTTING = robot_node["PLOTTING"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "dt_wbc");
- dt_wbc = robot_node["dt_wbc"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "dt_wbc");
+ dt_wbc = robot_node["dt_wbc"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "N_gait");
- N_gait = robot_node["N_gait"].as<int>();
+ assert_yaml_parsing(robot_node, "robot", "N_gait");
+ N_gait = robot_node["N_gait"].as<int>();
- assert_yaml_parsing(robot_node, "robot", "envID");
- envID = robot_node["envID"].as<int>();
+ assert_yaml_parsing(robot_node, "robot", "envID");
+ envID = robot_node["envID"].as<int>();
- assert_yaml_parsing(robot_node, "robot", "velID");
- velID = robot_node["velID"].as<int>();
+ assert_yaml_parsing(robot_node, "robot", "velID");
+ velID = robot_node["velID"].as<int>();
- assert_yaml_parsing(robot_node, "robot", "q_init");
- q_init = robot_node["q_init"].as<std::vector<double> >();
+ assert_yaml_parsing(robot_node, "robot", "q_init");
+ q_init = robot_node["q_init"].as<std::vector<double> >();
- assert_yaml_parsing(robot_node, "robot", "dt_mpc");
- dt_mpc = robot_node["dt_mpc"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "dt_mpc");
+ dt_mpc = robot_node["dt_mpc"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "T_gait");
- T_gait = robot_node["T_gait"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "T_gait");
+ T_gait = robot_node["T_gait"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "T_mpc");
- T_mpc = robot_node["T_mpc"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "T_mpc");
+ T_mpc = robot_node["T_mpc"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "N_SIMULATION");
- N_SIMULATION = robot_node["N_SIMULATION"].as<int>();
+ assert_yaml_parsing(robot_node, "robot", "N_SIMULATION");
+ N_SIMULATION = robot_node["N_SIMULATION"].as<int>();
- assert_yaml_parsing(robot_node, "robot", "type_MPC");
- type_MPC = robot_node["type_MPC"].as<int>();
+ assert_yaml_parsing(robot_node, "robot", "type_MPC");
+ type_MPC = robot_node["type_MPC"].as<int>();
- assert_yaml_parsing(robot_node, "robot", "use_flat_plane");
- use_flat_plane = robot_node["use_flat_plane"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "use_flat_plane");
+ use_flat_plane = robot_node["use_flat_plane"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "predefined_vel");
- predefined_vel = robot_node["predefined_vel"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "predefined_vel");
+ predefined_vel = robot_node["predefined_vel"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "kf_enabled");
- kf_enabled = robot_node["kf_enabled"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "kf_enabled");
+ kf_enabled = robot_node["kf_enabled"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "enable_pyb_GUI");
- enable_pyb_GUI = robot_node["enable_pyb_GUI"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "enable_pyb_GUI");
+ enable_pyb_GUI = robot_node["enable_pyb_GUI"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "enable_corba_viewer");
- enable_corba_viewer = robot_node["enable_corba_viewer"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "enable_corba_viewer");
+ enable_corba_viewer = robot_node["enable_corba_viewer"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "enable_multiprocessing");
- enable_multiprocessing = robot_node["enable_multiprocessing"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "enable_multiprocessing");
+ enable_multiprocessing = robot_node["enable_multiprocessing"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "perfect_estimator");
- perfect_estimator = robot_node["perfect_estimator"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "perfect_estimator");
+ perfect_estimator = robot_node["perfect_estimator"].as<bool>();
- assert_yaml_parsing(robot_node, "robot", "Kp_main");
- Kp_main = robot_node["Kp_main"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Kp_main");
+ Kp_main = robot_node["Kp_main"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "Kd_main");
- Kd_main = robot_node["Kd_main"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Kd_main");
+ Kd_main = robot_node["Kd_main"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "Kff_main");
- Kff_main = robot_node["Kff_main"].as<double>();
-
- assert_yaml_parsing(robot_node, "robot", "fc_v_esti");
- fc_v_esti = robot_node["fc_v_esti"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Kff_main");
+ Kff_main = robot_node["Kff_main"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "k_feedback");
- k_feedback = robot_node["k_feedback"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "fc_v_esti");
+ fc_v_esti = robot_node["fc_v_esti"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "max_height");
- max_height = robot_node["max_height"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "k_feedback");
+ k_feedback = robot_node["k_feedback"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "lock_time");
- lock_time = robot_node["lock_time"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "max_height");
+ max_height = robot_node["max_height"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "osqp_w_states");
- osqp_w_states = robot_node["osqp_w_states"].as<std::vector<double> >();
+ assert_yaml_parsing(robot_node, "robot", "lock_time");
+ lock_time = robot_node["lock_time"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "osqp_w_forces");
- osqp_w_forces = robot_node["osqp_w_forces"].as<std::vector<double> >();
+ assert_yaml_parsing(robot_node, "robot", "osqp_w_states");
+ osqp_w_states = robot_node["osqp_w_states"].as<std::vector<double> >();
- assert_yaml_parsing(robot_node, "robot", "osqp_Nz_lim");
- osqp_Nz_lim = robot_node["osqp_Nz_lim"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "osqp_w_forces");
+ osqp_w_forces = robot_node["osqp_w_forces"].as<std::vector<double> >();
- assert_yaml_parsing(robot_node, "robot", "Kp_flyingfeet");
- Kp_flyingfeet = robot_node["Kp_flyingfeet"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "osqp_Nz_lim");
+ osqp_Nz_lim = robot_node["osqp_Nz_lim"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "Kd_flyingfeet");
- Kd_flyingfeet = robot_node["Kd_flyingfeet"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Kp_flyingfeet");
+ Kp_flyingfeet = robot_node["Kp_flyingfeet"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "Q1");
- Q1 = robot_node["Q1"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Kd_flyingfeet");
+ Kd_flyingfeet = robot_node["Kd_flyingfeet"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "Q2");
- Q2 = robot_node["Q2"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Q1");
+ Q1 = robot_node["Q1"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "Fz_max");
- Fz_max = robot_node["Fz_max"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Q2");
+ Q2 = robot_node["Q2"].as<double>();
- assert_yaml_parsing(robot_node, "robot", "Fz_min");
- Fz_min = robot_node["Fz_min"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Fz_max");
+ Fz_max = robot_node["Fz_max"].as<double>();
+ assert_yaml_parsing(robot_node, "robot", "Fz_min");
+ Fz_min = robot_node["Fz_min"].as<double>();
}
diff --git a/src/QPWBC.cpp b/src/QPWBC.cpp
index b2ab70a0..b7dc6a4b 100644
--- a/src/QPWBC.cpp
+++ b/src/QPWBC.cpp
@@ -1,9 +1,8 @@
#include "qrw/QPWBC.hpp"
-
QPWBC::QPWBC() {
- /*
- Constructor of the QP solver. Initialization of matrices
+ /*
+ Constructor of the QP solver. Initialization of matrices
*/
// Slipping constraints
@@ -17,15 +16,14 @@ QPWBC::QPWBC() {
// Add slipping constraints to inequality matrix
for (int i = 0; i < 4; i++) {
- G.block(5*i, 3*i, 5, 3) = SC;
+ G.block(5 * i, 3 * i, 5, 3) = SC;
}
// Set OSQP settings to default
osqp_set_default_settings(settings);
-
}
-void QPWBC::initialize(Params& params) {
+void QPWBC::initialize(Params ¶ms) {
params_ = ¶ms;
Q1 = params.Q1 * Eigen::Matrix<double, 6, 6>::Identity();
Q2 = params.Q2 * Eigen::Matrix<double, 12, 12>::Identity();
@@ -33,10 +31,9 @@ void QPWBC::initialize(Params& params) {
// Set the lower and upper limits of the box
std::fill_n(v_NK_up, size_nz_NK, params_->Fz_max);
std::fill_n(v_NK_low, size_nz_NK, params_->Fz_min);
-}
+}
int QPWBC::create_matrices() {
-
// Create the constraint matrices
create_ML();
@@ -61,7 +58,6 @@ inline void QPWBC::add_to_P(int i, int j, double v, int *r_P, int *c_P, double *
}
int QPWBC::create_ML() {
-
int *r_ML = new int[size_nz_ML]; // row indexes of non-zero values in matrix ML
int *c_ML = new int[size_nz_ML]; // col indexes of non-zero values in matrix ML
double *v_ML = new double[size_nz_ML]; // non-zero values in matrix ML
@@ -84,7 +80,7 @@ int QPWBC::create_ML() {
int nst = cpt_ML; // number of non zero elements
int ncc = st_to_cc_size(nst, r_ML, c_ML); // number of CC values
// int m = 20; // number of rows
- int n = 12; // number of columns
+ int n = 12; // number of columns
// std::cout << "Number of CC values: " << ncc << std::endl;
@@ -121,9 +117,7 @@ int QPWBC::create_ML() {
return 0;
}
-
int QPWBC::create_weight_matrices() {
-
int *r_P = new int[size_nz_P]; // row indexes of non-zero values in matrix P
int *c_P = new int[size_nz_P]; // col indexes of non-zero values in matrix P
double *v_P = new double[size_nz_P]; // non-zero values in matrix P
@@ -147,7 +141,7 @@ int QPWBC::create_weight_matrices() {
int nst = cpt_P; // number of non zero elements
int ncc = st_to_cc_size(nst, r_P, c_P); // number of CC values
// int m = 12; // number of rows
- int n = 12; // number of columns
+ int n = 12; // number of columns
// std::cout << "Number of CC values: " << ncc << std::endl;
@@ -181,15 +175,14 @@ int QPWBC::create_weight_matrices() {
}
int QPWBC::call_solver() {
-
// Setup the solver (first iteration) then just update it
if (not initialized) // Setup the solver with the matrices
{
data = (OSQPData *)c_malloc(sizeof(OSQPData));
- data->n = 12; // number of variables
- data->m = 20; // number of constraints
- data->P = P; // the upper triangular part of the quadratic cost matrix P in csc format (size n x n)
- data->A = ML; // linear constraints matrix A in csc format (size m x n)
+ data->n = 12; // number of variables
+ data->m = 20; // number of constraints
+ data->P = P; // the upper triangular part of the quadratic cost matrix P in csc format (size n x n)
+ data->A = ML; // linear constraints matrix A in csc format (size m x n)
data->q = Q; // dense array for linear part of cost function (size n)
data->l = v_NK_low; // dense array for lower bound (size m)
data->u = v_NK_up; // dense array for upper bound (size m)
@@ -229,7 +222,6 @@ int QPWBC::call_solver() {
// Update upper bound of the OSQP solver
osqp_update_upper_bound(workspce, &v_NK_up[0]);
osqp_update_lower_bound(workspce, &v_NK_low[0]);
-
}
// Run the solver to solve the QP problem
@@ -241,7 +233,6 @@ int QPWBC::call_solver() {
}
int QPWBC::retrieve_result(const Eigen::MatrixXd &f_cmd) {
-
// Retrieve the solution of the QP problem
for (int k = 0; k < 12; k++) {
f_res(k, 0) = (workspce->solution->x)[k];
@@ -261,22 +252,21 @@ Getters
*/
Eigen::MatrixXd QPWBC::get_f_res() { return f_res; }
Eigen::MatrixXd QPWBC::get_ddq_res() { return ddq_res; }
-Eigen::MatrixXd QPWBC::get_H() {
- Eigen::MatrixXd Hxd = Eigen::MatrixXd::Zero(12, 12);
+Eigen::MatrixXd QPWBC::get_H() {
+ Eigen::MatrixXd Hxd = Eigen::MatrixXd::Zero(12, 12);
Hxd = H;
- return Hxd;
+ return Hxd;
}
-int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA,
- const Eigen::MatrixXd &k_contact) {
-
+int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd,
+ const Eigen::MatrixXd &RNEA, const Eigen::MatrixXd &k_contact) {
// Create the constraint and weight matrices used by the QP solver
// Minimize x^T.P.x + 2 x^T.Q with constraints M.X == N and L.X <= K
if (not initialized) {
create_matrices();
// std::cout << G << std::endl;
}
-
+
// Compute the different matrices involved in the box QP
compute_matrices(M, Jc, f_cmd, RNEA);
@@ -284,10 +274,10 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
update_PQ();
Eigen::Matrix<double, 20, 1> Gf = G * f_cmd;
-
+
for (int i = 0; i < G.rows(); i++) {
- v_NK_low[i] = - Gf(i, 0) + params_->Fz_min;
- v_NK_up[i] = - Gf(i, 0) + params_->Fz_max;
+ v_NK_low[i] = -Gf(i, 0) + params_->Fz_min;
+ v_NK_up[i] = -Gf(i, 0) + params_->Fz_max;
}
// Limit max force when contact is activated
@@ -296,17 +286,17 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
if (k_contact(0, i) < k_max) {
v_NK_up[5*i+4] -= Nz_max * (1.0 - k_contact(0, i) / k_max);
}*/
- /*else if (k_contact(0, i) == (k_max+10))
- {
- //char t_char[1] = {'M'};
- //cc_print( (data->A)->m, (data->A)->n, (data->A)->nzmax, (data->A)->i, (data->A)->p, (data->A)->x, t_char);
- std::cout << " ### " << k_contact(0, i) << std::endl;
-
- for (int i = 0; i < data->m; i++) {
- std::cout << data->l[i] << " | " << data->u[i] << " | " << f_cmd(i, 0) << std::endl;
- }
- }*/
-
+ /*else if (k_contact(0, i) == (k_max+10))
+ {
+ //char t_char[1] = {'M'};
+ //cc_print( (data->A)->m, (data->A)->n, (data->A)->nzmax, (data->A)->i, (data->A)->p, (data->A)->x, t_char);
+ std::cout << " ### " << k_contact(0, i) << std::endl;
+
+ for (int i = 0; i < data->m; i++) {
+ std::cout << data->l[i] << " | " << data->u[i] << " | " << f_cmd(i, 0) << std::endl;
+ }
+ }*/
+
//}
// Create an initial guess and call the solver to solve the QP problem
@@ -338,7 +328,6 @@ int QPWBC::run(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen:
}
void QPWBC::my_print_csc_matrix(csc *M, const char *name) {
-
c_int j, i, row_start, row_stop;
c_int k = 0;
@@ -357,14 +346,12 @@ void QPWBC::my_print_csc_matrix(csc *M, const char *name) {
int b = (int)j;
double c = M->x[k++];
printf("\t%3u [%3u,%3u] = %.3g\n", k - 1, a, b, c);
-
}
}
}
}
void QPWBC::save_csc_matrix(csc *M, std::string filename) {
-
c_int j, i, row_start, row_stop;
c_int k = 0;
@@ -391,7 +378,6 @@ void QPWBC::save_csc_matrix(csc *M, std::string filename) {
}
void QPWBC::save_dns_matrix(double *M, int size, std::string filename) {
-
// Open file
std::ofstream myfile;
myfile.open(filename + ".csv");
@@ -403,9 +389,8 @@ void QPWBC::save_dns_matrix(double *M, int size, std::string filename) {
myfile.close();
}
-
-void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd, const Eigen::MatrixXd &RNEA) {
-
+void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc, const Eigen::MatrixXd &f_cmd,
+ const Eigen::MatrixXd &RNEA) {
Y = M.block(0, 0, 6, 6);
X = Jc.block(0, 0, 12, 6).transpose();
Yinv = pseudoInverse(Y);
@@ -430,18 +415,15 @@ void QPWBC::compute_matrices(const Eigen::MatrixXd &M, const Eigen::MatrixXd &Jc
std::cout << g << std::endl;
std::cout << "H" << std::endl;
std::cout << H << std::endl;*/
-
-
}
void QPWBC::update_PQ() {
-
// Update P matrix of min xT P x + 2 xT Q
int cpt = 0;
for (int i = 0; i < 12; i++) {
for (int j = 0; j <= i; j++) {
- P->x[cpt] = H(j, i);
- cpt++;
+ P->x[cpt] = H(j, i);
+ cpt++;
}
}
@@ -454,36 +436,32 @@ void QPWBC::update_PQ() {
/*char t_char[1] = {'P'};
my_print_csc_matrix(P, t_char);*/
-
}
-
-
WbcWrapper::WbcWrapper()
- : M_(Eigen::Matrix<double, 18, 18>::Zero())
- , Jc_(Eigen::Matrix<double, 12, 6>::Zero())
- , k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero())
- , qdes_(Vector12::Zero())
- , vdes_(Vector12::Zero())
- , tau_ff_(Vector12::Zero())
- , ddq_cmd_(Vector18::Zero())
- , q_default_(Vector19::Zero())
- , f_with_delta_(Vector12::Zero())
- , ddq_with_delta_(Vector18::Zero())
- , posf_tmp_(Matrix43::Zero())
- , log_feet_pos_target(Matrix34::Zero())
- , log_feet_vel_target(Matrix34::Zero())
- , log_feet_acc_target(Matrix34::Zero())
- , k_log_(0)
-{}
-
-void WbcWrapper::initialize(Params& params)
-{
+ : M_(Eigen::Matrix<double, 18, 18>::Zero()),
+ Jc_(Eigen::Matrix<double, 12, 6>::Zero()),
+ k_since_contact_(Eigen::Matrix<double, 1, 4>::Zero()),
+ qdes_(Vector12::Zero()),
+ vdes_(Vector12::Zero()),
+ tau_ff_(Vector12::Zero()),
+ ddq_cmd_(Vector18::Zero()),
+ q_default_(Vector19::Zero()),
+ f_with_delta_(Vector12::Zero()),
+ ddq_with_delta_(Vector18::Zero()),
+ posf_tmp_(Matrix43::Zero()),
+ log_feet_pos_target(Matrix34::Zero()),
+ log_feet_vel_target(Matrix34::Zero()),
+ log_feet_acc_target(Matrix34::Zero()),
+ k_log_(0) {}
+
+void WbcWrapper::initialize(Params ¶ms) {
// Params store parameters
params_ = ¶ms;
// Path to the robot URDF (TODO: Automatic path)
- const std::string filename = std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
+ const std::string filename =
+ std::string("/opt/openrobots/share/example-robot-data/robots/solo_description/robots/solo12.urdf");
// Build model from urdf (base is not free flyer)
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model_, false);
@@ -494,7 +472,7 @@ void WbcWrapper::initialize(Params& params)
// Update all the quantities of the model
VectorN q_tmp = VectorN::Zero(model_.nq);
q_tmp(6, 0) = 1.0; // Quaternion (0, 0, 0, 1)
- pinocchio::computeAllTerms(model_, data_ , q_tmp, VectorN::Zero(model_.nv));
+ pinocchio::computeAllTerms(model_, data_, q_tmp, VectorN::Zero(model_.nv));
// Initialize inverse kinematic and box QP solvers
invkin_ = new InvKin();
@@ -514,14 +492,12 @@ void WbcWrapper::initialize(Params& params)
// Make mass matrix symetric
data_.M.triangularView<Eigen::StrictlyLower>() = data_.M.transpose().triangularView<Eigen::StrictlyLower>();
-
}
-void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_cmd, MatrixN const& contacts,
- MatrixN const& pgoals, MatrixN const& vgoals, MatrixN const& agoals)
-{
+void WbcWrapper::compute(VectorN const &q, VectorN const &dq, MatrixN const &f_cmd, MatrixN const &contacts,
+ MatrixN const &pgoals, MatrixN const &vgoals, MatrixN const &agoals) {
// Update nb of iterations since contact
- k_since_contact_ += contacts; // Increment feet in stance phase
+ k_since_contact_ += contacts; // Increment feet in stance phase
k_since_contact_ = k_since_contact_.cwiseProduct(contacts); // Reset feet in swing phase
// Store target positions, velocities and acceleration for logging purpose
@@ -538,17 +514,12 @@ void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_c
// Retrieve feet jacobian
posf_tmp_ = invkin_->get_posf();
- for (int i = 0; i < 4; i++)
- {
- if (contacts(0, i))
- {
+ for (int i = 0; i < 4; i++) {
+ if (contacts(0, i)) {
Jc_.block(3 * i, 0, 3, 3) = Matrix3::Identity();
- Jc_.block(3 * i, 3, 3, 3) << 0.0, posf_tmp_(i, 2), -posf_tmp_(i, 1),
- -posf_tmp_(i, 2), 0.0, posf_tmp_(i, 0),
- posf_tmp_(i, 1), -posf_tmp_(i, 0), 0.0;
- }
- else
- {
+ Jc_.block(3 * i, 3, 3, 3) << 0.0, posf_tmp_(i, 2), -posf_tmp_(i, 1), -posf_tmp_(i, 2), 0.0, posf_tmp_(i, 0),
+ posf_tmp_(i, 1), -posf_tmp_(i, 0), 0.0;
+ } else {
Jc_.block(3 * i, 0, 3, 6).setZero();
}
}
@@ -570,7 +541,8 @@ void WbcWrapper::compute(VectorN const& q, VectorN const& dq, MatrixN const& f_c
std::cout << k_since_contact_ << std::endl;*/
// Solve the QP problem
- box_qp_->run(data_.M, Jc_, Eigen::Map<const VectorN>(f_cmd.data(), f_cmd.size()), data_.tau.head(6), k_since_contact_);
+ box_qp_->run(data_.M, Jc_, Eigen::Map<const VectorN>(f_cmd.data(), f_cmd.size()), data_.tau.head(6),
+ k_since_contact_);
// Add to reference quantities the deltas found by the QP solver
f_with_delta_ = box_qp_->get_f_res();
diff --git a/src/StatePlanner.cpp b/src/StatePlanner.cpp
index 405f89af..520a58f4 100644
--- a/src/StatePlanner.cpp
+++ b/src/StatePlanner.cpp
@@ -1,64 +1,58 @@
#include "qrw/StatePlanner.hpp"
-StatePlanner::StatePlanner()
- : dt_(0.0)
- , h_ref_(0.0)
- , n_steps_(0)
- , RPY_(Vector3::Zero())
-{
- // Empty
+StatePlanner::StatePlanner() : dt_(0.0), h_ref_(0.0), n_steps_(0), RPY_(Vector3::Zero()) {
+ // Empty
}
-void StatePlanner::initialize(Params& params)
-{
- dt_ = params.dt_mpc;
- h_ref_ = params.h_ref;
- n_steps_ = (int)std::lround(params.T_mpc / dt_);
- referenceStates_ = MatrixN::Zero(12, 1 + n_steps_);
- dt_vector_ = VectorN::LinSpaced(n_steps_, dt_, params.T_mpc);
+void StatePlanner::initialize(Params& params) {
+ dt_ = params.dt_mpc;
+ h_ref_ = params.h_ref;
+ n_steps_ = (int)std::lround(params.T_mpc / dt_);
+ referenceStates_ = MatrixN::Zero(12, 1 + n_steps_);
+ dt_vector_ = VectorN::LinSpaced(n_steps_, dt_, params.T_mpc);
}
-void StatePlanner::computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average)
-{
- if (q.rows() != 6)
- {
- throw std::runtime_error("q should be a vector of size 6");
+void StatePlanner::computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref, double z_average) {
+ if (q.rows() != 6) {
+ throw std::runtime_error("q should be a vector of size 6");
+ }
+ RPY_ = q.tail(3);
+
+ // Update the current state
+ referenceStates_(0, 0) = 0.0; // In horizontal frame x = 0.0
+ referenceStates_(1, 0) = 0.0; // In horizontal frame y = 0.0
+ referenceStates_(2, 0) = q(2, 0); // We keep height
+ referenceStates_.block(3, 0, 2, 1) = RPY_.head(2); // We keep roll and pitch
+ referenceStates_(5, 0) = 0.0; // In horizontal frame yaw = 0.0
+ referenceStates_.block(6, 0, 3, 1) = v.head(3);
+ referenceStates_.block(9, 0, 3, 1) = v.tail(3);
+
+ for (int i = 0; i < n_steps_; i++) {
+ if (vref(5) != 0) {
+ referenceStates_(0, 1 + i) =
+ (vref(0) * std::sin(vref(5) * dt_vector_(i)) + vref(1) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) /
+ vref(5);
+ referenceStates_(1, 1 + i) =
+ (vref(1) * std::sin(vref(5) * dt_vector_(i)) - vref(0) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) /
+ vref(5);
+ } else {
+ referenceStates_(0, 1 + i) = vref(0) * dt_vector_(i);
+ referenceStates_(1, 1 + i) = vref(1) * dt_vector_(i);
}
- RPY_ = q.tail(3);
+ referenceStates_(0, 1 + i) += referenceStates_(0, 0);
+ referenceStates_(1, 1 + i) += referenceStates_(1, 0);
- // Update the current state
- referenceStates_(0, 0) = 0.0; // In horizontal frame x = 0.0
- referenceStates_(1, 0) = 0.0; // In horizontal frame y = 0.0
- referenceStates_(2, 0) = q(2, 0); // We keep height
- referenceStates_.block(3, 0, 2, 1) = RPY_.head(2); // We keep roll and pitch
- referenceStates_(5, 0) = 0.0; // In horizontal frame yaw = 0.0
- referenceStates_.block(6, 0, 3, 1) = v.head(3);
- referenceStates_.block(9, 0, 3, 1) = v.tail(3);
+ referenceStates_(2, 1 + i) = h_ref_ + z_average;
- for (int i = 0; i < n_steps_; i++)
- {
- if (vref(5) != 0)
- {
- referenceStates_(0, 1 + i) = (vref(0) * std::sin(vref(5) * dt_vector_(i)) + vref(1) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) / vref(5);
- referenceStates_(1, 1 + i) = (vref(1) * std::sin(vref(5) * dt_vector_(i)) - vref(0) * (std::cos(vref(5) * dt_vector_(i)) - 1.0)) / vref(5);
- }
- else
- {
- referenceStates_(0, 1 + i) = vref(0) * dt_vector_(i);
- referenceStates_(1, 1 + i) = vref(1) * dt_vector_(i);
- }
- referenceStates_(0, 1 + i) += referenceStates_(0, 0);
- referenceStates_(1, 1 + i) += referenceStates_(1, 0);
+ referenceStates_(5, 1 + i) = vref(5) * dt_vector_(i);
- referenceStates_(2, 1 + i) = h_ref_ + z_average;
+ referenceStates_(6, 1 + i) =
+ vref(0) * std::cos(referenceStates_(5, 1 + i)) - vref(1) * std::sin(referenceStates_(5, 1 + i));
+ referenceStates_(7, 1 + i) =
+ vref(0) * std::sin(referenceStates_(5, 1 + i)) + vref(1) * std::cos(referenceStates_(5, 1 + i));
- referenceStates_(5, 1 + i) = vref(5) * dt_vector_(i);
+ // referenceStates_(5, 1 + i) += RPY_(2);
- referenceStates_(6, 1 + i) = vref(0) * std::cos(referenceStates_(5, 1 + i)) - vref(1) * std::sin(referenceStates_(5, 1 + i));
- referenceStates_(7, 1 + i) = vref(0) * std::sin(referenceStates_(5, 1 + i)) + vref(1) * std::cos(referenceStates_(5, 1 + i));
-
- // referenceStates_(5, 1 + i) += RPY_(2);
-
- referenceStates_(11, 1 + i) = vref(5);
- }
+ referenceStates_(11, 1 + i) = vref(5);
+ }
}
diff --git a/src/main.cpp b/src/main.cpp
index 010b2d02..134c7442 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -10,7 +10,6 @@
#include "qrw/Gait.hpp"
#include "qrw/Params.hpp"
-
int main(int argc, char** argv) {
if (argc == 3) {
int arg_a = std::atoi(argv[1]), arg_b = std::atoi(argv[2]);
@@ -58,113 +57,121 @@ int main(int argc, char** argv) {
}
}*/
- /*EiquadprogFast qp;
- qp.reset(16, 0, 16);
- Eigen::MatrixXd Q_qp = Eigen::MatrixXd::Zero(16,16);
- Eigen::VectorXd C_qp = Eigen::VectorXd::Zero(16);
- Eigen::MatrixXd Aeq = Eigen::MatrixXd::Zero(0, 16);
- Eigen::VectorXd Beq = Eigen::VectorXd::Zero(0);
- Eigen::MatrixXd Aineq = Eigen::MatrixXd::Zero(16, 16);
- Eigen::VectorXd Bineq = Eigen::VectorXd::Zero(16);
- Eigen::VectorXd x_qp = Eigen::VectorXd::Zero(16);
-
- Q_qp <<
- 20.3068, 4.19814, 13.5679, -2.54082, 0, 0, 0, 0, 0, 0, 0, 0, 1.84671, -11.9805, -3.77491, -17.6021,
- 4.19814, 10.1805, -4.47586, 1.50651, 0, 0, 0, 0, 0, 0, 0, 0, 0.0678086, -0.0493709, -3.58335, -3.70053,
- 13.5679, -4.47586, 16.9657, -1.07805, 0, 0, 0, 0, 0, 0, 0, 0, 1.54046, -10.3655, -0.113754, -12.0197,
- -2.54082, 1.50651, -1.07805, 2.96928, 0, 0, 0, 0, 0, 0, 0, 0, -0.238438, 1.56562, 0.0778063, 1.88187,
- 0, 0, 0, 0, 2.62, 1, 1, -0.62, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 1, 2.62, -0.62, 1, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 1, -0.62, 2.62, 1, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, -0.62, 1, 1, 2.62, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 2.62, 1, 1, -0.62, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 1, 2.62, -0.62, 1, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 1, -0.62, 2.62, 1, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, -0.62, 1, 1, 2.62, 0, 0, 0, 0,
- 1.84671, 0.0678086, 1.54046, -0.238438, 0, 0, 0, 0, 0, 0, 0, 0, 2.96016, -1.05119, 1.49914, -2.5122,
- -11.9805, -0.0493709, -10.3655, 1.56562, 0, 0, 0, 0, 0, 0, 0, 0, -1.05119, 17.0288, -4.49649, 13.5835,
- -3.77491, -3.58335, -0.113754, 0.0778063, 0, 0, 0, 0, 0, 0, 0, 0, 1.49914, -4.49649, 10.2498, 4.25414,
- -17.6021, -3.70053, -12.0197, 1.88187, 0, 0, 0, 0, 0, 0, 0, 0, -2.5122, 13.5835, 4.25414, 20.3499;
-
- C_qp <<
- -12.97,
- -12.7995,
- -12.8596,
- -12.6892,
- -2.6356e-07,
- -2.6356e-07,
- -2.6356e-07,
- -2.6356e-07,
- -2.6356e-07,
- -2.6356e-07,
- -2.6356e-07,
- -2.6356e-07,
- -12.7546,
- -12.4232,
- -12.7665,
- -12.4351;
-
- for (int i = 0; i < 16; i++) {
- Aineq(i, i) = 1.;
- }
+ /*EiquadprogFast qp;
+ qp.reset(16, 0, 16);
+ Eigen::MatrixXd Q_qp = Eigen::MatrixXd::Zero(16,16);
+ Eigen::VectorXd C_qp = Eigen::VectorXd::Zero(16);
+ Eigen::MatrixXd Aeq = Eigen::MatrixXd::Zero(0, 16);
+ Eigen::VectorXd Beq = Eigen::VectorXd::Zero(0);
+ Eigen::MatrixXd Aineq = Eigen::MatrixXd::Zero(16, 16);
+ Eigen::VectorXd Bineq = Eigen::VectorXd::Zero(16);
+ Eigen::VectorXd x_qp = Eigen::VectorXd::Zero(16);
+
+ Q_qp <<
+ 20.3068, 4.19814, 13.5679, -2.54082, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1.84671, -11.9805, -3.77491, -17.6021, 4.19814, 10.1805, -4.47586, 1.50651, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0.0678086, -0.0493709, -3.58335, -3.70053,
+ 13.5679, -4.47586, 16.9657, -1.07805, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1.54046, -10.3655, -0.113754, -12.0197, -2.54082, 1.50651, -1.07805, 2.96928, 0, 0,
+ 0, 0, 0, 0, 0, 0, -0.238438, 1.56562, 0.0778063, 1.88187, 0,
+ 0, 0, 0, 2.62, 1, 1, -0.62, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1, 2.62, -0.62,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 1, -0.62, 2.62, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, -0.62, 1, 1, 2.62,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 2.62, 1, 1, -0.62, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+ 2.62, -0.62, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 1, -0.62, 2.62, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, -0.62, 1, 1,
+ 2.62, 0, 0, 0, 0, 1.84671, 0.0678086, 1.54046, -0.238438, 0, 0,
+ 0, 0, 0, 0, 0, 0, 2.96016, -1.05119, 1.49914, -2.5122,
+ -11.9805, -0.0493709, -10.3655, 1.56562, 0, 0, 0, 0, 0, 0, 0, 0,
+ -1.05119, 17.0288, -4.49649, 13.5835, -3.77491, -3.58335, -0.113754, 0.0778063, 0, 0,
+ 0, 0, 0, 0, 0, 0, 1.49914, -4.49649, 10.2498, 4.25414,
+ -17.6021, -3.70053, -12.0197, 1.88187, 0, 0, 0, 0, 0, 0, 0, 0,
+ -2.5122, 13.5835, 4.25414, 20.3499;
+
+ C_qp <<
+ -12.97,
+ -12.7995,
+ -12.8596,
+ -12.6892,
+ -2.6356e-07,
+ -2.6356e-07,
+ -2.6356e-07,
+ -2.6356e-07,
+ -2.6356e-07,
+ -2.6356e-07,
+ -2.6356e-07,
+ -2.6356e-07,
+ -12.7546,
+ -12.4232,
+ -12.7665,
+ -12.4351;
+
+ for (int i = 0; i < 16; i++) {
+ Aineq(i, i) = 1.;
+ }
+
+ std::cout << "Matrices:" << std::endl;
+ std::cout << Q_qp << std::endl << "--" << std::endl;
+ std::cout << C_qp << std::endl << "--" << std::endl;
+ std::cout << Aeq << std::endl << "--" << std::endl;
+ std::cout << Beq << std::endl << "--" << std::endl;
+ std::cout << Aineq << std::endl << "--" << std::endl;
+ std::cout << Bineq << std::endl << "--" << std::endl;
+
+ qp.solve_quadprog(Q_qp, C_qp, Aeq, Beq, Aineq, Bineq, x_qp);
+
+ Eigen::VectorXd dx = Eigen::VectorXd::Zero(16);
+ dx(0) = 0.01;
+ dx(1) = 0.01;
+ dx(2) = 0.01;
+ dx(3) = 0.01;
+ dx(12) = 0.01;
+ dx(13) = 0.01;
+ dx(14) = 0.01;
+ dx(15) = 0.01;
+
+ std::cout << "Cost for sol : " << 0.5 * x_qp.transpose() * Q_qp * x_qp + x_qp.transpose() * C_qp << std::endl;
+ std::cout << "Cost for sol-dx: " << 0.5 * (x_qp-dx).transpose() * Q_qp * (x_qp-dx) + (x_qp-dx).transpose() * C_qp
+ << std::endl; std::cout << "Cost for sol+dx: " << 0.5 * (x_qp+dx).transpose() * Q_qp * (x_qp+dx) +
+ (x_qp+dx).transpose() * C_qp << std::endl;*/
+
+ /*Eigen::Matrix<double, 20, 4> desiredGait_;
+ int N = 5;
+ Eigen::Matrix<double, 1, 4> sequence;
+ sequence << 0, 1, 1, 1;
+ desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1, 0, 1, 1;
+ desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1, 1, 0, 1;
+ desiredGait_.block(2*N, 0, N, 4) = sequence.colwise().replicate(N);
+ sequence << 1, 1, 1, 0;
+ desiredGait_.block(3*N, 0, N, 4) = sequence.colwise().replicate(N);
+
+ std::cout << desiredGait_ << std::endl;
+ std::cout << "##" << std::endl;
+ */
+ /*Gait gait = Gait();
+ gait.initialize(0.02, 0.32, 0.16);
- std::cout << "Matrices:" << std::endl;
- std::cout << Q_qp << std::endl << "--" << std::endl;
- std::cout << C_qp << std::endl << "--" << std::endl;
- std::cout << Aeq << std::endl << "--" << std::endl;
- std::cout << Beq << std::endl << "--" << std::endl;
- std::cout << Aineq << std::endl << "--" << std::endl;
- std::cout << Bineq << std::endl << "--" << std::endl;
-
- qp.solve_quadprog(Q_qp, C_qp, Aeq, Beq, Aineq, Bineq, x_qp);
-
- Eigen::VectorXd dx = Eigen::VectorXd::Zero(16);
- dx(0) = 0.01;
- dx(1) = 0.01;
- dx(2) = 0.01;
- dx(3) = 0.01;
- dx(12) = 0.01;
- dx(13) = 0.01;
- dx(14) = 0.01;
- dx(15) = 0.01;
-
- std::cout << "Cost for sol : " << 0.5 * x_qp.transpose() * Q_qp * x_qp + x_qp.transpose() * C_qp << std::endl;
- std::cout << "Cost for sol-dx: " << 0.5 * (x_qp-dx).transpose() * Q_qp * (x_qp-dx) + (x_qp-dx).transpose() * C_qp << std::endl;
- std::cout << "Cost for sol+dx: " << 0.5 * (x_qp+dx).transpose() * Q_qp * (x_qp+dx) + (x_qp+dx).transpose() * C_qp << std::endl;*/
-
- /*Eigen::Matrix<double, 20, 4> desiredGait_;
- int N = 5;
- Eigen::Matrix<double, 1, 4> sequence;
- sequence << 0, 1, 1, 1;
- desiredGait_.block(0, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1, 0, 1, 1;
- desiredGait_.block(N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1, 1, 0, 1;
- desiredGait_.block(2*N, 0, N, 4) = sequence.colwise().replicate(N);
- sequence << 1, 1, 1, 0;
- desiredGait_.block(3*N, 0, N, 4) = sequence.colwise().replicate(N);
-
- std::cout << desiredGait_ << std::endl;
- std::cout << "##" << std::endl;
- */
- /*Gait gait = Gait();
- gait.initialize(0.02, 0.32, 0.16);
-
- std::cout << gait.getPastGait() << std::endl << "##" << std::endl;
- std::cout << gait.getCurrentGait() << std::endl << "##" << std::endl;
- std::cout << gait.getDesiredGait() << std::endl << "##" << std::endl;
-
- for (int k = 0; k < 10; k++)
- {
- gait.updateGait(k, 1, VectorN::Zero(19), 0);
- std::cout << "## " << k << " ##" << std::endl;
std::cout << gait.getPastGait() << std::endl << "##" << std::endl;
std::cout << gait.getCurrentGait() << std::endl << "##" << std::endl;
std::cout << gait.getDesiredGait() << std::endl << "##" << std::endl;
- }*/
+ for (int k = 0; k < 10; k++)
+ {
+ gait.updateGait(k, 1, VectorN::Zero(19), 0);
+ std::cout << "## " << k << " ##" << std::endl;
+ std::cout << gait.getPastGait() << std::endl << "##" << std::endl;
+ std::cout << gait.getCurrentGait() << std::endl << "##" << std::endl;
+ std::cout << gait.getDesiredGait() << std::endl << "##" << std::endl;
+ }*/
- //std::cout << yaml_control_interface::RobotFromYamlFile(CONFIG_SOLO12_YAML) << std::endl;
+ // std::cout << yaml_control_interface::RobotFromYamlFile(CONFIG_SOLO12_YAML) << std::endl;
Params params = Params();
std::cout << params.interface << std::endl;
std::cout << params.SIMULATION << std::endl;
diff --git a/src/st_to_cc.cpp b/src/st_to_cc.cpp
index 2bcddff1..7e3869b5 100644
--- a/src/st_to_cc.cpp
+++ b/src/st_to_cc.cpp
@@ -1,15 +1,14 @@
-# include <stdlib.h>
-# include <stdio.h>
-# include <math.h>
-# include <time.h>
-# include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <time.h>
+#include <string.h>
-# include "other/st_to_cc.hpp"
+#include "other/st_to_cc.hpp"
/******************************************************************************/
-double *cc_mv ( int m, int n, int ncc, int icc[], int ccc[], double acc[],
- double x[] )
+double *cc_mv(int m, int n, int ncc, int icc[], int ccc[], double acc[], double x[])
/******************************************************************************/
/*
@@ -19,7 +18,7 @@ double *cc_mv ( int m, int n, int ncc, int icc[], int ccc[], double acc[],
Licensing:
- This code is distributed under the GNU LGPL license.
+ This code is distributed under the GNU LGPL license.
Modified:
@@ -59,17 +58,14 @@ double *cc_mv ( int m, int n, int ncc, int icc[], int ccc[], double acc[],
int j;
int k;
- b = ( double * ) malloc ( m * sizeof ( double ) );
+ b = (double *)malloc(m * sizeof(double));
- for ( i = 0; i < m; i++ )
- {
+ for (i = 0; i < m; i++) {
b[i] = 0.0;
}
- for ( j = 0; j < n; j++ )
- {
- for ( k = ccc[j]; k < ccc[j+1]; k++ )
- {
+ for (j = 0; j < n; j++) {
+ for (k = ccc[j]; k < ccc[j + 1]; k++) {
i = icc[k];
b[i] = b[i] + acc[k] * x[j];
}
@@ -79,8 +75,7 @@ double *cc_mv ( int m, int n, int ncc, int icc[], int ccc[], double acc[],
}
/******************************************************************************/
-void cc_print ( int m, int n, int ncc, int icc[], int ccc[], double acc[],
- char *title )
+void cc_print(int m, int n, int ncc, int icc[], int ccc[], double acc[], char *title)
/******************************************************************************/
/*
@@ -121,36 +116,29 @@ void cc_print ( int m, int n, int ncc, int icc[], int ccc[], double acc[],
int j;
int k;
- printf ( "\n" );
- printf ( "%s\n", title );
- printf ( " # I J A\n" );
- printf ( " ---- ---- ---- ----------------\n" );
- printf ( "\n" );
+ printf("\n");
+ printf("%s\n", title);
+ printf(" # I J A\n");
+ printf(" ---- ---- ---- ----------------\n");
+ printf("\n");
- if ( ccc[0] == 0 )
- {
+ if (ccc[0] == 0) {
j = 0;
- for ( k = 0; k < ncc; k++ )
- {
+ for (k = 0; k < ncc; k++) {
i = icc[k];
- while ( ccc[j+2] <= k )
- {
+ while (ccc[j + 2] <= k) {
j = j + 1;
}
- printf ( " %4d %4d %4d %16.8g\n", k, i, j, acc[k] );
+ printf(" %4d %4d %4d %16.8g\n", k, i, j, acc[k]);
}
- }
- else
- {
+ } else {
j = 1;
- for ( k = 0; k < ncc; k++ )
- {
+ for (k = 0; k < ncc; k++) {
i = icc[k];
- while ( ccc[j+1] <= k + 1 )
- {
+ while (ccc[j + 1] <= k + 1) {
j = j + 1;
}
- printf ( " %4d %4d %4d %16.8g\n", k + 1, i, j, acc[k] );
+ printf(" %4d %4d %4d %16.8g\n", k + 1, i, j, acc[k]);
}
}
@@ -158,7 +146,7 @@ void cc_print ( int m, int n, int ncc, int icc[], int ccc[], double acc[],
}
/******************************************************************************/
-int i4_max ( int i1, int i2 )
+int i4_max(int i1, int i2)
/******************************************************************************/
/*
@@ -187,19 +175,16 @@ int i4_max ( int i1, int i2 )
{
int value;
- if ( i2 < i1 )
- {
+ if (i2 < i1) {
value = i1;
- }
- else
- {
+ } else {
value = i2;
}
return value;
}
/******************************************************************************/
-int i4_min ( int i1, int i2 )
+int i4_min(int i1, int i2)
/******************************************************************************/
/*
@@ -228,19 +213,16 @@ int i4_min ( int i1, int i2 )
{
int value;
- if ( i1 < i2 )
- {
+ if (i1 < i2) {
value = i1;
- }
- else
- {
+ } else {
value = i2;
}
return value;
}
/******************************************************************************/
-int *i4vec_copy_new ( int n, int a1[] )
+int *i4vec_copy_new(int n, int a1[])
/******************************************************************************/
/*
@@ -276,17 +258,16 @@ int *i4vec_copy_new ( int n, int a1[] )
int *a2;
int i;
- a2 = ( int * ) malloc ( n * sizeof ( int ) );
+ a2 = (int *)malloc(n * sizeof(int));
- for ( i = 0; i < n; i++ )
- {
+ for (i = 0; i < n; i++) {
a2[i] = a1[i];
}
return a2;
}
/******************************************************************************/
-void i4vec_dec ( int n, int a[] )
+void i4vec_dec(int n, int a[])
/******************************************************************************/
/*
@@ -319,15 +300,14 @@ void i4vec_dec ( int n, int a[] )
{
int i;
- for ( i = 0; i < n; i++ )
- {
+ for (i = 0; i < n; i++) {
a[i] = a[i] - 1;
}
return;
}
/******************************************************************************/
-void i4vec_inc ( int n, int a[] )
+void i4vec_inc(int n, int a[])
/******************************************************************************/
/*
@@ -360,15 +340,14 @@ void i4vec_inc ( int n, int a[] )
{
int i;
- for ( i = 0; i < n; i++ )
- {
+ for (i = 0; i < n; i++) {
a[i] = a[i] + 1;
}
return;
}
/******************************************************************************/
-int i4vec_max ( int n, int a[] )
+int i4vec_max(int n, int a[])
/******************************************************************************/
/*
@@ -405,17 +384,14 @@ int i4vec_max ( int n, int a[] )
int i;
int value;
- if ( n <= 0 )
- {
+ if (n <= 0) {
return 0;
}
value = a[0];
- for ( i = 1; i < n; i++ )
- {
- if ( value < a[i] )
- {
+ for (i = 1; i < n; i++) {
+ if (value < a[i]) {
value = a[i];
}
}
@@ -424,7 +400,7 @@ int i4vec_max ( int n, int a[] )
}
/******************************************************************************/
-int i4vec_min ( int n, int a[] )
+int i4vec_min(int n, int a[])
/******************************************************************************/
/*
@@ -461,17 +437,14 @@ int i4vec_min ( int n, int a[] )
int i;
int value;
- if ( n <= 0 )
- {
+ if (n <= 0) {
return 0;
}
value = a[0];
- for ( i = 1; i < n; i++ )
- {
- if ( a[i] < value )
- {
+ for (i = 1; i < n; i++) {
+ if (a[i] < value) {
value = a[i];
}
}
@@ -479,7 +452,7 @@ int i4vec_min ( int n, int a[] )
}
/******************************************************************************/
-void i4vec_write ( char *output_filename, int n, int table[] )
+void i4vec_write(char *output_filename, int n, int table[])
/******************************************************************************/
/*
@@ -493,7 +466,7 @@ void i4vec_write ( char *output_filename, int n, int table[] )
Licensing:
- This code is distributed under the GNU LGPL license.
+ This code is distributed under the GNU LGPL license.
Modified:
@@ -514,35 +487,33 @@ void i4vec_write ( char *output_filename, int n, int table[] )
{
int j;
FILE *output;
-/*
- Open the file.
-*/
- output = fopen ( output_filename, "wt" );
-
- if ( !output )
- {
- fprintf ( stderr, "\n" );
- fprintf ( stderr, "I4VEC_WRITE - Fatal error!\n" );
- fprintf ( stderr, " Could not open the output file.\n" );
- exit ( 1 );
+ /*
+ Open the file.
+ */
+ output = fopen(output_filename, "wt");
+
+ if (!output) {
+ fprintf(stderr, "\n");
+ fprintf(stderr, "I4VEC_WRITE - Fatal error!\n");
+ fprintf(stderr, " Could not open the output file.\n");
+ exit(1);
}
-/*
- Write the data.
-*/
- for ( j = 0; j < n; j++ )
- {
- fprintf ( output, "%d\n", table[j] );
+ /*
+ Write the data.
+ */
+ for (j = 0; j < n; j++) {
+ fprintf(output, "%d\n", table[j]);
}
-/*
- Close the file.
-*/
- fclose ( output );
+ /*
+ Close the file.
+ */
+ fclose(output);
return;
}
/******************************************************************************/
-int i4vec2_compare ( int n, int a1[], int a2[], int i, int j )
+int i4vec2_compare(int n, int a1[], int a2[], int i, int j)
/******************************************************************************/
/*
@@ -581,27 +552,17 @@ int i4vec2_compare ( int n, int a1[], int a2[], int i, int j )
isgn = 0;
- if ( a1[i-1] < a1[j-1] )
- {
+ if (a1[i - 1] < a1[j - 1]) {
isgn = -1;
- }
- else if ( a1[i-1] == a1[j-1] )
- {
- if ( a2[i-1] < a2[j-1] )
- {
+ } else if (a1[i - 1] == a1[j - 1]) {
+ if (a2[i - 1] < a2[j - 1]) {
isgn = -1;
- }
- else if ( a2[i-1] < a2[j-1] )
- {
+ } else if (a2[i - 1] < a2[j - 1]) {
isgn = 0;
- }
- else if ( a2[j-1] < a2[i-1] )
- {
+ } else if (a2[j - 1] < a2[i - 1]) {
isgn = +1;
}
- }
- else if ( a1[j-1] < a1[i-1] )
- {
+ } else if (a1[j - 1] < a1[i - 1]) {
isgn = +1;
}
@@ -609,7 +570,7 @@ int i4vec2_compare ( int n, int a1[], int a2[], int i, int j )
}
/******************************************************************************/
-void i4vec2_sort_a ( int n, int a1[], int a2[] )
+void i4vec2_sort_a(int n, int a1[], int a2[])
/******************************************************************************/
/*
@@ -646,41 +607,36 @@ void i4vec2_sort_a ( int n, int a1[], int a2[] )
int isgn;
int j;
int temp;
-/*
- Initialize.
-*/
+ /*
+ Initialize.
+ */
i = 0;
indx = 0;
isgn = 0;
j = 0;
-/*
- Call the external heap sorter.
-*/
- for ( ; ; )
- {
- sort_heap_external ( n, &indx, &i, &j, isgn );
-/*
- Interchange the I and J objects.
-*/
- if ( 0 < indx )
- {
- temp = a1[i-1];
- a1[i-1] = a1[j-1];
- a1[j-1] = temp;
-
- temp = a2[i-1];
- a2[i-1] = a2[j-1];
- a2[j-1] = temp;
- }
-/*
- Compare the I and J objects.
-*/
- else if ( indx < 0 )
- {
- isgn = i4vec2_compare ( n, a1, a2, i, j );
+ /*
+ Call the external heap sorter.
+ */
+ for (;;) {
+ sort_heap_external(n, &indx, &i, &j, isgn);
+ /*
+ Interchange the I and J objects.
+ */
+ if (0 < indx) {
+ temp = a1[i - 1];
+ a1[i - 1] = a1[j - 1];
+ a1[j - 1] = temp;
+
+ temp = a2[i - 1];
+ a2[i - 1] = a2[j - 1];
+ a2[j - 1] = temp;
}
- else if ( indx == 0 )
- {
+ /*
+ Compare the I and J objects.
+ */
+ else if (indx < 0) {
+ isgn = i4vec2_compare(n, a1, a2, i, j);
+ } else if (indx == 0) {
break;
}
}
@@ -688,7 +644,7 @@ void i4vec2_sort_a ( int n, int a1[], int a2[] )
}
/******************************************************************************/
-int i4vec2_sorted_unique_count ( int n, int a1[], int a2[] )
+int i4vec2_sorted_unique_count(int n, int a1[], int a2[])
/******************************************************************************/
/*
@@ -727,22 +683,18 @@ int i4vec2_sorted_unique_count ( int n, int a1[], int a2[] )
int i;
int iu;
int unique_num;
-
+
unique_num = 0;
- if ( n <= 0 )
- {
+ if (n <= 0) {
return unique_num;
}
iu = 0;
unique_num = 1;
- for ( i = 1; i < n; i++ )
- {
- if ( a1[i] != a1[iu] ||
- a2[i] != a2[iu] )
- {
+ for (i = 1; i < n; i++) {
+ if (a1[i] != a1[iu] || a2[i] != a2[iu]) {
iu = i;
unique_num = unique_num + 1;
}
@@ -752,8 +704,7 @@ int i4vec2_sorted_unique_count ( int n, int a1[], int a2[] )
}
/******************************************************************************/
-void i4vec2_sorted_uniquely ( int n1, int a1[], int b1[], int n2, int a2[],
- int b2[] )
+void i4vec2_sorted_uniquely(int n1, int a1[], int b1[], int n2, int a2[], int b2[])
/******************************************************************************/
/*
@@ -800,18 +751,15 @@ void i4vec2_sorted_uniquely ( int n1, int a1[], int b1[], int n2, int a2[],
i1 = 0;
i2 = 0;
- if ( n1 <= 0 )
- {
+ if (n1 <= 0) {
return;
}
a2[i2] = a1[i1];
b2[i2] = b1[i1];
- for ( i1 = 1; i1 < n1; i1++ )
- {
- if ( a1[i1] != a2[i2] || b1[i1] != b2[i2] )
- {
+ for (i1 = 1; i1 < n1; i1++) {
+ if (a1[i1] != a2[i2] || b1[i1] != b2[i2]) {
i2 = i2 + 1;
a2[i2] = a1[i1];
b2[i2] = b1[i1];
@@ -822,7 +770,7 @@ void i4vec2_sorted_uniquely ( int n1, int a1[], int b1[], int n2, int a2[],
}
/******************************************************************************/
-double r8_uniform_01 ( int *seed )
+double r8_uniform_01(int *seed)
/******************************************************************************/
/*
@@ -900,20 +848,19 @@ double r8_uniform_01 ( int *seed )
k = *seed / 127773;
- *seed = 16807 * ( *seed - k * 127773 ) - k * 2836;
+ *seed = 16807 * (*seed - k * 127773) - k * 2836;
- if ( *seed < 0 )
- {
+ if (*seed < 0) {
*seed = *seed + i4_huge;
}
- r = ( ( double ) ( *seed ) ) * 4.656612875E-10;
+ r = ((double)(*seed)) * 4.656612875E-10;
return r;
}
/******************************************************************************/
-double r8vec_diff_norm ( int n, double a[], double b[] )
+double r8vec_diff_norm(int n, double a[], double b[])
/******************************************************************************/
/*
@@ -955,17 +902,16 @@ double r8vec_diff_norm ( int n, double a[], double b[] )
value = 0.0;
- for ( i = 0; i < n; i++ )
- {
- value = value + ( a[i] - b[i] ) * ( a[i] - b[i] );
+ for (i = 0; i < n; i++) {
+ value = value + (a[i] - b[i]) * (a[i] - b[i]);
}
- value = sqrt ( value );
+ value = sqrt(value);
return value;
}
/******************************************************************************/
-double *r8vec_uniform_01_new ( int n, int *seed )
+double *r8vec_uniform_01_new(int n, int *seed)
/******************************************************************************/
/*
@@ -1038,35 +984,32 @@ double *r8vec_uniform_01_new ( int n, int *seed )
int k;
double *r;
- if ( *seed == 0 )
- {
- fprintf ( stderr, "\n" );
- fprintf ( stderr, "R8VEC_UNIFORM_01_NEW - Fatal error!\n" );
- fprintf ( stderr, " Input value of SEED = 0.\n" );
- exit ( 1 );
+ if (*seed == 0) {
+ fprintf(stderr, "\n");
+ fprintf(stderr, "R8VEC_UNIFORM_01_NEW - Fatal error!\n");
+ fprintf(stderr, " Input value of SEED = 0.\n");
+ exit(1);
}
- r = ( double * ) malloc ( n * sizeof ( double ) );
+ r = (double *)malloc(n * sizeof(double));
- for ( i = 0; i < n; i++ )
- {
+ for (i = 0; i < n; i++) {
k = *seed / 127773;
- *seed = 16807 * ( *seed - k * 127773 ) - k * 2836;
+ *seed = 16807 * (*seed - k * 127773) - k * 2836;
- if ( *seed < 0 )
- {
+ if (*seed < 0) {
*seed = *seed + i4_huge;
}
- r[i] = ( double ) ( *seed ) * 4.656612875E-10;
+ r[i] = (double)(*seed) * 4.656612875E-10;
}
return r;
}
/******************************************************************************/
-void r8vec_write ( char *output_filename, int n, double x[] )
+void r8vec_write(char *output_filename, int n, double x[])
/******************************************************************************/
/*
@@ -1080,7 +1023,7 @@ void r8vec_write ( char *output_filename, int n, double x[] )
Licensing:
- This code is distributed under the GNU LGPL license.
+ This code is distributed under the GNU LGPL license.
Modified:
@@ -1101,35 +1044,33 @@ void r8vec_write ( char *output_filename, int n, double x[] )
{
int j;
FILE *output;
-/*
- Open the file.
-*/
- output = fopen ( output_filename, "wt" );
-
- if ( !output )
- {
- fprintf ( stderr, "\n" );
- fprintf ( stderr, "R8VEC_WRITE - Fatal error!\n" );
- fprintf ( stderr, " Could not open the output file.\n" );
- exit ( 1 );
+ /*
+ Open the file.
+ */
+ output = fopen(output_filename, "wt");
+
+ if (!output) {
+ fprintf(stderr, "\n");
+ fprintf(stderr, "R8VEC_WRITE - Fatal error!\n");
+ fprintf(stderr, " Could not open the output file.\n");
+ exit(1);
}
-/*
- Write the data.
-*/
- for ( j = 0; j < n; j++ )
- {
- fprintf ( output, " %24.16g\n", x[j] );
+ /*
+ Write the data.
+ */
+ for (j = 0; j < n; j++) {
+ fprintf(output, " %24.16g\n", x[j]);
}
-/*
- Close the file.
-*/
- fclose ( output );
+ /*
+ Close the file.
+ */
+ fclose(output);
return;
}
/******************************************************************************/
-void sort_heap_external ( int n, int *indx, int *i, int *j, int isgn )
+void sort_heap_external(int n, int *indx, int *i, int *j, int isgn)
/******************************************************************************/
/*
@@ -1197,27 +1138,22 @@ void sort_heap_external ( int n, int *indx, int *i, int *j, int isgn )
static int k = 0;
static int k1 = 0;
static int n1 = 0;
-/*
- INDX = 0: This is the first call.
-*/
- if ( *indx == 0 )
- {
-
+ /*
+ INDX = 0: This is the first call.
+ */
+ if (*indx == 0) {
i_save = 0;
j_save = 0;
k = n / 2;
k1 = k;
n1 = n;
}
-/*
- INDX < 0: The user is returning the results of a comparison.
-*/
- else if ( *indx < 0 )
- {
- if ( *indx == -2 )
- {
- if ( isgn < 0 )
- {
+ /*
+ INDX < 0: The user is returning the results of a comparison.
+ */
+ else if (*indx < 0) {
+ if (*indx == -2) {
+ if (isgn < 0) {
i_save = i_save + 1;
}
j_save = k1;
@@ -1228,24 +1164,19 @@ void sort_heap_external ( int n, int *indx, int *i, int *j, int isgn )
return;
}
- if ( 0 < isgn )
- {
+ if (0 < isgn) {
*indx = 2;
*i = i_save;
*j = j_save;
return;
}
- if ( k <= 1 )
- {
- if ( n1 == 1 )
- {
+ if (k <= 1) {
+ if (n1 == 1) {
i_save = 0;
j_save = 0;
*indx = 0;
- }
- else
- {
+ } else {
i_save = n1;
j_save = 1;
n1 = n1 - 1;
@@ -1258,30 +1189,24 @@ void sort_heap_external ( int n, int *indx, int *i, int *j, int isgn )
k = k - 1;
k1 = k;
}
-/*
- 0 < INDX: the user was asked to make an interchange.
-*/
- else if ( *indx == 1 )
- {
+ /*
+ 0 < INDX: the user was asked to make an interchange.
+ */
+ else if (*indx == 1) {
k1 = k;
}
- for ( ; ; )
- {
-
+ for (;;) {
i_save = 2 * k1;
- if ( i_save == n1 )
- {
+ if (i_save == n1) {
j_save = k1;
k1 = i_save;
*indx = -1;
*i = i_save;
*j = j_save;
return;
- }
- else if ( i_save <= n1 )
- {
+ } else if (i_save <= n1) {
j_save = i_save + 1;
*indx = -2;
*i = i_save;
@@ -1289,8 +1214,7 @@ void sort_heap_external ( int n, int *indx, int *i, int *j, int isgn )
return;
}
- if ( k <= 1 )
- {
+ if (k <= 1) {
break;
}
@@ -1298,16 +1222,13 @@ void sort_heap_external ( int n, int *indx, int *i, int *j, int isgn )
k1 = k;
}
- if ( n1 == 1 )
- {
+ if (n1 == 1) {
i_save = 0;
j_save = 0;
*indx = 0;
*i = i_save;
*j = j_save;
- }
- else
- {
+ } else {
i_save = n1;
j_save = 1;
n1 = n1 - 1;
@@ -1320,8 +1241,7 @@ void sort_heap_external ( int n, int *indx, int *i, int *j, int isgn )
}
/******************************************************************************/
-void st_data_read ( char *input_filename, int m, int n, int nst, int ist[],
- int jst[], double ast[] )
+void st_data_read(char *input_filename, int m, int n, int nst, int ist[], int jst[], double ast[])
/******************************************************************************/
/*
@@ -1363,18 +1283,16 @@ void st_data_read ( char *input_filename, int m, int n, int nst, int ist[],
int k;
int num;
- input = fopen ( input_filename, "rt" );
+ input = fopen(input_filename, "rt");
- for ( k = 0; k < nst; k++ )
- {
- num = fscanf ( input, "%i%i%lf", &i, &j, &aij );
+ for (k = 0; k < nst; k++) {
+ num = fscanf(input, "%i%i%lf", &i, &j, &aij);
- if ( num != 3 )
- {
- fprintf ( stderr, "\n" );
- fprintf ( stderr, "ST_DATA_READ - Fatal error!\n" );
- fprintf ( stderr, " I/O error reading data index %d\n", k );
- exit ( 1 );
+ if (num != 3) {
+ fprintf(stderr, "\n");
+ fprintf(stderr, "ST_DATA_READ - Fatal error!\n");
+ fprintf(stderr, " I/O error reading data index %d\n", k);
+ exit(1);
}
ist[k] = i;
@@ -1382,14 +1300,13 @@ void st_data_read ( char *input_filename, int m, int n, int nst, int ist[],
ast[k] = aij;
}
- fclose ( input );
+ fclose(input);
return;
}
/******************************************************************************/
-void st_header_print ( int i_min, int i_max, int j_min, int j_max, int m,
- int n, int nst )
+void st_header_print(int i_min, int i_max, int j_min, int j_max, int m, int n, int nst)
/******************************************************************************/
/*
@@ -1422,23 +1339,22 @@ void st_header_print ( int i_min, int i_max, int j_min, int j_max, int m,
Input, int NST, the number of nonzeros.
*/
{
- printf ( "\n" );
- printf ( " Sparse Triplet (ST) header information:\n" );
- printf ( "\n" );
- printf ( " Minimum row index I_MIN = %d\n", i_min );
- printf ( " Maximum row index I_MAX = %d\n", i_max );
- printf ( " Minimum col index J_MIN = %d\n", j_min );
- printf ( " Maximum col index J_MAX = %d\n", j_max );
- printf ( " Number of rows M = %d\n", m );
- printf ( " Number of columns N = %d\n", n );
- printf ( " Number of nonzeros NST = %d\n", nst );
+ printf("\n");
+ printf(" Sparse Triplet (ST) header information:\n");
+ printf("\n");
+ printf(" Minimum row index I_MIN = %d\n", i_min);
+ printf(" Maximum row index I_MAX = %d\n", i_max);
+ printf(" Minimum col index J_MIN = %d\n", j_min);
+ printf(" Maximum col index J_MAX = %d\n", j_max);
+ printf(" Number of rows M = %d\n", m);
+ printf(" Number of columns N = %d\n", n);
+ printf(" Number of nonzeros NST = %d\n", nst);
return;
}
/******************************************************************************/
-void st_header_read ( char *input_filename, int *i_min, int *i_max, int *j_min,
- int *j_max, int *m, int *n, int *nst )
+void st_header_read(char *input_filename, int *i_min, int *i_max, int *j_min, int *j_max, int *m, int *n, int *nst)
/******************************************************************************/
/*
@@ -1480,31 +1396,29 @@ void st_header_read ( char *input_filename, int *i_min, int *i_max, int *j_min,
int j;
int num;
- input = fopen ( input_filename, "rt" );
+ input = fopen(input_filename, "rt");
*nst = 0;
- *i_min = + i4_huge;
- *i_max = - i4_huge;
- *j_min = + i4_huge;
- *j_max = - i4_huge;
+ *i_min = +i4_huge;
+ *i_max = -i4_huge;
+ *j_min = +i4_huge;
+ *j_max = -i4_huge;
- for ( ; ; )
- {
- num = fscanf ( input, "%i%i%lf", &i, &j, &aij );
+ for (;;) {
+ num = fscanf(input, "%i%i%lf", &i, &j, &aij);
- if ( num != 3 )
- {
+ if (num != 3) {
break;
}
*nst = *nst + 1;
- *i_min = i4_min ( *i_min, i );
- *i_max = i4_max ( *i_max, i );
- *j_min = i4_min ( *j_min, j );
- *j_max = i4_max ( *j_max, j );
+ *i_min = i4_min(*i_min, i);
+ *i_max = i4_max(*i_max, i);
+ *j_min = i4_min(*j_min, j);
+ *j_max = i4_max(*j_max, j);
}
- fclose ( input );
+ fclose(input);
*m = *i_max - *i_min + 1;
*n = *j_max - *j_min + 1;
@@ -1513,8 +1427,7 @@ void st_header_read ( char *input_filename, int *i_min, int *i_max, int *j_min,
}
/******************************************************************************/
-double *st_mv ( int m, int n, int nst, int ist[], int jst[], double ast[],
- double x[] )
+double *st_mv(int m, int n, int nst, int ist[], int jst[], double ast[], double x[])
/******************************************************************************/
/*
@@ -1524,7 +1437,7 @@ double *st_mv ( int m, int n, int nst, int ist[], int jst[], double ast[],
Licensing:
- This code is distributed under the GNU LGPL license.
+ This code is distributed under the GNU LGPL license.
Modified:
@@ -1554,10 +1467,9 @@ double *st_mv ( int m, int n, int nst, int ist[], int jst[], double ast[],
int j;
int k;
- b = ( double * ) malloc ( m * sizeof ( double ) );
+ b = (double *)malloc(m * sizeof(double));
- for ( k = 0; k < nst; k++ )
- {
+ for (k = 0; k < nst; k++) {
i = ist[k];
j = jst[k];
b[i] = b[i] + ast[k] * x[j];
@@ -1567,8 +1479,7 @@ double *st_mv ( int m, int n, int nst, int ist[], int jst[], double ast[],
}
/******************************************************************************/
-void st_print ( int m, int n, int nst, int ist[], int jst[], double ast[],
- char *title )
+void st_print(int m, int n, int nst, int ist[], int jst[], double ast[], char *title)
/******************************************************************************/
/*
@@ -1605,21 +1516,20 @@ void st_print ( int m, int n, int nst, int ist[], int jst[], double ast[],
{
int k;
- printf ( "\n" );
- printf ( "%s\n", title );
- printf ( " # I J A\n" );
- printf ( " ---- ---- ---- --------------\n" );
- printf ( "\n" );
- for ( k = 0; k < nst; k++ )
- {
- printf ( " %4d %4d %4d %16.8g\n", k, ist[k], jst[k], ast[k] );
+ printf("\n");
+ printf("%s\n", title);
+ printf(" # I J A\n");
+ printf(" ---- ---- ---- --------------\n");
+ printf("\n");
+ for (k = 0; k < nst; k++) {
+ printf(" %4d %4d %4d %16.8g\n", k, ist[k], jst[k], ast[k]);
}
return;
}
/******************************************************************************/
-int st_to_cc_size ( int nst, int ist[], int jst[] )
+int st_to_cc_size(int nst, int ist[], int jst[])
/******************************************************************************/
/*
@@ -1629,7 +1539,7 @@ int st_to_cc_size ( int nst, int ist[], int jst[] )
Licensing:
- This code is distributed under the GNU LGPL license.
+ This code is distributed under the GNU LGPL license.
Modified:
@@ -1651,29 +1561,28 @@ int st_to_cc_size ( int nst, int ist[], int jst[] )
int *ist2;
int *jst2;
int ncc;
-/*
- Make copies so the sorting doesn't confuse the user.
-*/
- ist2 = i4vec_copy_new ( nst, ist );
- jst2 = i4vec_copy_new ( nst, jst );
-/*
- Sort by column first, then row.
-*/
- i4vec2_sort_a ( nst, jst2, ist2 );
-/*
- Count the unique pairs.
-*/
- ncc = i4vec2_sorted_unique_count ( nst, jst2, ist2 );
-
- free ( ist2 );
- free ( jst2 );
+ /*
+ Make copies so the sorting doesn't confuse the user.
+ */
+ ist2 = i4vec_copy_new(nst, ist);
+ jst2 = i4vec_copy_new(nst, jst);
+ /*
+ Sort by column first, then row.
+ */
+ i4vec2_sort_a(nst, jst2, ist2);
+ /*
+ Count the unique pairs.
+ */
+ ncc = i4vec2_sorted_unique_count(nst, jst2, ist2);
+
+ free(ist2);
+ free(jst2);
return ncc;
}
/******************************************************************************/
-void st_to_cc_index ( int nst, int ist[], int jst[], int ncc, int n,
- int icc[], int ccc[] )
+void st_to_cc_index(int nst, int ist[], int jst[], int ncc, int n, int icc[], int ccc[])
/******************************************************************************/
/*
@@ -1683,7 +1592,7 @@ void st_to_cc_index ( int nst, int ist[], int jst[], int ncc, int n,
Licensing:
- This code is distributed under the GNU LGPL license.
+ This code is distributed under the GNU LGPL license.
Modified:
@@ -1715,53 +1624,48 @@ void st_to_cc_index ( int nst, int ist[], int jst[], int ncc, int n,
int jlo;
int *jst2;
int k;
-/*
- Make copies so the sorting doesn't confuse the user.
-*/
- ist2 = i4vec_copy_new ( nst, ist );
- jst2 = i4vec_copy_new ( nst, jst );
-/*
- Sort the elements.
-*/
- i4vec2_sort_a ( nst, jst2, ist2 );
-/*
- Get the unique elements.
-*/
- jcc = ( int * ) malloc ( ncc * sizeof ( int ) );
- i4vec2_sorted_uniquely ( nst, jst2, ist2, ncc, jcc, icc );
-/*
- Compress the column index.
-*/
+ /*
+ Make copies so the sorting doesn't confuse the user.
+ */
+ ist2 = i4vec_copy_new(nst, ist);
+ jst2 = i4vec_copy_new(nst, jst);
+ /*
+ Sort the elements.
+ */
+ i4vec2_sort_a(nst, jst2, ist2);
+ /*
+ Get the unique elements.
+ */
+ jcc = (int *)malloc(ncc * sizeof(int));
+ i4vec2_sorted_uniquely(nst, jst2, ist2, ncc, jcc, icc);
+ /*
+ Compress the column index.
+ */
ccc[0] = 0;
jlo = 0;
- for ( k = 0; k < ncc; k++ )
- {
+ for (k = 0; k < ncc; k++) {
jhi = jcc[k];
- if ( jhi != jlo )
- {
- for ( j = jlo + 1; j <= jhi; j++ )
- {
+ if (jhi != jlo) {
+ for (j = jlo + 1; j <= jhi; j++) {
ccc[j] = k;
}
jlo = jhi;
}
}
jhi = n;
- for ( j = jlo + 1; j <= jhi; j++ )
- {
+ for (j = jlo + 1; j <= jhi; j++) {
ccc[j] = ncc;
}
- free ( ist2 );
- free ( jcc );
- free ( jst2 );
+ free(ist2);
+ free(jcc);
+ free(jst2);
return;
}
/******************************************************************************/
-double *st_to_cc_values ( int nst, int ist[], int jst[], double ast[], int ncc,
- int n, int icc[], int ccc[] )
+double *st_to_cc_values(int nst, int ist[], int jst[], double ast[], int ncc, int n, int icc[], int ccc[])
/******************************************************************************/
/*
@@ -1771,7 +1675,7 @@ double *st_to_cc_values ( int nst, int ist[], int jst[], double ast[], int ncc,
Licensing:
- This code is distributed under the GNU LGPL license.
+ This code is distributed under the GNU LGPL license.
Modified:
@@ -1809,52 +1713,46 @@ double *st_to_cc_values ( int nst, int ist[], int jst[], double ast[], int ncc,
int kcc;
int kst;
- acc = ( double * ) malloc ( ncc * sizeof ( double ) );
+ acc = (double *)malloc(ncc * sizeof(double));
- for ( i = 0; i < ncc; i++ )
- {
+ for (i = 0; i < ncc; i++) {
acc[i] = 0.0;
}
- for ( kst = 0; kst < nst; kst++ )
- {
+ for (kst = 0; kst < nst; kst++) {
i = ist[kst];
j = jst[kst];
clo = ccc[j];
- chi = ccc[j+1];
+ chi = ccc[j + 1];
fail = 1;
- for ( kcc = clo; kcc < chi; kcc++ )
- {
- if ( icc[kcc] == i )
- {
+ for (kcc = clo; kcc < chi; kcc++) {
+ if (icc[kcc] == i) {
acc[kcc] = acc[kcc] + ast[kst];
fail = 0;
break;
}
}
- if ( fail )
- {
- fprintf ( stderr, "\n" );
- fprintf ( stderr, "ST_TO_CC_VALUES - Fatal error!\n" );
- fprintf ( stderr, " ST entry cannot be located in CC array.\n" );
- fprintf ( stderr, " ST index KST = %d\n", kst );
- fprintf ( stderr, " ST row IST(KST) = %d\n", ist[kst] );
- fprintf ( stderr, " ST col JST(KST) = %d\n", jst[kst] );
- fprintf ( stderr, " ST val AST(KST) = %g\n", ast[kst] );
- exit ( 1 );
+ if (fail) {
+ fprintf(stderr, "\n");
+ fprintf(stderr, "ST_TO_CC_VALUES - Fatal error!\n");
+ fprintf(stderr, " ST entry cannot be located in CC array.\n");
+ fprintf(stderr, " ST index KST = %d\n", kst);
+ fprintf(stderr, " ST row IST(KST) = %d\n", ist[kst]);
+ fprintf(stderr, " ST col JST(KST) = %d\n", jst[kst]);
+ fprintf(stderr, " ST val AST(KST) = %g\n", ast[kst]);
+ exit(1);
}
-
}
return acc;
}
/******************************************************************************/
-void timestamp ( )
+void timestamp()
/******************************************************************************/
/*
@@ -1883,26 +1781,25 @@ void timestamp ( )
None
*/
{
-# define TIME_SIZE 40
+#define TIME_SIZE 40
static char time_buffer[TIME_SIZE];
const struct tm *tm;
time_t now;
- now = time ( NULL );
- tm = localtime ( &now );
+ now = time(NULL);
+ tm = localtime(&now);
- strftime ( time_buffer, TIME_SIZE, "%d %B %Y %I:%M:%S %p", tm );
+ strftime(time_buffer, TIME_SIZE, "%d %B %Y %I:%M:%S %p", tm);
- fprintf ( stdout, "%s\n", time_buffer );
+ fprintf(stdout, "%s\n", time_buffer);
return;
-# undef TIME_SIZE
+#undef TIME_SIZE
}
/******************************************************************************/
-double *wathen_st ( int nx, int ny, int nz_num, int *seed, int row[],
- int col[] )
+double *wathen_st(int nx, int ny, int nz_num, int *seed, int row[], int col[])
/******************************************************************************/
/*
@@ -1985,30 +1882,26 @@ double *wathen_st ( int nx, int ny, int nz_num, int *seed, int row[],
Parameters:
- Input, int NX, NY, values which determine the size of
+ Input, int NX, NY, values which determine the size of
the matrix.
- Input, int NZ_NUM, the number of values used to
+ Input, int NZ_NUM, the number of values used to
describe the matrix.
Input/output, int *SEED, the random number seed.
- Output, int ROW[NZ_NUM], COL[NZ_NUM], the row and
+ Output, int ROW[NZ_NUM], COL[NZ_NUM], the row and
column indices of the nonzero entries.
Output, double WATHEN_ST[NZ_NUM], the nonzero entries of the matrix.
*/
{
double *a;
- const double em[8*8] = {
- 6.0, -6.0, 2.0, -8.0, 3.0, -8.0, 2.0, -6.0,
- -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0,
- 2.0, -6.0, 6.0, -6.0, 2.0, -8.0, 3.0, -8.0,
- -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0,
- 3.0, -8.0, 2.0, -6.0, 6.0, -6.0, 2.0, -8.0,
- -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0,
- 2.0, -8.0, 3.0, -8.0, 2.0, -6.0, 6.0, -6.0,
- -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 };
+ const double em[8 * 8] = {6.0, -6.0, 2.0, -8.0, 3.0, -8.0, 2.0, -6.0, -6.0, 32.0, -6.0, 20.0, -8.0,
+ 16.0, -8.0, 20.0, 2.0, -6.0, 6.0, -6.0, 2.0, -8.0, 3.0, -8.0, -8.0, 20.0,
+ -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, 3.0, -8.0, 2.0, -6.0, 6.0, -6.0, 2.0,
+ -8.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, 2.0, -8.0, 3.0, -8.0,
+ 2.0, -6.0, 6.0, -6.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0};
int i;
int j;
int k;
@@ -2017,10 +1910,9 @@ double *wathen_st ( int nx, int ny, int nz_num, int *seed, int row[],
int node[8];
double rho;
- a = ( double * ) malloc ( nz_num * sizeof ( double ) );
-
- for ( k = 0; k < nz_num; k++ )
- {
+ a = (double *)malloc(nz_num * sizeof(double));
+
+ for (k = 0; k < nz_num; k++) {
row[k] = 0;
col[k] = 0;
a[k] = 0.0;
@@ -2028,28 +1920,24 @@ double *wathen_st ( int nx, int ny, int nz_num, int *seed, int row[],
k = 0;
- for ( j = 0; j < nx; j++ )
- {
- for ( i = 0; i < nx; i++ )
- {
- node[0] = 3 * ( j + 1 ) * nx + 2 * ( j + 1 ) + 2 * ( i + 1 );
+ for (j = 0; j < nx; j++) {
+ for (i = 0; i < nx; i++) {
+ node[0] = 3 * (j + 1) * nx + 2 * (j + 1) + 2 * (i + 1);
node[1] = node[0] - 1;
node[2] = node[0] - 2;
- node[3] = ( 3 * ( j + 1 ) - 1 ) * nx + 2 * ( j + 1 ) + ( i + 1 ) - 2;
- node[4] = ( 3 * ( j + 1 ) - 3 ) * nx + 2 * ( j + 1 ) + 2 * ( i + 1 ) - 4;
+ node[3] = (3 * (j + 1) - 1) * nx + 2 * (j + 1) + (i + 1) - 2;
+ node[4] = (3 * (j + 1) - 3) * nx + 2 * (j + 1) + 2 * (i + 1) - 4;
node[5] = node[4] + 1;
node[6] = node[4] + 2;
node[7] = node[3] + 1;
- rho = 100.0 * r8_uniform_01 ( seed );
+ rho = 100.0 * r8_uniform_01(seed);
- for ( krow = 0; krow < 8; krow++ )
- {
- for ( kcol = 0; kcol < 8; kcol++ )
- {
+ for (krow = 0; krow < 8; krow++) {
+ for (kcol = 0; kcol < 8; kcol++) {
row[k] = node[krow];
col[k] = node[kcol];
- a[k] = rho * em[krow+kcol*8];
+ a[k] = rho * em[krow + kcol * 8];
k = k + 1;
}
}
@@ -2060,7 +1948,7 @@ double *wathen_st ( int nx, int ny, int nz_num, int *seed, int row[],
}
/******************************************************************************/
-int wathen_st_size ( int nx, int ny )
+int wathen_st_size(int nx, int ny)
/******************************************************************************/
/*
--
GitLab