37 QuadrotorDynamicsSolver::QuadrotorDynamicsSolver()
44 J_.diagonal() = Eigen::Vector3d(0.0023, 0.0023, 0.004);
47 J_inv_.diagonal() = Eigen::Vector3d(1. / 0.0023, 1. / 0.0023, 1. / 0.0040);
50 void QuadrotorDynamicsSolver::AssignScene(
ScenePtr scene_in)
52 const int num_positions_in = scene_in->GetKinematicTree().GetNumControlledJoints();
54 if ((num_positions_in) != 6 || scene_in->GetKinematicTree().GetControlledBaseType() != BaseType::FLOATING)
58 Eigen::VectorXd QuadrotorDynamicsSolver::f(
const StateVector& x,
const ControlVector& u)
60 Eigen::Quaterniond quaternion;
63 if (x.segment<4>(3).isApprox(Eigen::Vector4d::Zero()))
64 quaternion = Eigen::Quaterniond(1, 0, 0, 0);
66 quaternion = Eigen::Quaterniond(x.segment<4>(3)).normalized();
68 const Eigen::Vector3d v = x.segment<3>(6);
69 const Eigen::Vector3d
omega = x.tail<3>();
75 const Eigen::Vector3d F(0, 0, F_1 + F_2 + F_3 + F_4);
81 const Eigen::Vector3d
tau(
L_ * (F_2 - F_4),
L_ * (F_3 - F_1), (M_1 - M_2 + M_3 - M_4));
83 StateVector
x_dot(13);
85 x_dot.segment<4>(3) = 0.5 * (quaternion * Eigen::Quaterniond(0,
omega(0),
omega(1),
omega(2))).coeffs();
86 x_dot.segment<3>(7) = Eigen::Vector3d(0, 0, -
g_) + (1. /
mass_) * (quaternion * F);
87 x_dot.segment<3>(10) = J_inv_ * (
tau - omega.cross(J_ * omega));
107 Eigen::Matrix<double, 6, 1> xyz_rpy;
108 xyz_rpy.head<3>() = x_in.head<3>();
109 xyz_rpy.tail<3>() = Eigen::Quaterniond(x_in.segment<4>(3)).toRotationMatrix().eulerAngles(0, 1, 2);
#define REGISTER_DYNAMICS_SOLVER_TYPE(TYPE, DERIV)
std::shared_ptr< Scene > ScenePtr
Quadrotor dynamics with quaternion representation Based on D. Mellinger, N. Michael, and V. Kumar, "Trajectory generation and control for precise aggressive maneuvers with quadrotors", Proceedings of the 12th International Symposium on Experimental Robotics (ISER 2010), 2010. Cf. https://journals.sagepub.com/doi/abs/10.1177/0278364911434236.
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst