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>
 
 
#if defined(__cplusplus)
extern "C"
{
#endif
 
// doxygen requires these stacked namespaces, you CANNOT use RSI::RapidCode::Cartesian {
namespace RSI
{
namespace RapidCode
{
namespace Cartesian
{
 
  static constexpr uint32_t MotionFrameBufferSizeMinimum=50;// Minimum size the trajectory executor buffer must be  
 
 
  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 CartesianAxis
  {
    X = 0,      
    Y = 1,      
    Z = 2,      
    Roll = 3,   
    Pitch = 4,  
    Yaw = 5,    
  };
 
 
 
  class RSI_API Vector3d
  {
  public:
    double X;
    
    double Y;
    
    double Z;
 
     Vector3d();
 
     Vector3d(double x, double y, double z);
 
     Vector3d(const Vector3d& rhs);
 
     double Dot(const Vector3d& rhs) const;
 
     Vector3d Cross(const Vector3d& rhs) const;
 
     Vector3d& Set(double x, double y, double z);
 
     Vector3d& Set(const Vector3d& rhs);
 
     bool Equals(const Vector3d& rhs, const double tolerance) const;
 
     double SumSquares() const;
 
     double Length() const;
 
     Vector3d Normal() const;
 
     Vector3d& Normalize();
 
     Vector3d& operator=(const Vector3d& rhs);
     bool operator==(const Vector3d& rhs) const;
     bool operator!=(const Vector3d& rhs) const;
     Vector3d operator*(double scalar) const;
     Vector3d& operator*=(double scalar);
     Vector3d operator+(const Vector3d& rhs) const;
     Vector3d& operator+=(const Vector3d& rhs);
     Vector3d operator-(const Vector3d& rhs) const;
     Vector3d operator-() const;
     Vector3d& operator-=(const Vector3d& rhs);
     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& rhs);
 
     Quaternion(double w, const Vector3d& v);
 
     Quaternion(double x, double y, double z);
 
     Quaternion(const Vector3d& rpy);
 
     bool operator==(const Quaternion& rhs) const;
 
     bool operator!=(const Quaternion& rhs) const;
 
     Quaternion& operator=(const Quaternion& rhs);
 
     Quaternion operator+(const Quaternion& rhs) const;
 
     Quaternion& operator+=(const Quaternion& rhs);
 
     Quaternion operator-(const Quaternion& rhs) const;
 
     Quaternion& operator-=(const Quaternion& rhs);
    
     Quaternion operator*(const Quaternion& rhs) const;
 
     Quaternion operator*(double s) const;
 
     Quaternion& operator*=(double s);
 
     Vector3d operator*(const Vector3d& rhs) const;
 
     Quaternion operator-() const;
 
     double Dot(const Quaternion& rhs) const;
 
     Quaternion& Set(const Quaternion& rhs);
 
     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& rhs, 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& out_angle, Vector3d& out_axis) const;
 
     Vector3d ToEuler() const;
 
 
    static Quaternion FromMatrix(const double* const mat);
    
 
     void ToMatrix(double* mat) const;
 
 
    //Quaternion& lerp(const Quaternion& q0, const Quaternion& q1, double time);
 
    //static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, double time, bool forceShortArc = true);
 
 
    //static Quaternion rotationFromTo(const Vector3d& from, const Vector3d& to);
  };
 
  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& rhs, double tolerance) const;
 
     Pose Rotate(const Pose& rhs) const;
 
     Vector3d operator*(const Vector3d& rhs) const;
 
     Pose operator*(const Pose& rhs) const;
 
     Pose 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 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 SolveInverse(const Pose& inputPose, void* outputPointerSolutionVector) const = 0;
 
    // ouput function
    // inputJointPositions requires this to be in robot centric coordinates.
    // apply tool offset and robot base offset
    virtual void SolveForward(const double* inputJointPositions, Pose& outputPose) const = 0;
  };
 
 
  class RSI_API KinematicModelBuilder
  {
  private:
    struct Storage;
    Storage* storage;
 
  protected:
     KinematicModelBuilder(const char* const label);
    virtual ~KinematicModelBuilder();
 
    // Disabled copy constructors
     KinematicModelBuilder(const KinematicModelBuilder&);
     KinematicModelBuilder& operator=(const KinematicModelBuilder&) = delete;
 
     Storage* StorageGet();
     const Storage* StorageGet() const;
     void StorageSet(Storage*);
 
  public:
 
    virtual const KinematicModel& ModelBuild() = 0;
 
     const KinematicModel& ModelGet();
 
     void UnitsSet(LinearUnits units);
 
     void LabelSet(const char* const label);
 
     void TransformFromKinematicOriginToRobotOriginSet(const Pose& KinematicOriginToRobotOrigin);
 
    // Move constructors
     KinematicModelBuilder(KinematicModelBuilder&&) noexcept;
     KinematicModelBuilder& operator=(KinematicModelBuilder&&) noexcept;
 
  
 
    
  };
 
  struct RSI_API LinearJointMapping
  {
  public:
    int JointIndex;
 
    CartesianAxis CartesianCoordinate;
 
    double Scaling;
    static constexpr double DEFAULT_SCALING = 1.0;
 
    double Offset;
    static constexpr double DEFAULT_OFFSET = 0.0;
 
    static constexpr int MAX_LABEL_SIZE = 16;
    static constexpr int EXPECTED_LABEL_BUFFER_SIZE = MAX_LABEL_SIZE + 1;
 
    char ExpectedLabel[EXPECTED_LABEL_BUFFER_SIZE];
 
     LinearJointMapping(int jointIndex, CartesianAxis cartesianCoordinate);
  };
 
 
  class RSI_API LinearModelBuilder : public KinematicModelBuilder
  {
  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;
 
    struct Storage;
    Storage* storage;
  };
 
  // UNSUPPORTED
  class RSI_API ArticulatedModelBuilder : public KinematicModelBuilder
  {
  public:
     const KinematicModel& ModelBuild() override;
     ArticulatedModelBuilder();
     ~ArticulatedModelBuilder();
 
    struct Storage;
    Storage* storage;
  };
 
 
 
  class RSI_API Robot : public virtual RapidCodeObject
  {
  public:
 
 
    // RSI RA5 (Hans)          "RSI_RA5";
    static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier, uint32_t  motionFrameBufferSize);
   
    static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier);
 
    static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, KinematicModelBuilder* builder, uint32_t motionFrameBufferSize);
 
 
    static void RobotDelete(MotionController* controller, Robot* robot);
 
 
 
 
 
 
    static constexpr uint64_t TimeoutForever = 0xFFFFFFFFFFFFFFFF;
 
 
 
 
    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 ampActiveTimoutMilliseconds) = 0;
 
    virtual void EStop() = 0;
 
    virtual void ClearFaults() = 0;
 
 
 
    
    virtual Pose ActualPoseGet() = 0;
 
    virtual Pose CommandPoseGet() = 0;
 
 
    virtual void JointsActualPositionsGet(double* positions, uint16_t sz) = 0;
 
    virtual void JointsCommandPositionsGet(double* positions, uint16_t sz) = 0;
 
    virtual void InverseKinematics(Pose pose, double* outJoints, uint16_t outJointsSize) = 0;
 
    virtual Pose ForwardKinematics(double* inJoints, uint16_t inJointsSize) = 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 Pose& target, const Vector3d& center, RotationDirection direction) = 0;
 
    //  @brief Appends a line on the current path
    virtual void PathLine(const Pose& target) = 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 uint64_t PathPlannedPointsGet(double* points, double* times, uint64_t startFrame, uint64_t frameCount) = 0;
 
    virtual double PathDurationGet() = 0;
 
    virtual void Run() = 0;
    
    virtual uint32_t  MotionFrameBufferSizeGet()=0;
 
 
    virtual bool GcodeFileLoad(const char* const filePath) = 0;
 
    virtual bool GcodeStringLoad(const char* const text) = 0;
 
    virtual const char* const GcodeProgramGet() = 0;
 
    virtual void GcodeFeedRateSet(double unitsPerMinute) = 0;
 
    virtual double GcodeFeedRateGet() = 0;
 
    virtual void GcodeAccelerationRateSet(double unitsPerMinuteSquared) = 0;
 
    virtual double GcodeAccelerationRateGet() = 0;
 
    virtual int32_t GcodeExecutingLineNumberGet() = 0;
 
 
    virtual void GcodeUnitsSet(LinearUnits units) = 0;
 
    virtual LinearUnits GcodeUnitsGet() = 0;
 
    virtual void GcodeProgrammingModeSet(PathMode mode) = 0;
 
    virtual PathMode GcodeProgrammingModeGet() = 0;
 
    virtual int32_t GcodeErrorLineNumberGet() = 0;
 
    virtual int32_t GcodeNumberOfLinesGet() = 0;
 
    virtual void GcodeReload() = 0;
 
    virtual void GcodeReload(int32_t startLine, int32_t endLine) = 0;
 
    virtual void GcodeReload(int32_t startLine) = 0;
 
    virtual const char* const GcodeLineTextGet(uint32_t lineNumber) = 0;
 
    virtual void GcodePlaneSet(Plane plane) = 0;
 
    virtual void GcodeWorkOffsetConfigure(GCodeWorkOffset offset, Vector3d offsetValues) = 0;
 
    virtual void GcodeWorkOffsetSelect(GCodeWorkOffset offset) = 0;
 
    virtual GCodeWorkOffset GcodeWorkOffsetGet() = 0;
 
    virtual const Pose& GcodeWorkOffsetValuesGet() = 0;
 
  };
 
 
  
 
 
} // Cartesian namespace
} // RapidCode namespace
} // RSI namespace
#if defined(__cplusplus)
} // extern C
#endif // defined(__cplusplus)
#endif // defined(HAS_CARTESIAN_ROBOT)
#endif // _CARTESIANROBOT_H