@@ -31,18 +31,20 @@ void FootTrajectoryGenerator::update(State& state) {
3131 Scalar desired_theta = start_theta + (end_theta - start_theta) * cubic (time_in_step);
3232 swing_foot.pose = Pose3 (RotationMatrix::aroundZ (desired_theta), Vector3 (desired_pos (0 ), desired_pos (1 ), 0 ));
3333 swing_foot.pose .euler = Vector3 (0 , 0 , desired_theta);
34-
34+
3535 // Linear Velocity with cubic polynomial interpolation
3636 swing_foot.lin_vel .segment (0 , 2 ) = (end_pos - start_pos) * cubic_dot (time_in_step) / ss_duration;
37- swing_foot.lin_acc .segment (0 , 2 ) = (end_pos - start_pos) * cubic_ddot (time_in_step) / (std::pow (ss_duration, 2 ));
37+ swing_foot.lin_acc .segment (0 , 2 ) =
38+ (end_pos - start_pos) * cubic_ddot (time_in_step) / (std::pow (ss_duration, 2 ));
3839 // Height with quartic polynomial interpolation
3940 swing_foot.pose .translation (2 ) = step_height * quartic (time_in_step);
4041 swing_foot.lin_vel (2 ) = step_height * quartic_dot (time_in_step) / ss_duration;
4142 swing_foot.lin_acc (2 ) = step_height * quartic_ddot (time_in_step) / (ss_duration * ss_duration);
4243
4344 // Angular Velocity with cubic polynomial interpolation
4445 swing_foot.ang_vel = Vector3 (0 , 0 , (end_theta - start_theta) * cubic_dot (time_in_step) / ss_duration);
45- swing_foot.ang_acc = Vector3 (0 , 0 , (end_theta - start_theta) * cubic_ddot (time_in_step) / (std::pow (ss_duration, 2 )));
46+ swing_foot.ang_acc =
47+ Vector3 (0 , 0 , (end_theta - start_theta) * cubic_ddot (time_in_step) / (std::pow (ss_duration, 2 )));
4648 }
4749
4850 state.setDesiredSwingFoot (swing_foot);
0 commit comments