Program Listing for File robot.hpp

↰ Return to documentation for file (include/crx_kinematics/robot.hpp)

#pragma once
#include <Eigen/Geometry>

namespace crx_kinematics
{

enum class RobotNameEnum
{
    crx3ia = -1,
    crx5ia = 0,
    crx10ia,
    crx10ia_l,
    crx20ia_l,
    crx30ia
};

struct DHParams
{
    double a = 0;      // Translation along previous frames X-axis
    double alpha = 0;  // Rotation around previous frames X-axis
    double r = 0;      // Translation along this frames Z-axis
    double theta = 0;  // Rotation around this frames Z-axis

    Eigen::Isometry3d T(const double joint_angle) const;
};

struct CircleEvaluation
{
    explicit CircleEvaluation(const double q,
                              const Eigen::Isometry3d& T_R0_tool,
                              const double r4,
                              const double r5,
                              const double r6,
                              const double a3,
                              const Eigen::Vector3d& O5);

    double q;
    Eigen::Vector3d O4;
    bool triangle_inequality_holds;
    Eigen::Vector3d O3UP;
    Eigen::Vector3d O3DOWN;
    double dot_product_up;
    double dot_product_down;
};

class CRXRobot
{
  public:
    CRXRobot();
    CRXRobot(const RobotNameEnum& robot_name, const bool couple_j2_j3 = true);
    Eigen::Isometry3d fk(const std::array<double, 6>& joint_values) const;
    std::vector<std::array<double, 6>> ik(const Eigen::Isometry3d& desired_pose) const;

  private:
    std::array<DHParams, 6> dh_params;
    bool couple_j2_j3;  // Whether to couple J2/J3 (like the Fanuc controller and paper does), or
                        // leave them independent (like the official Fanuc URDFs do). If coupled,
                        // J3=0 will always correspond to a horizontal forearm, regardless of the
                        // value of J2.
                        // Note: The internals follow the paper (so calculates a coupled J3 value).
                        //       This bool thus governs what conversions should happen as pre/post
                        //       processing within the fk and ik functions.
};

std::array<double, 6> to_xyzwpr(const Eigen::Isometry3d& T);
Eigen::Isometry3d from_xyzwpr(const std::array<double, 6>& xyzwpr);
std::array<double, 6> deg2rad(const std::array<double, 6>& joint_values);

}  // namespace crx_kinematics