cartesianrobot.h

#pragma once
#ifndef _CARTESIANROBOT_H
#define _CARTESIANROBOT_H
 
#include "rsi.h" // HAS_CARTESIAN_ROBOT defined in rsi.h
#if defined(HAS_CARTESIAN_ROBOT)
#include <cmath>
 
 
// doxygen requires these stacked namespaces, you CANNOT use RSI::RapidCode::Cartesian {
namespace RSI
{
namespace RapidCode
{

namespace Cartesian
{

  enum class PathState
  {
    Uninitialized, 
    Idle, 
    Moving, 
    Stopping, 
  };
 
  constexpr double pi = 3.1415926535897932384626433832795028841971693993751;

  enum class PathMode
  {
    Absolute,   
    Relative,   
  };

  enum class RotationDirection
  {
    Clockwise,    
    CounterClockwise,   
  };

  enum class Plane
  {
    None,   
    XY,   
    XZ,   
    YZ,   
  }; 

  enum class GCodeWorkOffset {
    Machine = 0,
    G54 = 54,
    G55 = 55,
    G56 = 56,
    G57 = 57,
    G58 = 58,
    G59 = 59,
  };

  enum class LinearUnits
  {
    None,         
    Millimeters,  
    Inches,       
    Feet,         
    Centimeters,  
    Meters,       
  };

  enum class RotationalUnits
  {
    None,         
    Degrees,      
    Radians,      
    Revolutions,  
  };

  enum class CartesianAxis
  {
    X = 0,      
    Y = 1,      
    Z = 2,      
    Roll = 3,   
    Pitch = 4,  
    Yaw = 5,    
  };

 
  class Gcode; // forward declaration


  class RSI_API Vector3d
  {
  public:
    double X;
    
    double Y;
    
    double Z;

     Vector3d();

     Vector3d(double x, double y, double z);

     Vector3d(const Vector3d& rightHandSide);

     double Dot(const Vector3d& rightHandSide) const;

     Vector3d Cross(const Vector3d& rightHandSide) const;

     Vector3d& Set(double x, double y, double z);

     Vector3d& Set(const Vector3d& rightHandSide);

     bool Equals(const Vector3d& rightHandSide, const double tolerance) const;

     double SumSquares() const;

     double Length() const;

     Vector3d Normal() const;

     Vector3d& Normalize();

     Vector3d& operator=(const Vector3d& rightHandSide);
     bool operator==(const Vector3d& rightHandSide) const;
     bool operator!=(const Vector3d& rightHandSide) const;
     Vector3d operator*(double scalar) const;
     Vector3d& operator*=(double scalar);
     Vector3d operator+(const Vector3d& rightHandSide) const;
     Vector3d& operator+=(const Vector3d& rightHandSide);
     Vector3d operator-(const Vector3d& rightHandSide) const;
     Vector3d operator-() const;
     Vector3d& operator-=(const Vector3d& rightHandSide);
     Vector3d operator/(double scalar) const;
     Vector3d& operator/=(double scalar);
     Vector3d Inverse() const;
  };
  
  class RSI_API Quaternion
  {
  public:
    Vector3d V; 

    double W; 

    Quaternion();

    Quaternion(double w, double x, double y, double z);

    Quaternion(const Quaternion& otherQuaternion);

    Quaternion(double w, const Vector3d& v);

    Quaternion(double x, double y, double z);

    Quaternion(const Vector3d& rpy, RotationalUnits units = RotationalUnits::Radians);

    bool operator==(const Quaternion& rightHandSide) const;

    bool operator!=(const Quaternion& rightHandSide) const;

    Quaternion& operator=(const Quaternion& rightHandSide);

    Quaternion operator+(const Quaternion& rightHandSide) const;

    Quaternion& operator+=(const Quaternion& rightHandSide);

    Quaternion operator-(const Quaternion& rightHandSide) const;

    Quaternion& operator-=(const Quaternion& rightHandSide);
    
    Quaternion operator*(const Quaternion& rightHandSide) const;

    Quaternion operator*(double s) const;

    Quaternion& operator*=(double s);

