Go to the documentation of this file.
81 ThrowPretty(
"L1Rate has wrong size: expected " <<
scene_->get_num_controls() <<
", 1, or 0 (default), got " <<
parameters_.L1Rate.size());
114 if (!
scene_->GetDynamicsSolver())
ThrowPretty(
"DynamicsSolver is not initialised!");
116 const int NX =
scene_->get_num_positions() +
scene_->get_num_velocities(),
117 NDX = 2 *
scene_->get_num_velocities(),
118 NU =
scene_->get_num_controls();
119 Qf_ = Eigen::MatrixXd::Identity(NDX, NDX);
133 R_ = Eigen::MatrixXd::Identity(
scene_->get_num_controls(),
scene_->get_num_controls());
142 ThrowNamed(
"R dimension mismatch! Expected " <<
scene_->get_num_controls() <<
", got " << this->parameters_.R.rows());
183 bool full_noise_set =
false;
184 Ci_.assign(NX, Eigen::MatrixXd::Zero(NX, NU));
189 for (
int i = 0; i < NU; ++i)
194 ThrowPretty(
"Noise does not work for systems that have NU > NX. This should be fixed in the future.");
202 Eigen::Map<Eigen::MatrixXd> C_map(
parameters_.C.data(), NU, NX);
204 for (
int i = 0; i < NX; ++i)
205 Ci_[i].row(i) = C_map.col(i).transpose();
206 full_noise_set =
true;
214 CW_ = this->
parameters_.CW_rate * Eigen::MatrixXd::Identity(NX, NX);
220 full_noise_set =
true;
238 const long double fmod_tau_dt = std::fmod(
static_cast<long double>(1000. *
tau_),
static_cast<long double>(1000. *
scene_->GetDynamicsSolver()->get_dt()));
239 if (fmod_tau_dt > 1e-5)
ThrowPretty(
"tau is not a multiple of dt: tau=" <<
tau_ <<
", dt=" <<
scene_->GetDynamicsSolver()->get_dt() <<
", mod(" << fmod_tau_dt <<
")");
253 const int NX =
scene_->get_num_positions() +
scene_->get_num_velocities(), NDX = 2 *
scene_->get_num_velocities(), NU =
scene_->get_num_controls();
255 X_ = Eigen::MatrixXd::Zero(NX,
T_);
257 X_diff_ = Eigen::MatrixXd::Zero(NDX,
T_);
258 U_ = Eigen::MatrixXd::Zero(
scene_->get_num_controls(),
T_ - 1);
261 if (
scene_->get_has_quaternion_floating_base())
263 for (
int t = 0;
t <
T_; ++
t)
273 Eigen::MatrixXd goal_state(
X_star_);
285 for (
int t = 0;
t <
T_; ++
t)
287 goal_state.col(
t) = this->
parameters_.GoalState.segment(
t * NX, NX);
292 ThrowPretty(
"GoalState has " << this->
parameters_.GoalState.rows() <<
" rows, but expected either NX=" << NX <<
" or NQ=" <<
scene_->get_num_positions() <<
", or NX*T=" << NX *
T_);
300 Eigen::MatrixXd start_state(
X_);
307 for (
int t = 0;
t <
T_; ++
t)
314 for (
int t = 0;
t <
T_; ++
t)
316 start_state.col(
t) = this->
parameters_.StartState.segment(
t * NX, NX);
321 ThrowPretty(
"StartState has " << this->
parameters_.StartState.rows() <<
" rows, but expected either NX=" << NX <<
" or NQ=" <<
scene_->get_num_positions() <<
", or NX*T=" << NX *
T_);
326 Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(NDX, NDX);
358 Phi.assign(
T_, y_ref_);
376 dxdiff_.assign(
T_, Eigen::MatrixXd::Zero(NDX, NDX));
396 ThrowNamed(
"Invalid number of timesteps: " << T_in);
410 for (
int i = 0; i <
tasks_.size(); ++i)
tasks_[i]->is_used =
false;
418 for (
int i = 0; i < T_; ++i) kinematic_solutions_[i] = std::make_shared<KinematicResponse>(*
scene_->GetKinematicTree().GetKinematicResponse());
420 if (this->
parameters_.WarmStartWithInverseDynamics)
422 for (
int t = 0;
t <
T_ - 1; ++
t)
424 U_.col(
t) =
scene_->GetDynamicsSolver()->InverseDynamics(
X_.col(
t));
449 if (X_in.rows() !=
X_.rows() || X_in.cols() !=
X_.cols())
ThrowPretty(
"Sizes don't match! " <<
X_.rows() <<
"x" <<
X_.cols() <<
" vs " << X_in.rows() <<
"x" << X_in.cols());
453 if (
scene_->get_has_quaternion_floating_base())
455 for (
int t = 0;
t <
T_; ++
t)
475 if (U_in.rows() !=
U_.rows() || U_in.cols() !=
U_.cols())
ThrowPretty(
"Sizes don't match! " <<
U_.rows() <<
"x" <<
U_.cols() <<
" vs " << U_in.rows() <<
"x" << U_in.cols());
486 if (X_star_in.rows() !=
X_star_.rows() || X_star_in.cols() !=
X_star_.cols())
ThrowPretty(
"Sizes don't match! " <<
X_star_.rows() <<
"x" <<
X_star_.cols() <<
" vs " << X_star_in.rows() <<
"x" << X_star_in.cols());
490 if (
scene_->get_has_quaternion_floating_base())
492 for (
int t = 0;
t <
T_; ++
t)
518 if (Q_in.rows() !=
Q_[
t].rows() || Q_in.cols() !=
Q_[
t].cols())
ThrowPretty(
"Dimension mismatch!");
530 if (
t >= (
T_ - 1) ||
t < -1)
532 ThrowPretty(
"Requested t=" <<
t <<
" out of range, needs to be 0 =< t < " <<
T_ - 1);
539 if (u_in.rows() !=
scene_->get_num_controls())
541 ThrowPretty(
"Mismatching in size of control vector: " << u_in.rows() <<
" given, expected: " <<
scene_->get_num_controls());
544 if (x_in.rows() !=
scene_->get_num_positions() +
scene_->get_num_velocities())
546 ThrowPretty(
"Mismatching in size of state vector vector: " << x_in.rows() <<
" given, expected: " <<
scene_->get_num_positions() +
scene_->get_num_velocities());
578 if (
scene_->GetDynamicsSolver()->get_has_state_limits())
580 scene_->GetDynamicsSolver()->ClampToStateLimits(
X_.col(
t + 1));
589 Eigen::VectorXd noise(
scene_->get_num_positions() +
scene_->get_num_velocities());
590 for (
int i = 0; i <
scene_->get_num_positions() +
scene_->get_num_velocities(); ++i)
593 Eigen::VectorXd control_dependent_noise = std::sqrt(
scene_->GetDynamicsSolver()->get_dt()) *
get_F(
t) * noise;
595 for (
int i = 0; i <
scene_->get_num_positions() +
scene_->get_num_velocities(); ++i)
597 Eigen::VectorXd white_noise = std::sqrt(
scene_->GetDynamicsSolver()->get_dt()) *
CW_ * noise;
599 X_.col(
t + 1) =
X_.col(
t + 1) + white_noise + control_dependent_noise;
616 if (
t >= (
T_ - 1) ||
t < -1)
618 ThrowPretty(
"Requested t=" <<
t <<
" out of range, needs to be 0 =< t < " <<
T_ - 1);
632 if (x_in.rows() !=
scene_->get_num_positions() +
scene_->get_num_velocities())
634 ThrowPretty(
"Mismatching in size of state vector vector: " << x_in.rows() <<
" given, expected: " <<
scene_->get_num_positions() +
scene_->get_num_velocities());
668 const Eigen::VectorXd q =
scene_->GetDynamicsSolver()->GetPosition(
x);
740 return state_cost + general_cost;
767 if (
scene_->get_has_quaternion_floating_base())
769 Eigen::RowVectorXd xdiffTQ =
X_diff_.col(
t).transpose() *
Q_[
t];
771 for (
int i = 0; i < ddxdiff.size(); ++i)
795 if (
t >=
T_ - 1 ||
t < -1)
797 ThrowPretty(
"Requested t=" <<
t <<
" out of range, needs to be 0 =< t < " <<
T_ - 1);
810 for (
int iu = 0; iu <
scene_->get_num_controls(); ++iu)
828 if (
t >=
T_ - 1 ||
t < -1)
830 ThrowPretty(
"Requested t=" <<
t <<
" out of range, needs to be 0 =< t < " <<
T_ - 1);
842 cost +=
U_.col(
t).cwiseAbs2().cwiseProduct(
R_.diagonal()).sum();
845 for (
int iu = 0; iu <
scene_->get_num_controls(); ++iu)
860 if (!std::isfinite(
cost))
869 if (
t >=
T_ - 1 ||
t < -1)
871 ThrowPretty(
"Requested t=" <<
t <<
" out of range, needs to be 0 =< t < " <<
T_ - 1);
885 for (
int iu = 0; iu <
scene_->get_num_controls(); ++iu)
905 if (
t >=
T_ - 1 ||
t < -1)
907 ThrowPretty(
"Requested t=" <<
t <<
" out of range, needs to be 0 =< t < " <<
T_ - 1);
910 const int NX =
scene_->get_num_positions() +
scene_->get_num_velocities();
911 Eigen::MatrixXd F(NX, NX);
913 for (
int i = 0; i < NX; ++i)
914 F.col(i) =
Ci_[i] *
U_.col(
t);
922 if (column_idx < 0 || column_idx >=
scene_->get_num_velocities())
923 ThrowPretty(
"Requested column_idx=" << column_idx <<
" out of range; needs to be 0 <= column_idx < " <<
scene_->get_num_velocities() - 1);
924 return Ci_[column_idx];
void Update(Eigen::VectorXdRefConst u, int t)
Eigen::VectorXd ApplyStartState(bool update_traj=true) override
unsigned int number_of_problem_updates_
void ReinitializeVariables(int _T, std::shared_ptr< PlanningProblem > _prob, const TaskSpaceVector &_Phi)
double pseudo_huber_jacobian(double x, double beta)
void AppendVector(std::vector< Val > &orig, const std::vector< Val > &extra)
DynamicTimeIndexedShootingProblemInitializer parameters_
double smooth_l1_hessian(double x, double beta)
KinematicRequestFlags flags_
Eigen::VectorXd smooth_l1_std_
std::vector< Eigen::VectorXd > control_cost_jacobian_
void set_X(Eigen::MatrixXdRefConst X_in)
Sets the state trajectory X (can be used as the initial guess)
double huber_jacobian(double x, double beta)
double control_cost_weight_
Eigen::MatrixXd GetStateCostHessian(int t)
lxx
int length_jacobian
Length of tangent vector to Phi.
std::vector< Eigen::MatrixXd > general_cost_hessian_
std::normal_distribution< double > standard_normal_noise_
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)
virtual ~DynamicTimeIndexedShootingProblem()
void ValidateTimeIndex(int &t_in) const
Checks the desired time index for bounds and supports -1 indexing.
void NormalizeQuaternionInConfigurationVector(Eigen::Ref< Eigen::VectorXd > q)
std::vector< Eigen::VectorXd > general_cost_jacobian_
const Eigen::MatrixXd & get_Q(int t) const
Returns the precision matrix at time step t.
void UpdateMultipleTaskKinematics(std::vector< std::shared_ptr< KinematicResponse >> responses)
void Instantiate(const DynamicTimeIndexedShootingProblemInitializer &init) override
const Eigen::MatrixXd & get_Qf() const
Returns the cost weight matrix at time N.
Eigen::VectorXd start_state_
void set_Q(Eigen::MatrixXdRefConst Q_in, int t)
Sets the precision matrix for time step t.
ControlCostLossTermType loss_type_
void EnableStochasticUpdates()
std::vector< Eigen::MatrixXd > dxdiff_
double smooth_l1_cost(double x, double beta)
void set_X_star(Eigen::MatrixXdRefConst X_star_in)
Sets the target state trajectory X.
double smooth_l1_jacobian(double x, double beta)
Eigen::Array< Eigen::MatrixXd, Eigen::Dynamic, 1 > Hessian
Eigen::MatrixXd X_star_
Goal state trajectory (i.e., positions, velocities). Size: num-states x T.
const Eigen::MatrixXd & get_X() const
Returns the state trajectory X.
DynamicTimeIndexedShootingProblem()
std::vector< Hessian > ddPhi_ddu
Stacked TaskMap Hessian w.r.t. control.
std::vector< Eigen::MatrixXd > dPhi_du
Stacked TaskMap Jacobian w.r.t. control.
Eigen::MatrixXd Qf_
Final state cost.
#define HIGHLIGHT_NAMED(name, x)
std::vector< Eigen::MatrixXd > state_cost_hessian_
const Eigen::MatrixXd & GetControlNoiseJacobian(int column_idx) const
F[i]_u.
Eigen::MatrixXd CW_
White noise covariance.
bool stochastic_matrices_specified_
std::vector< Eigen::MatrixXd > control_cost_hessian_
double GetControlCost(int t) const
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
std::vector< TaskSpaceVector > Phi
Stacked TaskMap vector.
#define REGISTER_PROBLEM_TYPE(TYPE, DERIV)
std::vector< std::shared_ptr< KinematicResponse > > kinematic_solutions_
std::vector< Hessian > ddPhi_dxdu
Stacked TaskMap Hessian w.r.t. state and control.
std::vector< Eigen::MatrixXd > dPhi_dx
void set_Qf(Eigen::MatrixXdRefConst Q_in)
Sets the cost weight matrix for time N.
void SetDefaultQuaternionInConfigurationVector(Eigen::Ref< Eigen::VectorXd > q)
const typedef Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
void set_U(Eigen::MatrixXdRefConst U_in)
Sets the control trajectory U (can be used as the initial guess)
std::vector< TaskVectorEntry > map
const Eigen::MatrixXd & get_R() const
Returns the control weight matrix.
virtual void Initialize(const std::vector< exotica::Initializer > &inits, std::shared_ptr< PlanningProblem > prob, TaskSpaceVector &Phi)
TimeIndexedTask cost
Cost task.
void DisableStochasticUpdates()
Eigen::VectorXd GetControlCostJacobian(int t)
lu
Eigen::VectorXd GetStateCostJacobian(int t)
lx
std::vector< Eigen::MatrixXd > dPhi_dx
Stacked TaskMap Jacobian w.r.t. state.
int length_Phi
Length of TaskSpaceVector (Phi => stacked task-maps)
void UpdateTerminalState(Eigen::VectorXdRefConst x)
Eigen::MatrixXd U_
Control trajectory. Size: num-controls x (T-1)
const Eigen::MatrixXd & get_U() const
Returns the control trajectory U.
std::vector< Eigen::MatrixXd > S
std::vector< Eigen::VectorXd > ydiff
void ReinitializeVariables()
const Eigen::MatrixXd & get_X_star() const
Returns the target state trajectory X.
void PreUpdate() override
std::vector< Hessian > ddPhi_ddx
double tau_
Time step duration.
Eigen::MatrixXd GetControlCostHessian(int t)
luu
Eigen::VectorXd smooth_l1_mean_
virtual Eigen::VectorXd ApplyStartState(bool update_traj=true)
Eigen::MatrixXd X_
State trajectory (i.e., positions, velocities). Size: num-states x T.
void init(const M_string &remappings)
int T_
Number of time steps.
void InstantiateCostTerms(const DynamicTimeIndexedShootingProblemInitializer &init)
double huber_cost(double x, double beta)
std::vector< Eigen::MatrixXd > Q_
State space penalty matrix (precision matrix), per time index.
double pseudo_huber_hessian(double x, double beta)
void set_T(const int &T_in)
Sets the number of timesteps in the state trajectory.
int num_tasks
Number of TaskMaps.
const int & get_T() const
Returns the number of timesteps in the state trajectory.
Eigen::MatrixXd R_
Control space penalty matrix.
double pseudo_huber_cost(double x, double beta)
const double & get_tau() const
Returns the discretization timestep tau.
std::vector< Eigen::VectorXd > state_cost_jacobian_
geometry_msgs::TransformStamped t
Eigen::MatrixXd get_F(int t) const
Returns the noise weight matrix at time t.
Eigen::VectorXd huber_rate_
std::vector< Eigen::MatrixXd > Ci_
Noise weight terms.
double huber_hessian(double x, double beta)
bool stochastic_updates_enabled_
double GetStateCost(int t) const
void UpdateTaskMaps(Eigen::VectorXdRefConst x, Eigen::VectorXdRefConst u, int t)
void SetZero(const int n)
std::vector< Hessian > ddPhi_ddx
Stacked TaskMap Hessian w.r.t. state.
Eigen::MatrixXd X_diff_
Difference between X_ and X_star_. Size: ndx x T.
exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Oct 20 2023 02:59:49