kinematics.hpp

Go to the documentation of this file.

#pragma once

#include "hebi_kinematics.h"
#include "Eigen/Eigen"
#include "util.hpp"
#include <vector>
#include <memory>

using namespace Eigen;

namespace hebi {
namespace kinematics {

typedef std::vector<Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > Matrix4fVector;
typedef std::vector<MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> > MatrixXfVector;

class Kinematics;

class KinematicBody
{
  friend Kinematics;

  private:
    HebiBodyPtr internal_;

    bool manage_pointer_lifetime_ = true;

    HebiBodyPtr getInternal() const { return internal_; }

    void consume() { manage_pointer_lifetime_ = false; }

  protected:
    #ifndef DOXYGEN_OMIT_INTERNAL

    KinematicBody(HebiBodyPtr internal) : internal_(internal) {};
    #endif // DOXYGEN_OMIT_INTERNAL

  public:
    virtual ~KinematicBody() noexcept
    {
      if (manage_pointer_lifetime_)
        hebiBodyRelease(internal_);
    }
   
    static std::shared_ptr<KinematicBody> createX5();

    static std::shared_ptr<KinematicBody> createX5Link(float length, float twist);

    static std::shared_ptr<KinematicBody> createGenericLink(const Eigen::Vector3f& com, const Eigen::Matrix4f& output);
};

class Kinematics final
{
  private:
    const HebiKinematicsPtr internal_;

  public:
    Kinematics();

    virtual ~Kinematics() noexcept;

    void setBaseFrame(const Eigen::Matrix4f& base_frame);

    Eigen::Matrix4f getBaseFrame() const;

    size_t getFrameCount(HebiFrameType frame_type) const;

    size_t getDoFCount() const;

    bool addBody(std::shared_ptr<KinematicBody> new_body);

    void getForwardKinematics(HebiFrameType, const Eigen::VectorXd& positions, Matrix4fVector& frames) const;
    void getFK(HebiFrameType, const Eigen::VectorXd& positions, Matrix4fVector& frames) const;

    void getEndEffector(HebiFrameType, const Eigen::VectorXd& positions, Eigen::Matrix4f& transform) const;

    void solveInverseKinematics(const Eigen::Vector3f& target_xyz, const Eigen::VectorXd& positions, Eigen::VectorXd& result) const;
    void solveIK(const Eigen::Vector3f& target_xyz, const Eigen::VectorXd& initial_positions, Eigen::VectorXd& result) const;

    void getJacobians(HebiFrameType, const Eigen::VectorXd& positions, MatrixXfVector& jacobians) const;
    void getJ(HebiFrameType, const Eigen::VectorXd& positions, MatrixXfVector& jacobians) const;

    void getJacobianEndEffector(HebiFrameType, const Eigen::VectorXd& positions, Eigen::MatrixXf& jacobian) const;
    void getJEndEffector(HebiFrameType, const Eigen::VectorXd& positions, Eigen::MatrixXf& jacobian) const;

  private:
    HEBI_DISABLE_COPY_MOVE(Kinematics)
};

} // namespace kinematics
} // namespace hebi