CartesianPose.h

Go to the documentation of this file.

/* ============================================================
 *
 * This file is a part of RCI project
 *
 * Copyright (C) 2011 by Arne Nordmann <anordman at cor-lab dot uni-bielefeld dot de>
 *
 * This file may be licensed under the terms of the
 * GNU Lesser General Public License Version 3 (the ``LGPL''),
 * or (at your option) any later version.
 *
 * Software distributed under the License is distributed
 * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
 * express or implied. See the LGPL for the specific language
 * governing rights and limitations.
 *
 * You should have received a copy of the LGPL along with this
 * program. If not, go to http://www.gnu.org/licenses/lgpl.html
 * or write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The development of this software was supported by:
 *   CoR-Lab, Research Institute for Cognition and Robotics
 *     Bielefeld University
 *
 * ============================================================ */

#pragma once

#include <iostream>
#include <sstream>

#include <rsc/math/MathUtils.h>

#include <nemo/Matrix.h>

#include "rci/dto/CartesianValues.h"

namespace rci {

class Pose;
typedef boost::shared_ptr<Pose> PosePtr;
class Orientation;
typedef boost::shared_ptr<Orientation> OrientationPtr;
class Translation;
typedef boost::shared_ptr<Translation> TranslationPtr;

class CartesianVelocity;
typedef boost::shared_ptr<CartesianVelocity> CartesianVelocityPtr;
class CartesianAcceleration;
typedef boost::shared_ptr<CartesianAcceleration> CartesianAccelerationPtr;

class Translation: public CartesianTranslational {

public:
    Translation();
    Translation(double valx, double valy, double valz);
    Translation(const nemo::RealVector& values);
    Translation(const Translation& translation);
    Translation(const TranslationPtr& translation);

    static TranslationPtr fromMeters(double x, double y, double z);

    virtual std::string print() const;
};

class Orientation: public CartesianRotational {

public:
    Orientation();
    Orientation(double q0, double q1, double q2, double q3);
    Orientation(const nemo::RealVector& values);
    Orientation(const Orientation& orientation);
    Orientation(const OrientationPtr& orientation);
    Orientation(const ::boost::math::quaternion<double>& quaternion);
    Orientation(double rxx, double rxy, double rxz, double ryx, double ryy,
            double ryz, double rzx, double rzy, double rzz);

    static OrientationPtr
    fromEulerAngles(double rotX, double rotY, double rotZ);

    static OrientationPtr
    fromAxisAngle(double angle, double vexX, double vecY, double vecZ);

    static OrientationPtr
    fromCompactAxisAngle(double x, double y, double z);

    static OrientationPtr
    fromQuaternion(double q0, double q1, double q2, double q3);

    static OrientationPtr
    fromQuaternion(const ::boost::math::quaternion<double>& quaternion);

    static OrientationPtr
    fromRotationMatrix(double rxx, double rxy, double rxz, double ryx,
            double ryy, double ryz, double rzx, double rzy, double rzz);
    virtual OrientationPtr rotateBy(const OrientationPtr& rot) const;

    virtual std::string print() const;
};

class Pose: public CartesianValue {

public:
    Pose();
    Pose(double valx, double valy, double valz, double vala, double valb,
            double valc, double vald);
    Pose(double valx, double valy, double valz, double a, double b,
            double c);
    Pose(const nemo::RealVector& values);
    Pose(const Pose& pose);
    Pose(const TranslationPtr& transl, const OrientationPtr& rot);

    virtual TranslationPtr getTranslation() const;

    virtual OrientationPtr getOrientation() const;

    virtual double x() const;

    virtual double y() const;

    virtual double z() const;

    virtual double q0() const;

    virtual double q1() const;

    virtual double q2() const;

    virtual double q3() const;

    virtual double a() const;

    virtual double b() const;

    virtual double c() const;

    static PosePtr fromMetersAndRadians(double x, double y, double z, double q0,
            double q1, double q2, double q3);
    static PosePtr fromMetersAndRadians(double x, double y, double z, double a,
            double b, double c);

    static PosePtr fromMetersAndRadiansHomogeneous(double rxx, double rxy,
            double rxz, double tx, double ryx, double ryy, double ryz,
            double ty, double rzx, double rzy, double rzz, double tz);

    static PosePtr fromMetersAndRadiansHomogeneous(
            const nemo::RealMatrix& homogeneous);

    virtual std::string print() const;
};

}