Joint Acceleration Constraint

[ben-greenberg]

Took a stab at adding this acceleration constraint, to smooth trajectories instead of using the velocity constraint (which was causing solutions to enter an 'unfeasible' state). I basically just modified the joint_velocity_constraint to make this. Seems like I can use it, though I'm not sure if its doing what I want. Happy to get any feedback on this.

[mpowelson]

So adding variables for velocity is something that we have talked about. However this is probably not what you are wanting. Since all of the other constraints operate on positions, you won't have any way of tying the goal waypoints to the velocity variables. Trajopt may optimize it, but you'll just end up with velocities that don't correspond to the position variables.

What you probably want is to implement acceleration constraints using central finite difference like we do in the old trajopt version. You can check out the old cost in trajectory_costs.cpp. I think the math should be pretty much the same minus the squaring of the results.

DblVec JointAccEqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff =
      (diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))).rowwise() -
      targets_.transpose();
  // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector
  return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal());
}

[ben-greenberg]

Thanks for the guidance! A few questions as I've been trying to get up to speed on what was done for the velocity constraint.

1. joint_velocity_constraint.h defines the velocity as Joint velocity is calculated as v = th_1 - th_0 Where did this equation come from, and is there a similar one for acceleration?

2. I'm also unclear on what the single_step calculated in joint_velocity_constraint.cpp is representing. I see it is defined as Eigen::VectorXd single_step = vals1 - vals2;? I would have thought that the step would be vals2 - vals1 if it is representing the difference between joint positions at a certain point, but that change seems to break things when running.

3. If we use the central finite distance as you suggested, I think we'll need to change add an additional step for the forward and backward differences in position. Does that sound like the correct approach?

[mpowelson]

Thanks for the guidance! A few questions as I've been trying to get up to speed on what was done for the velocity constraint.

1. joint_velocity_constraint.h defines the velocity as Joint velocity is calculated as v = th_1 - th_0

This is just saying that the velocity is the position at t1 - position at t0. However, the dt is assumed to be 1.

Where did this equation come from, and is there a similar one for acceleration?

2. I'm also unclear on what the single_step calculated in joint_velocity_constraint.cpp is representing. I see it is defined as Eigen::VectorXd single_step = vals1 - vals2;? I would have thought that the step would be vals2 - vals1 if it is representing the difference between joint positions at a certain point, but that change seems to break things when running.

I'm not sure why this is that way. It might be a bug.

3. If we use the central finite distance as you suggested, I think we'll need to change add an additional step for the forward and backward differences in position. Does that sound like the correct approach?

You should be able to use the math I posted above (or here for example). Calculating velocity is baked into the equation.

[Levi-Armstrong]

@ben-greenberg Can you rebase on master?

[Levi-Armstrong]

replaced by #275