Skip to content

Commit 6739eb0

Browse files
Copilotastomodynamics
Copilot
and
astomodynamics
authored
Remove redundant dynamics-model comments
Agent-Logs-Url: https://github.com/astomodynamics/cddp-cpp/sessions/4bbe59ce-e0d4-4b27-a43e-d412404c2078 Co-authored-by: astomodynamics <49183997+astomodynamics@users.noreply.github.com>
1 parent a562702 commit 6739eb0

7 files changed

Lines changed: 10 additions & 243 deletions

File tree

‎src/dynamics_model/bicycle.cpp‎

Lines changed: 1 addition & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -32,15 +32,12 @@ Eigen::VectorXd Bicycle::getContinuousDynamics(const Eigen::VectorXd &state,
3232

3333
Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM);
3434

35-
// Extract state variables
3635
const double theta = state(STATE_THETA); // heading angle
3736
const double v = state(STATE_V); // velocity
3837

39-
// Extract control variables
4038
const double a = control(CONTROL_ACC); // acceleration
4139
const double delta = control(CONTROL_DELTA); // steering angle
4240

43-
// Kinematic bicycle model equations
4441
state_dot(STATE_X) = v * std::cos(theta); // dx/dt
4542
state_dot(STATE_Y) = v * std::sin(theta); // dy/dt
4643
state_dot(STATE_THETA) = (v / wheelbase_) * std::tan(delta); // dtheta/dt
@@ -56,16 +53,12 @@ Bicycle::getContinuousDynamicsAutodiff(const VectorXdual2nd &state,
5653

5754

5855
VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM);
59-
60-
// Extract state variables (now dual2nd types)
6156
const autodiff::dual2nd theta = state(STATE_THETA); // heading angle
6257
const autodiff::dual2nd v = state(STATE_V); // velocity
6358

64-
// Extract control variables (now dual2nd types)
6559
const autodiff::dual2nd a = control(CONTROL_ACC); // acceleration
6660
const autodiff::dual2nd delta = control(CONTROL_DELTA); // steering angle
6761

68-
// Use ADL for math functions
6962
state_dot(STATE_X) = v * cos(theta);
7063
state_dot(STATE_Y) = v * sin(theta);
7164
state_dot(STATE_THETA) = (v / wheelbase_) * tan(delta);
@@ -80,25 +73,17 @@ Eigen::MatrixXd Bicycle::getStateJacobian(const Eigen::VectorXd &state,
8073

8174
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM);
8275

83-
// Extract state variables
8476
const double theta = state(STATE_THETA); // heading angle
8577
const double v = state(STATE_V); // velocity
8678

87-
// Extract control variables
8879
const double delta = control(CONTROL_DELTA); // steering angle
8980

90-
// Compute partial derivatives with respect to state variables
91-
// df1/dtheta = d(dx/dt)/dtheta
9281
A(STATE_X, STATE_THETA) = -v * std::sin(theta);
93-
// df1/dv = d(dx/dt)/dv
9482
A(STATE_X, STATE_V) = std::cos(theta);
9583

96-
// df2/dtheta = d(dy/dt)/dtheta
9784
A(STATE_Y, STATE_THETA) = v * std::cos(theta);
98-
// df2/dv = d(dy/dt)/dv
9985
A(STATE_Y, STATE_V) = std::sin(theta);
10086

101-
// df3/dv = d(dtheta/dt)/dv
10287
A(STATE_THETA, STATE_V) = std::tan(delta) / wheelbase_;
10388

10489
return A;
@@ -110,17 +95,12 @@ Eigen::MatrixXd Bicycle::getControlJacobian(const Eigen::VectorXd &state,
11095

11196
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM);
11297

113-
// Extract state variables
11498
const double v = state(STATE_V); // velocity
11599

116-
// Extract control variables
117100
const double delta = control(CONTROL_DELTA); // steering angle
118101

119-
// Compute partial derivatives with respect to control variables
120-
// df4/da = d(dv/dt)/da
121102
B(STATE_V, CONTROL_ACC) = 1.0;
122103

