state.h

Go to the documentation of this file.

/******************************************************************************
Copyright (c) 2017, Alexander W. Winkler. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#ifndef _XPP_STATES_STATE_H_
#define _XPP_STATES_STATE_H_

#include <iostream>
#include <Eigen/Dense>

#include <xpp_states/cartesian_declarations.h>

namespace xpp {

class StateLin1d;
class StateLin2d;
class StateLin3d;
class StateAng3d;
class State3d;
class State3dEuler;

using Vector2d    = Eigen::Vector2d;
using Vector3d    = Eigen::Vector3d;
using Vector6d    = Eigen::Matrix<double,6,1>;
using VectorXd    = Eigen::VectorXd;
using Quaterniond = Eigen::Quaterniond;

enum MotionDerivative { kPos=0, kVel, kAcc, kJerk };

class StateLinXd {
public:
  VectorXd p_, v_, a_; 

  explicit StateLinXd(int _dim = 0);

  explicit StateLinXd(const VectorXd& p, const VectorXd& v, const VectorXd& a);

  StateLinXd(const VectorXd& p);
  virtual ~StateLinXd() = default;

  const VectorXd GetByIndex(MotionDerivative deriv) const;

  VectorXd& GetByIndex(MotionDerivative deriv);

  bool operator==(const StateLinXd& other) const;

  bool operator!=(const StateLinXd& other) const;

  int kNumDim = 0;  
};


// some state classes of explicit dimensions for type safety as well as
// conversions from the general base class.
class StateLin1d  : public StateLinXd {
public:
  StateLin1d() : StateLinXd(1) {};
  virtual ~StateLin1d() {};
};

class StateLin2d  : public StateLinXd {
public:
  StateLin2d() : StateLinXd(2) {};
  virtual ~StateLin2d() {};
};

class StateLin3d : public StateLinXd {
public:
  StateLin3d() : StateLinXd(3) {};
  StateLin3d(const StateLinXd&);
  virtual ~StateLin3d() {};

  StateLin2d Get2D() const;
};


class StateAng3d {
public:
  Quaterniond q;  
  Vector3d    w;  
  Vector3d   wd;  

  explicit StateAng3d(Quaterniond _q = Quaterniond(1.0, 0.0, 0.0, 0.0),
                      Vector3d   _w  = Vector3d::Zero(),
                      Vector3d   _wd = Vector3d::Zero())
  : q(_q), w(_w), wd(_wd) {}
};


class State3d {
public:
  StateLin3d lin; 
  StateAng3d ang; 

  Vector6d Get6dVel() const;
  Vector6d Get6dAcc() const;
};

class State3dEuler {
public:
  StateLin3d lin; 
  StateLin3d ang; 
};



// Conversions between Euler angles and Quaternions
static Vector3d GetEulerZYXAngles(const Quaterniond& q)
{
  Vector3d yaw_pitch_roll = q.normalized().toRotationMatrix().eulerAngles(Z, Y, X);
  return yaw_pitch_roll.reverse();
}

static Quaterniond GetQuaternionFromEulerZYX(double yaw, double pitch, double roll)
{
  using namespace Eigen;
  Quaterniond q;

  q =   AngleAxisd(yaw,   Vector3d::UnitZ())
      * AngleAxisd(pitch, Vector3d::UnitY())
      * AngleAxisd(roll,  Vector3d::UnitX());

  return q;
}



// convenience functions for easy readability
inline std::ostream& operator<<(std::ostream& out, const StateLinXd& pos)
{
  out << "p=" << pos.p_.transpose() << "  "
      << "v=" << pos.v_.transpose() << "  "
      << "a=" << pos.a_.transpose();
  return out;
}

inline StateLinXd operator+(const StateLinXd& lhs, const StateLinXd& rhs)
{
  StateLinXd ret(lhs.kNumDim);
  ret.p_ = lhs.p_ + rhs.p_;
  ret.v_ = lhs.v_ + rhs.v_;
  ret.a_ = lhs.a_ + rhs.a_;
  return ret;
}

inline StateLinXd operator*(double mult, const StateLinXd& rhs)
{
  StateLinXd ret(rhs.kNumDim);
  ret.p_ = mult * rhs.p_;
  ret.v_ = mult * rhs.v_;
  ret.a_ = mult * rhs.a_;
  return ret;
}

inline bool StateLinXd::operator==(const StateLinXd &other) const
{
  bool all_equal = (p_==other.p_
                 && v_==other.v_
                 && a_==other.a_);
  return all_equal;
}

inline bool StateLinXd::operator!=(const StateLinXd &other) const
{
  return !(*this == other);
}

inline std::ostream& operator<<(std::ostream& out, const StateAng3d& ori)
{
  Vector3d rpy_rad;
  rpy_rad = GetEulerZYXAngles(ori.q);
  out << "rpy=" << rpy_rad.transpose() << "  "
      << "v="   << ori.w.transpose() << "  "
      << "a="   << ori.wd.transpose();
  return out;
}

inline std::ostream& operator<<(std::ostream& out, const State3d& pose)
{
  out << "\tPos: " << pose.lin << "\n"
      << "\tOri: " << pose.ang;
  return out;
}

} // namespace xpp

#endif // _XPP_STATES_STATE_H_