DiffWristKinematics.h

#pragma once
 
#include <Eigen/Dense>
 
namespace kinematics {
struct gearpos_t {
    float left;
    float right;
    explicit gearpos_t(float left, float right);
    gearpos_t(const Eigen::Vector2f& vec);
    Eigen::Vector2f vec() const;
};

struct jointpos_t {
    float pitch;
    float roll;
    explicit jointpos_t(float pitch, float roll);
    jointpos_t(const Eigen::Vector2f& vec);
    Eigen::Vector2f vec() const;
};

class DiffWristKinematics {
public:
    jointpos_t gearPosToJointPos(const gearpos_t& gearPos) const;
    gearpos_t jointPosToGearPos(const jointpos_t& jointPos) const;
    jointpos_t gearPowerToJointPower(const gearpos_t& gearPwr) const;
    gearpos_t jointPowerToGearPower(const jointpos_t& jointPwr) const;
};
 
} // namespace kinematics