euclidean.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 namespace crocoddyl {
10 
11 template <typename Scalar>
12 StateVectorTpl<Scalar>::StateVectorTpl(const std::size_t& nx) : StateAbstractTpl<Scalar>(nx, nx) {}
13 
14 template <typename Scalar>
15 StateVectorTpl<Scalar>::~StateVectorTpl() {}
16 
17 template <typename Scalar>
18 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::zero() const {
19  return MathBase::VectorXs::Zero(nx_);
20 }
21 
22 template <typename Scalar>
23 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::rand() const {
24  return MathBase::VectorXs::Random(nx_);
25 }
26 
27 template <typename Scalar>
28 void StateVectorTpl<Scalar>::diff(const Eigen::Ref<const typename MathBase::VectorXs>& x0,
29  const Eigen::Ref<const typename MathBase::VectorXs>& x1,
30  Eigen::Ref<typename MathBase::VectorXs> dxout) const {
31  if (static_cast<std::size_t>(x0.size()) != nx_) {
32  throw_pretty("Invalid argument: "
33  << "x0 has wrong dimension (it should be " + std::to_string(nx_) + ")");
34  }
35  if (static_cast<std::size_t>(x1.size()) != nx_) {
36  throw_pretty("Invalid argument: "
37  << "x1 has wrong dimension (it should be " + std::to_string(nx_) + ")");
38  }
39  if (static_cast<std::size_t>(dxout.size()) != ndx_) {
40  throw_pretty("Invalid argument: "
41  << "dxout has wrong dimension (it should be " + std::to_string(ndx_) + ")");
42  }
43  dxout = x1 - x0;
44 }
45 
46 template <typename Scalar>
47 void StateVectorTpl<Scalar>::integrate(const Eigen::Ref<const typename MathBase::VectorXs>& x,
48  const Eigen::Ref<const typename MathBase::VectorXs>& dx,
49  Eigen::Ref<typename MathBase::VectorXs> xout) const {
50  if (static_cast<std::size_t>(x.size()) != nx_) {
51  throw_pretty("Invalid argument: "
52  << "x has wrong dimension (it should be " + std::to_string(nx_) + ")");
53  }
54  if (static_cast<std::size_t>(dx.size()) != ndx_) {
55  throw_pretty("Invalid argument: "
56  << "dx has wrong dimension (it should be " + std::to_string(ndx_) + ")");
57  }
58  if (static_cast<std::size_t>(xout.size()) != nx_) {
59  throw_pretty("Invalid argument: "
60  << "xout has wrong dimension (it should be " + std::to_string(nx_) + ")");
61  }
62  xout = x + dx;
63 }
64 
65 template <typename Scalar>
66 void StateVectorTpl<Scalar>::Jdiff(const Eigen::Ref<const typename MathBase::VectorXs>&,
67  const Eigen::Ref<const typename MathBase::VectorXs>&,
68  Eigen::Ref<typename MathBase::MatrixXs> Jfirst,
69  Eigen::Ref<typename MathBase::MatrixXs> Jsecond, Jcomponent firstsecond) const {
70  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
71  if (firstsecond == first || firstsecond == both) {
72  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
73  throw_pretty("Invalid argument: "
74  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
75  ")");
76  }
77  Jfirst.setZero();
78  Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, -1.);
79  }
80  if (firstsecond == second || firstsecond == both) {
81  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
82  throw_pretty("Invalid argument: "
83  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
84  std::to_string(ndx_) + ")");
85  }
86  Jsecond.setZero();
87  Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
88  }
89 }
90 
91 template <typename Scalar>
92 void StateVectorTpl<Scalar>::Jintegrate(const Eigen::Ref<const typename MathBase::VectorXs>&,
93  const Eigen::Ref<const typename MathBase::VectorXs>&,
94  Eigen::Ref<typename MathBase::MatrixXs> Jfirst,
95  Eigen::Ref<typename MathBase::MatrixXs> Jsecond,
96  Jcomponent firstsecond) const {
97  assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
98  if (firstsecond == first || firstsecond == both) {
99  if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
100  throw_pretty("Invalid argument: "
101  << "Jfirst has wrong dimension (it should be " + std::to_string(ndx_) + "," + std::to_string(ndx_) +
102  ")");
103  }
104  Jfirst.setZero();
105  Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
106  }
107  if (firstsecond == second || firstsecond == both) {
108  if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
109  throw_pretty("Invalid argument: "
110  << "Jsecond has wrong dimension (it should be " + std::to_string(ndx_) + "," +
111  std::to_string(ndx_) + ")");
112  }
113  Jsecond.setZero();
114  Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
115  }
116 }
117 
118 } // namespace crocoddyl
Definition: action-base.hxx:11