is there a multi-body equivalent of a posture change constraint?

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.

Leave a Comment

Your email address will not be published.

Scroll to Top