123-
// df3/ddelta = d(dtheta/dt)/ddelta
124104
B(STATE_THETA, CONTROL_DELTA) =
125105
v / (wheelbase_ * std::pow(std::cos(delta), 2));
126106

@@ -133,22 +113,16 @@ Bicycle::getStateHessian(const Eigen::VectorXd &state,
133113

134114
auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM);
135115

136-
// Extract state variables
137116
const double theta = state(STATE_THETA); // heading angle
138117
const double v = state(STATE_V); // velocity
139118

140-
// Second derivatives with respect to states
141-
// d²(dx/dt)/dtheta²
142119
hessians[STATE_X](STATE_THETA, STATE_THETA) = -v * std::cos(theta);
143120

144-
// d²(dx/dt)/(dtheta*dv)
145121
hessians[STATE_X](STATE_THETA, STATE_V) = -std::sin(theta);
146122
hessians[STATE_X](STATE_V, STATE_THETA) = -std::sin(theta);
147123

148-
// d²(dy/dt)/dtheta²
149124
hessians[STATE_Y](STATE_THETA, STATE_THETA) = -v * std::sin(theta);
150125

151-
// d²(dy/dt)/(dtheta*dv)
152126
hessians[STATE_Y](STATE_THETA, STATE_V) = std::cos(theta);
153127
hessians[STATE_Y](STATE_V, STATE_THETA) = std::cos(theta);
154128

@@ -161,18 +135,14 @@ Bicycle::getControlHessian(const Eigen::VectorXd &state,
161135

162136
auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM);
163137

164-
// Extract state variables
165138
const double v = state(STATE_V); // velocity
166139

167-
// Extract control variables
168140
const double delta = control(CONTROL_DELTA); // steering angle
169141

170-
// Second derivatives with respect to controls
171-
// d²(dtheta/dt)/ddelta²
172142
hessians[STATE_THETA](CONTROL_DELTA, CONTROL_DELTA) =
173143
2.0 * v * std::sin(delta) / (wheelbase_ * std::pow(std::cos(delta), 3));
174144

175145
return hessians;
176146
}
177147

178-
} // namespace cddp
148+
} // namespace cddp

‎src/dynamics_model/car.cpp‎

Lines changed: 1 addition & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -13,22 +13,17 @@ Car::Car(double timestep, double wheelbase, std::string integration_type)
1313

1414
Eigen::VectorXd Car::getDiscreteDynamics(
1515
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {
16-
17-
// Extract states
1816
const double x = state(STATE_X); // x position
1917
const double y = state(STATE_Y); // y position
2018
const double theta = state(STATE_THETA); // car angle
2119
const double v = state(STATE_V); // velocity
2220

23-
// Extract controls
2421
const double delta = control(CONTROL_DELTA); // steering angle
2522
const double a = control(CONTROL_A); // acceleration
2623

27-
// Constants
2824
const double d = wheelbase_; // distance between back and front axles
2925
const double h = timestep_; // timestep
3026

31-
// Compute unit vector in car direction
3227
const double cos_theta = std::cos(theta);
3328
const double sin_theta = std::sin(theta);
3429
Eigen::Vector2d z(cos_theta, sin_theta);
@@ -45,40 +40,30 @@ Eigen::VectorXd Car::getDiscreteDynamics(
4540
// dtheta = asin(sin(w)*f/d)
4641
const double dtheta = std::asin(std::sin(delta) * f / d);
4742

48-
// Compute state change
4943
Eigen::VectorXd dy = Eigen::VectorXd::Zero(STATE_DIM);
5044
dy(STATE_X) = b * cos_theta;
5145
dy(STATE_Y) = b * sin_theta;
5246
dy(STATE_THETA) = dtheta;
5347
dy(STATE_V) = h * a;
5448

55-
// Return next state
5649
return state + dy;
5750
}
5851

5952
Eigen::MatrixXd Car::getStateJacobian(
6053
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {
61-
62-
// Convert inputs to autodiff types
6354
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
6455
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();
65-
66-
// Initialize Jacobian
6756
Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM);
6857

69-
// Calculate Jacobian using autodiff
7058
for (int i = 0; i < STATE_DIM; ++i) {
71-
// Create a lambda that returns the i-th component of the dynamics
7259
auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd {
7360
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time);
7461
return dynamics(i);
7562
};
7663

77-
// Calculate gradient of the i-th output with respect to state
7864
J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(state_dual), at(state_dual));
7965
}
8066

81-
// Convert discrete Jacobian to continuous time Jacobian
8267
J.diagonal().array() -= 1.0;
8368
J /= timestep_;
8469

@@ -87,54 +72,37 @@ Eigen::MatrixXd Car::getStateJacobian(
8772

8873
Eigen::MatrixXd Car::getControlJacobian(
8974
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {
90-
91-
// Convert inputs to autodiff types
9275
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
9376
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();
94-
95-
// Initialize Jacobian
9677
Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM);
9778

98-
// Calculate Jacobian using autodiff
9979
for (int i = 0; i < STATE_DIM; ++i) {
100-
// Create a lambda that returns the i-th component of the dynamics
10180
auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd {
10281
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time);
10382
return dynamics(i);
10483
};
10584

106-
// Calculate gradient of the i-th output with respect to control
10785
J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(control_dual), at(control_dual));
10886
}
10987

110-
// Convert discrete Jacobian to continuous time Jacobian
11188
J /= timestep_;
11289

11390
return J;
11491
}
11592

