refact with reference
This commit is contained in:
Huang Zhenbiao
parent
657029864e
commit
85be4715ab
|
|
@ -45,6 +45,10 @@ add_library(${PROJECT_NAME} SHARED
|
|||
src/quadProgpp/Array.cc
|
||||
src/quadProgpp/QuadProg++.cc
|
||||
|
||||
src/gait/WaveGenerator.cpp
|
||||
src/gait/FootEndCtrl.cpp
|
||||
src/gait/GaitGenerator.cpp
|
||||
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC
|
||||
|
|
|
|||
|
|
@ -22,12 +22,15 @@ public:
|
|||
private:
|
||||
void calcTorque();
|
||||
|
||||
KDL::Vector pcd_;
|
||||
KDL::Vector pcdInit_;
|
||||
Estimator& estimator_;
|
||||
QuadrupedRobot& robot_model_;
|
||||
BalanceCtrl& balance_ctrl_;
|
||||
|
||||
Vec3 pcd_, pcd_init_;
|
||||
RotMat Rd_;
|
||||
KDL::Rotation init_rotation_;
|
||||
|
||||
KDL::Vector pose_body_, vel_body_;
|
||||
Vec3 pose_body_, vel_body_;
|
||||
|
||||
double kp_w_;
|
||||
Mat3 Kp_p_, Kd_p_, Kd_w_;
|
||||
|
|
|
|||
|
|
@ -23,10 +23,16 @@ enum class FSMMode {
|
|||
CHANGE
|
||||
};
|
||||
|
||||
enum class FrameType{
|
||||
enum class FrameType {
|
||||
BODY,
|
||||
HIP,
|
||||
GLOBAL
|
||||
};
|
||||
|
||||
enum class WaveStatus {
|
||||
STANCE_ALL,
|
||||
SWING_ALL,
|
||||
WAVE_ALL
|
||||
};
|
||||
|
||||
#endif //ENUMCLASS_H
|
||||
|
|
|
|||
|
|
@ -5,8 +5,6 @@
|
|||
#ifndef BALANCECTRL_H
|
||||
#define BALANCECTRL_H
|
||||
|
||||
#include <kdl/frames.hpp>
|
||||
|
||||
#include "unitree_guide_controller/common/mathTypes.h"
|
||||
class QuadrupedRobot;
|
||||
|
||||
|
|
@ -18,11 +16,20 @@ public:
|
|||
|
||||
void init(const QuadrupedRobot &robot);
|
||||
|
||||
Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
|
||||
const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact);
|
||||
/**
|
||||
* Calculate the desired feet end force
|
||||
* @param ddPcd desired body acceleration
|
||||
* @param dWbd desired body angular acceleration
|
||||
* @param rot_matrix current body rotation matrix
|
||||
* @param feet_pos_2_body feet positions to body under world frame
|
||||
* @param contact feet contact
|
||||
* @return
|
||||
*/
|
||||
Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
|
||||
const Vec34 &feet_pos_2_body, const std::vector<int> &contact);
|
||||
|
||||
private:
|
||||
void calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &rotM);
|
||||
void calMatrixA(const Vec34 &feet_pos_2_body, const RotMat &rotM);
|
||||
|
||||
void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM);
|
||||
|
||||
|
|
@ -34,7 +41,7 @@ private:
|
|||
Mat6 S_;
|
||||
Mat3 Ib_;
|
||||
Vec6 bd_;
|
||||
Vec3 _g, _pcb;
|
||||
Vec3 g_, pcb_;
|
||||
Vec12 F_, F_prev_, g0T_;
|
||||
double mass_, alpha_, beta_, friction_ratio_;
|
||||
Eigen::MatrixXd CE_, CI_;
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
#include <hardware_interface/loaned_command_interface.hpp>
|
||||
#include <hardware_interface/loaned_state_interface.hpp>
|
||||
#include <control_input_msgs/msg/inputs.hpp>
|
||||
#include <unitree_guide_controller/gait/WaveGenerator.h>
|
||||
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
|
||||
|
||||
#include "BalanceCtrl.h"
|
||||
|
|
@ -50,8 +51,11 @@ struct CtrlComponent {
|
|||
BalanceCtrl default_balance_ctrl_;
|
||||
std::reference_wrapper<BalanceCtrl> balance_ctrl_;
|
||||
|
||||
WaveGenerator default_wave_generator_;
|
||||
std::reference_wrapper<WaveGenerator> wave_generator_;
|
||||
|
||||
CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_),
|
||||
estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_) {
|
||||
estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_), wave_generator_(default_wave_generator_) {
|
||||
}
|
||||
|
||||
void clear() {
|
||||
|
|
|
|||
|
|
@ -4,10 +4,9 @@
|
|||
|
||||
#ifndef ESTIMATOR_H
|
||||
#define ESTIMATOR_H
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <kdl/frames.hpp>
|
||||
#include <unitree_guide_controller/common/mathTypes.h>
|
||||
|
||||
#include "LowPassFilter.h"
|
||||
|
||||
|
|
@ -23,16 +22,16 @@ public:
|
|||
* Get the estimated robot central position
|
||||
* @return robot central position
|
||||
*/
|
||||
KDL::Vector getPosition() {
|
||||
return {x_hat_(0), x_hat_(1), x_hat_(2)};
|
||||
Vec3 getPosition() {
|
||||
return x_hat_.segment(0, 3);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the estimated robot central velocity
|
||||
* @return robot central velocity
|
||||
*/
|
||||
KDL::Vector getVelocity() {
|
||||
return {x_hat_(3), x_hat_(4), x_hat_(5)};
|
||||
Vec3 getVelocity() {
|
||||
return x_hat_.segment(3, 3);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -40,33 +39,19 @@ public:
|
|||
* @param index leg index
|
||||
* @return foot position in world frame
|
||||
*/
|
||||
KDL::Vector getFootPos(const int index) {
|
||||
return getPosition() + rotation_ * foot_poses_[index].p;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get all estimated foot positions in world frame
|
||||
* @return all foot positions in world frame
|
||||
*/
|
||||
std::vector<KDL::Vector> getFootPos() {
|
||||
std::vector<KDL::Vector> foot_pos;
|
||||
foot_pos.resize(4);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
foot_pos[i] = getFootPos(i);
|
||||
}
|
||||
return foot_pos;
|
||||
Vec3 getFootPos(const int index) {
|
||||
return getPosition() + Vec3((rotation_ * foot_poses_[index].p).data);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the estimated foot position in body frame
|
||||
* @return
|
||||
*/
|
||||
std::vector<KDL::Vector> getFootPos2Body() {
|
||||
std::vector<KDL::Vector> foot_pos;
|
||||
foot_pos.resize(4);
|
||||
const KDL::Vector body_pos = getPosition();
|
||||
Vec34 getFootPos2Body() {
|
||||
Vec34 foot_pos;
|
||||
const Vec3 body_pos = getPosition();
|
||||
for (int i = 0; i < 4; i++) {
|
||||
foot_pos[i] = getFootPos(i) - body_pos;
|
||||
foot_pos.col(i) = getFootPos(i) - body_pos;
|
||||
}
|
||||
return foot_pos;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,24 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
|
||||
#ifndef FOOTENDCTRL_H
|
||||
#define FOOTENDCTRL_H
|
||||
#include <unitree_guide_controller/common/mathTypes.h>
|
||||
|
||||
|
||||
class FootEndCtrl {
|
||||
public:
|
||||
FootEndCtrl();
|
||||
|
||||
~FootEndCtrl() = default;
|
||||
|
||||
Vec3 calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase);
|
||||
|
||||
private:
|
||||
double k_x_, k_y_, k_yaw_;
|
||||
};
|
||||
|
||||
|
||||
#endif //FOOTENDCTRL_H
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
|
||||
#ifndef GAITGENERATOR_H
|
||||
#define GAITGENERATOR_H
|
||||
|
||||
|
||||
class Gaitgenerator {
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //GAITGENERATOR_H
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
|
||||
#ifndef WAVEGENERATOR_H
|
||||
#define WAVEGENERATOR_H
|
||||
#include <sys/time.h>
|
||||
#include <unitree_guide_controller/common/enumClass.h>
|
||||
#include <unitree_guide_controller/common/mathTypes.h>
|
||||
|
||||
inline long long getSystemTime() {
|
||||
timeval t{};
|
||||
gettimeofday(&t, nullptr);
|
||||
return 1000000 * t.tv_sec + t.tv_usec;
|
||||
}
|
||||
|
||||
class WaveGenerator {
|
||||
public:
|
||||
WaveGenerator();
|
||||
|
||||
~WaveGenerator() = default;
|
||||
|
||||
void init(double period, double st_ratio, const Vec4 &bias);
|
||||
|
||||
void calculate(Vec4 &phase_result, VecInt4 &contact_result, WaveStatus status);
|
||||
|
||||
[[nodiscard]] double get_t_stance() const { return period_ * st_ratio_; }
|
||||
[[nodiscard]] double get_t_swing() const { return period_ * (1 - st_ratio_); }
|
||||
[[nodiscard]] double get_t() const { return period_; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* Update phase, contact and status based on current time.
|
||||
* @param phase foot phase
|
||||
* @param contact foot contact
|
||||
* @param status Wave Status
|
||||
*/
|
||||
void calcWave(Vec4 &phase, VecInt4 &contact, WaveStatus status);
|
||||
|
||||
double period_{};
|
||||
double st_ratio_{}; // stance phase ratio
|
||||
Vec4 bias_;
|
||||
|
||||
Vec4 normal_t_; // normalize time [0,1)
|
||||
Vec4 phase_, phase_past_; // foot phase
|
||||
VecInt4 contact_, contact_past_; // foot contact
|
||||
VecInt4 switch_status_;
|
||||
WaveStatus status_past_;
|
||||
|
||||
long start_t_{};
|
||||
};
|
||||
|
||||
|
||||
#endif //WAVEGENERATOR_H
|
||||
|
|
@ -7,7 +7,10 @@
|
|||
#include <unitree_guide_controller/common/mathTools.h>
|
||||
|
||||
StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test",
|
||||
std::move(ctrlComp)) {
|
||||
std::move(ctrlComp)),
|
||||
estimator_(ctrlComp.estimator_.get()),
|
||||
robot_model_(ctrlComp.robot_model_.get()),
|
||||
balance_ctrl_(ctrlComp.balance_ctrl_.get()) {
|
||||
_xMax = 0.05;
|
||||
_xMin = -_xMax;
|
||||
_yMax = 0.05;
|
||||
|
|
@ -25,20 +28,20 @@ StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateNa
|
|||
}
|
||||
|
||||
void StateBalanceTest::enter() {
|
||||
pcdInit_ = ctrl_comp_.estimator_.get().getPosition();
|
||||
pcd_ = pcdInit_;
|
||||
init_rotation_ = ctrl_comp_.estimator_.get().getRotation();
|
||||
pcd_init_ = estimator_.getPosition();
|
||||
pcd_ = pcd_init_;
|
||||
init_rotation_ = estimator_.getRotation();
|
||||
}
|
||||
|
||||
void StateBalanceTest::run() {
|
||||
pcd_(0) = pcdInit_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
|
||||
pcd_(1) = pcdInit_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
|
||||
pcd_(2) = pcdInit_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
|
||||
pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
|
||||
pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
|
||||
pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
|
||||
const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
|
||||
Rd_ = Mat3((KDL::Rotation::RPY(0, 0, yaw).Inverse() * init_rotation_).data);
|
||||
|
||||
pose_body_ = ctrl_comp_.estimator_.get().getPosition();
|
||||
vel_body_ = ctrl_comp_.estimator_.get().getVelocity();
|
||||
pose_body_ = estimator_.getPosition();
|
||||
vel_body_ = estimator_.getVelocity();
|
||||
|
||||
for (int i = 0; i < 12; i++) {
|
||||
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8);
|
||||
|
|
@ -63,25 +66,24 @@ FSMStateName StateBalanceTest::checkChange() {
|
|||
}
|
||||
|
||||
void StateBalanceTest::calcTorque() {
|
||||
const auto B2G_Rotation = Eigen::Matrix3d(ctrl_comp_.estimator_.get().getRotation().data);
|
||||
const auto B2G_Rotation = Eigen::Matrix3d(estimator_.getRotation().data);
|
||||
const RotMat G2B_Rotation = B2G_Rotation.transpose();
|
||||
|
||||
// expected body acceleration
|
||||
dd_pcd_ = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3((-vel_body_).data);
|
||||
dd_pcd_ = Kp_p_ * (pcd_ - pose_body_) + Kd_p_ * -vel_body_;
|
||||
|
||||
// expected body angular acceleration
|
||||
d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
|
||||
Kd_w_ * (Vec3(0, 0, 0) - Vec3((-ctrl_comp_.estimator_.get().getGlobalGyro()).data));
|
||||
d_wbd_ = -kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
|
||||
Kd_w_ * Vec3((-estimator_.getGlobalGyro()).data);
|
||||
|
||||
// calculate foot force
|
||||
const std::vector contact(4, 1);
|
||||
const Vec34 foot_force = G2B_Rotation * ctrl_comp_.balance_ctrl_.get().calF(dd_pcd_, -d_wbd_, B2G_Rotation,
|
||||
ctrl_comp_.estimator_.get().
|
||||
getFootPos2Body(), contact);
|
||||
const Vec34 foot_force = G2B_Rotation * balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
|
||||
estimator_.getFootPos2Body(), contact);
|
||||
|
||||
std::vector<KDL::JntArray> current_joints = ctrl_comp_.robot_model_.get().current_joint_pos_;
|
||||
std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(-foot_force.col(i), i);
|
||||
KDL::JntArray torque = robot_model_.getTorque(-foot_force.col(i), i);
|
||||
for (int j = 0; j < 3; j++) {
|
||||
ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
|
||||
ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ BalanceCtrl::BalanceCtrl() {
|
|||
mass_ = 15;
|
||||
alpha_ = 0.001;
|
||||
beta_ = 0.1;
|
||||
_g << 0, 0, -9.81;
|
||||
g_ << 0, 0, -9.81;
|
||||
friction_ratio_ = 0.4;
|
||||
friction_mat_ << 1, 0, friction_ratio_, -1, 0, friction_ratio_, 0, 1, friction_ratio_, 0, -1,
|
||||
friction_ratio_, 0, 0, 1;
|
||||
|
|
@ -21,7 +21,7 @@ BalanceCtrl::BalanceCtrl() {
|
|||
|
||||
void BalanceCtrl::init(const QuadrupedRobot &robot) {
|
||||
mass_ = robot.mass_;
|
||||
_pcb = Vec3(0.0, 0.0, 0.0);
|
||||
pcb_ = Vec3(0.0, 0.0, 0.0);
|
||||
Ib_ = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
|
||||
|
||||
Vec6 s;
|
||||
|
|
@ -37,10 +37,10 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
|
|||
F_prev_.setZero();
|
||||
}
|
||||
|
||||
Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
|
||||
const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact) {
|
||||
calMatrixA(feetPos2B, rotM);
|
||||
calVectorBd(ddPcd, dWbd, rotM);
|
||||
Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
|
||||
const Vec34 &feet_pos_2_body, const std::vector<int> &contact) {
|
||||
calMatrixA(feet_pos_2_body, rot_matrix);
|
||||
calVectorBd(ddPcd, dWbd, rot_matrix);
|
||||
calConstraints(contact);
|
||||
|
||||
G_ = A_.transpose() * S_ * A_ + alpha_ * W_ + beta_ * U_;
|
||||
|
|
@ -52,15 +52,15 @@ Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
|
|||
return vec12ToVec34(F_);
|
||||
}
|
||||
|
||||
void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &rotM) {
|
||||
void BalanceCtrl::calMatrixA(const Vec34 &feet_pos_2_body, const RotMat &rotM) {
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
A_.block(0, 3 * i, 3, 3) = I3;
|
||||
A_.block(3, 3 * i, 3, 3) = skew(Vec3(feetPos2B[i].data) - rotM * _pcb);
|
||||
A_.block(3, 3 * i, 3, 3) = skew(Vec3(feet_pos_2_body.col(i)) - rotM * pcb_);
|
||||
}
|
||||
}
|
||||
|
||||
void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM) {
|
||||
bd_.head(3) = mass_ * (ddPcd - _g);
|
||||
bd_.head(3) = mass_ * (ddPcd - g_);
|
||||
bd_.tail(3) = rotM * Ib_ * rotM.transpose() * dWbd;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,14 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/gait/FootEndCtrl.h"
|
||||
|
||||
FootEndCtrl::FootEndCtrl() {
|
||||
k_x_ = 0.005;
|
||||
k_y_ = 0.005;
|
||||
k_yaw_ = 0.005;
|
||||
}
|
||||
|
||||
Vec3 FootEndCtrl::calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase) {
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/gait/GaitGenerator.h"
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "unitree_guide_controller/gait/WaveGenerator.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
WaveGenerator::WaveGenerator() {
|
||||
phase_past_ << 0.5, 0.5, 0.5, 0.5;
|
||||
contact_past_.setZero();
|
||||
status_past_ = WaveStatus::SWING_ALL;
|
||||
}
|
||||
|
||||
void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
|
||||
period_ = period;
|
||||
st_ratio_ = st_ratio;
|
||||
bias_ = bias;
|
||||
|
||||
if (st_ratio_ >= 1 || st_ratio_ <= 0) {
|
||||
std::cerr << "[ERROR] The stancePhaseRatio of WaveGenerator should between (0, 1)"
|
||||
<< std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
for (int i(0); i < bias_.rows(); i++) {
|
||||
if (bias_(i) > 1 || bias_(i) < 0) {
|
||||
std::cerr << "[ERROR] The bias of WaveGenerator should between [0, 1]"
|
||||
<< std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
start_t_ = getSystemTime();
|
||||
}
|
||||
|
||||
auto WaveGenerator::calculate(Vec4 &phase_result, VecInt4 &contact_result, const WaveStatus status) -> void {
|
||||
calcWave(phase_, contact_, status);
|
||||
|
||||
if (status != status_past_) {
|
||||
if (switch_status_.sum() == 0) {
|
||||
switch_status_.setOnes();
|
||||
}
|
||||
calcWave(phase_past_, contact_past_, status_past_);
|
||||
|
||||
if (status == WaveStatus::STANCE_ALL && status_past_ == WaveStatus::SWING_ALL) {
|
||||
contact_past_.setOnes();
|
||||
} else if (status == WaveStatus::SWING_ALL && status_past_ == WaveStatus::STANCE_ALL) {
|
||||
contact_past_.setZero();
|
||||
}
|
||||
}
|
||||
|
||||
if (switch_status_.sum() != 0) {
|
||||
for (int i(0); i < 4; ++i) {
|
||||
if (contact_(i) == contact_past_(i)) {
|
||||
switch_status_(i) = 0;
|
||||
} else {
|
||||
contact_(i) = contact_past_(i);
|
||||
phase_(i) = phase_past_(i);
|
||||
}
|
||||
}
|
||||
if (switch_status_.sum() == 0) {
|
||||
status_past_ = status;
|
||||
}
|
||||
}
|
||||
|
||||
phase_result = phase_;
|
||||
contact_result = contact_;
|
||||
}
|
||||
|
||||
void WaveGenerator::calcWave(Vec4 &phase, VecInt4 &contact, const WaveStatus status) {
|
||||
switch (status) {
|
||||
case WaveStatus::WAVE_ALL: {
|
||||
const double past_t = static_cast<double>(getSystemTime() - start_t_) * 1e-6;
|
||||
for (int i(0); i < 4; ++i) {
|
||||
normal_t_(i) =
|
||||
fmod(past_t + period_ - period_ * bias_(i), period_) / period_;
|
||||
if (normal_t_(i) < st_ratio_) {
|
||||
contact(i) = 1;
|
||||
phase(i) = normal_t_(i) / st_ratio_;
|
||||
} else {
|
||||
contact(i) = 0;
|
||||
phase(i) = (normal_t_(i) - st_ratio_) / (1 - st_ratio_);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case WaveStatus::SWING_ALL: {
|
||||
contact.setZero();
|
||||
phase << 0.5, 0.5, 0.5, 0.5;
|
||||
break;
|
||||
}
|
||||
case WaveStatus::STANCE_ALL: {
|
||||
contact.setOnes();
|
||||
phase << 0.5, 0.5, 0.5, 0.5;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue