TermTaskspacePose.hpp

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#ifdef CPPADCG

#include <ct/optcon/costfunction/term/TermBase.hpp>
#include <ct/optcon/costfunction/utility/utilities.hpp>

#include <iit/rbd/traits/TraitSelector.h>
#include <ct/rbd/robot/Kinematics.h>
#include <ct/rbd/state/RBDState.h>


namespace ct {
namespace rbd {


template <class KINEMATICS, bool FB, size_t STATE_DIM, size_t CONTROL_DIM>
class TermTaskspacePose : public optcon::TermBase<STATE_DIM, CONTROL_DIM, double, ct::core::ADCGScalar>
{
public:
    using SCALAR = ct::core::ADCGScalar;
    using BASE = optcon::TermBase<STATE_DIM, CONTROL_DIM, double, ct::core::ADCGScalar>;

    using RBDStateTpl = ct::rbd::RBDState<KINEMATICS::NJOINTS, SCALAR>;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    TermTaskspacePose() {}
    TermTaskspacePose(size_t eeInd,
        const Eigen::Matrix<double, 3, 3>& Qpos,
        const double& Qrot,
        const ct::rbd::RigidBodyPose& rbdPose,
        const std::string& name = "TermTaskSpace")
        // delegate constructor
        : TermTaskspacePose(eeInd,
              Qpos,
              Qrot,
              rbdPose.position().toImplementation(),
              rbdPose.getRotationQuaternion().toImplementation(),
              name)
    {
    }
    TermTaskspacePose(size_t eeInd,
        const Eigen::Matrix<double, 3, 3>& Qpos,
        const double& Qrot,
        const core::StateVector<3, double>& w_pos_des,
        const Eigen::Quaternion<double>& w_q_des,
        const std::string& name = "TermTaskSpace")
        : BASE(name),
          eeInd_(eeInd),
          Q_pos_(Qpos),
          Q_rot_(Qrot),
          w_p_ref_(w_pos_des),
          w_R_ref_(w_q_des.normalized().toRotationMatrix())
    {
    }

    TermTaskspacePose(size_t eeInd,
        const Eigen::Matrix<double, 3, 3>& Qpos,
        const double& Qrot,
        const core::StateVector<3, double>& w_pos_des,
        const Eigen::Matrix<double, 3, 1>& eulerXyz,
        const std::string& name = "TermTaskSpace")
        // delegate constructor
        : TermTaskspacePose(eeInd,
              Qpos,
              Qrot,
              w_pos_des,
              Eigen::Quaternion<double>(Eigen::AngleAxisd(eulerXyz(0), Eigen::Vector3d::UnitX()) *
                                        Eigen::AngleAxisd(eulerXyz(1), Eigen::Vector3d::UnitY()) *
                                        Eigen::AngleAxisd(eulerXyz(2), Eigen::Vector3d::UnitZ())),
              name)
    {
    }


    TermTaskspacePose(std::string& configFile, const std::string& termName, bool verbose = false)
    {
        loadConfigFile(configFile, termName, verbose);
    }

    TermTaskspacePose(const TermTaskspacePose& arg)
        : BASE(arg),
          eeInd_(arg.eeInd_),
          kinematics_(KINEMATICS()),
          Q_pos_(arg.Q_pos_),
          Q_rot_(arg.Q_rot_),
          w_p_ref_(arg.w_p_ref_),
          w_R_ref_(arg.w_R_ref_)
    {
    }

    virtual ~TermTaskspacePose() {}
    virtual TermTaskspacePose<KINEMATICS, FB, STATE_DIM, CONTROL_DIM>* clone() const override
    {
        return new TermTaskspacePose(*this);
    }

    virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
        const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
        const SCALAR& t) override
    {
        return evalLocal<SCALAR>(x, u, t);
    }

    virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
        const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
        ct::core::ADCGScalar t) override
    {
        return evalLocal<ct::core::ADCGScalar>(x, u, t);
    }

