diff --git a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp index b89b63facabc17e7b73114c3bb56a22ce074cc18..d6f2c24499f884d04ba118bbde74afe2a28311c9 100644 --- a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp +++ b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp @@ -122,6 +122,14 @@ namespace armarx this->init(q); } + Quaternion::Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz) + { + this->qw = qw; + this->qx = qx; + this->qy = qy; + this->qz = qz; + } + Matrix3f Quaternion::toEigen() const { Matrix3f rot; diff --git a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h index 34abe0f16a5b33f9dcd76ebc670dee7a623f67c3..98131676c2b475f35a706c69860543664c1bd608 100644 --- a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h +++ b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h @@ -133,6 +133,7 @@ namespace armarx Quaternion(const Eigen::Matrix4f &); Quaternion(const Eigen::Matrix3f &); Quaternion(const Eigen::Quaternionf &); + Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz); Eigen::Matrix3f toEigen() const; Eigen::Matrix3f slerp(float, const Eigen::Matrix3f &);