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,
    G92 = 92,
  };
 
  enum class LinearUnits
  {
    None,         
    Millimeters,  
    Inches,       
    Feet,         
    Centimeters,  
    Meters,       
 
  };
 
  enum class CartesianAxis
  {
    X = 0,      
    Y = 1,      
    Z = 2,      
    Roll = 3,   
    Pitch = 4,  
    Yaw = 5,    
  };
 
 
  class Gcode; // forward declaration
 
  template<typename ContainerClass>
  class RSI_API RapidVectorIterator
  {
  public:
    typedef typename ContainerClass::ValueType ValueType;
 
    typedef typename ContainerClass::PointerType PointerType;
 
    ValueType& operator*();
 
    const ValueType& operator*() const;
 
    RapidVectorIterator& operator++();
 
    RapidVectorIterator operator++(int);
 
    bool operator==(const RapidVectorIterator&) const;
 
    bool operator!=(const RapidVectorIterator&) const;
 
    RapidVectorIterator(PointerType pointerArgument, PointerType startArgument, const size_t containerSize);
 
    RapidVectorIterator(const RapidVectorIterator&) = default;
 
    RapidVectorIterator& operator=(const RapidVectorIterator&) = default;
 
    RapidVectorIterator(RapidVectorIterator&&) noexcept = default;
 
    RapidVectorIterator& operator=(RapidVectorIterator&&) noexcept = default;
  private:
    PointerType pointer;
    PointerType start, end;
    void VerifyPointer() const;
  };
 
  template<typename Type>
  class RapidVectorImplementation;
 
 
  template<typename Type>
  class RSI_API RapidVector
  {
  private:
    // The pointer to the implementation.
    RapidVectorImplementation<Type>* pImpl;
  public:
    Type& operator[](const size_t index);
 
    const Type& operator[](const size_t index) const;
 
    Type& At(const size_t index);
 
    const Type& At(const size_t index) const;
 
    void PushBack(const Type& element);
 
    const size_t Size() const;
 
    void Clear();
 
    void PopBack();
 
    Type& Front();
 
    const Type& Front() const;
 
    Type& Back();
 
    const Type& Back() const;
 
    RapidVector();
 
    RapidVector(const size_t size);
 
    RapidVector(RapidVectorImplementation<Type>*);
 
    ~RapidVector();
 
    RapidVector(const RapidVector& other);
 
    RapidVector& operator=(const RapidVector& other);
 
    RapidVector(RapidVector&& other) noexcept;
 
    RapidVector& operator=(RapidVector&& other) noexcept;
 
    typedef typename Type* PointerType;
    typedef typename Type ValueType;
 
    typedef typename RapidVectorIterator<RapidVector> Iterator;
 
    Iterator begin() noexcept;
 
    Iterator begin() const noexcept;
 
    Iterator end() const noexcept;
  };
 
  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);
 
    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);
 
    bool Equals(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() 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;
 
    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:
    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;
  };
 
  class RSI_API KinematicModel
  {
  public:
    virtual const char* const NameGet() const = 0;
    virtual uint32_t AxisCountGet() const = 0;
 
    virtual LinearUnits UnitsGet() const = 0;
 
    virtual bool IsConfigured() const = 0;
 
    virtual const char* const* const ExpectedAxisNamesGet() const = 0;
    virtual uint32_t ExpectedAxisNamesCountGet() 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;
 
  };
 
  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
  {
  private:
    struct Storage;
    Storage* storage;
 
  protected:
    KinematicModelBuilder(const char* const label);
 
    // Protect the copy constructor, for internal use only.
    KinematicModelBuilder(const KinematicModelBuilder&);
    
 
    class ProtectedAccessor;
    // Provide derived classes with protected access to certain members.
    ProtectedAccessor ProtectedAccessGet();
 
  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 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
  {
    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
  {
    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 bool MotionCounterWait(uint64_t motionWaitFor, uint64_t timeoutMilliseconds) = 0;
 
    virtual bool MotionCounterWaitChange(uint64_t motionWaitChange, 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 void AmpEnableSet(bool enable, int32_t ampActiveTimeoutMilliseconds) = 0;
 
    virtual void EStop() = 0;
 
    virtual void ClearFaults() = 0;
 
 
 
    
    virtual Pose ActualPoseGet() = 0;
 
    virtual RobotPosition ActualPositionGet() = 0;
    
 
    virtual Pose ActualPoseGet(LinearUnits units) = 0;
    
    virtual RobotPosition ActualPositionGet(LinearUnits units) = 0;
 
    
    virtual Pose CommandPoseGet() = 0;
 
    virtual RobotPosition CommandPositionGet() = 0;
 
    virtual Pose CommandPoseGet(LinearUnits units) = 0;
 
    virtual RobotPosition CommandPositionGet(LinearUnits units) = 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 OriginTransformSet(const Pose& transform) = 0;
    
    virtual const Pose& EndEffectorTransformGet() = 0;
 
    virtual const Pose& OriginTransformGet() = 0;
 
 
    virtual const double ScaleFactorBetweenUnitsGet(LinearUnits from, LinearUnits 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 bool Load(const char* const text) = 0;
    virtual bool LoadFile(const char* const file) = 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 void FeedRateSet(double programmingUnitsPerMinute) = 0;
 
    virtual double FeedRateGet() = 0;
 
    virtual void AccelerationRateSet(double programmingUnitsPerMinuteSquared) = 0;
 
    virtual double AccelerationRateGet() = 0;
 
    virtual void Run() = 0;
    
    virtual double DurationGet() = 0;
    
    virtual void Cancel() = 0;
 
    virtual uint64_t DoneWait(uint64_t timeoutMilliseconds) = 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 Pose& ActiveWorkOffsetValuesGet() = 0;
 
    virtual bool IsRunning() = 0;
 
    virtual int32_t ExecutingLineNumberGet() = 0;
 
    virtual void FreeAxisLetterSet(const char gcodeLetter, const int freeAxisIndex) = 0;
 
    virtual void CallbackRegister(Cartesian::GcodeCallback* callback) = 0;
  };
 
 
} // Cartesian namespace
} // RapidCode namespace
} // RSI namespace
 
#endif // defined(HAS_CARTESIAN_ROBOT)
#endif // _CARTESIANROBOT_H