| 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, |
| 28 | const double des_yaw_dot, const Eigen::Vector3d& kx, |
| 29 | const Eigen::Vector3d& kv) { |
| 30 | // ROS_INFO("Error %lf %lf %lf", (des_pos - pos_).norm(), |
| 31 | // (des_vel - vel_).norm(), (des_acc - acc_).norm()); |
| 32 | |
| 33 | Eigen::Vector3d totalError = (des_pos - pos_) + (des_vel - vel_) + (des_acc - acc_); |
| 34 | |
| 35 | Eigen::Vector3d ka(fabs(totalError[0]) > 3 ? 0 : (fabs(totalError[0]) * 0.2), |
| 36 | fabs(totalError[1]) > 3 ? 0 : (fabs(totalError[1]) * 0.2), |
| 37 | fabs(totalError[2]) > 3 ? 0 : (fabs(totalError[2]) * 0.2)); |
| 38 | |
| 39 | force_.noalias() = kx.asDiagonal() * (des_pos - pos_) + kv.asDiagonal() * (des_vel - vel_) + |
| 40 | mass_ * /*(Eigen::Vector3d(1, 1, 1) - ka).asDiagonal() **/ (des_acc) + |
| 41 | mass_ * ka.asDiagonal() * (des_acc - acc_) + mass_ * g_ * Eigen::Vector3d(0, 0, 1); |
| 42 | |
| 43 | // Limit control angle to 45 degree |
| 44 | double theta = M_PI / 2; |
| 45 | double c = cos(theta); |
| 46 | Eigen::Vector3d f; |
| 47 | f.noalias() = kx.asDiagonal() * (des_pos - pos_) + kv.asDiagonal() * (des_vel - vel_) + // |
| 48 | mass_ * des_acc + // |
| 49 | mass_ * ka.asDiagonal() * (des_acc - acc_); |
| 50 | if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c) { |
| 51 | double nf = f.norm(); |
| 52 | double A = c * c * nf * nf - f(2) * f(2); |
| 53 | double B = 2 * (c * c - 1) * f(2) * mass_ * g_; |
| 54 | double C = (c * c - 1) * mass_ * mass_ * g_ * g_; |
| 55 | double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A); |
| 56 | force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1); |
| 57 | } |
| 58 | // Limit control angle to 45 degree |
| 59 | |
| 60 | Eigen::Vector3d b1c, b2c, b3c; |
| 61 | Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0); |
| 62 | |
| 63 | if (force_.norm() > 1e-6) |
| 64 | b3c.noalias() = force_.normalized(); |
| 65 | else |
| 66 | b3c.noalias() = Eigen::Vector3d(0, 0, 1); |
| 67 | |
| 68 | b2c.noalias() = b3c.cross(b1d).normalized(); |
| 69 | b1c.noalias() = b2c.cross(b3c).normalized(); |
| 70 | |
| 71 | Eigen::Matrix3d R; |
| 72 | R << b1c, b2c, b3c; |
| 73 | |
| 74 | orientation_ = Eigen::Quaterniond(R); |
| 75 | } |
| 76 | |
| 77 | const Eigen::Vector3d& SO3Control::getComputedForce(void) { |
| 78 | return force_; |
no test coverage detected