SpinBoxToPose.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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    RobotAPI::ArmarXObjects::NJointControllerGuiPluginUtility
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <SimoxUtility/math/convert.h>
 
#include "SpinBoxToVector.h"
 
namespace armarx
{
    template <class Qwid = QDoubleSpinBox>
    class SpinBoxToPose
    {
        //set
    public:
        void
        setPos(const Eigen::Vector3f& pos)
        {
            _xyz.set(pos);
        }
 
        void
        setOri(const Eigen::Vector3f& rpy)
        {
            _rpy.set(rpy);
        }
 
        void
        setOri(const Eigen::Quaternionf& q)
        {
            setOri(simox::math::quat_to_rpy(q));
        }
 
        void
        setOri(const Eigen::Matrix3f& m)
        {
            setOri(simox::math::mat3f_to_rpy(m));
        }
 
        void
        setPose(const auto& pos, const auto& ori)
        {
            setPos(pos);
            setOri(ori);
        }
 
        void
        setPose(const Eigen::Matrix4f& m)
        {
            setPos(simox::math::mat4f_to_pos(m));
            setOri(simox::math::mat4f_to_rpy(m));
        }
 
        //get
    public:
        Eigen::Vector3f
        getPos() const
        {
            return _xyz.template get<Eigen::Vector3f>();
        }
 
        Eigen::Vector3f
        getRPY() const
        {
            return _rpy.template get<Eigen::Vector3f>();
        }
 
        Eigen::Quaternionf
        getQuat() const
        {
            return simox::math::rpy_to_quat(getRPY());
        }
 
        Eigen::Matrix3f
        getMat3() const
        {
            return simox::math::rpy_to_mat3f(getRPY());
        }
 
        Eigen::Matrix4f
        getMat4() const
        {
            return simox::math::pos_rpy_to_mat4f(getPos(), getRPY());
        }
 
        //set up
    public:
        void
        setXYZWidgets(Qwid* x, Qwid* y, Qwid* z)
        {
            _xyz.addWidget(x);
            _xyz.addWidget(y);
            _xyz.addWidget(z);
        }
 
        void
        setRPYWidgets(Qwid* r, Qwid* p, Qwid* y)
        {
            _rpy.addWidget(r);
            _rpy.addWidget(p);
            _rpy.addWidget(y);
        }
 
        SpinBoxToVector<Qwid, 3>&
        xyzWidgets()
        {
            return _xyz;
        }
 
        SpinBoxToVector<Qwid, 3>&
        rpyWidgets()
        {
            return _rpy;
        }
 
        void
        setDefaultLimits()
        {
            _xyz.setMinMax(-100'000, 100'000);
            _rpy.setMinMax(-9, 9);
        }
 
    private:
        SpinBoxToVector<Qwid, 3> _xyz;
        SpinBoxToVector<Qwid, 3> _rpy;
    };
} // namespace armarx