41 N =
scene_->GetKinematicTree().GetNumControlledJoints();
81 Phi.assign(
T_, y_ref_);
83 x.assign(
T_, Eigen::VectorXd::Zero(
N));
84 xdiff.assign(
T_, Eigen::VectorXd::Zero(
N));
108 for (
int i = 0; i <
tasks_.size(); ++i)
tasks_[i]->is_used =
false;
116 for (
int i = 0; i < T_; ++i) kinematic_solutions_[i] = std::make_shared<KinematicResponse>(*
scene_->GetKinematicTree().GetKinematicResponse());
141 scene_->Update(x_in, static_cast<double>(t) *
tau_);
185 if (t > 0)
xdiff[t] =
x[t] -
x[t - 1];
std::vector< TaskVectorEntry > map
KinematicRequestFlags flags_
virtual Eigen::VectorXd ApplyStartState(bool update_traj=true)
unsigned int number_of_problem_updates_
bool IsValid() override
Evaluates whether the problem is valid.
void ReinitializeVariables() override
void SetZero(const int n)
std::vector< Eigen::VectorXd > xdiff
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
void Update(const TaskSpaceVector &big_Phi, Eigen::MatrixXdRefConst big_dPhi_dx, Eigen::MatrixXdRefConst big_dPhi_du, HessianRefConst big_ddPhi_ddx, HessianRefConst big_ddPhi_ddu, HessianRefConst big_ddPhi_dxdu, int t)
std::vector< Hessian > hessian
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
std::vector< Eigen::VectorXd > x
Current internal problem state.
double w_scale_
Kinematic system transition error covariance multiplier (constant throughout the trajectory) ...
void ValidateTimeIndex(int &t_in) const
Checks the desired time index for bounds and supports -1 indexing.
int T_
Number of time steps.
UnconstrainedTimeIndexedProblemInitializer parameters_
void Instantiate(const UnconstrainedTimeIndexedProblemInitializer &init) override
Instantiates the problem from an Initializer.
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
void UpdateMultipleTaskKinematics(std::vector< std::shared_ptr< KinematicResponse >> responses)
TimeIndexedTask cost
Cost task.
std::vector< Eigen::MatrixXd > jacobian
void ReinitializeVariables(int _T, std::shared_ptr< PlanningProblem > _prob, const TaskSpaceVector &_Phi)
#define HIGHLIGHT_NAMED(name, x)
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void Update(Eigen::VectorXdRefConst x_in, int t) override
Updates an individual timestep from a given state vector.
double tau_
Time step duration.
std::vector< std::shared_ptr< KinematicResponse > > kinematic_solutions_
Eigen::VectorXd xdiff_max_
Maximum change in the variables in a single timestep tau_. Gets set/updated via SetJointVelocityLimit...
Unconstrained time-indexed problem.
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
void PreUpdate() override
Updates internal variables before solving, e.g., after setting new values for Rho.
std::vector< TaskSpaceVector > Phi
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)
double ct
Normalisation of scalar cost and Jacobian over trajectory length.
Eigen::VectorXd q_dot_max_
Joint velocity limit (rad/s)
std::vector< Eigen::VectorXd > initial_trajectory_