yujin_ocs/yocs_velocity_smoother/src/velocity_smoother_nodelet.cpp's velocity code

[lahyeon]

yujin_ocs/yocs_velocity_smoother/src/velocity_smoother_nodelet.cpp's velocity code's

I do not understand the calculation in void VelocitySmoother::velocityCB(const geometry_msgs::Twist::ConstPtr& msg)

v_inc = target_vel.linear.x – last_cmd_vel.linear.x; w_inc = target_vel.angular.z – last_cmd_vel.angular.z;

// Calculate and normalise vectors A (desired velocity increment) and B (maximum velocity increment), // where v acts as coordinate x and w as coordinate y; the sign of the angle from A to B determines // which velocity (v or w) must be overconstrained to keep the direction provided as command double MA = sqrt( v_inc v_inc + w_inc w_inc); double MB = sqrt(max_v_inc max_v_inc + max_w_inc max_w_inc);

  double Av = std::abs(v_inc) / MA;
  double Aw = std::abs(w_inc) / MA;
  double Bv = max_v_inc / MB;
  double Bw = max_w_inc / MB;
  double theta = atan2(Bw, Bv) - atan2(Aw, Av);

if (theta < 0) { // overconstrain linear velocity max_v_inc = (max_w_incstd::abs(v_inc))/std::abs(w_inc); } else { // overconstrain angular velocity max_w_inc = (max_v_incstd::abs(w_inc))/std::abs(v_inc); }

  if (std::abs(v_inc) > max_v_inc)
  {
    // we must limit linear velocity
    cmd_vel->linear.x  = last_cmd_vel.linear.x  + sign(v_inc)*max_v_inc;
  }

  if (std::abs(w_inc) > max_w_inc)
  {
    // we must limit angular velocity
    cmd_vel->angular.z = last_cmd_vel.angular.z + sign(w_inc)*max_w_inc;
  }

what is mean?

[corot]

If you constraint linear xor angular velocity, being both non zero, you will change the shape of your trajectory. These lines constraint the other velocity (not need to respect the acceleration limits) to keep executing the same trajectory (but slower)

[lahyeon]

I appreciate your help!