11693
std::vector<Eigen::MatrixXd> Car::getStateHessian(
11794
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {
118-
119-
// Convert inputs to autodiff types
12095
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
12196
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();
122-
123-
// Initialize Hessians
12497
auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM);
12598

126-
// Calculate Hessians using autodiff
12799
for (int i = 0; i < STATE_DIM; ++i) {
128-
// Create a lambda that returns the i-th component of the dynamics
129100
auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd {
130101
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time);
131102
return dynamics(i);
132103
};
133104

134-
// Calculate Hessian of the i-th output with respect to state
135105
hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(state_dual), at(state_dual));
136-
137-
// Convert discrete Hessian to continuous time Hessian
138106
hessians[i] /= timestep_;
139107
}
140108

@@ -143,50 +111,36 @@ std::vector<Eigen::MatrixXd> Car::getStateHessian(
143111

144112
std::vector<Eigen::MatrixXd> Car::getControlHessian(
145113
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {
146-
147-
// Convert inputs to autodiff types
148114
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
149115
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();
150-
151-
// Initialize Hessians
152116
auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM);
153117

154-
// Calculate Hessians using autodiff
155118
for (int i = 0; i < STATE_DIM; ++i) {
156-
// Create a lambda that returns the i-th component of the dynamics
157119
auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd {
158120
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time);
159121
return dynamics(i);
160122
};
161123

162-
// Calculate Hessian of the i-th output with respect to control
163124
hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(control_dual), at(control_dual));
164-
165-
// Convert discrete Hessian to continuous time Hessian
166125
hessians[i] /= timestep_;
167126
}
168127

169128
return hessians;
170129
}
171130

172-
// Helper: Autodiff version of discrete dynamics
173131
cddp::VectorXdual2nd Car::getDiscreteDynamicsAutodiff(
174132
const VectorXdual2nd& state, const VectorXdual2nd& control, double time) const {
175133

176-
// Extract states (dual2nd)
177134
const autodiff::dual2nd theta = state(STATE_THETA);
178135
const autodiff::dual2nd v = state(STATE_V);
179136

180-
// Extract controls (dual2nd)
181137
const autodiff::dual2nd delta = control(CONTROL_DELTA);
182138
const autodiff::dual2nd a = control(CONTROL_A);
183139

184-
// Constants
185140
const double d_double = wheelbase_;
186141
const double h = timestep_;
187142
const autodiff::dual2nd d = wheelbase_;
188143

189-
// Use ADL for math functions
190144
const autodiff::dual2nd cos_theta = cos(theta);
191145
const autodiff::dual2nd sin_theta = sin(theta);
192146

@@ -228,4 +182,4 @@ cddp::VectorXdual2nd Car::getContinuousDynamicsAutodiff(
228182
return (next_state - state) / timestep_;
229183
}
230184

231-
} // namespace cddp
185+
} // namespace cddp

0 commit comments

Comments
 (0)