43 -cos(M_PI/6), 0, cos(M_PI/6), cos(M_PI/6), 0, -cos(M_PI/6),
54 const double rotor_force_constant = 0.00000854858;
55 const double rotor_moment_constant = 0.016;
58 const double arm_length = 0.215;
62 K(0, 0) = arm_length * rotor_force_constant;
63 K(1, 1) = arm_length * rotor_force_constant;
64 K(2, 2) = rotor_force_constant * rotor_moment_constant;
65 K(3, 3) = rotor_force_constant;
73 initialized_params_ =
true;
77 assert(rotor_velocities);
78 assert(initialized_params_);
80 rotor_velocities->resize(amount_rotors_);
82 Eigen::Vector3d angular_acceleration;
85 Eigen::Vector4d angular_acceleration_thrust;
86 angular_acceleration_thrust.block<3,1>(0,0) = angular_acceleration;
87 angular_acceleration_thrust(3) = control_rate_thrust_reference_(3);
90 *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
91 *rotor_velocities = rotor_velocities->cwiseSqrt();
97 assert(angular_acceleration);
99 Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
100 angular_rate_des[0] = control_rate_thrust_reference_(0);
101 angular_rate_des[1] = control_rate_thrust_reference_(1);
102 angular_rate_des[2] = control_rate_thrust_reference_(2);
104 Eigen::Vector3d angular_rate_error = angular_rate_ - angular_rate_des;
107 + angular_rate_.cross(angular_rate_);
Eigen::Matrix4Xd allocation_matrix_
void ComputeDesiredAngularAcc(Eigen::Vector3d *angular_acceleration) const
virtual void InitializeParams()
virtual std::shared_ptr< ControllerBase > Clone()
virtual ~RateController()
Eigen::MatrixX4d angular_acc_to_rotor_velocities_
virtual void CalculateRotorVelocities(Eigen::VectorXd *rotor_velocities) const
Eigen::Matrix3d inertia_matrix_
Eigen::Vector3d gain_angular_rate_
ROTORS_CONTROL_REGISTER_CONTROLLER(RateController)