    void loadConfigFile(const std::string& filename, const std::string& termName, bool verbose = false) override
    {
        if (verbose)
            std::cout << "Loading TermTaskspacePose from file " << filename << std::endl;

        ct::optcon::loadScalarCF(filename, "eeId", eeInd_, termName);
        ct::optcon::loadScalarCF(filename, "Q_rot", Q_rot_, termName);

        ct::optcon::loadMatrixCF(filename, "Q_pos", Q_pos_, termName);
        ct::optcon::loadMatrixCF(filename, "x_des", w_p_ref_, termName);

        // try loading euler angles
        if (verbose)
            std::cout << "trying to load euler angles" << std::endl;
        try
        {
            Eigen::Vector3d eulerXyz;
            ct::optcon::loadMatrixCF(filename, "eulerXyz_des", eulerXyz, termName);
            Eigen::Quaternion<double> quat_des(Eigen::AngleAxisd(eulerXyz(0), Eigen::Vector3d::UnitX()) *
                                               Eigen::AngleAxisd(eulerXyz(1), Eigen::Vector3d::UnitY()) *
                                               Eigen::AngleAxisd(eulerXyz(2), Eigen::Vector3d::UnitZ()));
            w_R_ref_ = quat_des.toRotationMatrix();
            if (verbose)
                std::cout << "Read desired Euler Angles Xyz as  = \n" << eulerXyz.transpose() << std::endl;
        } catch (const std::exception& e)
        {
            throw std::runtime_error(
                "Failed to load TermTaskspacePose, could not find a desired end effector orientation in file.");
        }

        if (verbose)
        {
            std::cout << "Read eeId as eeId = \n" << eeInd_ << std::endl;
            std::cout << "Read Q_pos as Q_pos = \n" << Q_pos_ << std::endl;
            std::cout << "Read Q_rot as Q_rot = \n" << Q_rot_ << std::endl;
            std::cout << "Read x_des as x_des = \n" << w_p_ref_.transpose() << std::endl;
        }
    }


    const Eigen::Matrix<double, 3, 1>& getReferencePosition() const { return w_p_ref_; }
    const Eigen::Quaterniond getReferenceOrientation() const { return Eigen::Quaterniond(w_R_ref_); }
    void setReferencePosition(const Eigen::Matrix<double, 3, 1>& w_p_ref) { w_p_ref_ = w_p_ref; }
    void setReferenceOrientation(const Eigen::Matrix<double, 3, 3> w_R_ref) { w_R_ref_ = w_R_ref; }
    const ct::rbd::RigidBodyPose getReferencePose() const
    {
        return ct::rbd::RigidBodyPose(getReferenceOrientation(), getReferencePosition());
    }


protected:
    virtual RBDStateTpl setStateFromVector(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x)
    {
        return setStateFromVector_specialized<FB>(x);
    }

private:
    template <bool T>
    RBDStateTpl setStateFromVector_specialized(const Eigen::Matrix<SCALAR, -1, 1>& x,
        typename std::enable_if<T, bool>::type = true)
    {
        RBDStateTpl rbdState;
        rbdState.fromStateVectorEulerXyz(x);

        return rbdState;
    }

    template <bool T>
    RBDStateTpl setStateFromVector_specialized(const Eigen::Matrix<SCALAR, -1, 1>& x,
        typename std::enable_if<!T, bool>::type = true)
    {
        RBDStateTpl rbdState;
        rbdState.joints().toImplementation() = x.head(STATE_DIM);
        return rbdState;
    }

    template <typename SC>
    SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t)
    {
        using Matrix3Tpl = Eigen::Matrix<SC, 3, 3>;

        // transform the robot state vector into a CT RBDState
        RBDState<KINEMATICS::NJOINTS, SC> rbdState = setStateFromVector(x);

        // position difference in world frame
        Eigen::Matrix<SC, 3, 1> xDiff =
            kinematics_.getEEPositionInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions())
                .toImplementation() -
            w_p_ref_.template cast<SC>();

        // compute the cost based on the position error
        SC pos_cost = (xDiff.transpose() * Q_pos_.template cast<SC>() * xDiff)(0, 0);


        // get current end-effector rotation in world frame
        Matrix3Tpl ee_rot = kinematics_.getEERotInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions());

        // compute a measure for the difference between current rotation and desired rotation and compute cost based on the orientation error
        // for the intuition behind, consider the following posts:
        // https://math.stackexchange.com/a/87698
        // https://math.stackexchange.com/a/773635
        Matrix3Tpl ee_rot_diff = w_R_ref_.template cast<SC>().transpose() * ee_rot;

        SC rot_cost =
            (SC)Q_rot_ * (ee_rot_diff - Matrix3Tpl::Identity()).squaredNorm();  // the frobenius norm of (R_diff-I)

        return pos_cost + rot_cost;
    }

    size_t eeInd_;

    KINEMATICS kinematics_;

    Eigen::Matrix<double, 3, 3> Q_pos_;

    double Q_rot_;

    Eigen::Matrix<double, 3, 1> w_p_ref_;

    Eigen::Matrix<double, 3, 3> w_R_ref_;
};


}  // namespace rbd
}  // namespace ct

#endif