CartesianValues.cpp

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
 *
 * ============================================================ */

#include "CartesianValues.h"

#include <iostream>
#include <sstream>

#include "boost/quaterion_conversions.h"

using namespace nemo;
using namespace boost;
using namespace boost::math;

namespace rci {

CartesianTranslational::CartesianTranslational() :
        DataTransferObject(), ContainingDoubles(3, 0.0) {
}

CartesianTranslational::~CartesianTranslational() {
}

CartesianTranslational::CartesianTranslational(double valx, double valy,
        double valz) :
        DataTransferObject(), ContainingDoubles(3, 0.0) {
    this->_values[0] = valx;
    this->_values[1] = valy;
    this->_values[2] = valz;
}

CartesianTranslational::CartesianTranslational(
        const CartesianTranslational& transl) :
        DataTransferObject(), ContainingDoubles(3, 0.0) {
    this->_values[0] = transl.asDouble(0);
    this->_values[1] = transl.asDouble(1);
    this->_values[2] = transl.asDouble(2);
}

CartesianTranslational::CartesianTranslational(
        const CartesianTranslationalPtr& transl) :
        DataTransferObject(), ContainingDoubles(3, 0.0) {
    this->_values[0] = transl->asDouble(0);
    this->_values[1] = transl->asDouble(1);
    this->_values[2] = transl->asDouble(2);
}

CartesianTranslational::CartesianTranslational(const RealVector& values) :
        DataTransferObject(), ContainingDoubles(values) {
    if (values.dimension() != 3) {
        throw std::invalid_argument("Wrong number of values given (need 3).");
    }
}

double CartesianTranslational::x() const {
    return this->asDouble(0);
}

double CartesianTranslational::y() const {
    return this->asDouble(1);
}

double CartesianTranslational::z() const {
    return this->asDouble(2);
}

std::string CartesianTranslational::print() const {
    std::ostringstream outstream(std::ostringstream::out);
    outstream.precision(3); // Precision when printing double values
    outstream << "Translation<x=" << this->x() << ",y=" << this->y() << ",z="
            << this->z() << ">(m)";
    return outstream.str();
}

CartesianRotational::CartesianRotational() :
        DataTransferObject(), ContainingDoubles(4, 0.0), _quaternion() {
}

CartesianRotational::~CartesianRotational() {
}

CartesianRotational::CartesianRotational(double rotX, double rotY, double rotZ) :
        DataTransferObject(), ContainingDoubles(4, 0.0), _quaternion() {
    double qq0 = cos(rotX / 2) * cos(rotY / 2) * cos(rotZ / 2)
            + sin(rotX / 2) * sin(rotY / 2) * sin(rotZ / 2);
    double qq1 = sin(rotX / 2) * cos(rotY / 2) * cos(rotZ / 2)
            - cos(rotX / 2) * sin(rotY / 2) * sin(rotZ / 2);
    double qq2 = cos(rotX / 2) * sin(rotY / 2) * cos(rotZ / 2)
            + sin(rotX / 2) * cos(rotY / 2) * sin(rotZ / 2);
    double qq3 = cos(rotX / 2) * cos(rotY / 2) * sin(rotZ / 2)
            - sin(rotX / 2) * sin(rotY / 2) * cos(rotZ / 2);

    _quaternion = quaternion<double>(qq0, qq1, qq2, qq3);
}

CartesianRotational::CartesianRotational(double vala, double valb, double valc,
        double vald) :
        DataTransferObject(), ContainingDoubles(4, 0.0), _quaternion(vala, valb,
                valc, vald) {
}

CartesianRotational::CartesianRotational(const RealVector& values) :
        DataTransferObject(), ContainingDoubles(4, 0.0), _quaternion(values[0],
                values[1], values[2], values[3]) {
    if (values.dimension() != 4) {
        throw std::invalid_argument("Wrong number of values given (need 4).");
    }
}

CartesianRotational::CartesianRotational(
        const ::boost::math::quaternion<double> quaternion) :
        DataTransferObject(), ContainingDoubles(4, 0.0), _quaternion(quaternion) {
}

CartesianRotational::CartesianRotational(const CartesianRotational& rot) :
        DataTransferObject(), ContainingDoubles(4, 0.0), _quaternion(
                rot.asDouble(0), rot.asDouble(1), rot.asDouble(2),
                rot.asDouble(3)) {
}

CartesianRotational::CartesianRotational(const CartesianRotationalPtr& rot) :
        DataTransferObject(), ContainingDoubles(4, 0.0), _quaternion(
                rot->asDouble(0), rot->asDouble(1), rot->asDouble(2),
                rot->asDouble(3)) {
}

CartesianRotational::CartesianRotational(double rxx, double rxy, double rxz,
        double ryx, double ryy, double ryz, double rzx, double rzy, double rzz) :
        _quaternion() {

    R3_matrix<double> tmp;
    tmp.a11 = rxx;
    tmp.a12 = rxy;
    tmp.a13 = rxz;
    tmp.a21 = ryx;
    tmp.a22 = ryy;
    tmp.a23 = ryz;
    tmp.a31 = rzx;
    tmp.a32 = rzy;
    tmp.a33 = rzz;

    this->_quaternion = R3_rotation_to_quaternion(tmp);

    // Test for invalid quaternion
    // HACK - Perturb if in a pole
    bool valid = ((_quaternion.R_component_1() == _quaternion.R_component_1())
            && (_quaternion.R_component_2() == _quaternion.R_component_2())
            && (_quaternion.R_component_3() == _quaternion.R_component_3())
            && (_quaternion.R_component_4() == _quaternion.R_component_4()));
    while (!valid) {
        rxx *= 0.999;
        rxy *= 0.999;
        rxz *= 0.999;
        ryx *= 0.999;
        ryy *= 0.999;
        ryz *= 0.999;
        rzx *= 0.999;
        rzy *= 0.999;
        rzz *= 0.999;
        tmp.a11 = rxx;
        tmp.a12 = rxy;
        tmp.a13 = rxz;
        tmp.a21 = ryx;
        tmp.a22 = ryy;
        tmp.a23 = ryz;
        tmp.a31 = rzx;
        tmp.a32 = rzy;
        tmp.a33 = rzz;
        this->_quaternion = R3_rotation_to_quaternion(tmp);
        valid = ((_quaternion.R_component_1()
                == _quaternion.R_component_1())
                && (_quaternion.R_component_2() == _quaternion.R_component_2())
                && (_quaternion.R_component_3() == _quaternion.R_component_3())
                && (_quaternion.R_component_4() == _quaternion.R_component_4()));
    }

}

double CartesianRotational::q0() const {
    return this->_quaternion.R_component_1();
}

double CartesianRotational::q1() const {
    return this->_quaternion.R_component_2();
}

double CartesianRotational::q2() const {
    return this->_quaternion.R_component_3();
}

double CartesianRotational::q3() const {
    return this->_quaternion.R_component_4();
}

double CartesianRotational::a() const {
    return atan2(2 * (q0() * q1() + q2() * q3()),
            1 - 2 * (q1() * q1() + q2() * q2()));
}

double CartesianRotational::b() const {
    return asin(std::max(-1.0, std::min(1.0, 2 * (q0() * q2() - q3() * q1()))));
}

double CartesianRotational::c() const {
    return atan2(2 * (q0() * q3() + q1() * q2()),
            1 - 2 * (q2() * q2() + q3() * q3()));
}

nemo::RealVector CartesianRotational::asEulerAngles() const {
    RealVector vector(dim(3));

    vector[0] = this->a();
    vector[1] = this->b();
    vector[2] = this->c();

    return vector;
}

nemo::RealVector CartesianRotational::asAxisAngle() const {
    RealVector vector(dim(4));

    double f = sqrt(1 - q0() * q0());

    if (f < CartesianRotational::epsilon()) {
        return RealVector(dim(4), 0.0);
    }

    vector[0] = 2 * acos(q0());
    vector[1] = q1() / f;
    vector[2] = q2() / f;
    vector[3] = q3() / f;

    return vector;
}

nemo::RealVector CartesianRotational::asCompactAxisAngle() const {
    RealVector aa = this->asAxisAngle();
    return RealVector(aa[1] * aa[0], aa[2] * aa[0], aa[3] * aa[0]);
}

::boost::math::quaternion<double> CartesianRotational::asQuaternion() const {
    return _quaternion;
}

RealVector CartesianRotational::asDoubleVector() const {
    RealVector vector(nemo::dim(4));

    vector[0] = this->q0();
    vector[1] = this->q1();
    vector[2] = this->q2();
    vector[3] = this->q3();

    return vector;
}

double CartesianRotational::asDouble(unsigned int index) const {
    switch (index) {
    case 0:
        return this->q0();
        break;
    case 1:
        return this->q1();
        break;
    case 2:
        return this->q2();
        break;
    case 3:
        return this->q3();
        break;
    default:
        throw std::invalid_argument(
                "Rotational value expressed as quaternion, so index has to be between 0 and 3.");
    }
}

void CartesianRotational::setValue(double value) {
    throw std::runtime_error(
            "Multi-dimensional object, don`t know which value to set. Specify index.");
}

void CartesianRotational::setValue(unsigned int index, double value) {
    switch (index) {
    case 0:
        this->_quaternion = quaternion<double>(value,
                _quaternion.R_component_2(), _quaternion.R_component_3(),
                _quaternion.R_component_4());
        break;
    case 1:
        this->_quaternion = quaternion<double>(_quaternion.R_component_1(),
                value, _quaternion.R_component_3(),
                _quaternion.R_component_4());
        break;
    case 2:
        this->_quaternion = quaternion<double>(_quaternion.R_component_1(),
                _quaternion.R_component_2(), value,
                _quaternion.R_component_4());
        break;
    case 3:
        this->_quaternion = quaternion<double>(_quaternion.R_component_1(),
                _quaternion.R_component_2(), _quaternion.R_component_3(),
                value);
        break;
    default:
        throw std::range_error("Index exceeds dimensions.");
    }
}

std::string CartesianRotational::print() const {
    std::ostringstream outstream(std::ostringstream::out);
    outstream.precision(3); // Precision when printing double values
    outstream << "CartesianRotational<a=" << this->a() << ",b=" << this->b()
            << ",c=" << this->c() << ">(rad)";
    return outstream.str();
}

CartesianValue::CartesianValue() :
        DataTransferObject(), _transl(), _rot() {
}

CartesianValue::~CartesianValue() {
}

CartesianValue::CartesianValue(double valx, double valy, double valz,
        double vala, double valb, double valc, double vald) :
        DataTransferObject(), _transl(valx, valy, valz), _rot(vala, valb, valc,
                vald) {
}

CartesianValue::CartesianValue(const CartesianValue& val) :
        DataTransferObject(), _transl(val._transl), _rot(val._rot) {
}

CartesianValue::CartesianValue(const CartesianTranslationalPtr& transl,
        const CartesianRotationalPtr& rot) :
        DataTransferObject(), _transl(transl), _rot(rot) {
}

CartesianValue::CartesianValue(const RealVector& values) :
        DataTransferObject(), _transl(), _rot() {
    if (values.dimension() != 7) {
        throw std::invalid_argument("Wrong number of values given (need 7).");
    }
    this->_transl = CartesianTranslational(values[0], values[1], values[2]);
    this->_rot = CartesianRotational(values[3], values[4], values[5],
            values[6]);
}

double CartesianValue::asDouble(unsigned int index) const {
    if (index > 6) {
        // Dimensions doesn`t agree, throw exception
    }
    if (index < 3) {
        return this->_transl.asDouble(index);
    } else {
        return this->_rot.asDouble(index - 3);
    }
}

RealVector CartesianValue::asDoubleVector() const {
    RealVector vector(dim(7));

    vector[0] = this->_transl.asDouble(0);
    vector[1] = this->_transl.asDouble(1);
    vector[2] = this->_transl.asDouble(2);
    vector[3] = this->_rot.asDouble(0);
    vector[4] = this->_rot.asDouble(1);
    vector[5] = this->_rot.asDouble(2);
    vector[6] = this->_rot.asDouble(3);

    return vector;
}

void CartesianValue::setValue(unsigned int index, double value) {
    if (index > 6) {
        throw std::range_error("Index exceeds dimensions");
    }
    if (index < 3) {
        this->_transl.setValue(index, value);
    } else {
        this->_rot.setValue(index - 3, value);
    }
}

double CartesianValue::x() const {
    return this->_transl.x();
}

double CartesianValue::y() const {
    return this->_transl.y();
}

double CartesianValue::z() const {
    return this->_transl.z();
}

double CartesianValue::q0() const {
    return this->_rot.q0();
}

double CartesianValue::q1() const {
    return this->_rot.q1();
}

double CartesianValue::q2() const {
    return this->_rot.q2();
}

double CartesianValue::q3() const {
    return this->_rot.q3();
}

double CartesianValue::a() const {
    return this->_rot.a();
}

double CartesianValue::b() const {
    return this->_rot.b();
}

double CartesianValue::c() const {
    return this->_rot.c();
}

unsigned int CartesianValue::getDimension() const {
    return 7;
}

}