40 lines
1.2 KiB
C++
40 lines
1.2 KiB
C++
#ifndef ARM_CONTROLLER_H
|
|
#define ARM_CONTROLLER_H
|
|
|
|
#include "ControllerBase.hpp"
|
|
|
|
namespace control {
|
|
|
|
class UpperBodyController : public ControllerBase {
|
|
public:
|
|
using Ptr = std::shared_ptr<UpperBodyController>;
|
|
using ConstPtr = std::shared_ptr<const UpperBodyController>;
|
|
|
|
UpperBodyController(model::UpperBodyModel::Ptr model);
|
|
~UpperBodyController() 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 setArmTargetPosition(bool leftArm, const Eigen::Vector3d& targetPos);
|
|
|
|
private:
|
|
model::UpperBodyModel::Ptr model_;
|
|
std::unique_ptr<ocs2::Ocs2Solver> solver_;
|
|
ocs2::TargetTrajectories targetTrajectories_;
|
|
|
|
// 控制器参数
|
|
double positionTolerance_ = 0.01; // 位置容差 (m)
|
|
double velocityTolerance_ = 0.05; // 速度容差 (m/s)
|
|
};
|
|
|
|
} // namespace control
|
|
|
|
#endif // UPPER_BODY_CONTROLLER_H
|