search

Failed Attempt on Acceleration- and Velocity-Limited Trajectory Generation


  float dt = 1 / 8000.;
  float accel = 30;

  float k_p = 0.002;
  float k_d = 0.001;

  float dx = controller->position_target - controller->position_measured;
  controller->dx = dx;
  float decel_distance = 
      (controller->velocity_measured * controller->velocity_measured) / (2 * accel);
  controller->decel_distance = decel_distance;
  if (controller->velocity_measured * dx > 0) {
    if (fabs(dx) > 1.2 * decel_distance) { // accelerate
      accel = copysignf(accel, dx);
    }
    else { // decelerate
      accel = copysignf(accel, -controller->velocity_measured);
    }
  }
  else {
    accel = copysignf(accel, -controller->velocity_measured);
  }


  controller->velocity_setpoint += accel * dt;
  controller->position_setpoint += controller->velocity_setpoint * dt;
//  controller->position_setpoint = clampf(controller->position_setpoint, -2*M_PI, 2*M_PI);

  if (mode != MODE_TORQUE) {
    controller->torque_setpoint = k_p * (controller->position_setpoint - controller->position_measured) + k_d * (controller->velocity_setpoint - controller->velocity_measured);

    controller->accel = accel;

    controller->torque_setpoint = clampf(
        controller->torque_setpoint,
        -controller->torque_limit,
        controller->torque_limit);


//    controller->torque_setpoint = (controller->position_controller.torque_setpoint * (float)controller->motor.kv_rating) / 8.3;
//    controller->current_controller.i_d_target = 0;

//    controller->torque_setpoint =
//        controller->torque_target - controller->torque_measured;
  }
  else {
    controller->torque_setpoint = controller->torque_target;
  }

The actuator will oscillate around the target position

After manually posing some finish constraint the actuator work as expected. However, this method will introduce too many variables that needs to be tuned for every actuator. So we will abandon this method and use cascaded controller instead.

hashtag
SAFETY REMINDER

The above code does not handle velocity / torque bounds. So if the position target cannot be satisfied (bridge is powered off in this case), the velocity and position setpoint will grow out of bound, which will lead to abrupt motion when the motor is powered on again, and the motor will overshoot again on the other side if allowed to move freely.

Here's some previous not-working codes for reference

After switching to cascaded controller, there's still some oscillations

Last updated