diff --git a/src/cddp_core/logddp_solver.cpp b/src/cddp_core/logddp_solver.cpp index a5d86d7..1809d2a 100644 --- a/src/cddp_core/logddp_solver.cpp +++ b/src/cddp_core/logddp_solver.cpp @@ -114,7 +114,7 @@ void LogDDPSolver::initialize(CDDP &context) { // Warm starts may reuse gains with a user-modified state trajectory. // Re-roll the nominal state sequence so the linearization point stays - // dynamically consistent after defect tracking removal. + // dynamically consistent. rollOutNominalTrajectory(context); // Evaluate current trajectory and reset filter @@ -368,11 +368,6 @@ bool LogDDPSolver::backwardPass(CDDP &context) { const double timestep = context.getTimestep(); const auto &constraint_set = context.getConstraintSet(); - // Pre-compute dynamics jacobians and hessians for all time steps - // Note: LogDDP needs continuous-time Jacobians for the backward pass - // because it handles defect terms (d = F[t] - X[t+1]) which require - // the A = dt*Fx + I, B = dt*Fu formulation. We compute these inline - // since the base precomputeDynamicsDerivatives stores discrete-time versions. const int MIN_HORIZON_FOR_PARALLEL = 20; const bool use_parallel = options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL; diff --git a/src/dynamics_model/bicycle.cpp b/src/dynamics_model/bicycle.cpp index 0d08869..9a38b2b 100644 --- a/src/dynamics_model/bicycle.cpp +++ b/src/dynamics_model/bicycle.cpp @@ -32,15 +32,12 @@ Eigen::VectorXd Bicycle::getContinuousDynamics(const Eigen::VectorXd &state, Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM); - // Extract state variables const double theta = state(STATE_THETA); // heading angle const double v = state(STATE_V); // velocity - // Extract control variables const double a = control(CONTROL_ACC); // acceleration const double delta = control(CONTROL_DELTA); // steering angle - // Kinematic bicycle model equations state_dot(STATE_X) = v * std::cos(theta); // dx/dt state_dot(STATE_Y) = v * std::sin(theta); // dy/dt state_dot(STATE_THETA) = (v / wheelbase_) * std::tan(delta); // dtheta/dt @@ -56,16 +53,12 @@ Bicycle::getContinuousDynamicsAutodiff(const VectorXdual2nd &state, VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); - - // Extract state variables (now dual2nd types) const autodiff::dual2nd theta = state(STATE_THETA); // heading angle const autodiff::dual2nd v = state(STATE_V); // velocity - // Extract control variables (now dual2nd types) const autodiff::dual2nd a = control(CONTROL_ACC); // acceleration const autodiff::dual2nd delta = control(CONTROL_DELTA); // steering angle - // Use ADL for math functions state_dot(STATE_X) = v * cos(theta); state_dot(STATE_Y) = v * sin(theta); state_dot(STATE_THETA) = (v / wheelbase_) * tan(delta); @@ -80,25 +73,17 @@ Eigen::MatrixXd Bicycle::getStateJacobian(const Eigen::VectorXd &state, Eigen::MatrixXd A = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - // Extract state variables const double theta = state(STATE_THETA); // heading angle const double v = state(STATE_V); // velocity - // Extract control variables const double delta = control(CONTROL_DELTA); // steering angle - // Compute partial derivatives with respect to state variables - // df1/dtheta = d(dx/dt)/dtheta A(STATE_X, STATE_THETA) = -v * std::sin(theta); - // df1/dv = d(dx/dt)/dv A(STATE_X, STATE_V) = std::cos(theta); - // df2/dtheta = d(dy/dt)/dtheta A(STATE_Y, STATE_THETA) = v * std::cos(theta); - // df2/dv = d(dy/dt)/dv A(STATE_Y, STATE_V) = std::sin(theta); - // df3/dv = d(dtheta/dt)/dv A(STATE_THETA, STATE_V) = std::tan(delta) / wheelbase_; return A; @@ -110,17 +95,12 @@ Eigen::MatrixXd Bicycle::getControlJacobian(const Eigen::VectorXd &state, Eigen::MatrixXd B = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM); - // Extract state variables const double v = state(STATE_V); // velocity - // Extract control variables const double delta = control(CONTROL_DELTA); // steering angle - // Compute partial derivatives with respect to control variables - // df4/da = d(dv/dt)/da B(STATE_V, CONTROL_ACC) = 1.0; - // df3/ddelta = d(dtheta/dt)/ddelta B(STATE_THETA, CONTROL_DELTA) = v / (wheelbase_ * std::pow(std::cos(delta), 2)); @@ -133,22 +113,16 @@ Bicycle::getStateHessian(const Eigen::VectorXd &state, auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); - // Extract state variables const double theta = state(STATE_THETA); // heading angle const double v = state(STATE_V); // velocity - // Second derivatives with respect to states - // d²(dx/dt)/dtheta² hessians[STATE_X](STATE_THETA, STATE_THETA) = -v * std::cos(theta); - // d²(dx/dt)/(dtheta*dv) hessians[STATE_X](STATE_THETA, STATE_V) = -std::sin(theta); hessians[STATE_X](STATE_V, STATE_THETA) = -std::sin(theta); - // d²(dy/dt)/dtheta² hessians[STATE_Y](STATE_THETA, STATE_THETA) = -v * std::sin(theta); - // d²(dy/dt)/(dtheta*dv) hessians[STATE_Y](STATE_THETA, STATE_V) = std::cos(theta); hessians[STATE_Y](STATE_V, STATE_THETA) = std::cos(theta); @@ -161,18 +135,14 @@ Bicycle::getControlHessian(const Eigen::VectorXd &state, auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); - // Extract state variables const double v = state(STATE_V); // velocity - // Extract control variables const double delta = control(CONTROL_DELTA); // steering angle - // Second derivatives with respect to controls - // d²(dtheta/dt)/ddelta² hessians[STATE_THETA](CONTROL_DELTA, CONTROL_DELTA) = 2.0 * v * std::sin(delta) / (wheelbase_ * std::pow(std::cos(delta), 3)); return hessians; } -} // namespace cddp \ No newline at end of file +} // namespace cddp diff --git a/src/dynamics_model/car.cpp b/src/dynamics_model/car.cpp index 37b08a5..11d6dc4 100644 --- a/src/dynamics_model/car.cpp +++ b/src/dynamics_model/car.cpp @@ -13,22 +13,17 @@ Car::Car(double timestep, double wheelbase, std::string integration_type) Eigen::VectorXd Car::getDiscreteDynamics( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Extract states const double x = state(STATE_X); // x position const double y = state(STATE_Y); // y position const double theta = state(STATE_THETA); // car angle const double v = state(STATE_V); // velocity - // Extract controls const double delta = control(CONTROL_DELTA); // steering angle const double a = control(CONTROL_A); // acceleration - // Constants const double d = wheelbase_; // distance between back and front axles const double h = timestep_; // timestep - // Compute unit vector in car direction const double cos_theta = std::cos(theta); const double sin_theta = std::sin(theta); Eigen::Vector2d z(cos_theta, sin_theta); @@ -45,40 +40,30 @@ Eigen::VectorXd Car::getDiscreteDynamics( // dtheta = asin(sin(w)*f/d) const double dtheta = std::asin(std::sin(delta) * f / d); - // Compute state change Eigen::VectorXd dy = Eigen::VectorXd::Zero(STATE_DIM); dy(STATE_X) = b * cos_theta; dy(STATE_Y) = b * sin_theta; dy(STATE_THETA) = dtheta; dy(STATE_V) = h * a; - // Return next state return state + dy; } Eigen::MatrixXd Car::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Jacobian Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - // Calculate Jacobian using autodiff for (int i = 0; i < STATE_DIM; ++i) { - // Create a lambda that returns the i-th component of the dynamics auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time); return dynamics(i); }; - // Calculate gradient of the i-th output with respect to state J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(state_dual), at(state_dual)); } - // Convert discrete Jacobian to continuous time Jacobian J.diagonal().array() -= 1.0; J /= timestep_; @@ -87,27 +72,19 @@ Eigen::MatrixXd Car::getStateJacobian( Eigen::MatrixXd Car::getControlJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Jacobian Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM); - // Calculate Jacobian using autodiff for (int i = 0; i < STATE_DIM; ++i) { - // Create a lambda that returns the i-th component of the dynamics auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time); return dynamics(i); }; - // Calculate gradient of the i-th output with respect to control J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(control_dual), at(control_dual)); } - // Convert discrete Jacobian to continuous time Jacobian J /= timestep_; return J; @@ -115,26 +92,17 @@ Eigen::MatrixXd Car::getControlJacobian( std::vector Car::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Hessians auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); - // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { - // Create a lambda that returns the i-th component of the dynamics auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time); return dynamics(i); }; - // Calculate Hessian of the i-th output with respect to state hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(state_dual), at(state_dual)); - - // Convert discrete Hessian to continuous time Hessian hessians[i] /= timestep_; } @@ -143,50 +111,36 @@ std::vector Car::getStateHessian( std::vector Car::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Hessians auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); - // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { - // Create a lambda that returns the i-th component of the dynamics auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time); return dynamics(i); }; - // Calculate Hessian of the i-th output with respect to control hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(control_dual), at(control_dual)); - - // Convert discrete Hessian to continuous time Hessian hessians[i] /= timestep_; } return hessians; } -// Helper: Autodiff version of discrete dynamics cddp::VectorXdual2nd Car::getDiscreteDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control, double time) const { - // Extract states (dual2nd) const autodiff::dual2nd theta = state(STATE_THETA); const autodiff::dual2nd v = state(STATE_V); - // Extract controls (dual2nd) const autodiff::dual2nd delta = control(CONTROL_DELTA); const autodiff::dual2nd a = control(CONTROL_A); - // Constants const double d_double = wheelbase_; const double h = timestep_; const autodiff::dual2nd d = wheelbase_; - // Use ADL for math functions const autodiff::dual2nd cos_theta = cos(theta); const autodiff::dual2nd sin_theta = sin(theta); @@ -228,4 +182,4 @@ cddp::VectorXdual2nd Car::getContinuousDynamicsAutodiff( return (next_state - state) / timestep_; } -} // namespace cddp \ No newline at end of file +} // namespace cddp diff --git a/src/dynamics_model/forklift.cpp b/src/dynamics_model/forklift.cpp index 9b32bf1..f694f9c 100644 --- a/src/dynamics_model/forklift.cpp +++ b/src/dynamics_model/forklift.cpp @@ -16,19 +16,15 @@ Forklift::Forklift(double timestep, double wheelbase, std::string integration_ty Eigen::VectorXd Forklift::getDiscreteDynamics( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Extract states const double x = state(STATE_X); // x position const double y = state(STATE_Y); // y position const double theta = state(STATE_THETA); // forklift angle const double v = state(STATE_V); // velocity const double delta = state(STATE_DELTA); // steering angle - // Extract controls const double a = control(CONTROL_A); // acceleration const double ddelta = control(CONTROL_DDELTA); // steering rate - // Constants const double L = wheelbase_; // wheelbase const double h = timestep_; // timestep @@ -41,7 +37,6 @@ Eigen::VectorXd Forklift::getDiscreteDynamics( const double sin_theta = std::sin(theta); const double tan_delta = std::tan(effective_delta); - // State derivatives Eigen::VectorXd dy = Eigen::VectorXd::Zero(STATE_DIM); dy(STATE_X) = h * v * cos_theta; dy(STATE_Y) = h * v * sin_theta; @@ -49,21 +44,15 @@ Eigen::VectorXd Forklift::getDiscreteDynamics( dy(STATE_V) = h * a; dy(STATE_DELTA) = h * ddelta; - // Return next state return state + dy; } Eigen::MatrixXd Forklift::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Jacobian Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - // Calculate Jacobian using autodiff for (int i = 0; i < STATE_DIM; ++i) { auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time); @@ -72,7 +61,6 @@ Eigen::MatrixXd Forklift::getStateJacobian( J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(state_dual), at(state_dual)); } - // Convert discrete Jacobian to continuous time Jacobian J.diagonal().array() -= 1.0; J /= timestep_; @@ -81,15 +69,10 @@ Eigen::MatrixXd Forklift::getStateJacobian( Eigen::MatrixXd Forklift::getControlJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Jacobian Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM); - // Calculate Jacobian using autodiff for (int i = 0; i < STATE_DIM; ++i) { auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time); @@ -105,15 +88,10 @@ Eigen::MatrixXd Forklift::getControlJacobian( std::vector Forklift::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Hessians auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); - // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time); @@ -129,15 +107,10 @@ std::vector Forklift::getStateHessian( std::vector Forklift::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - - // Convert inputs to autodiff types VectorXdual2nd state_dual = state.cast(); VectorXdual2nd control_dual = control.cast(); - - // Initialize Hessians auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); - // Calculate Hessians using autodiff for (int i = 0; i < STATE_DIM; ++i) { auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd { VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time); @@ -151,22 +124,18 @@ std::vector Forklift::getControlHessian( return hessians; } -// Helper: Autodiff version of discrete dynamics cddp::VectorXdual2nd Forklift::getDiscreteDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control, double time) const { - // Extract states (dual2nd) const autodiff::dual2nd x = state(STATE_X); const autodiff::dual2nd y = state(STATE_Y); const autodiff::dual2nd theta = state(STATE_THETA); const autodiff::dual2nd v = state(STATE_V); const autodiff::dual2nd delta_raw = state(STATE_DELTA); - // Extract controls (dual2nd) const autodiff::dual2nd a = control(CONTROL_A); const autodiff::dual2nd ddelta = control(CONTROL_DDELTA); - // Constants const double L = wheelbase_; const double h = timestep_; @@ -174,12 +143,10 @@ cddp::VectorXdual2nd Forklift::getDiscreteDynamicsAutodiff( const double steer_sign = rear_steer_ ? -1.0 : 1.0; const autodiff::dual2nd effective_delta = steer_sign * delta_raw; - // Use ADL for math functions const autodiff::dual2nd cos_theta = cos(theta); const autodiff::dual2nd sin_theta = sin(theta); const autodiff::dual2nd tan_delta = tan(effective_delta); - // Compute state changes using standard bicycle model VectorXdual2nd dy = VectorXdual2nd::Zero(STATE_DIM); dy(STATE_X) = h * v * cos_theta; dy(STATE_Y) = h * v * sin_theta; @@ -190,11 +157,10 @@ cddp::VectorXdual2nd Forklift::getDiscreteDynamicsAutodiff( return state + dy; } -// Required continuous dynamics using autodiff discrete dynamics cddp::VectorXdual2nd Forklift::getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control, double time) const { VectorXdual2nd next_state = this->getDiscreteDynamicsAutodiff(state, control, time); return (next_state - state) / timestep_; } -} // namespace cddp \ No newline at end of file +} // namespace cddp diff --git a/src/dynamics_model/pendulum.cpp b/src/dynamics_model/pendulum.cpp index cd0c819..94b20f7 100644 --- a/src/dynamics_model/pendulum.cpp +++ b/src/dynamics_model/pendulum.cpp @@ -31,17 +31,11 @@ Eigen::VectorXd Pendulum::getContinuousDynamics( Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM); - // Extract state variables const double theta = state(STATE_THETA); const double theta_dot = state(STATE_THETA_DOT); - - // Extract control variable const double torque = control(CONTROL_TORQUE); - - // Precompute constants const double inertia = mass_ * length_ * length_; - // Pendulum dynamics equations state_dot(STATE_THETA) = theta_dot; state_dot(STATE_THETA_DOT) = (torque - damping_ * theta_dot + mass_ * gravity_ * length_ * std::sin(theta)) / inertia; @@ -53,16 +47,9 @@ Eigen::MatrixXd Pendulum::getStateJacobian( Eigen::MatrixXd A = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - // Extract state variables const double theta = state(STATE_THETA); - - // Compute partial derivatives with respect to state variables A(STATE_THETA, STATE_THETA_DOT) = 1.0; - - // d(dtheta_dot/dt)/dtheta A(STATE_THETA_DOT, STATE_THETA) = (gravity_ / length_) * std::cos(theta); - - // d(dtheta_dot/dt)/dtheta_dot A(STATE_THETA_DOT, STATE_THETA_DOT) = -damping_ / (mass_ * length_ * length_); return A; @@ -73,8 +60,6 @@ Eigen::MatrixXd Pendulum::getControlJacobian( Eigen::MatrixXd B = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM); - // Compute partial derivatives with respect to control variable - // d(dtheta_dot/dt)/dtorque B(STATE_THETA_DOT, CONTROL_TORQUE) = 1.0 / (mass_ * length_ * length_); return B; @@ -83,14 +68,8 @@ Eigen::MatrixXd Pendulum::getControlJacobian( std::vector Pendulum::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - // Initialize a vector of matrices (one matrix per state dimension) auto hessian = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); - - // Extract state variables const double theta = state(STATE_THETA); - - // For the pendulum, only the second derivative of theta_dot with respect to theta is non-zero - // d^2(dtheta_dot/dt)/dtheta^2 = -g/l * sin(theta) const double inertia = mass_ * length_ * length_; hessian[STATE_THETA_DOT](STATE_THETA, STATE_THETA) = -(gravity_ / length_) * std::sin(theta); @@ -100,12 +79,7 @@ std::vector Pendulum::getStateHessian( std::vector Pendulum::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - // Initialize a vector of matrices (one matrix per state dimension) auto hessian = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); - - // For the pendulum, all second derivatives with respect to control are zero - // No need to set any values as the matrices are already initialized to zero - return hessian; } @@ -125,4 +99,4 @@ VectorXdual2nd Pendulum::getContinuousDynamicsAutodiff( return state_dot; } -} // namespace cddp \ No newline at end of file +} // namespace cddp diff --git a/src/dynamics_model/quadrotor.cpp b/src/dynamics_model/quadrotor.cpp index c128f42..a07f249 100644 --- a/src/dynamics_model/quadrotor.cpp +++ b/src/dynamics_model/quadrotor.cpp @@ -35,12 +35,8 @@ Eigen::VectorXd Quadrotor::getContinuousDynamics( Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM); - // --- Position Derivative --- - // The derivative of the position is the linear velocity. state_dot.segment<3>(STATE_X) = state.segment<3>(STATE_VX); - // --- Quaternion Derivative --- - // Extract the quaternion (assumed to be [qw, qx, qy, qz]) double qw = state(STATE_QW); double qx = state(STATE_QX); double qy = state(STATE_QY); @@ -74,20 +70,16 @@ Eigen::VectorXd Quadrotor::getContinuousDynamics( state_dot(STATE_QY) = 0.5 * (qw * omega_y - qx * omega_z + qz * omega_x); state_dot(STATE_QZ) = 0.5 * (qw * omega_z + qx * omega_y - qy * omega_x); - // --- Velocity Derivative --- - // Extract control variables (motor forces) const double f1 = control(CONTROL_F1); const double f2 = control(CONTROL_F2); const double f3 = control(CONTROL_F3); const double f4 = control(CONTROL_F4); - // Compute total thrust and moments const double thrust = f1 + f2 + f3 + f4; const double tau_x = arm_length_ * (f1 - f3); const double tau_y = arm_length_ * (f2 - f4); const double tau_z = 0.1 * (f1 - f2 + f3 - f4); - // Compute rotation matrix from the normalized quaternion Eigen::Matrix3d R = getRotationMatrix(qw, qx, qy, qz); // Thrust is applied along the body z-axis. @@ -95,7 +87,6 @@ Eigen::VectorXd Quadrotor::getContinuousDynamics( Eigen::Vector3d acceleration = (1.0/mass_) * (R * F_thrust) - Eigen::Vector3d(0, 0, gravity_); state_dot.segment<3>(STATE_VX) = acceleration; - // --- Angular Velocity Derivative --- Eigen::Vector3d omega(omega_x, omega_y, omega_z); Eigen::Vector3d tau(tau_x, tau_y, tau_z); Eigen::Vector3d angular_acc = inertia_.inverse() * (tau - omega.cross(inertia_ * omega)); @@ -105,7 +96,6 @@ Eigen::VectorXd Quadrotor::getContinuousDynamics( } Eigen::Matrix3d Quadrotor::getRotationMatrix(double qw, double qx, double qy, double qz) const { - // Compute the rotation matrix from a unit quaternion. // The quaternion is assumed to be normalized and in the form [qw, qx, qy, qz] Eigen::Matrix3d R; R(0, 0) = 1 - 2 * (qy * qy + qz * qz); @@ -126,7 +116,6 @@ Eigen::Matrix3d Quadrotor::getRotationMatrix(double qw, double qx, double qy, do Eigen::MatrixXd Quadrotor::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - // Use autodiff to compute state Jacobian VectorXdual2nd x = state; VectorXdual2nd u = control; @@ -140,7 +129,6 @@ Eigen::MatrixXd Quadrotor::getStateJacobian( Eigen::MatrixXd Quadrotor::getControlJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - // Use autodiff to compute control Jacobian VectorXdual2nd x = state; VectorXdual2nd u = control; @@ -154,7 +142,6 @@ Eigen::MatrixXd Quadrotor::getControlJacobian( Eigen::Matrix Quadrotor::getRotationMatrixAutodiff( const autodiff::dual2nd& qw, const autodiff::dual2nd& qx, const autodiff::dual2nd& qy, const autodiff::dual2nd& qz) const { - // Compute the rotation matrix from a unit quaternion. // The quaternion is assumed to be normalized and in the form [qw, qx, qy, qz] Eigen::Matrix R; R(0, 0) = 1 - 2 * (qy * qy + qz * qz); @@ -176,12 +163,8 @@ VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control, double time) const { VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); - // --- Position Derivative --- - // The derivative of the position is the linear velocity. state_dot.segment<3>(STATE_X) = state.segment<3>(STATE_VX); - // --- Quaternion Derivative --- - // Extract the quaternion (assumed to be [qw, qx, qy, qz]) autodiff::dual2nd qw = state(STATE_QW); autodiff::dual2nd qx = state(STATE_QX); autodiff::dual2nd qy = state(STATE_QY); @@ -199,7 +182,6 @@ VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( qw = 1.0; qx = 0.0; qy = 0.0; qz = 0.0; } - // Extract body angular velocity components autodiff::dual2nd omega_x = state(STATE_OMEGA_X); autodiff::dual2nd omega_y = state(STATE_OMEGA_Y); autodiff::dual2nd omega_z = state(STATE_OMEGA_Z); @@ -210,20 +192,16 @@ VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( state_dot(STATE_QY) = 0.5 * (qw * omega_y - qx * omega_z + qz * omega_x); state_dot(STATE_QZ) = 0.5 * (qw * omega_z + qx * omega_y - qy * omega_x); - // --- Velocity Derivative --- - // Extract control variables (motor forces) const autodiff::dual2nd f1 = control(CONTROL_F1); const autodiff::dual2nd f2 = control(CONTROL_F2); const autodiff::dual2nd f3 = control(CONTROL_F3); const autodiff::dual2nd f4 = control(CONTROL_F4); - // Compute total thrust and moments const autodiff::dual2nd thrust = f1 + f2 + f3 + f4; const autodiff::dual2nd tau_x = arm_length_ * (f1 - f3); const autodiff::dual2nd tau_y = arm_length_ * (f2 - f4); const autodiff::dual2nd tau_z = 0.1 * (f1 - f2 + f3 - f4); - // Compute rotation matrix from the normalized quaternion Eigen::Matrix R = getRotationMatrixAutodiff(qw, qx, qy, qz); // Thrust is applied along the body z-axis. @@ -232,14 +210,10 @@ VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( Eigen::Matrix acceleration = (1.0/mass_) * (R * F_thrust) - gravity; state_dot.segment<3>(STATE_VX) = acceleration; - // --- Angular Velocity Derivative --- Eigen::Matrix omega(omega_x, omega_y, omega_z); Eigen::Matrix tau(tau_x, tau_y, tau_z); - // Convert inertia matrix to autodiff type Eigen::Matrix inertia = inertia_.cast(); - - // Calculate angular acceleration Eigen::Matrix angular_acc = inertia.inverse() * (tau - omega.cross(inertia * omega)); state_dot.segment<3>(STATE_OMEGA_X) = angular_acc; @@ -249,19 +223,15 @@ VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( // TODO: Implement a more accurate version if needed std::vector Quadrotor::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - // We'll use autodiff to compute Hessians VectorXdual2nd x = state; VectorXdual2nd u = control; std::vector hessians(STATE_DIM); for (int i = 0; i < STATE_DIM; ++i) { - // Define lambda for the ith component of the dynamics auto fi = [&, i, time](const VectorXdual2nd& x_ad) -> autodiff::dual2nd { return getContinuousDynamicsAutodiff(x_ad, u, time)(i); }; - - // Compute Hessian of ith component w.r.t. state hessians[i] = autodiff::hessian(fi, wrt(x), at(x)); } @@ -271,19 +241,15 @@ std::vector Quadrotor::getStateHessian( // TODO: Implement a more accurate version if needed std::vector Quadrotor::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - // We'll use autodiff to compute Hessians VectorXdual2nd x = state; VectorXdual2nd u = control; std::vector hessians(STATE_DIM); for (int i = 0; i < STATE_DIM; ++i) { - // Define lambda for the ith component of the dynamics auto fi = [&, i, time](const VectorXdual2nd& u_ad) -> autodiff::dual2nd { return getContinuousDynamicsAutodiff(x, u_ad, time)(i); }; - - // Compute Hessian of ith component w.r.t. control hessians[i] = autodiff::hessian(fi, wrt(u), at(u)); } @@ -292,26 +258,18 @@ std::vector Quadrotor::getControlHessian( std::vector Quadrotor::getCrossHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { - // For mixed partial derivatives, we need a different approach - // We compute derivatives of Jacobian w.r.t. control VectorXdual2nd x = state; VectorXdual2nd u = control; std::vector cross_hessians(STATE_DIM); for (int i = 0; i < STATE_DIM; ++i) { - // Define a function that returns the gradient of the ith component w.r.t. state auto gradient_i = [&, i, time](const VectorXdual2nd& u_ad) -> VectorXdual2nd { - // Capture the current u_ad in a lambda auto fi_x = [&, u_ad, i, time](const VectorXdual2nd& x_ad) -> autodiff::dual2nd { return getContinuousDynamicsAutodiff(x_ad, u_ad, time)(i); }; - - // Return the gradient of fi with respect to x at the current x return autodiff::gradient(fi_x, wrt(x), at(x)); }; - - // Compute Jacobian of gradient w.r.t. control (this is the cross Hessian) cross_hessians[i] = autodiff::jacobian(gradient_i, wrt(u), at(u)); } diff --git a/src/dynamics_model/quaternion_attitude.cpp b/src/dynamics_model/quaternion_attitude.cpp index d4c9447..dcfd010 100644 --- a/src/dynamics_model/quaternion_attitude.cpp +++ b/src/dynamics_model/quaternion_attitude.cpp @@ -35,11 +35,9 @@ namespace cddp { Eigen::VectorXd state_dot(STATE_DIM); - // Extract states Eigen::Vector4d quat = state.segment<4>(STATE_QUAT_W); Eigen::Vector3d omega = state.segment<3>(STATE_OMEGA_X); - // Extract control Eigen::Vector3d tau = control.segment<3>(CONTROL_TAU_X); // Normalize quaternion to prevent drift @@ -66,41 +64,34 @@ namespace cddp Eigen::MatrixXd QuaternionAttitude::getStateJacobian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - // Use autodiff to compute state Jacobian - VectorXdual2nd x = state; // Cast state to autodiff type - VectorXdual2nd u = control; // Cast control to autodiff type + VectorXdual2nd x = state; + VectorXdual2nd u = control; - // Define lambda for dynamics w.r.t. state auto dynamics_wrt_x = [&](const VectorXdual2nd &x_ad) -> VectorXdual2nd { return this->getContinuousDynamicsAutodiff(x_ad, u, time); }; - // Compute Jacobian return autodiff::jacobian(dynamics_wrt_x, wrt(x), at(x)); } Eigen::MatrixXd QuaternionAttitude::getControlJacobian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - // Use autodiff to compute control Jacobian - VectorXdual2nd x = state; // Cast state to autodiff type - VectorXdual2nd u = control; // Cast control to autodiff type + VectorXdual2nd x = state; + VectorXdual2nd u = control; - // Define lambda for dynamics w.r.t. control auto dynamics_wrt_u = [&](const VectorXdual2nd &u_ad) -> VectorXdual2nd { return this->getContinuousDynamicsAutodiff(x, u_ad, time); }; - // Compute Jacobian return autodiff::jacobian(dynamics_wrt_u, wrt(u), at(u)); } std::vector QuaternionAttitude::getStateHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - // Use autodiff to compute state Hessian VectorXdual2nd x = state; VectorXdual2nd u = control; @@ -108,13 +99,11 @@ namespace cddp for (int i = 0; i < STATE_DIM; ++i) { - // Define lambda for the i-th component of dynamics w.r.t. state auto fi_x = [&, i, time](const VectorXdual2nd &x_ad) -> autodiff::dual2nd { return this->getContinuousDynamicsAutodiff(x_ad, u, time)(i); }; - // Compute Hessian for the i-th component hessians[i] = autodiff::hessian(fi_x, wrt(x), at(x)); } @@ -124,7 +113,6 @@ namespace cddp std::vector QuaternionAttitude::getControlHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - // Use autodiff to compute control Hessian VectorXdual2nd x = state; VectorXdual2nd u = control; @@ -132,13 +120,11 @@ namespace cddp for (int i = 0; i < STATE_DIM; ++i) { - // Define lambda for the i-th component of dynamics w.r.t. control auto fi_u = [&, i, time](const VectorXdual2nd &u_ad) -> autodiff::dual2nd { return this->getContinuousDynamicsAutodiff(x, u_ad, time)(i); }; - // Compute Hessian for the i-th component hessians[i] = autodiff::hessian(fi_u, wrt(u), at(u)); } @@ -148,7 +134,6 @@ namespace cddp std::vector QuaternionAttitude::getCrossHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control, double time) const { - // Use autodiff to compute cross Hessian (Jacobian of gradient) VectorXdual2nd x = state; VectorXdual2nd u = control; @@ -156,43 +141,34 @@ namespace cddp for (int i = 0; i < STATE_DIM; ++i) { - // Define lambda that computes the gradient of the i-th component w.r.t. state auto gradient_fi_x = [&, i, time](const VectorXdual2nd &u_ad) -> VectorXdual2nd { - // Inner lambda: i-th component of dynamics w.r.t state (holding u_ad constant) auto fi_x = [&, u_ad, i, time](const VectorXdual2nd &x_ad) -> autodiff::dual2nd { return this->getContinuousDynamicsAutodiff(x_ad, u_ad, time)(i); }; - // Return the gradient w.r.t. x return autodiff::gradient(fi_x, wrt(x), at(x)); }; - // Compute the Jacobian of this gradient function w.r.t. control cross_hessians[i] = autodiff::jacobian(gradient_fi_x, wrt(u), at(u)); } return cross_hessians; } - // Autodiff version of the continuous dynamics VectorXdual2nd QuaternionAttitude::getContinuousDynamicsAutodiff( const VectorXdual2nd &state, const VectorXdual2nd &control, double time) const { - // Cast member variables to autodiff types autodiff::Matrix3dual2nd inertia_ad = this->inertia_.cast(); autodiff::Matrix3dual2nd inertia_inv_ad = this->inertia_inv_.cast(); - // Extract states autodiff::Vector4dual2nd quat = state.segment<4>(STATE_QUAT_W); autodiff::Vector3dual2nd omega = state.segment<3>(STATE_OMEGA_X); - // Extract control autodiff::Vector3dual2nd tau = control.segment<3>(CONTROL_TAU_X); - // Initialize state derivative vector VectorXdual2nd state_dot(STATE_DIM); // Quaternion Kinematics: dq/dt = 0.5 * Omega(omega) * q @@ -204,4 +180,4 @@ namespace cddp return state_dot; } -} // namespace cddp \ No newline at end of file +} // namespace cddp diff --git a/src/dynamics_model/unicycle.cpp b/src/dynamics_model/unicycle.cpp index db51fa3..e92d416 100644 --- a/src/dynamics_model/unicycle.cpp +++ b/src/dynamics_model/unicycle.cpp @@ -30,14 +30,9 @@ Eigen::VectorXd Unicycle::getContinuousDynamics( Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM); - // Extract state variables const double theta = state(STATE_THETA); // heading angle - - // Extract control variables const double v = control(CONTROL_V); // velocity const double omega = control(CONTROL_OMEGA); // angular velocity - - // Unicycle dynamics equations state_dot(STATE_X) = v * std::cos(theta); // dx/dt state_dot(STATE_Y) = v * std::sin(theta); // dy/dt state_dot(STATE_THETA) = omega; // dtheta/dt @@ -50,17 +45,9 @@ Eigen::MatrixXd Unicycle::getStateJacobian( Eigen::MatrixXd A = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); - // Extract state variables const double theta = state(STATE_THETA); // heading angle - - // Extract control variables const double v = control(CONTROL_V); // velocity - - // Compute partial derivatives with respect to state variables - // df1/dtheta = d(dx/dt)/dtheta A(STATE_X, STATE_THETA) = -v * std::sin(theta); - - // df2/dtheta = d(dy/dt)/dtheta A(STATE_Y, STATE_THETA) = v * std::cos(theta); return A; @@ -70,18 +57,9 @@ Eigen::MatrixXd Unicycle::getControlJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const { Eigen::MatrixXd B = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM); // Note: Using 2 for control dim as per original - - // Extract state variables const double theta = state(STATE_THETA); // heading angle - - // Compute partial derivatives with respect to control variables - // df1/dv = d(dx/dt)/dv B(STATE_X, CONTROL_V) = std::cos(theta); - - // df2/dv = d(dy/dt)/dv B(STATE_Y, CONTROL_V) = std::sin(theta); - - // df3/domega = d(dtheta/dt)/domega B(STATE_THETA, CONTROL_OMEGA) = 1.0; return B; @@ -92,13 +70,9 @@ std::vector Unicycle::getStateHessian( auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM); - // Non-zero elements - // Hessian of x w.r.t. theta twice const double v = control(CONTROL_V); const double theta = state(STATE_THETA); hessians[STATE_X](STATE_THETA, STATE_THETA) = -v * std::cos(theta); - - // Hessian of y w.r.t. theta twice hessians[STATE_Y](STATE_THETA, STATE_THETA) = -v * std::sin(theta); return hessians; @@ -109,8 +83,6 @@ std::vector Unicycle::getControlHessian( auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM); - // No non-zero terms in control Hessian for this model - return hessians; } @@ -119,14 +91,11 @@ VectorXdual2nd Unicycle::getContinuousDynamicsAutodiff( VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); - // Extract state (dual2nd) const autodiff::dual2nd theta = state(STATE_THETA); - // Extract control (dual2nd) const autodiff::dual2nd v = control(CONTROL_V); const autodiff::dual2nd omega = control(CONTROL_OMEGA); - // Dynamics using ADL for math state_dot(STATE_X) = v * cos(theta); state_dot(STATE_Y) = v * sin(theta); state_dot(STATE_THETA) = omega; @@ -134,4 +103,4 @@ VectorXdual2nd Unicycle::getContinuousDynamicsAutodiff( return state_dot; } -} // namespace cddp \ No newline at end of file +} // namespace cddp