    Vector3d operator*(const Vector3d& rightHandSide) const;

    Quaternion operator-() const;

    double Dot(const Quaternion& rightHandSide) const;

    Quaternion& Set(const Quaternion& otherQuaternion);

    Quaternion& Set(double w, double x, double y, double z);

    Quaternion& Set(double w, const Vector3d& vec);

    Quaternion& Set(double x, double y, double z);

    Quaternion& Set(const Vector3d& vec, RotationalUnits units = RotationalUnits::Radians);

    bool Equals(const Quaternion& rightHandSide, const double tolerance) const;

    bool EqualsOrientation(const Quaternion& rightHandSide, const double tolerance) const;

    Quaternion& Normalize();

    Quaternion Normalized() const;
 

    double SumSquares() const;

    double Magnitude() const;

    Quaternion Inverse() const;

    Quaternion Conjugate() const;
 
    

    static Quaternion FromAngleAxis(double angle, const Vector3d& axis);

    void ToAngleAxis(double& outAngle, Vector3d& outAxis) const;

    Vector3d ToEuler(RotationalUnits units = RotationalUnits::Radians) const;

    static Quaternion FromMatrix(const double* const matrix);
    

    void ToMatrix(double* matrix) const;
  };

  class RSI_API Pose {
  public:
    Vector3d Position;
    
    Quaternion Orientation;

    Pose();

    Pose(const Pose& pose);

    Pose(const Vector3d position, const Quaternion& quaternion);

    Pose(const Vector3d& position);

    Pose(double x, double y, double z);

    Pose(const Quaternion& quaternion);

    bool Equals(const Pose& rightHandSide, double tolerance) const;

    bool EqualsOrientation(const Pose& rightHandSide, double tolerance) const;

    Pose Rotate(const Pose& rightHandSide) const;

    Vector3d operator*(const Vector3d& rightHandSide) const;

    Pose operator*(const Pose& rightHandSide) const;

    Pose Inverse() const;
  };

  class RSI_API RobotPosition
  {
  public:
    RSI::RapidCode::Cartesian::Pose Pose;

    RapidVector<double> FreeAxes;

    RobotPosition();
    RobotPosition(const Cartesian::RobotPosition& otherPosition);
    RobotPosition(const Cartesian::Pose& newPose, const size_t numberOfFreeAxes = 0);
    RobotPosition(const Cartesian::Pose& newPose, const RapidVector<double>& freeAxes);
    Cartesian::RobotPosition operator*(const Cartesian::RobotPosition& otherPosition) const;
    Cartesian::RobotPosition operator*(const Cartesian::Pose& otherPose) const;
    void AxisValueSet(uint32_t index, double value);
    double AxisValueGet(uint32_t index) const;
    Cartesian::RobotPosition Inverse() const;
  };

  class RSI_API KinematicModel
  {
  public:
    virtual const char* const NameGet() const = 0;
    virtual uint32_t AxisCountGet() const = 0;
 
    virtual LinearUnits UnitsGet() const = 0;
    virtual RotationalUnits RotationalUnitsGet() const = 0;
 
    virtual bool IsConfigured() const = 0;
 
    virtual const char* const* const ExpectedAxisNamesGet() const = 0;
    virtual uint32_t ExpectedAxisNamesCountGet() const = 0;
 
    virtual uint32_t FreeAxisCountGet() const = 0;
 
    // input function
    // inputPose requires this to be in robot centric coordinates.
    // remove tool offset and robot base offset
    virtual bool InverseKinematicsSolve(const Pose& inputPose, void* outputPointerSolutionVector) const = 0;
 
    virtual bool InverseKinematicsSolve(const RobotPosition& inputPosition, void* vpSols) const = 0;
 
    // ouput function
    // inputJointPositions requires this to be in robot centric coordinates.
    // apply tool offset and robot base offset
    virtual void ForwardKinematicsSolve(const double* inputJointPositions, Pose& outputPose) const = 0;
 
    virtual void ForwardKinematicsSolve(const double* inputJointPositions, RobotPosition& outputPosition) const = 0;
 
