cartesianrobot.h

#pragma once
#ifndef _CARTESIANROBOT_H
#define _CARTESIANROBOT_H
 
#if !__INTIME__
#include "rsi.h"
#include <cmath>
 
 
#ifdef RSIDLL
#  define RSI_API __declspec(dllexport)
#else
#  define RSI_API __declspec(dllimport)
#endif
 
 
#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
#endif // !__INTIME__
#endif // _CARTESIANROBOT_H