state.h
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2017, Alexander W. Winkler. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6 
7 * Redistributions of source code must retain the above copyright notice, this
8  list of conditions and the following disclaimer.
9 
10 * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13 
14 * Neither the name of the copyright holder nor the names of its
15  contributors may be used to endorse or promote products derived from
16  this software without specific prior written permission.
17 
18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ******************************************************************************/
29 
30 #ifndef _XPP_STATES_STATE_H_
31 #define _XPP_STATES_STATE_H_
32 
33 #include <iostream>
34 #include <Eigen/Dense>
35 
37 
38 namespace xpp {
39 
40 class StateLin1d;
41 class StateLin2d;
42 class StateLin3d;
43 class StateAng3d;
44 class State3d;
45 class State3dEuler;
46 
49 using Vector6d = Eigen::Matrix<double,6,1>;
52 
53 enum MotionDerivative { kPos=0, kVel, kAcc, kJerk };
54 
58 class StateLinXd {
59 public:
60  VectorXd p_, v_, a_;
61 
68  explicit StateLinXd(int _dim = 0);
69 
78  explicit StateLinXd(const VectorXd& p, const VectorXd& v, const VectorXd& a);
79 
83  StateLinXd(const VectorXd& p);
84  virtual ~StateLinXd() = default;
85 
91  const VectorXd GetByIndex(MotionDerivative deriv) const;
92 
99 
103  bool operator==(const StateLinXd& other) const;
104 
108  bool operator!=(const StateLinXd& other) const;
109 
110  int kNumDim = 0;
111 };
112 
113 
114 // some state classes of explicit dimensions for type safety as well as
115 // conversions from the general base class.
116 class StateLin1d : public StateLinXd {
117 public:
118  StateLin1d() : StateLinXd(1) {};
119  virtual ~StateLin1d() {};
120 };
121 
122 class StateLin2d : public StateLinXd {
123 public:
124  StateLin2d() : StateLinXd(2) {};
125  virtual ~StateLin2d() {};
126 };
127 
128 class StateLin3d : public StateLinXd {
129 public:
130  StateLin3d() : StateLinXd(3) {};
131  StateLin3d(const StateLinXd&);
132  virtual ~StateLin3d() {};
133 
137  StateLin2d Get2D() const;
138 };
139 
140 
147 class StateAng3d {
148 public:
150  Vector3d w;
152 
163  explicit StateAng3d(Quaterniond _q = Quaterniond(1.0, 0.0, 0.0, 0.0),
164  Vector3d _w = Vector3d::Zero(),
165  Vector3d _wd = Vector3d::Zero())
166  : q(_q), w(_w), wd(_wd) {}
167 };
168 
169 
174 class State3d {
175 public:
178 
179  Vector6d Get6dVel() const;
180  Vector6d Get6dAcc() const;
181 };
182 
187 class State3dEuler {
188 public:
189  StateLin3d lin;
191 };
192 
193 
194 
195 // Conversions between Euler angles and Quaternions
206 static Vector3d GetEulerZYXAngles(const Quaterniond& q)
207 {
208  Vector3d yaw_pitch_roll = q.normalized().toRotationMatrix().eulerAngles(Z, Y, X);
209  return yaw_pitch_roll.reverse();
210 }
211 
219 static Quaterniond GetQuaternionFromEulerZYX(double yaw, double pitch, double roll)
220 {
221  using namespace Eigen;
222  Quaterniond q;
223 
224  q = AngleAxisd(yaw, Vector3d::UnitZ())
225  * AngleAxisd(pitch, Vector3d::UnitY())
226  * AngleAxisd(roll, Vector3d::UnitX());
227 
228  return q;
229 }
230 
231 
232 
233 // convenience functions for easy readability
234 inline std::ostream& operator<<(std::ostream& out, const StateLinXd& pos)
235 {
236  out << "p=" << pos.p_.transpose() << " "
237  << "v=" << pos.v_.transpose() << " "
238  << "a=" << pos.a_.transpose();
239  return out;
240 }
241 
242 inline StateLinXd operator+(const StateLinXd& lhs, const StateLinXd& rhs)
243 {
244  StateLinXd ret(lhs.kNumDim);
245  ret.p_ = lhs.p_ + rhs.p_;
246  ret.v_ = lhs.v_ + rhs.v_;
247  ret.a_ = lhs.a_ + rhs.a_;
248  return ret;
249 }
250 
251 inline StateLinXd operator*(double mult, const StateLinXd& rhs)
252 {
253  StateLinXd ret(rhs.kNumDim);
254  ret.p_ = mult * rhs.p_;
255  ret.v_ = mult * rhs.v_;
256  ret.a_ = mult * rhs.a_;
257  return ret;
258 }
259 
260 inline bool StateLinXd::operator==(const StateLinXd &other) const
261 {
262  bool all_equal = (p_==other.p_
263  && v_==other.v_
264  && a_==other.a_);
265  return all_equal;
266 }
267 
268 inline bool StateLinXd::operator!=(const StateLinXd &other) const
269 {
270  return !(*this == other);
271 }
272 
273 inline std::ostream& operator<<(std::ostream& out, const StateAng3d& ori)
274 {
275  Vector3d rpy_rad;
276  rpy_rad = GetEulerZYXAngles(ori.q);
277  out << "rpy=" << rpy_rad.transpose() << " "
278  << "v=" << ori.w.transpose() << " "
279  << "a=" << ori.wd.transpose();
280  return out;
281 }
282 
283 inline std::ostream& operator<<(std::ostream& out, const State3d& pose)
284 {
285  out << "\tPos: " << pose.lin << "\n"
286  << "\tOri: " << pose.ang;
287  return out;
288 }
289 
290 } // namespace xpp
291 
292 #endif // _XPP_STATES_STATE_H_
xpp::StateLinXd::GetByIndex
const VectorXd GetByIndex(MotionDerivative deriv) const
Read either position, velocity of acceleration by index.
Definition: state.cc:88
xpp::StateLin2d::StateLin2d
StateLin2d()
Definition: state.h:151
xpp::kAcc
@ kAcc
Definition: state.h:80
xpp::StateLin1d::StateLin1d
StateLin1d()
Definition: state.h:145
xpp::operator+
StateLinXd operator+(const StateLinXd &lhs, const StateLinXd &rhs)
Definition: state.h:269
xpp::VectorXd
Eigen::VectorXd VectorXd
Definition: state.h:77
xpp::State3d
6D-State (linear+angular) of an object in 3-dimensional space, where the angular part is expressed by...
Definition: state.h:201
xpp::operator<<
std::ostream & operator<<(std::ostream &stream, Endeffectors< T > endeffectors)
Definition: endeffectors.h:284
xpp::State3d::lin
StateLin3d lin
linear position, velocity and acceleration
Definition: state.h:203
xpp::StateLinXd::a_
VectorXd a_
position, velocity and acceleration
Definition: state.h:87
xpp::State3dEuler
6D-state (linear+angular) of an object in 3-dimensional space, where the angular part is expressed by...
Definition: state.h:214
xpp::StateAng3d
Angular state of an object in 3-dimensional space.
Definition: state.h:174
xpp::Y
@ Y
Definition: cartesian_declarations.h:49
xpp::kJerk
@ kJerk
Definition: state.h:80
xpp::StateLin3d
Definition: state.h:155
xpp::StateLinXd::p_
VectorXd p_
Definition: state.h:87
xpp::Z
@ Z
Definition: cartesian_declarations.h:49
xpp::State3dEuler::lin
StateLin3d lin
linear position, velocity and acceleration
Definition: state.h:216
xpp::MotionDerivative
MotionDerivative
Definition: state.h:80
cartesian_declarations.h
xpp::StateAng3d::w
Vector3d w
angular velocity (omega).
Definition: state.h:177
xpp::GetEulerZYXAngles
static Vector3d GetEulerZYXAngles(const Quaterniond &q)
Converts an orientation to Euler ZY'X'' convention.
Definition: state.h:233
xpp::StateLinXd::operator==
bool operator==(const StateLinXd &other) const
Returns true if this state has all same pos,vel and acc as other.
Definition: state.h:287
xpp::StateLin3d::Get2D
StateLin2d Get2D() const
Extracts only the 2-dimensional part (x,y) from this 3-D state.
Definition: state.cc:119
xpp::State3d::Get6dVel
Vector6d Get6dVel() const
Definition: state.cc:129
xpp::State3d::Get6dAcc
Vector6d Get6dAcc() const
Definition: state.cc:138
xpp::X
@ X
Definition: cartesian_declarations.h:49
xpp::StateLin3d::StateLin3d
StateLin3d()
Definition: state.h:157
xpp::kPos
@ kPos
Definition: state.h:80
xpp::kVel
@ kVel
Definition: state.h:80
xpp::State3d::ang
StateAng3d ang
Quaternion, velocity and acceleration.
Definition: state.h:204
xpp::State3dEuler::ang
StateLin3d ang
roll-pitch-yaw Euler angles, rates- and rate derivatives.
Definition: state.h:217
xpp::StateLinXd::operator!=
bool operator!=(const StateLinXd &other) const
Returns true if just one value in this state differs from other.
Definition: state.h:295
xpp
Definition: cartesian_declarations.h:41
xpp::operator*
StateLinXd operator*(double mult, const StateLinXd &rhs)
Definition: state.h:278
xpp::StateLinXd::~StateLinXd
virtual ~StateLinXd()=default
xpp::StateLin2d
Definition: state.h:149
xpp::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: state.h:76
xpp::GetQuaternionFromEulerZYX
static Quaterniond GetQuaternionFromEulerZYX(double yaw, double pitch, double roll)
Converts an orientation to Quaternion from Euler ZY'X'' convention.
Definition: state.h:246
xpp::StateLinXd
Represents position, velocity and acceleration in x-dimensions.
Definition: state.h:85
xpp::StateAng3d::StateAng3d
StateAng3d(Quaterniond _q=Quaterniond(1.0, 0.0, 0.0, 0.0), Vector3d _w=Vector3d::Zero(), Vector3d _wd=Vector3d::Zero())
Builds an angular state.
Definition: state.h:190
xpp::StateLinXd::StateLinXd
StateLinXd(int _dim=0)
Constructs an object of dimensions _dim.
Definition: state.cc:63
xpp::Quaterniond
Eigen::Quaterniond Quaterniond
Definition: state.h:78
xpp::StateLin1d::~StateLin1d
virtual ~StateLin1d()
Definition: state.h:146
xpp::StateLinXd::v_
VectorXd v_
Definition: state.h:87
xpp::StateAng3d::wd
Vector3d wd
angular acceleration (omega dot).
Definition: state.h:178
xpp::StateLin3d::~StateLin3d
virtual ~StateLin3d()
Definition: state.h:159
xpp::StateLin2d::~StateLin2d
virtual ~StateLin2d()
Definition: state.h:152
xpp::StateAng3d::q
Quaterniond q
orientation expressed as Quaternion.
Definition: state.h:176
xpp::StateLinXd::kNumDim
int kNumDim
the number of dimenions this state represents.
Definition: state.h:137
xpp::StateLin1d
Definition: state.h:143
xpp::Vector2d
Eigen::Vector2d Vector2d
Definition: state.h:74
xpp::Vector3d
Eigen::Vector3d Vector3d
Definition: state.h:75


xpp_states
Author(s): Alexander W. Winkler
autogenerated on Wed Mar 2 2022 01:14:14