    virtual bool IsArticulated() const = 0;
  };

  struct RSI_API ModelAxisMapping
  {
 
  public:
    int JointIndex;

    double Scaling;
    static constexpr double DefaultScaling = 1.0;

    double Offset;
    static constexpr double DefaultOffset = 0.0;

    static constexpr int MaxLabelSize = 16;
    static constexpr int ExpectedLabelBufferSize = MaxLabelSize + 1;

    char ExpectedLabel[ExpectedLabelBufferSize];

    ModelAxisMapping(int jointIndex);

    ModelAxisMapping(int jointIndex, double scaling, double offset, const char* const label);
 
    // ModelAxisMapping Destructor.
    virtual ~ModelAxisMapping() = default;
  };

  class RSI_API KinematicModelBuilder
  {
  protected:
    // Stores implementation details for the KinematicModelBuilder. (Pointer to implementation pattern)
    struct Storage;
    Storage* storage;

    KinematicModelBuilder(Storage* storage);
 
    // Protect the copy constructor, for internal use only.
    KinematicModelBuilder(const KinematicModelBuilder&);
    
  public:
    virtual ~KinematicModelBuilder();
 
    // Delete the copy-assignment constructor.
    KinematicModelBuilder& operator=(const KinematicModelBuilder&) = delete;
 
    // Move constructors
    KinematicModelBuilder(KinematicModelBuilder&&) noexcept;
    KinematicModelBuilder& operator=(KinematicModelBuilder&&) noexcept;

    virtual const KinematicModel& ModelBuild() = 0;

    const KinematicModel& ModelGet();

    void UnitsSet(LinearUnits units);

    LinearUnits UnitsGet() const;

    void RotationalUnitsSet(RotationalUnits units);

    RotationalUnits RotationalUnitsGet() const;

    void LabelSet(const char* const label);

    const char* const LabelGet() const;

    void TransformFromKinematicOriginToRobotOriginSet(const Pose& KinematicOriginToRobotOrigin);

    Pose TransformFromKinematicOriginToRobotOriginGet() const;

    void FreeAxisAdd(const ModelAxisMapping& freeAxis);
  };

  struct RSI_API LinearJointMapping : public ModelAxisMapping
  {
  public:
    CartesianAxis CartesianCoordinate;
 
    LinearJointMapping(int jointIndex, CartesianAxis cartesianCoordinate);
    LinearJointMapping(int jointIndex, CartesianAxis cartesianCoordinate, double scaling, double offset, const char* const label);
  };

  class RSI_API LinearModelBuilder : public KinematicModelBuilder
  {
    // Stores implementation details for the LinearModelBuilder. (Pointer to implementation pattern)
    struct Storage;
    Storage* storage;
 
  public:

    const KinematicModel& ModelBuild() override;

    void JointAdd(const LinearJointMapping& joint);

    LinearModelBuilder(const char* const label);
    ~LinearModelBuilder();
 
    // Move constructors
    LinearModelBuilder(LinearModelBuilder&& other) noexcept;
    LinearModelBuilder& operator=(LinearModelBuilder&&) noexcept;
 
    // Disable copy constructors
    LinearModelBuilder(const LinearModelBuilder&) = delete;
    LinearModelBuilder& operator=(const LinearModelBuilder&) = delete;
  };
 
  // UNSUPPORTED
  class RSI_API ArticulatedModelBuilder : public KinematicModelBuilder
  {
    // Stores implementation details for the ArticulatedModelBuilder. (Pointer to implementation pattern)
    struct Storage;
    Storage* storage;
 
  public:
    const KinematicModel& ModelBuild() override;
    ArticulatedModelBuilder();
    ~ArticulatedModelBuilder();
  };

  struct GcodeCallbackData {
    int32_t LineNumber;
    
    const char* LineText;
    
    RsiError* UserError;
  };

  class RSI_API GcodeCallback 
  {
  public:
    virtual void Execute(GcodeCallbackData* data) {};
 
    virtual ~GcodeCallback() = default;     // Virtual destructor for C++ base classes
  };
 
