50 -cos(M_PI/6), 0, cos(M_PI/6), cos(M_PI/6), 0, -cos(M_PI/6),
64 const double rotor_force_constant = 0.00000854858;
65 const double rotor_moment_constant = 0.016;
68 const double arm_length = 0.215;
72 K(0, 0) = arm_length * rotor_force_constant;
73 K(1, 1) = arm_length * rotor_force_constant;
74 K(2, 2) = rotor_force_constant * rotor_moment_constant;
75 K(3, 3) = rotor_force_constant;
83 initialized_params_ =
true;
87 assert(rotor_velocities);
88 assert(initialized_params_);
90 rotor_velocities->resize(amount_rotors_);
92 Eigen::Vector3d angular_acceleration;
95 Eigen::Vector4d angular_acceleration_thrust;
96 angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
97 angular_acceleration_thrust(3) = control_attitude_thrust_reference_(3);
100 *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Ones(rotor_velocities->rows()));
101 *rotor_velocities = rotor_velocities->cwiseSqrt();
106 assert(angular_acceleration);
108 Eigen::Matrix3d R = attitude_.toRotationMatrix();
111 Eigen::Matrix3d R_des;
112 double yaw = atan2(R(1, 0), R(0, 0));
113 R_des = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
114 * Eigen::AngleAxisd(control_attitude_thrust_reference_(0), Eigen::Vector3d::UnitX())
115 * Eigen::AngleAxisd(control_attitude_thrust_reference_(1), Eigen::Vector3d::UnitY());
117 Eigen::Vector3d b3_des = R.transpose() * R_des.col(2);
118 Eigen::Vector3d angle_error = b3_des.cross(Eigen::Vector3d::UnitZ());
120 Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
121 angular_rate_des[2] = control_attitude_thrust_reference_(2);
123 Eigen::Vector3d angular_rate_error = angular_rate_ - R_des.transpose() * R * angular_rate_des;
125 *angular_acceleration = -1 * angle_error.cwiseProduct(
gain_attitude_)
127 + angular_rate_.cross(angular_rate_);
virtual void CalculateRotorVelocities(Eigen::VectorXd *rotor_velocities) const
Eigen::MatrixX4d angular_acc_to_rotor_velocities_
Eigen::Vector3d gain_attitude_
Eigen::Matrix4Xd allocation_matrix_
Eigen::Vector3d gain_angular_rate_
virtual ~AttitudeControllerSamy()
void ComputeDesiredAngularAcc(Eigen::Vector3d *angular_acceleration) const
virtual std::shared_ptr< ControllerBase > Clone()
virtual void InitializeParams()
ROTORS_CONTROL_REGISTER_CONTROLLER(AttitudeControllerSamy)
Eigen::Matrix3d inertia_matrix_