CODEWITHSUNDEEP
drake
xing bing
xing bing

Reputation: 13

Question about KinematicTrajectoryOptimization AddPathVelocityConstraint

I want to constraints velocity at a specify position, for example set start and end velocity to zero.

KinematicTrajectoryOptimization provider two api: AddPathVelocityConstraint and AddVelocityConstraintAtNormalizedTime.

my question is how to constrain q̇(t) using liner constraint ?

is it ok to use below formula ? :

 q̇(t) = drds * dsdt = ṙ(s) / duration,
 duration * lb <= ṙ(s) <= duration * ub.

and corrspends implment will be

KinematicTrajectoryOptimization::AddPathVelocityConstraint(
    const Eigen::Ref<const Eigen::VectorXd>& lb,
    const Eigen::Ref<const Eigen::VectorXd>& ub, double s) {
  const VectorX<symbolic::Expression> sym_rdot_value = sym_rdot_->value(s);
  auto binding =
      prog_.AddLinearConstraint(lb * duration_ <= sym_rdot_value && sym_rdot_value <= ub * duration_);
  binding.evaluator()->set_description("path real velocity constraint");
  return binding;
}

Upvotes: 1

Views: 31

Answers (0)

Related Questions