  class RSI_API Robot : public virtual RapidCodeObject
  {
  public:


    static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, KinematicModelBuilder* builder, uint32_t motionFrameBufferSize);
   
    static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier);

    static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier, uint32_t motionFrameBufferSize);

    static void RobotDelete(MotionController* controller, Robot* robot);

 


    static constexpr uint64_t TimeoutForever = 0xFFFFFFFFFFFFFFFF;

    static constexpr uint32_t MotionFrameBufferSizeDefault = 256; 

    static constexpr uint32_t MotionFrameBufferSizeMinimum = 50; 
 


    virtual PathState PathStateGet() = 0;

    virtual bool PathStateWait(PathState stateWaitFor, uint64_t timeoutMilliseconds) = 0;

    virtual bool PathStateWaitChange(PathState stateWaitChange, uint64_t timeoutMilliseconds) = 0;
 

    virtual uint64_t MotionCounterGet() = 0;

    virtual void MotionCounterWait(uint64_t motionCount, uint64_t timeoutMilliseconds) = 0;

    virtual uint64_t SampleCounterGet() = 0;

    virtual void SampleCounterWait(uint64_t sampleCount, uint64_t timeoutMilliseconds) = 0;

    virtual bool IsRunning() = 0;

    virtual bool PathIsRunning() = 0;

    virtual bool MotionDoneGet() = 0;

    virtual uint64_t MotionDoneWait() = 0;

    virtual uint64_t MotionDoneWait(uint64_t timeoutMilliseconds) = 0;

    virtual bool HasError() = 0;

    virtual const char* const ErrorMessageGet() = 0;

 


    virtual void Resume() = 0;

    virtual void Stop() = 0;

    virtual void EStopAbort() = 0;

    virtual void Abort() = 0;

    virtual int32_t AmpEnableSet(bool enable, int32_t ampActiveTimeoutMilliseconds = RapidCodeMotion::AmpEnableTimeoutMillisecondsDefault, bool overrideRestrictedState = false) = 0;

    virtual void EStop() = 0;

    virtual void ClearFaults() = 0;
 
 

  
    virtual Pose ActualPoseGet(LinearUnits units = LinearUnits::None) = 0;
    
    virtual RobotPosition ActualPositionGet(LinearUnits units = LinearUnits::None) = 0;

    virtual Pose CommandPoseGet(LinearUnits units = LinearUnits::None) = 0;

    virtual RobotPosition CommandPositionGet(LinearUnits units = LinearUnits::None) = 0;

    virtual RapidVector<double> JointsActualPositionsGet() = 0;

    virtual RapidVector<double> JointsCommandPositionsGet() = 0;

    virtual RapidVector<double> InverseKinematics(Pose pose) = 0;

    virtual RapidVector<double> InverseKinematics(RobotPosition pose) = 0;

    virtual Pose ForwardKinematics(const RapidVector<double>& joints) = 0;

    virtual RobotPosition ForwardKinematicsPosition(const RapidVector<double>& joints) = 0;
  

    virtual const KinematicModel& ModelGet() = 0;

    virtual void EndEffectorTransformSet(const Pose& transform) = 0;

    virtual void EndEffectorTransformSet(const RobotPosition& transformPosition) = 0;

    virtual void OriginTransformSet(const Pose& transform) = 0;
    virtual void OriginTransformSet(const RobotPosition& transformPosition) = 0;
    
    virtual const RobotPosition& EndEffectorTransformGet() = 0;

    virtual const RobotPosition& OriginTransformGet() = 0;

    virtual const double ScaleFactorBetweenUnitsGet(LinearUnits from, LinearUnits to) = 0;

    virtual const double ScaleFactorBetweenUnitsGet(RotationalUnits from, RotationalUnits to) = 0;


    virtual void MoveAbsolute(const Pose& absolutePose) = 0;
 

    virtual void MoveRelative(const Pose& relativePose) = 0;
 


    virtual void PathClear() = 0;

    virtual void PathVelocitySet(double unitsPerSecond) = 0;

    virtual double PathVelocityGet() = 0;

