49 lines
1.5 KiB
C++
49 lines
1.5 KiB
C++
#ifndef LOWER_BODY_CONTROLLER_H
|
|
#define LOWER_BODY_CONTROLLER_H
|
|
|
|
#include "robot_control/control/ControllerBase.h"
|
|
#include "robot_control/model/LowerBodyModel.h"
|
|
#include <ocs2_oc/oc_solver/Ocs2Solver.h>
|
|
#include <ocs2_core/reference/TargetTrajectories.h>
|
|
|
|
namespace robot_control {
|
|
namespace control {
|
|
|
|
class LowerBodyController : public ControllerBase {
|
|
public:
|
|
using Ptr = std::shared_ptr<LowerBodyController>;
|
|
using ConstPtr = std::shared_ptr<const LowerBodyController>;
|
|
|
|
LowerBodyController(model::LowerBodyModel::Ptr model);
|
|
~LowerBodyController() override = default;
|
|
|
|
// 从基类继承的方法
|
|
ocs2::vector_t computeControl(const ocs2::scalar_t t, const ocs2::vector_t& x) override;
|
|
void reset() override;
|
|
bool isGoalReached(const ocs2::vector_t& x) const override;
|
|
|
|
// 设置目标轨迹
|
|
void setTargetTrajectories(const ocs2::TargetTrajectories& targetTrajectories);
|
|
|
|
// 设置轮子速度
|
|
void setWheelVelocities(const Eigen::Vector4d& wheelVels);
|
|
|
|
// 设置单腿目标位置
|
|
void setLegTargetPosition(int legIndex, const Eigen::Vector3d& targetPos);
|
|
|
|
private:
|
|
model::LowerBodyModel::Ptr model_;
|
|
std::unique_ptr<ocs2::Ocs2Solver> solver_;
|
|
ocs2::TargetTrajectories targetTrajectories_;
|
|
Eigen::Vector4d wheelVelocities_;
|
|
|
|
// 控制器参数
|
|
double positionTolerance_ = 0.01; // 位置容差 (m)
|
|
double velocityTolerance_ = 0.05; // 速度容差 (m/s)
|
|
};
|
|
|
|
} // namespace control
|
|
} // namespace robot_control
|
|
|
|
#endif // LOWER_BODY_CONTROLLER_H
|