| 20 | } |
| 21 | |
| 22 | void SO3Control::setVelocity(const Eigen::Vector3d& velocity) { |
| 23 | vel_ = velocity; |
| 24 | } |
| 25 | |
| 26 | void SO3Control::calculateControl(const Eigen::Vector3d& des_pos, const Eigen::Vector3d& des_vel, |
| 27 | const Eigen::Vector3d& des_acc, const double des_yaw, |