Pose.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#define EIGEN_NO_STATIC_ASSERT
#include "Pose.h"
 
#include <sstream>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
 
template class ::IceInternal::Handle<::armarx::Pose>;
template class ::IceInternal::Handle<::armarx::Vector2>;
template class ::IceInternal::Handle<::armarx::Vector3>;
template class ::IceInternal::Handle<::armarx::Quaternion>;
 
namespace armarx
{
    Vector2::Vector2()
    {
        x = 0;
        y = 0;
    }
 
    Vector2::Vector2(const Eigen::Vector2f& v)
    {
        x = v(0);
        y = v(1);
    }
 
    Vector2::Vector2(::Ice::Float x, ::Ice::Float y)
    {
        this->x = x;
        this->y = y;
    }
 
    Eigen::Vector2f
    Vector2::toEigen() const
    {
        Eigen::Vector2f v;
        v << this->x, this->y;
        return v;
    }
 
    void
    Vector2::operator=(const Eigen::Vector2f& vec)
    {
        x = vec[0];
        y = vec[1];
    }
 
    std::string
    Vector2::output(const Ice::Current&) const
    {
        std::stringstream s;
        s << toEigen();
        return s.str();
    }
 
    void
    Vector2::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        obj->setFloat("x", x);
        obj->setFloat("y", y);
    }
 
    void
    Vector2::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        x = obj->getFloat("x");
        y = obj->getFloat("y");
    }
 
    Vector3::Vector3()
    {
        x = 0;
        y = 0;
        z = 0;
    }
 
    Vector3::Vector3(const Eigen::Vector3f& v)
    {
        x = v(0);
        y = v(1);
        z = v(2);
    }
 
    Vector3::Vector3(const Eigen::Matrix4f& m)
    {
        x = m(0, 3);
        y = m(1, 3);
        z = m(2, 3);
    }
 
    Vector3::Vector3(::Ice::Float x, ::Ice::Float y, ::Ice::Float z)
    {
        this->x = x;
        this->y = y;
        this->z = z;
    }
 
    Eigen::Vector3f
    Vector3::toEigen() const
    {
        Eigen::Vector3f v;
        v << this->x, this->y, this->z;
        return v;
    }
 
    void
    Vector3::operator=(const Eigen::Vector3f& vec)
    {
        x = vec[0];
        y = vec[1];
        z = vec[2];
    }
 
    std::string
    Vector3::output(const Ice::Current&) const
    {
        std::stringstream s;
        s << toEigen().format(ArmarXEigenFormat);
        return s.str();
    }
 
    void
    Vector3::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        obj->setFloat("x", x);
        obj->setFloat("y", y);
        obj->setFloat("z", z);
    }
 
    void
    Vector3::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        x = obj->getFloat("x");
        y = obj->getFloat("y");
        z = obj->getFloat("z");
    }
 
    Quaternion::Quaternion() = default;
 
    Quaternion::Quaternion(const Eigen::Matrix4f& m4)
    {
        Eigen::Matrix3f m3 = m4.block<3, 3>(0, 0);
        this->init(m3);
    }
 
    Quaternion::Quaternion(const Eigen::Matrix3f& m3)
    {
        this->init(m3);
    }
 
    Quaternion::Quaternion(const Eigen::Quaternionf& q)
    {
        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;
    }
 
    Eigen::Matrix3f
    Quaternion::toEigen() const
    {
        return toEigenQuaternion().toRotationMatrix();
    }
 
    Eigen::Quaternionf
    Quaternion::toEigenQuaternion() const
    {
        return {this->qw, this->qx, this->qy, this->qz};
    }
 
    void
    Quaternion::init(const Eigen::Matrix3f& m3)
    {
        Eigen::Quaternionf q;
        q = m3;
        init(q);
    }
 
    void
    Quaternion::init(const Eigen::Quaternionf& q)
    {
        this->qw = q.w();
        this->qx = q.x();
        this->qy = q.y();
        this->qz = q.z();
    }
 
    Eigen::Matrix3f
    Quaternion::slerp(float alpha, const Eigen::Matrix3f& m)
    {
        return Quaternion::slerp(alpha, this->toEigen(), m);
    }
 
    Eigen::Matrix3f
    Quaternion::slerp(float alpha, const Eigen::Matrix3f& m1, const Eigen::Matrix3f& m2)
    {
        Eigen::Matrix3f result;
        Eigen::Quaternionf q1, q2;
        q1 = m1;
        q2 = m2;
        result = q1.slerp(alpha, q2);
        return result;
    }
 
    std::string
    Quaternion::output(const Ice::Current& c) const
    {
        std::stringstream s;
        s << toEigen() /*.format(ArmarXEigenFormat)*/;
        return s.str();
    }
 
    void
    Quaternion::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        obj->setFloat("qx", qx);
        obj->setFloat("qy", qy);
        obj->setFloat("qz", qz);
        obj->setFloat("qw", qw);
    }
 
    void
    Quaternion::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        qx = obj->getFloat("qx");
        qy = obj->getFloat("qy");
        qz = obj->getFloat("qz");
        qw = obj->getFloat("qw");
    }
 
    Pose::Pose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v)
    {
        this->orientation = new Quaternion(m);
        this->position = new Vector3(v);
        init();
    }
 
    Pose::Pose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori)
    {
        this->orientation = new Quaternion(*QuaternionPtr::dynamicCast(ori));
        this->position = new Vector3(*Vector3Ptr::dynamicCast(pos));
        init();
    }
 
    Pose::Pose()
    {
        this->orientation = new Quaternion();
        this->position = new Vector3();
        init();
    }
 
    Pose::Pose(const Pose& source) :
        IceUtil::Shared(source),
        armarx::Serializable(source),
        armarx::VariantDataClass(source),
        PoseBase(source)
    {
        ARMARX_CHECK_NOT_NULL(source.orientation) << "Pose being copied has a null orientation.";
        ARMARX_CHECK_NOT_NULL(source.position) << "Pose being copied has a null position.";
        orientation = QuaternionBasePtr::dynamicCast(source.orientation->clone());
        position = Vector3Ptr::dynamicCast(source.position->clone());
        ARMARX_CHECK_NOT_NULL(orientation) << "Cloned orientation is not a QuaternionBasePtr.";
        ARMARX_CHECK_NOT_NULL(position) << "Cloned position is not a Vector3Ptr.";
        init();
    }
 
    Pose::Pose(const Eigen::Matrix4f& m)
    {
        this->orientation = new Quaternion(m);
        this->position = new Vector3(m);
        init();
    }
 
    Pose::Pose(const Eigen::Vector3f& position, const Eigen::Quaternionf& quaternion)
    {
        this->position = new Vector3(position);
        this->orientation = new Quaternion(quaternion);
        init();
    }
 
    void
    Pose::operator=(const Eigen::Matrix4f& matrix)
    {
        this->orientation = new Quaternion(matrix);
        this->position = new Vector3(matrix);
        init();
    }
 
    Eigen::Matrix4f
    Pose::toEigen() const
    {
        Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
        ARMARX_CHECK_EXPRESSION(c_orientation);
        ARMARX_CHECK_EXPRESSION(c_position);
        m.block<3, 3>(0, 0) = c_orientation->toEigen();
        m.block<3, 1>(0, 3) = c_position->toEigen();
        return m;
    }
 
    std::string
    Pose::output(const Ice::Current&) const
    {
        std::stringstream s;
        s << toEigen() /*.format(ArmarXEigenFormat)*/;
        return s.str();
    }
 
    void
    Pose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
    {
        position->serialize(serializer, c);
        orientation->serialize(serializer, c);
    }
 
    void
    Pose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        position->deserialize(obj);
        orientation->deserialize(obj);
    }
 
    void
    Pose::init()
    {
        this->c_position = Vector3Ptr::dynamicCast(this->position);
        this->c_orientation = QuaternionPtr::dynamicCast(this->orientation);
    }
 
    void
    Pose::ice_postUnmarshal()
    {
        init();
    }
 
} // namespace armarx
 
