30 #ifndef _XPP_STATES_STATE_H_ 31 #define _XPP_STATES_STATE_H_ 34 #include <Eigen/Dense> 166 : q(_q), w(_w), wd(_wd) {}
208 Vector3d yaw_pitch_roll = q.normalized().toRotationMatrix().eulerAngles(
Z,
Y,
X);
209 return yaw_pitch_roll.reverse();
221 using namespace Eigen;
224 q = AngleAxisd(yaw, Vector3d::UnitZ())
225 * AngleAxisd(pitch, Vector3d::UnitY())
226 * AngleAxisd(roll, Vector3d::UnitX());
236 out <<
"p=" << pos.
p_.transpose() <<
" " 237 <<
"v=" << pos.
v_.transpose() <<
" " 238 <<
"a=" << pos.
a_.transpose();
254 ret.
p_ = mult * rhs.
p_;
255 ret.
v_ = mult * rhs.
v_;
256 ret.
a_ = mult * rhs.
a_;
262 bool all_equal = (
p_==other.
p_ 270 return !(*
this == other);
277 out <<
"rpy=" << rpy_rad.transpose() <<
" " 278 <<
"v=" << ori.
w.transpose() <<
" " 279 <<
"a=" << ori.
wd.transpose();
285 out <<
"\tPos: " << pose.
lin <<
"\n" 286 <<
"\tOri: " << pose.
ang;
292 #endif // _XPP_STATES_STATE_H_
Quaterniond q
orientation expressed as Quaternion.
6D-state (linear+angular) of an object in 3-dimensional space, where the angular part is expressed by...
Represents position, velocity and acceleration in x-dimensions.
std::ostream & operator<<(std::ostream &stream, Endeffectors< T > endeffectors)
const VectorXd GetByIndex(MotionDerivative deriv) const
Read either position, velocity of acceleration by index.
static Quaterniond GetQuaternionFromEulerZYX(double yaw, double pitch, double roll)
Converts an orientation to Quaternion from Euler ZY'X'' convention.
VectorXd a_
position, velocity and acceleration
static Vector3d GetEulerZYXAngles(const Quaterniond &q)
Converts an orientation to Euler ZY'X'' convention.
StateAng3d ang
Quaternion, velocity and acceleration.
Eigen::Quaterniond Quaterniond
StateLinXd operator+(const StateLinXd &lhs, const StateLinXd &rhs)
virtual ~StateLinXd()=default
Vector3d w
angular velocity (omega).
6D-State (linear+angular) of an object in 3-dimensional space, where the angular part is expressed by...
StateLin3d lin
linear position, velocity and acceleration
bool operator==(const StateLinXd &other) const
Returns true if this state has all same pos,vel and acc as other.
int kNumDim
the number of dimenions this state represents.
Angular state of an object in 3-dimensional space.
Eigen::Matrix< double, 6, 1 > Vector6d
StateAng3d(Quaterniond _q=Quaterniond(1.0, 0.0, 0.0, 0.0), Vector3d _w=Vector3d::Zero(), Vector3d _wd=Vector3d::Zero())
Builds an angular state.
bool operator!=(const StateLinXd &other) const
Returns true if just one value in this state differs from other.
Vector3d wd
angular acceleration (omega dot).
StateLinXd operator*(double mult, const StateLinXd &rhs)
StateLin3d ang
roll-pitch-yaw Euler angles, rates- and rate derivatives.
StateLin3d lin
linear position, velocity and acceleration
StateLinXd(int _dim=0)
Constructs an object of dimensions _dim.