CartesianPose.cpp

Go to the documentation of this file.
00001 /* ============================================================
00002  *
00003  * This file is a part of RCI project
00004  *
00005  * Copyright (C) 2011 by Arne Nordmann <anordman at cor-lab dot uni-bielefeld dot de>
00006  *
00007  * This file may be licensed under the terms of the
00008  * GNU Lesser General Public License Version 3 (the ``LGPL''),
00009  * or (at your option) any later version.
00010  *
00011  * Software distributed under the License is distributed
00012  * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
00013  * express or implied. See the LGPL for the specific language
00014  * governing rights and limitations.
00015  *
00016  * You should have received a copy of the LGPL along with this
00017  * program. If not, go to http://www.gnu.org/licenses/lgpl.html
00018  * or write to the Free Software Foundation, Inc.,
00019  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
00020  *
00021  * The development of this software was supported by:
00022  *   CoR-Lab, Research Institute for Cognition and Robotics
00023  *     Bielefeld University
00024  *
00025  * ============================================================ */
00026 
00027 #include "CartesianPose.h"
00028 
00029 #include <math.h>
00030 
00031 #include <boost/math/quaternion.hpp>
00032 
00033 using namespace std;
00034 using namespace nemo;
00035 using namespace rsc::math;
00036 
00037 namespace rci {
00038 
00039 Translation::Translation() :
00040         CartesianTranslational() {
00041 }
00042 
00043 Translation::Translation(double valx, double valy, double valz) :
00044         CartesianTranslational(valx, valy, valz) {
00045 }
00046 
00047 Translation::Translation(const Translation& transl) :
00048         CartesianTranslational(transl) {
00049 }
00050 
00051 Translation::Translation(const TranslationPtr& transl) :
00052         CartesianTranslational(transl) {
00053 }
00054 
00055 TranslationPtr Translation::fromMeters(double x, double y, double z) {
00056     return TranslationPtr(new Translation(x, y, z));
00057 }
00058 
00059 Translation::Translation(const RealVector& values) :
00060         CartesianTranslational(values) {
00061 }
00062 
00063 std::string Translation::print() const {
00064     ostringstream outstream(ostringstream::out);
00065     outstream.precision(3); // Precision when printing double values
00066     outstream << "Translation<x=" << this->asDouble(0) << ",y="
00067             << this->asDouble(1) << ",z=" << this->asDouble(2) << ">(m)"
00068             << std::endl;
00069     return outstream.str();
00070 }
00071 
00072 Orientation::Orientation() :
00073         CartesianRotational() {
00074 }
00075 
00076 Orientation::Orientation(double vala, double valb, double valc, double vald) :
00077         CartesianRotational(vala, valb, valc, vald) {
00078 }
00079 
00080 Orientation::Orientation(const RealVector& values) :
00081         CartesianRotational(values) {
00082 }
00083 
00084 Orientation::Orientation(const Orientation& orient) :
00085         CartesianRotational(orient) {
00086 }
00087 
00088 Orientation::Orientation(const OrientationPtr& orient) :
00089         CartesianRotational(orient) {
00090 }
00091 
00092 Orientation::Orientation(const ::boost::math::quaternion<double>& quaternion) :
00093         CartesianRotational(quaternion) {
00094 }
00095 
00096 Orientation::Orientation(double rxx, double rxy, double rxz, double ryx,
00097         double ryy, double ryz, double rzx, double rzy, double rzz) :
00098         CartesianRotational(rxx, rxy, rxz, ryx, ryy, ryz, rzx, rzy, rzz) {
00099 }
00100 
00101 OrientationPtr Orientation::rotateBy(const OrientationPtr& rot) const {
00102     return Orientation::fromQuaternion(this->_quaternion * rot->asQuaternion());
00103 }
00104 
00105 std::string Orientation::print() const {
00106     ostringstream outstream(ostringstream::out);
00107     outstream.precision(3); // Precision when printing double values
00108     outstream << "Orientation<q0=" << this->q0() << ",q1=" << this->q1()
00109             << ",q2=" << this->q2() << ",q3=" << this->q3() << ">" << std::endl;
00110     return outstream.str();
00111 }
00112 
00113 OrientationPtr Orientation::fromEulerAngles(double rotX, double rotY,
00114         double rotZ) {
00115     double q0 = cos(rotX / 2) * cos(rotY / 2) * cos(rotZ / 2)
00116             + sin(rotX / 2) * sin(rotY / 2) * sin(rotZ / 2);
00117     double q1 = sin(rotX / 2) * cos(rotY / 2) * cos(rotZ / 2)
00118             - cos(rotX / 2) * sin(rotY / 2) * sin(rotZ / 2);
00119     double q2 = cos(rotX / 2) * sin(rotY / 2) * cos(rotZ / 2)
00120             + sin(rotX / 2) * cos(rotY / 2) * sin(rotZ / 2);
00121     double q3 = cos(rotX / 2) * cos(rotY / 2) * sin(rotZ / 2)
00122             - sin(rotX / 2) * sin(rotY / 2) * cos(rotZ / 2);
00123 
00124     return OrientationPtr(new Orientation(q0, q1, q2, q3));
00125 }
00126 
00127 OrientationPtr Orientation::fromAxisAngle(double angle, double vecX,
00128         double vecY, double vecZ) {
00129 
00130     if (fabs(angle) < epsilon()) {
00131         // Angle is zero, direction vector has no effect
00132         return OrientationPtr(new Orientation(1.0, 0.0, 0.0, 0.0));
00133     }
00134 
00135     double qx = vecX * sin(angle / 2);
00136     double qy = vecY * sin(angle / 2);
00137     double qz = vecZ * sin(angle / 2);
00138     double qw = cos(angle / 2);
00139 
00140     return OrientationPtr(new Orientation(qw, qx, qy, qz));
00141 }
00142 
00143 OrientationPtr Orientation::fromCompactAxisAngle(double aaX, double aaY,
00144         double aaZ) {
00145     double angle = sqrt(aaX * aaX + aaY * aaY + aaZ * aaZ);
00146     return Orientation::fromAxisAngle(angle, aaX / angle, aaY / angle,
00147             aaZ / angle);
00148 }
00149 
00150 OrientationPtr Orientation::fromQuaternion(double q0, double q1, double q2,
00151         double q3) {
00152     ::boost::math::quaternion<double> quaternion(q0, q1, q2, q3);
00153     return OrientationPtr(new Orientation(quaternion));
00154 }
00155 
00156 OrientationPtr Orientation::fromQuaternion(
00157         const ::boost::math::quaternion<double>& quaternion) {
00158     return OrientationPtr(new Orientation(quaternion));
00159 }
00160 
00161 OrientationPtr Orientation::fromRotationMatrix(double rxx, double rxy,
00162         double rxz, double ryx, double ryy, double ryz, double rzx, double rzy,
00163         double rzz) {
00164     return OrientationPtr(
00165             new Orientation(rxx, rxy, rxz, ryx, ryy, ryz, rzx, rzy, rzz));
00166 }
00167 
00168 Pose::Pose() :
00169         CartesianValue() {
00170 }
00171 
00172 Pose::Pose(double valx, double valy, double valz, double vala, double valb,
00173         double valc, double vald) :
00174         CartesianValue(valx, valy, valz, vala, valb, valc, vald) {
00175 }
00176 
00177 Pose::Pose(double valx, double valy, double valz, double vala, double valb,
00178         double valc) :
00179         CartesianValue(
00180                 CartesianTranslationalPtr(
00181                         new CartesianTranslational(valx, valy, valz)),
00182                 CartesianRotationalPtr(
00183                         new CartesianRotational(vala, valb, valc))) {
00184 }
00185 
00186 Pose::Pose(const Pose& pose) :
00187         CartesianValue(pose) {
00188 }
00189 
00190 Pose::Pose(const RealVector& values) :
00191         CartesianValue(values) {
00192 }
00193 
00194 Pose::Pose(const TranslationPtr& transl, const OrientationPtr& rot) :
00195         CartesianValue(transl, rot) {
00196 }
00197 
00198 TranslationPtr Pose::getTranslation() const {
00199     return Translation::fromMeters(this->x(), this->y(), this->z());
00200 }
00201 
00202 OrientationPtr Pose::getOrientation() const {
00203     return Orientation::fromQuaternion(this->q0(), this->q1(), this->q2(),
00204             this->q3());
00205 }
00206 
00207 double Pose::x() const {
00208     return this->_transl.x();
00209 }
00210 
00211 double Pose::y() const {
00212     return this->_transl.y();
00213 }
00214 
00215 double Pose::z() const {
00216     return this->_transl.z();
00217 }
00218 
00219 double Pose::a() const {
00220     return this->_rot.a();
00221 }
00222 
00223 double Pose::b() const {
00224     return this->_rot.b();
00225 }
00226 
00227 double Pose::c() const {
00228     return this->_rot.c();
00229 }
00230 
00231 double Pose::q0() const {
00232     return this->_rot.q0();
00233 }
00234 
00235 double Pose::q1() const {
00236     return this->_rot.q1();
00237 }
00238 
00239 double Pose::q2() const {
00240     return this->_rot.q2();
00241 }
00242 
00243 double Pose::q3() const {
00244     return this->_rot.q3();
00245 }
00246 
00247 PosePtr Pose::fromMetersAndRadians(double x, double y, double z, double q0,
00248         double q1, double q2, double q3) {
00249     PosePtr pose(new Pose(x, y, z, q0, q1, q2, q3));
00250     return pose;
00251 }
00252 
00253 PosePtr Pose::fromMetersAndRadians(double x, double y, double z, double a,
00254         double b, double c) {
00255     TranslationPtr transl = Translation::fromMeters(x, y, z);
00256     OrientationPtr rot = boost::static_pointer_cast<Orientation>(
00257             Orientation::fromEulerAngles(a, b, c));
00258     return PosePtr(new Pose(transl, rot));
00259 }
00260 
00261 PosePtr Pose::fromMetersAndRadiansHomogeneous(double rxx, double rxy,
00262         double rxz, double tx, double ryx, double ryy, double ryz, double ty,
00263         double rzx, double rzy, double rzz, double tz) {
00264     TranslationPtr transl = Translation::fromMeters(tx, ty, tz);
00265     OrientationPtr rot = boost::static_pointer_cast<Orientation>(
00266             Orientation::fromRotationMatrix(rxx, rxy, rxz, ryx, ryy, ryz, rzx,
00267                     rzy, rzz));
00268     return PosePtr(new Pose(transl, rot));
00269 }
00270 
00271 PosePtr Pose::fromMetersAndRadiansHomogeneous(const RealMatrix& homogeneous) {
00272     return Pose::fromMetersAndRadiansHomogeneous(homogeneous(0, 0),
00273             homogeneous(0, 1), homogeneous(0, 2), homogeneous(0, 3),
00274             homogeneous(1, 0), homogeneous(1, 1), homogeneous(1, 2),
00275             homogeneous(1, 3), homogeneous(2, 0), homogeneous(2, 1),
00276             homogeneous(2, 2), homogeneous(2, 3));
00277 }
00278 
00279 std::string Pose::print() const {
00280     ostringstream outstream(ostringstream::out);
00281     outstream.precision(3); // Precision when printing double values
00282     outstream << "Pose<" << this->_transl << " , " << this->_rot << ">";
00283     return outstream.str();
00284 }
00285 
00286 }
Generated on Thu Aug 2 14:02:47 2012 for RCI by  doxygen 1.6.3