void
armarx::fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector)
{
    vector = fromIce(ice);
}
 
void
armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion)
{
    quaternion = fromIce(ice);
}
 
void
armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation)
{
    rotation = fromIce(ice).toRotationMatrix();
}
 
void
armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation)
{
    Eigen::Matrix3f helper;
    fromIce(ice, helper);
    rotation = helper;
}
 
void
armarx::fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose)
{
    pose = fromIce(ice);
}
 
void
armarx::fromIce(const PoseBasePtr& ice, Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose)
{
    Eigen::Matrix4f helper;
    fromIce(ice, helper);
    pose = helper;
}
 
Eigen::Vector3f
armarx::fromIce(const Vector3BasePtr& pose)
{
    auto cast = Vector3Ptr::dynamicCast(pose);
    ARMARX_CHECK_NOT_NULL(cast) << "Vector3BasePtr must be a Vector3Ptr.";
    return cast->toEigen();
}
 
Eigen::Quaternionf
armarx::fromIce(const QuaternionBasePtr& pose)
{
    auto cast = QuaternionPtr::dynamicCast(pose);
    ARMARX_CHECK_NOT_NULL(cast) << "QuaternionBasePtr must be a QuaternionPtr.";
    return cast->toEigenQuaternion();
}
 
Eigen::Matrix4f
armarx::fromIce(const PoseBasePtr& pose)
{
    auto cast = PosePtr::dynamicCast(pose);
    ARMARX_CHECK_NOT_NULL(cast) << "PoseBasePtr must be a PosePtr.";
    return cast->toEigen();
}
 
void
armarx::toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector)
{
    ice = new Vector3(vector);
}
 
void
armarx::toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation)
{
    ice = new Quaternion(rotation);
}
 
void
armarx::toIce(QuaternionBasePtr& ice, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation)
{
    Eigen::Matrix3f helper = rotation;
    toIce(ice, helper);
}
 
void
armarx::toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion)
{
    ice = new Quaternion(quaternion);
}
 
void
armarx::toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose)
{
    ice = new Pose(pose);
}
 
void
armarx::toIce(PoseBasePtr& ice, const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose)
{
    Eigen::Matrix4f helper = pose;
    toIce(ice, helper);
}
 
armarx::Vector3Ptr
armarx::toIce(const Eigen::Vector3f& vector)
{
    return new Vector3(vector);
}
 
armarx::QuaternionPtr
armarx::toIce(const Eigen::Matrix3f& rotation)
{
    return new Quaternion(rotation);
}
 
armarx::QuaternionPtr
armarx::toIce(const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation)
{
    Eigen::Matrix3f helper = rotation;
    return toIce(helper);
}
 
armarx::QuaternionPtr
armarx::toIce(const Eigen::Quaternionf& quaternion)
{
    return new Quaternion(quaternion);
}
 
armarx::PosePtr
armarx::toIce(const Eigen::Matrix4f& pose)
{
    return new Pose(pose);
}
 
armarx::PosePtr
armarx::toIce(const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose)
{
    Eigen::Matrix4f helper = pose;
    return toIce(helper);
}
 
armarx::PosePtr
armarx::toIce(const Eigen::Isometry3f& pose)
{
    return armarx::toIce(pose.matrix());
}