Add Forklift dynamics model and parking example

[astomodynamics]

Summary

• Implement Forklift dynamics model with rear-steering kinematic bicycle model
• Add forklift parking example using IPDDP solver
• Include comprehensive unit tests

Implementation Details

• Forklift class: 5-state (x, y, theta, v, delta) and 2-control (acceleration, steering_rate) dynamics
• Autodiff support: Full gradient computation capabilities
• Constraints: Max acceleration ±1.5 m/s², max steering rate ±0.4 rad/s
• Cost weights: Bryson-scaled
• StateConstraint class: Added to handle velocity and steering angle constraints

Test Results

All unit tests pass, verifying:

• Forward dynamics computation
• Jacobian calculations
• Control and state constraint enforcement
• Proper rear-steering kinematics

Example

The forklift parking example demonstrates successful back-in parking maneuver using MSIPDDP solver.

[cursor]

Bug: Incorrect Jacobian Implementations in `StateConstraint`

The StateConstraint class has incorrect Jacobian implementations. The getStateJacobian method incorrectly returns a zero matrix; it should return [-I; I] * scale_factor_ as the constraint g = [-state; state] * scale_factor_ depends on state. Conversely, the getControlJacobian method incorrectly returns the state-dependent Jacobian [-I; I] with wrong dimensions (2*state.size(), state.size()); it should return a zero matrix with dimensions (2*state.size(), control.size()) since the constraint does not depend on control.

include/cddp-cpp/cddp_core/constraint.hpp#L538-L556

cddp-cpp/include/cddp-cpp/cddp_core/constraint.hpp

Lines 538 to 556 in 8273c7f

	Eigen::MatrixXd
	getStateJacobian(const Eigen::VectorXd &state,
	const Eigen::VectorXd &control) const override
	{
	return Eigen::MatrixXd::Zero(dim_, state.size());
	}
	Eigen::MatrixXd
	getControlJacobian(const Eigen::VectorXd &state,
	const Eigen::VectorXd &control) const override
	{
	Eigen::MatrixXd jac(2 * state.size(), state.size());
	jac.topLeftCorner(state.size(), state.size()) =
	-Eigen::MatrixXd::Identity(state.size(), state.size());
	jac.bottomRightCorner(state.size(), state.size()) =
	Eigen::MatrixXd::Identity(state.size(), state.size());
	return jac;
	}

Fix in Cursor • Fix in Web

---

BugBot free trial expires on July 22, 2025
You have used $0.00 of your $10.00 spend limit so far. Manage your spend limit in the Cursor dashboard.

Was this report helpful? Give feedback by reacting with 👍 or 👎