controlUtils.h

Go to the documentation of this file.

#ifndef _OCRACONTROL_UTILS_H_
#define _OCRACONTROL_UTILS_H_

#include <ocra/util/MathTypes.h>
#include <Eigen/Lgsm>
#include "ocra/control/Model.h"

namespace ocra
{

  namespace utils
  {
    Eigen::Matrix<double,6,1> cartesianPDCoupling(const Eigen::Matrix<double,6,1>& kp, const Eigen::Displacementd& Hdes, const Eigen::Displacementd& H,
                                  const Eigen::Matrix<double,6,1>& kv, const Eigen::Twistd& Tdes, const Eigen::Twistd& T);

    Eigen::Matrix<double,6,1> cartesianPCoupling(const Eigen::Matrix<double,6,1>& kp, const Eigen::Displacementd& Hdes, const Eigen::Displacementd& H);

    Eigen::Matrix<double,6,1> cartesianDCoupling(const Eigen::Matrix<double,6,1>& kv, const Eigen::Twistd& Tdes, const Eigen::Twistd& T);

    Eigen::VectorXd adaptKToInertia(const Eigen::VectorXd& K, const Model& model, double scale = 1.);

    Eigen::Displacementd compute_H_2_in_1(const Eigen::Displacementd& H_1_in_ref, const Eigen::Displacementd& H_2_in_ref);

    Eigen::Displacementd computeEgoFrame(Model& model, const Eigen::Vector3d& fwdDirection_in_phantom);

    Eigen::Displacementd interpolate(const Eigen::Displacementd& Hstart, const Eigen::Displacementd& Hend, double t);

    Eigen::Displacementd projectRotation(const Eigen::Displacementd& H, const Eigen::Vector3d& n, double& theta);
  }
}


#endif //_OCRACONTROL_UTILS_H_

// cmake:sourcegroup=Api