37 void TimeIndexedProblem::Instantiate(
const TimeIndexedProblemInitializer& init)
39 this->parameters_ = init;
41 N = scene_->GetKinematicTree().GetNumControlledJoints();
43 w_scale_ = this->parameters_.Wrate;
44 W = Eigen::MatrixXd::Identity(N, N) * w_scale_;
45 if (this->parameters_.W.rows() > 0)
47 if (this->parameters_.W.rows() == N)
49 W.diagonal() = this->parameters_.W * w_scale_;
53 ThrowNamed(
"W dimension mismatch! Expected " << N <<
", got " << this->parameters_.W.rows());
57 if (init.LowerBound.rows() == N)
59 scene_->GetKinematicTree().SetJointLimitsLower(init.LowerBound);
61 else if (init.LowerBound.rows() != 0)
63 ThrowNamed(
"Lower bound size incorrect! Expected " << N <<
" got " << init.LowerBound.rows());
65 if (init.UpperBound.rows() == N)
67 scene_->GetKinematicTree().SetJointLimitsUpper(init.UpperBound);
69 else if (init.UpperBound.rows() != 0)
71 ThrowNamed(
"Lower bound size incorrect! Expected " << N <<
" got " << init.UpperBound.rows());
74 use_bounds = this->parameters_.UseBounds;
76 cost.Initialize(this->parameters_.Cost, shared_from_this(), cost_Phi);
77 inequality.Initialize(this->parameters_.Inequality, shared_from_this(), inequality_Phi);
78 equality.Initialize(this->parameters_.Equality, shared_from_this(), equality_Phi);
80 T_ = this->parameters_.T;
81 tau_ = this->parameters_.tau;
82 SetJointVelocityLimits(this->parameters_.JointVelocityLimits);
83 ApplyStartState(
false);
84 ReinitializeVariables();
87 bool TimeIndexedProblem::IsValid()
89 bool succeeded =
true;
90 auto bounds = scene_->GetKinematicTree().GetJointLimits();
92 std::cout.precision(4);
95 for (
int t = 0; t < T_; ++t)
100 for (
int i = 0; i < N; ++i)
102 constexpr
double tolerance = 1.e-3;
103 if (
x[t](i) < bounds(i, 0) - tolerance ||
x[t](i) > bounds(i, 1) + tolerance)
105 if (debug_)
HIGHLIGHT_NAMED(
"TimeIndexedProblem::IsValid",
"State at timestep " << t <<
" is out of bounds: joint #" << i <<
": " << bounds(i, 0) <<
" < " <<
x[t](i) <<
" < " << bounds(i, 1));
112 if (GetInequality(t).rows() > 0)
114 if (GetInequality(t).maxCoeff() > this->parameters_.InequalityFeasibilityTolerance)
116 if (debug_)
HIGHLIGHT_NAMED(
"TimeIndexedProblem::IsValid",
"Violated inequality constraints at timestep " << t <<
": " << GetInequality(t).transpose());
122 if (GetEquality(t).rows() > 0)
124 if (GetEquality(t).cwiseAbs().maxCoeff() > this->parameters_.EqualityFeasibilityTolerance)
126 if (debug_)
HIGHLIGHT_NAMED(
"TimeIndexedProblem::IsValid",
"Violated equality constraints at timestep " << t <<
": " << GetEquality(t).cwiseAbs().maxCoeff());
132 if (q_dot_max_.maxCoeff() > 0 && t > 0)
134 if (((
x[t] -
x[t - 1]).cwiseAbs() - xdiff_max_).maxCoeff() > 1.e-5)
136 if (debug_)
HIGHLIGHT_NAMED(
"TimeIndexedProblem::IsValid",
"Violated joint velocity constraints at timestep " << t <<
": (" << (
x[t] -
x[t - 1]).transpose() <<
"), (limit=" << xdiff_max_.transpose() <<
"), violation: (" << ((
x[t] -
x[t - 1]).cwiseAbs() - xdiff_max_).transpose() <<
")");
#define HIGHLIGHT_NAMED(name, x)
Time-indexed problem with bound, joint velocity, and general equality/inequality constraints.
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)