robot_model.hpp

Go to the documentation of this file.

#pragma once

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

using namespace Eigen;

namespace hebi {
namespace robot_model {

using Matrix4dVector = std::vector<Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >;
using MatrixXdVector = std::vector<MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd> >;
// The result of an IK operation.  More fields will be added to this structure
// in future API releases.
struct IKResult
{
  HebiStatusCode result; // Success or failure
};

class RobotModel;

class Objective
{
friend RobotModel;
public:
  virtual ~Objective() {}
protected:
  virtual HebiStatusCode addObjective(HebiIKPtr ik) const = 0;
};

class EndEffectorPositionObjective final : public Objective
{
public:
  EndEffectorPositionObjective(const Eigen::Vector3d&);
  EndEffectorPositionObjective(double weight, const Eigen::Vector3d&);
private:
  HebiStatusCode addObjective(HebiIKPtr ik) const override;
  double _weight, _x, _y, _z;
};

class EndEffectorSO3Objective final : public Objective
{
public:
  EndEffectorSO3Objective(const Eigen::Matrix3d&);
  EndEffectorSO3Objective(double weight, const Eigen::Matrix3d&);
private:
  HebiStatusCode addObjective(HebiIKPtr ik) const override;
  double _weight;
  const double _matrix[9];
};

class EndEffectorTipAxisObjective final : public Objective
{
public:
  EndEffectorTipAxisObjective(const Eigen::Vector3d&);
  EndEffectorTipAxisObjective(double weight, const Eigen::Vector3d&);
private:
  HebiStatusCode addObjective(HebiIKPtr ik) const override;
  double _weight, _x, _y, _z;
};

class JointLimitConstraint final : public Objective
{
public:
  JointLimitConstraint(const Eigen::VectorXd& min_positions, const Eigen::VectorXd& max_positions);
  JointLimitConstraint(double weight, const Eigen::VectorXd& min_positions, const Eigen::VectorXd& max_positions);
private:
  HebiStatusCode addObjective(HebiIKPtr ik) const override;
  double _weight;
  Eigen::VectorXd _min_positions;
  Eigen::VectorXd _max_positions;
public:
  // Allow Eigen member variables:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class RobotModel final
{
  friend Objective;

  private:
    HebiRobotModelPtr const internal_;

    template<typename T>
    HebiStatusCode addObjectives(HebiIKPtr ik, const T& objective) const
    {
      static_assert(std::is_convertible<T*, Objective*>::value,
                    "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
      return static_cast<const Objective*>(&objective)->addObjective(ik);
    }

    template<typename T, typename ... Args>
    HebiStatusCode addObjectives(HebiIKPtr ik, const T& objective, Args ... args) const
    {
      static_assert(std::is_convertible<T*, Objective*>::value,
                    "Must pass arguments of base type hebi::robot_model::Objective to the variable args of solveIK!");
      auto res = static_cast<const Objective*>(&objective)->addObjective(ik);
      if (res != HebiStatusSuccess)
        return res;
      return addObjectives(ik, args ...);
    }

    static void setTranslate(Eigen::Matrix4d& matrix, double x, double y, double z);

    static void setRotX(Eigen::Matrix4d& matrix, double radians);

    static void setSphereInertia(Eigen::VectorXd& inertia, double mass, double radius);

    static void setRodXAxisInertia(Eigen::VectorXd& inertia, double mass, double length);

    bool tryAdd(HebiRobotModelElementPtr element, bool combine);

  public:
    enum class ActuatorType
    {
      X5_1,
      X5_4,
      X5_9,
      X8_3,
      X8_9,
      X8_16
    };

    enum class LinkType
    {
      X5
    };

    enum class BracketType
    {
      X5LightLeft,
      X5LightRight,
      X5HeavyLeftInside,
      X5HeavyLeftOutside,
      X5HeavyRightInside,
      X5HeavyRightOutside
    };

    RobotModel();

    ~RobotModel() noexcept;

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

    Eigen::Matrix4d getBaseFrame() const;

    size_t getFrameCount(HebiFrameType frame_type) const;

    size_t getDoFCount() const;

    bool addRigidBody(
      const Eigen::Matrix4d& com,
      const Eigen::VectorXd& inertia,
      double mass,
      const Eigen::Matrix4d& output,
      bool combine);

    bool addJoint(
      HebiJointType joint_type,
      bool combine);

    bool addActuator(ActuatorType actuator_type);

    bool addLink(LinkType link_type, double extension, double twist);

    bool addBracket(BracketType bracket_type);

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

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

    template<typename ... Args>
    IKResult solveInverseKinematics(const Eigen::VectorXd& initial_positions, Eigen::VectorXd& result, Args ... args) const
    {
      return solveIK(initial_positions, result, args ...);
    }

    template<typename ... Args>
    IKResult solveIK(const Eigen::VectorXd& initial_positions,
                     Eigen::VectorXd& result,
                     Args ... objectives) const
    {
      // Create a HEBI C library IK object
      auto ik = hebiIKCreate();

      // (Try) to add objectives to the IK object
      IKResult res;
      res.result = addObjectives(ik, objectives ...);
      if (res.result != HebiStatusSuccess)
      {
        hebiIKRelease(ik);
        return res;
      }

      // Transfer/initialize from Eigen to C arrays
      auto positions_array = new double[initial_positions.size()];
      {
        Map<Eigen::VectorXd> tmp(positions_array, initial_positions.size());
        tmp = initial_positions;
      }
      auto result_array = new double[initial_positions.size()];

      // Call into C library to solve
      res.result = hebiIKSolve(ik, internal_, positions_array, result_array, nullptr);

      // Transfer/cleanup from C arrays to Eigen
      delete[] positions_array;
      {
        Map<Eigen::VectorXd> tmp(result_array, initial_positions.size());
        result = tmp;
      }
      delete[] result_array;

      hebiIKRelease(ik);

      return res;
    }

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

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

    void getMasses(Eigen::VectorXd& masses) const;

  private:
    static const std::map<ActuatorType, double> actuator_masses_;

    HEBI_DISABLE_COPY_MOVE(RobotModel)
};

} // namespace robot_model
} // namespace hebi