Search code examples
drake

How to use AddVelocityConstraintAtNormalizedTime in pydrake?


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.


Solution

  • 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)