You could add that constraint through MathematicalProgram::AddLinearConstraint
. For example, say you have a trajectory optimization problem prog
, and you want to constraint that the difference between q[n+1]
and q[n]
are less than a bound max_delta_q
. You could do it as
nq = plant.num_positions();
A = Eigen::MatrixXd(nq, 2*nq);
A << Eigen::MatrixXd::Identity(nq), -Eigen::MatrixXd::Identity(nq)
// Add the constraint -max_delta_q <= q[n+1] - q[n] <= max_delta_q
prog.AddLinearConstraint(A, -max_delta_q, max_delta_q, {q[n], q[n+1]});
CLICK HERE to find out more related problems solutions.