I want to constrain \dot{q} at t=0 when using KinematicTrajectoryOptimization, can I use this constraint? Are there any examples available?
I see the argument is pydrake.solvers.Constraint
. However, I have no idea how to use it properly.
You can do
# Imposes the linear constraint
# lower <= [0 I] * [q] = upper
# [v]
# where both lower and upper are numpy arrays of shape (num_positions,)
constraint = pydrake.solvers.LinearConstraint(
np.concantenate((np.zeros(num_positions), np.eye(num_positions)), axis=1),
lower, upper)
kinematic_trajectory_optimization.AddVelocityConstraintAtNormalizedTime(constraint, 0)