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 
54 
58 class StateLinXd {
59 public:
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:
119  virtual ~StateLin1d() {};
120 };
121 
122 class StateLin2d : public StateLinXd {
123 public:
125  virtual ~StateLin2d() {};
126 };
127 
128 class StateLin3d : public StateLinXd {
129 public:
131  StateLin3d(const StateLinXd&);
132  virtual ~StateLin3d() {};
133 
137  StateLin2d Get2D() const;
138 };
139 
140 
147 class StateAng3d {
148 public:
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 
188 public:
191 };
192 
193 
194 
195 // Conversions between Euler angles and Quaternions
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_
Quaterniond q
orientation expressed as Quaternion.
Definition: state.h:149
Eigen::Vector2d Vector2d
Definition: state.h:47
6D-state (linear+angular) of an object in 3-dimensional space, where the angular part is expressed by...
Definition: state.h:187
Represents position, velocity and acceleration in x-dimensions.
Definition: state.h:58
VectorXd v_
Definition: state.h:60
virtual ~StateLin3d()
Definition: state.h:132
std::ostream & operator<<(std::ostream &stream, Endeffectors< T > endeffectors)
Definition: endeffectors.h:257
virtual ~StateLin2d()
Definition: state.h:125
const VectorXd GetByIndex(MotionDerivative deriv) const
Read either position, velocity of acceleration by index.
Definition: state.cc:61
static Quaterniond GetQuaternionFromEulerZYX(double yaw, double pitch, double roll)
Converts an orientation to Quaternion from Euler ZY&#39;X&#39;&#39; convention.
Definition: state.h:219
VectorXd a_
position, velocity and acceleration
Definition: state.h:60
Eigen::Vector3d Vector3d
Definition: state.h:48
static Vector3d GetEulerZYXAngles(const Quaterniond &q)
Converts an orientation to Euler ZY&#39;X&#39;&#39; convention.
Definition: state.h:206
StateAng3d ang
Quaternion, velocity and acceleration.
Definition: state.h:177
Eigen::Quaterniond Quaterniond
Definition: state.h:51
StateLinXd operator+(const StateLinXd &lhs, const StateLinXd &rhs)
Definition: state.h:242
VectorXd p_
Definition: state.h:60
virtual ~StateLinXd()=default
Vector3d w
angular velocity (omega).
Definition: state.h:150
MotionDerivative
Definition: state.h:53
6D-State (linear+angular) of an object in 3-dimensional space, where the angular part is expressed by...
Definition: state.h:174
StateLin3d lin
linear position, velocity and acceleration
Definition: state.h:176
bool operator==(const StateLinXd &other) const
Returns true if this state has all same pos,vel and acc as other.
Definition: state.h:260
int kNumDim
the number of dimenions this state represents.
Definition: state.h:110
Angular state of an object in 3-dimensional space.
Definition: state.h:147
virtual ~StateLin1d()
Definition: state.h:119
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: state.h:49
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:163
bool operator!=(const StateLinXd &other) const
Returns true if just one value in this state differs from other.
Definition: state.h:268
Vector3d wd
angular acceleration (omega dot).
Definition: state.h:151
StateLinXd operator*(double mult, const StateLinXd &rhs)
Definition: state.h:251
StateLin3d ang
roll-pitch-yaw Euler angles, rates- and rate derivatives.
Definition: state.h:190
Eigen::VectorXd VectorXd
Definition: state.h:50
StateLin3d lin
linear position, velocity and acceleration
Definition: state.h:189
StateLinXd(int _dim=0)
Constructs an object of dimensions _dim.
Definition: state.cc:36


xpp_states
Author(s): Alexander W. Winkler
autogenerated on Tue Mar 1 2022 00:07:15