    virtual void PathAccelerationSet(double unitsPerSecondSquared) = 0;


    virtual double PathAccelerationGet() = 0;

    virtual void PathProgrammingModeSet(PathMode mode) = 0;

    virtual PathMode PathProgrammingModeGet() = 0;

    virtual void PathArc(const Pose& target, double radius, RotationDirection direction) = 0;
    
    virtual void PathArc(const RobotPosition& target, double radius, RotationDirection direction) = 0;

    virtual void PathArc(const Pose& target, const Vector3d& center, RotationDirection direction) = 0;
    
    virtual void PathArc(const RobotPosition& RobotPosition, const Vector3d& center, RotationDirection direction) = 0;
 
 
    //  @brief Appends a line on the current path
    virtual void PathLine(const Pose& target) = 0;
 
    //  @brief Appends a line on the current path
    virtual void PathLine(const RobotPosition& position) = 0;

    virtual void PathPlaneSet(Plane plane) = 0;

    virtual Plane PathPlaneGet() = 0;

    virtual void PathLinearScalingSet(double scaleFactor) = 0;

    virtual double PathLinearScalingGet()const = 0;

    virtual void PathUnitsSet(LinearUnits units) = 0;

    virtual LinearUnits PathUnitsGet()const = 0;

    virtual double PathProcessLoadedMoves() = 0;

    virtual RapidVector<RobotPosition> PathPlannedPositionsGet(uint64_t startFrame, uint64_t frameCount) = 0;

    virtual double PathDurationGet() = 0;

    virtual void Run() = 0;
    
    virtual uint32_t  MotionFrameBufferSizeGet()=0;

 
    RSI::RapidCode::Cartesian::Gcode* Gcode;
 
  };

  class RSI_API Gcode : public virtual RapidCodeObject
  {
  public:

    virtual void FeedRateSet(double programmingUnitsPerMinute) = 0;

    virtual void AccelerationRateSet(double programmingUnitsPerMinuteSquared) = 0;      


    virtual void Load(const char* const text) = 0;
    
    virtual void LoadFile(const char* const file) = 0;

    virtual void Run() = 0;

    virtual uint64_t DoneWait(uint64_t timeoutMilliseconds) = 0;    
    


    virtual void ReloadLines(int32_t start = 0, int32_t end = -1) = 0;
    
    virtual int32_t LineCountGet() = 0;
   
       virtual const char* const ProgramGet() = 0;
    
    virtual int32_t SyntaxErrorLineNumberGet() = 0;

    virtual const char* const LineTextGet(uint32_t lineNumber) = 0;

    virtual double FeedRateGet() = 0;

    virtual double AccelerationRateGet() = 0;
    
    virtual double DurationGet() = 0;
    
    virtual void Cancel() = 0;
   
    virtual RapidVector<RobotPosition> PlannedPositionsGet(uint64_t startFrame, uint64_t frameCount) = 0;

    virtual void UnitsSet(LinearUnits units) = 0;
    
    virtual LinearUnits UnitsGet() = 0;

    virtual void WorkOffsetConfigure(GCodeWorkOffset workOffset, Vector3d offsetValues) = 0;
    
    virtual void ActiveWorkOffsetSelect(GCodeWorkOffset workOffset) = 0;
    
    virtual GCodeWorkOffset ActiveWorkOffsetGet() = 0;

    virtual const RobotPosition& ActiveWorkOffsetValuesGet() = 0;

    virtual bool IsRunning() = 0;

    virtual int32_t ExecutingLineNumberGet() = 0;

    virtual void FreeAxisLetterSet(const char gcodeLetter, const int32_t freeAxisIndex) = 0;

    virtual void CallbackRegister(Cartesian::GcodeCallback* callback, bool containsMotion = false) = 0;

    virtual void MCodeCallbackRegister(uint32_t mCode, Cartesian::GcodeCallback* callback, bool containsMotion = false) = 0;

  };

 
} // Cartesian namespace
} // RapidCode namespace
} // RSI namespace
 
#endif // defined(HAS_CARTESIAN_ROBOT)
#endif // _CARTESIANROBOT_H