The RMP Motion Controller APIs
cartesianrobot.h
1 #pragma once
2 #ifndef _CARTESIANROBOT_H
3 #define _CARTESIANROBOT_H
4 
5 #if !__INTIME__
6 #include "rsi.h"
7 #include <cmath>
8 
9 
10 #ifdef RSIDLL
11 # define RSI_API __declspec(dllexport)
12 #else
13 # define RSI_API __declspec(dllimport)
14 #endif
15 
16 
17 #if defined(__cplusplus)
18 extern "C"
19 {
20 #endif
21 
22 // doxygen requires these stacked namespaces, you CANNOT use RSI::RapidCode::Cartesian {
23 namespace RSI
24 {
25 namespace RapidCode
26 {
28 namespace Cartesian
29 {
30 
31  static constexpr uint32_t MotionFrameBufferSizeMinimum=50;// Minimum size the trajectory executor buffer must be
32 
35 
37  enum class PathState
38  {
39  Uninitialized,
40  Idle,
41  Moving,
42  Stopped,
43  Stopping,
44  Finishing,
45  };
46 
47  constexpr double pi = 3.1415926535897932384626433832795028841971693993751;
48 
50  enum class PathMode
51  {
52  Absolute,
53  Relative,
54  };
55 
57  enum class RotationDirection
58  {
59  Clockwise,
61  };
62 
64  enum class Plane
65  {
66  None,
67  XY,
68  XZ,
69  YZ,
70  };
71 
73  enum class GCodeWorkOffset {
74  Machine = 0,
75  G54 = 54,
76  G55 = 55,
77  G56 = 56,
78  G57 = 57,
79  G58 = 58,
80  G59 = 59,
81  };
82 
84  enum class LinearUnits
85  {
86  None,
87  Millimeters,
88  Inches,
89  Feet,
90  Centimeters,
91  Meters,
92 
93  };
94 
96 
99 
101  class RSI_API Vector3d
102  {
103  public:
105  double X;
106 
108  double Y;
109 
111  double Z;
112 
115 
117  Vector3d(double x, double y, double z);
118 
120  Vector3d(const Vector3d& rhs);
121 
123  double Dot(const Vector3d& rhs) const;
124 
129  Vector3d Cross(const Vector3d& rhs) const;
130 
132  Vector3d& Set(double x, double y, double z);
133 
135  Vector3d& Set(const Vector3d& rhs);
136 
139  bool Equals(const Vector3d& rhs, const double tolerance) const;
140 
142  double SumSquares() const;
143 
145  double Length() const;
146 
148  Vector3d Normal() const;
149 
152 
156  bool operator==(const Vector3d& rhs) const;
158  bool operator!=(const Vector3d& rhs) const;
160  Vector3d operator*(double scalar) const;
162  Vector3d& operator*=(double scalar);
164  Vector3d operator+(const Vector3d& rhs) const;
168  Vector3d operator-(const Vector3d& rhs) const;
174  Vector3d operator/(double scalar) const;
176  Vector3d& operator/=(double scalar);
177  };
178 
180  class RSI_API Quaternion
181  {
182  public:
185 
187  double W;
188 
191 
197  Quaternion(double w, double x, double y, double z);
198 
201  Quaternion(const Quaternion& rhs);
202 
206  Quaternion(double w, const Vector3d& v);
207 
212  Quaternion(double x, double y, double z);
213 
216  Quaternion(const Vector3d& rpy);
217 
221  bool operator==(const Quaternion& rhs) const;
222 
226  bool operator!=(const Quaternion& rhs) const;
227 
232 
236  Quaternion operator+(const Quaternion& rhs) const;
237 
242 
246  Quaternion operator-(const Quaternion& rhs) const;
247 
252 
256  Quaternion operator*(const Quaternion& rhs) const;
257 
261  Quaternion operator*(double s) const;
262 
266  Quaternion& operator*=(double s);
267 
271  Vector3d operator*(const Vector3d& rhs) const;
272 
276 
280  double Dot(const Quaternion& rhs) const;
281 
285  Quaternion& Set(const Quaternion& rhs);
286 
293  Quaternion& Set(double w, double x, double y, double z);
294 
299  Quaternion& Set(double w, const Vector3d& vec);
300 
307  Quaternion& Set(double x, double y, double z);
308 
312  Quaternion& Set(const Vector3d& vec);
313 
318  bool Equals(const Quaternion& rhs, const double tolerance) const;
319 
323 
327 
328 
331  double SumSquares() const;
332 
335  double Magnitude() const;
336 
340 
344 
345 
346 
351  static Quaternion FromAngleAxis(double angle, const Vector3d& axis);
352 
356  void ToAngleAxis(double& out_angle, Vector3d& out_axis) const;
357 
360  Vector3d ToEuler() const;
361 
362 
369  static Quaternion FromMatrix(const double* const mat);
370 
371 
377  void ToMatrix(double* mat) const;
378 
379 
385  //Quaternion& lerp(const Quaternion& q0, const Quaternion& q1, double time);
386 
393  //static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, double time, bool forceShortArc = true);
394 
395 
400  //static Quaternion rotationFromTo(const Vector3d& from, const Vector3d& to);
401  };
402 
404  class RSI_API Pose {
405  public:
408 
411 
413  Pose();
414 
417  Pose(const Pose& pose);
418 
422  Pose(const Vector3d position, const Quaternion& quaternion);
423 
426  Pose(const Vector3d& position);
427 
430  Pose(const Quaternion& quaternion);
431 
436  bool Equals(const Pose& rhs, double tolerance) const;
437 
441  Pose Rotate(const Pose& rhs) const;
442 
446  Vector3d operator*(const Vector3d& rhs) const;
447 
451  Pose operator*(const Pose& rhs) const;
452 
455  Pose Inverse() const;
456  };
457 
459  class RSI_API KinematicModel
460  {
461  public:
462  virtual const char*const NameGet() const = 0;
463  virtual uint32_t AxisCountGet() const = 0;
464  virtual const Pose& TransformToEndOfLastJointGet() const = 0;
465  virtual const Pose& TransformToEndOfLastJointInverseGet() const = 0;
466 
467  virtual LinearUnits UnitsGet() const = 0;
468 
469  virtual bool IsConfigured() const = 0;
470 
471  virtual const char* const* const ExpectedAxisNamesGet() const = 0;
472  virtual uint32_t ExpectedAxisNamesCountGet() const = 0;
473 
474  // input function
475  virtual bool SolveInverse(const double* in_eepos, const double* in_eerot, void* vpSols) const = 0;
476  // ouput function
477  virtual void SolveForward(const double* j, double* out_eepos, double* out_eerot) const = 0;
478  };
479 
482  class RSI_API Robot : public virtual RapidCodeObject
483  {
484  public:
485 
490 
495  // RSI RA5 (Hans) "RSI_RA5";
496  static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier, uint32_t motionFrameBufferSize);
497 
499  static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier);
500 
501 
503  static void RobotDelete(MotionController* controller, Robot* robot);
504 
505 
506 
508 
509 
514 
516  static constexpr uint64_t TimeoutForever = 0xFFFFFFFFFFFFFFFF;
517 
519 
520 
525 
528  virtual PathState PathStateGet() = 0;
529 
534  virtual bool PathStateWait(PathState stateWaitFor, uint64_t timeoutMilliseconds) = 0;
535 
540  virtual bool PathStateWaitChange(PathState stateWaitChange, uint64_t timeoutMilliseconds) = 0;
541 
542 
547  virtual uint64_t MotionCounterGet() = 0;
548 
553  virtual bool MotionCounterWait(uint64_t motionWaitFor, uint64_t timeoutMilliseconds) = 0;
554 
559  virtual bool MotionCounterWaitChange(uint64_t motionWaitChange, uint64_t timeoutMilliseconds) = 0;
560 
561 
562 
566  virtual bool IsRunning() = 0;
567 
574  virtual bool MotionDoneGet() = 0;
575 
582  virtual uint64_t MotionDoneWait() = 0;
583 
591  virtual uint64_t MotionDoneWait(uint64_t timeoutMilliseconds) = 0;
592 
598  virtual bool HasError() = 0;
599 
604  virtual const char* const ErrorMessageGet() = 0;
605 
607 
608 
612 
619  virtual void PathEStop() = 0;
620 
622  virtual void ClearFaults() = 0;
623 
624 
625 
629 
634  virtual Pose ActualPoseGet() = 0;
635 
640  virtual Pose CommandPoseGet() = 0;
641 
642 
647  virtual void JointsActualPositionsGet(double* positions, uint16_t sz) = 0;
648 
653  virtual void JointsCommandPositionsGet(double* positions, uint16_t sz) = 0;
654 
659  virtual void InverseKinematics(Pose pose, double* outJoints, uint16_t outJointsSize) = 0;
660 
666  virtual Pose ForwardKinematics(double* inJoints, uint16_t inJointsSize) = 0;
667 
668 
672 
675  virtual const KinematicModel& ModelGet() = 0;
676 
677 
681  virtual void GlobalTranformSet(const Pose& transform) = 0;
682 
685  virtual const Pose& GlobalTranformGet() = 0;
686 
689  virtual void EndEffectorTransformSet(const Pose& transform) = 0;
690 
692  virtual const Pose& EndEffectorTransformGet() = 0;
693 
694 
699  virtual const double ScaleFactorBetweenUnitsGet(LinearUnits from, LinearUnits to) = 0;
700 
701 
705 
714  virtual void Move(const Pose& absolutePose) = 0;
715 
716 
726  virtual void MoveRelative(const Pose& relativePose) = 0;
727 
728 
732 
736  virtual void PathClear() = 0;
737 
742  virtual void PathVelocitySet(double velocity) = 0;
743 
748  virtual double PathVelocityGet() = 0;
749 
754  virtual void PathAccelerationSet(double acceleration) = 0;
755 
761  virtual double PathAccelerationGet() = 0;
762 
769  virtual void PathProgrammingModeSet(PathMode mode) = 0;
770 
777 
783  virtual void PathArc(const Pose& target, double radius, RotationDirection direction) = 0;
784 
789  virtual void PathArc(const Pose& target, const Vector3d& center, RotationDirection direction) = 0;
790 
791  // Target is absolute wrt world origin and in Linear Programming Units
792  // Uses robot model end effector offset!
793  // If world offset is not set, Identity TF is used
794  virtual void PathLine(const Pose& target) = 0;
795 
796  // Target is absolute wrt world origin and in Linear Programming Units
797  // Uses specified End Effector Offset!
798  // If world offset is not set, Identity TF is used
799  virtual void PathLine(const Pose& target, const Pose& tool_offset) = 0;
800 
804  virtual void PathPlaneSet(Plane plane) = 0;
805 
809  virtual Plane PathPlaneGet() = 0;
810 
811 
817  virtual void PathLinearScalingSet(double scaleFactor) = 0;
818 
825  virtual double PathLinearScalingGet()const = 0;
826 
831  virtual void PathUnitsSet(LinearUnits units) = 0;
832 
836  virtual LinearUnits PathUnitsGet()const = 0;
837 
838 
841  virtual void PathProcessLoadedMoves() = 0;
842 
855  virtual uint64_t PathPlannedPointsGet(double* points, double* times, uint64_t startFrame, uint64_t frameCount) = 0;
856 
861  virtual double PathLoadedMovesExecutionTimeGet() = 0;
862 
869  virtual void PathRun(bool blocking) = 0;
870 
872  virtual void PathRun() = 0;
873 
879  virtual uint32_t MotionFrameBufferSizeGet()=0;
880 
884 
894  virtual bool GcodeFileLoad(const char* const filePath) = 0;
895 
905  virtual bool GcodeStringLoad(const char* const text) = 0;
906 
912  virtual const char* const GcodeProgramGet() = 0;
913 
919  virtual void GcodeFeedRateSet(double feedRate) = 0;
920 
928  virtual double GcodeFeedrateGet() = 0;
929 
935  virtual void GcodeAccelerationSet(double acceleration) = 0;
936 
943  virtual double GcodeAccelerationGet() = 0;
944 
951  virtual void GcodeRun(bool blocking) = 0;
952 
959  virtual void GcodeRun() = 0;
960 
969  virtual int32_t GcodeExecutingLineNumberGet() = 0;
970 
971 
980  virtual void GcodeUnitsSet(LinearUnits units) = 0;
981 
989  virtual LinearUnits GcodeUnitsGet() = 0;
990 
998  virtual void GcodeProgrammingModeSet(PathMode mode) = 0;
999 
1006 
1015  virtual int32_t GcodeErrorLineNumberGet() = 0;
1016 
1023  virtual int32_t GcodeNumberOfLinesGet() = 0;
1024 
1030  virtual void GcodeReload() = 0;
1031 
1037  virtual void GcodeReload(int32_t startLine, int32_t endLine) = 0;
1038 
1044  virtual void GcodeReload(int32_t startLine) = 0;
1045 
1054  virtual const char* const GcodeLineTextGet(uint32_t lineNumber) = 0;
1055 
1062  virtual void GcodePlaneSet(Plane plane) = 0;
1063 
1070  virtual void GcodeWorkOffsetConfigure(GCodeWorkOffset offset, Vector3d offsetValues) = 0;
1071 
1078  virtual void GcodeWorkOffsetSelect(GCodeWorkOffset offset) = 0;
1079 
1087 
1094  virtual const Pose& GcodeWorkOffsetValuesGet() = 0;
1095 
1096  };
1097 
1099 
1100 } // Cartesian namespace
1101 } // RapidCode namespace
1102 } // RSI namespace
1103 #if defined(__cplusplus)
1104 } // extern C
1105 #endif
1106 #endif // !__INTIME__
1107 #endif // _CARTESIANROBOT_H
1108 
RSI::RapidCode::Cartesian::Robot::GcodeFeedrateGet
double GcodeFeedrateGet()=0
Gets the target feed rate for the machine (GcodeUnitsGet()/minute).
RSI::RapidCode::Cartesian::Robot::GcodeRun
void GcodeRun()=0
Run the last loaded G-Code lines. Default behavior is blocking. This is an overloaded member function...
RSI::RapidCode::Cartesian::Robot::PathLinearScalingSet
void PathLinearScalingSet(double scaleFactor)=0
Sets scaling between the input to path motion and output of path motion to the kinematic model.
RSI::RapidCode::Cartesian::Robot::GcodeWorkOffsetGet
GCodeWorkOffset GcodeWorkOffsetGet()=0
Gets the enum representing the location where the current offset is saved.
RSI::RapidCode::MultiAxis
The MultiAxis object allows you to map two or more axes together and command synchronized motion.
Definition: rsi.h:11028
RSI::RapidCode::Cartesian::Robot::PathProcessLoadedMoves
void PathProcessLoadedMoves()=0
Processes our loaded moves. Used internally and to check whether loaded moves are valid,...
RSI::RapidCode::Cartesian::Quaternion::ToMatrix
void ToMatrix(double *mat) const
Quaternion To Matrix.
RSI::RapidCode::Cartesian::Robot
The Robot class for Path, G-Code and RapidRobot motion for any actuator. Use RobotCreate() to make on...
Definition: cartesianrobot.h:483
RSI::RapidCode::Cartesian::Robot::GcodeProgrammingModeGet
PathMode GcodeProgrammingModeGet()=0
Gets the G-Code programming mode (Relative/Absolute).
RSI::RapidCode::Cartesian::Quaternion::operator!=
bool operator!=(const Quaternion &rhs) const
Exact inequality check. See equals for approximately equal to.
RSI::RapidCode::Cartesian::Pose::Orientation
Quaternion Orientation
The orientation component of the Pose.
Definition: cartesianrobot.h:410
RSI::RapidCode::Cartesian::Quaternion::Conjugate
Quaternion Conjugate() const
Negates the imaginary component of the Quaternion.
RSI::RapidCode::Cartesian::RotationDirection
RotationDirection
Rotational directions about an axis.
Definition: cartesianrobot.h:58
RSI::RapidCode::Cartesian::Robot::GcodeFileLoad
bool GcodeFileLoad(const char *const filePath)=0
Open a file at the specified file path. Calls GcodeStringLoad() with the contents of the file.
RSI::RapidCode::Cartesian::Quaternion::operator*
Quaternion operator*(double s) const
Multiplication (scalar) operator. w*s, v*s.
RSI::RapidCode::Cartesian::Vector3d::SumSquares
double SumSquares() const
Returns the sum of squares. X*X+Y*Y+Z*Z.
RSI::RapidCode::Cartesian::Pose::Position
Vector3d Position
The position component of the Pose.
Definition: cartesianrobot.h:407
RSI::RapidCode::Cartesian::Robot::GcodeReload
void GcodeReload(int32_t startLine, int32_t endLine)=0
Reloads the lines from the last call to GcodeStringLoad() but only between start and end line numbers...
RSI::RapidCode::Cartesian::Robot::ModelGet
const KinematicModel & ModelGet()=0
Get the model this robot was created with Visit our Topic Page for more information.
RSI::RapidCode::Cartesian::Robot::GcodeRun
void GcodeRun(bool blocking)=0
Run the last loaded item. Optional argument to prevent blocking. Check GcodeIsRunning(); to see if ex...
RSI::RapidCode::Cartesian::Robot::GcodeAccelerationGet
double GcodeAccelerationGet()=0
Gets the target acceleration for the machine (GcodeUnitsGet()/minute^2).
RSI::RapidCode::Cartesian::Robot::PathVelocityGet
double PathVelocityGet()=0
Gets the target velocity for the machine (UserUnits/second).
RSI::RapidCode::Cartesian::Quaternion::Dot
double Dot(const Quaternion &rhs) const
Returns the dot product of each element. w*rhs.w+v.dot(rhs.v)
RSI::RapidCode::Cartesian::Vector3d::operator/
Vector3d operator/(double scalar) const
Returns a Vector3d scaled scaled by. X/scalar, Y/scalar, Z/scalar.
RSI::RapidCode::Cartesian::Quaternion::Quaternion
Quaternion(const Quaternion &rhs)
Constructor.
RSI::RapidCode::Cartesian::Robot::GcodeProgrammingModeSet
void GcodeProgrammingModeSet(PathMode mode)=0
Sets the programming mode (Relative/Absolute).
RSI::RapidCode::Cartesian::Quaternion::operator+
Quaternion operator+(const Quaternion &rhs) const
Adds two quaternions together. this.w + rhs.w, this.v+rhs.v.
RSI::RapidCode::Cartesian::Robot::EndEffectorTransformSet
void EndEffectorTransformSet(const Pose &transform)=0
End of Robot to TCP.
RSI::RapidCode::Cartesian::Quaternion::ToEuler
Vector3d ToEuler() const
Gets the Euler Angle representation of this quaternion.
RSI::RapidCode::Cartesian::Quaternion::Magnitude
double Magnitude() const
Gets the magnitude of this. sqrt(SumSquares())
RSI::RapidCode::Cartesian::Vector3d::X
double X
The X coordinate.
Definition: cartesianrobot.h:105
RSI::RapidCode::Cartesian::Vector3d::operator/=
Vector3d & operator/=(double scalar)
Scales each element by scalar. X/=scalar, Y/=scalar, Z/=scalar.
RSI::RapidCode::Cartesian::Vector3d::Set
Vector3d & Set(const Vector3d &rhs)
Sets X=rhs.X, Y=rhs.Y, Z=rhs.Z.
RSI::RapidCode::Cartesian::Plane
Plane
Rotational directions about and axis.
Definition: cartesianrobot.h:65
RSI::RapidCode::Cartesian::Quaternion::operator*
Quaternion operator*(const Quaternion &rhs) const
Multiplication (rotation) operator. Rotates other by this.
RSI::RapidCode::Cartesian::Quaternion
Quaternion for representing rotations.
Definition: cartesianrobot.h:181
RSI::RapidCode::Cartesian::Vector3d::operator!=
bool operator!=(const Vector3d &rhs) const
Returns whether this is not exactly equal to rhs.
RSI::RapidCode::Cartesian::KinematicModel
Describes the mathematical kinematic model of a robot.
Definition: cartesianrobot.h:460
RSI::RapidCode::Cartesian::Vector3d::operator*=
Vector3d & operator*=(double scalar)
Scales each element by scalar. X*=scalar, Y*=scalar, Z*=scalar.
RSI::RapidCode::Cartesian::Robot::MotionDoneWait
uint64_t MotionDoneWait(uint64_t timeoutMilliseconds)=0
Waits for a move to complete.
RSI::RapidCode::Cartesian::Robot::PathPlaneSet
void PathPlaneSet(Plane plane)=0
Sets the plane for the purposes of ambigous cases (arcs >= 180deg). Last set plane or XY plane is def...
RSI::RapidCode::Cartesian::Vector3d::Y
double Y
The Y coordinate.
Definition: cartesianrobot.h:108
RSI::RapidCode::Cartesian::Robot::GcodeReload
void GcodeReload()=0
Reloads the all lines from the last call to GcodeStringLoad(). Automatically called by GcodeStringLoa...
RSI::RapidCode::Cartesian::Vector3d::Length
double Length() const
Returns the length of the vector. Aka the magnitude =sqrt(SumSquares)
RSI::RapidCode::Cartesian::Quaternion::Quaternion
Quaternion(double w, double x, double y, double z)
Constructor.
RSI::RapidCode::Cartesian::Robot::PathRun
void PathRun()=0
RSI::RapidCode::Cartesian::Quaternion::Quaternion
Quaternion(const Vector3d &rpy)
Euler Constructor (Alternate)
RSI::RapidCode::Cartesian::Vector3d::operator+
Vector3d operator+(const Vector3d &rhs) const
Returns a copy of rhs added to this. this.X+rhs.X, this.Y+rhs.Y, this.Z+rhs.Z.
RSI::RapidCode::Cartesian::Quaternion::ToAngleAxis
void ToAngleAxis(double &out_angle, Vector3d &out_axis) const
Gets the axis angle representation of this quaternion.
RSI::RapidCode::Cartesian::Quaternion::operator-=
Quaternion & operator-=(const Quaternion &rhs)
Subtraction assignment. Subtracts other from this. See operator-.
RSI::RapidCode::Cartesian::Pose::Inverse
Pose Inverse() const
Inverts this. this * this.inverse() = I.
RSI::RapidCode::Cartesian::Quaternion::operator*
Vector3d operator*(const Vector3d &rhs) const
Multiplication (vector rotation) operator. Rotates vector by this.
RSI::RapidCode::Cartesian::Robot::MotionDoneGet
bool MotionDoneGet()=0
Check to see if motion is done and settled.
RSI::RapidCode::Cartesian::Robot::GcodeWorkOffsetConfigure
void GcodeWorkOffsetConfigure(GCodeWorkOffset offset, Vector3d offsetValues)=0
Save an XYZ offset to be used with specified G-Code work offset.
RSI::RapidCode::Cartesian::Robot::ForwardKinematics
Pose ForwardKinematics(double *inJoints, uint16_t inJointsSize)=0
Get the Actual pose of the robot from the the joint MultiAxis positions.
RSI::RapidCode::Cartesian::Robot::PathUnitsSet
void PathUnitsSet(LinearUnits units)=0
Defines the units Cartesian Path Motion is in.
RSI::RapidCode::Cartesian::Quaternion::V
Vector3d V
Vector3d (imaginary) component of Quaternion. 1->x, 2->y, 3->z. Do not modify this unless you know wh...
Definition: cartesianrobot.h:184
RSI::RapidCode::Cartesian::Vector3d::operator=
Vector3d & operator=(const Vector3d &rhs)
Sets this = rhs.
RSI::RapidCode::Cartesian::Robot::GcodeExecutingLineNumberGet
int32_t GcodeExecutingLineNumberGet()=0
Returns the line number of the currently executing G-Code line or -1 if there is none.
RSI::RapidCode::Cartesian::Pose::Pose
Pose(const Pose &pose)
Constructor from an existing Pose.
RSI::RapidCode::Cartesian::Vector3d::Dot
double Dot(const Vector3d &rhs) const
Dot Product. this.X*rhs.X + this.Y*rhs.Y + this.Z*rhs.Z.
RSI::RapidCode::Cartesian::Quaternion::operator-
Quaternion operator-(const Quaternion &rhs) const
Subtraction operator. this.w - rhs.w, this.v-rhs.v.
RSI::RapidCode::Cartesian::Quaternion::operator*=
Quaternion & operator*=(double s)
Multiplication (scalar) assignment operator. See operator*(double)
RSI::RapidCode::Cartesian::Vector3d::Vector3d
Vector3d()
Default constructor. X=Y=Z=0.
RSI::RapidCode::Cartesian::RotationDirection::Clockwise
@ Clockwise
RSI::RapidCode::RapidCodeObject
The RapidCode base class. All non-error objects are derived from this class.
Definition: rsi.h:145
RSI::RapidCode::Cartesian::Robot::ClearFaults
void ClearFaults()=0
Clears the MultiAxis fault then the Robot's error bit.
RSI::RapidCode::Cartesian::Vector3d::operator*
Vector3d operator*(double scalar) const
Returns a Vector3d scaled scaled by. X*scalar, Y*scalar, Z*scalar.
RSI::RapidCode::Cartesian::Robot::GcodeNumberOfLinesGet
int32_t GcodeNumberOfLinesGet()=0
Returns the total line number count in the last loaded item.
RSI::RapidCode::Cartesian::Robot::PathEStop
void PathEStop()=0
Commands a joint EStop and clears the loaded moves.
RSI::RapidCode::Cartesian::Vector3d::Equals
bool Equals(const Vector3d &rhs, const double tolerance) const
Checks whether this.X==rhs.X && this.Y==rhs.Y && this.Z==rhs.Z within tolerance abs(a-b) < tolerance.
RSI::RapidCode::Cartesian::Robot::PathStateWait
bool PathStateWait(PathState stateWaitFor, uint64_t timeoutMilliseconds)=0
Waits for a specific path state.
RSI::RapidCode::Cartesian::Vector3d::operator==
bool operator==(const Vector3d &rhs) const
Returns whether this is exactly equal to rhs.
RSI::RapidCode::Cartesian::Quaternion::Set
Quaternion & Set(double w, double x, double y, double z)
Assigns each component into this.
RSI::RapidCode::Cartesian::Robot::MotionFrameBufferSizeGet
uint32_t MotionFrameBufferSizeGet()=0
Gets the frame buffer size that is set at creation.
RSI::RapidCode::Cartesian::Robot::JointsActualPositionsGet
void JointsActualPositionsGet(double *positions, uint16_t sz)=0
Get the actual position of the axes in the underlying MultiAxis.
RSI::RapidCode::Cartesian::Pose
Pose.
Definition: cartesianrobot.h:404
RSI::RapidCode::Cartesian::Robot::RobotCreate
static Robot * RobotCreate(MotionController *controller, MultiAxis *multiAxis, const char *const modelIdentifier, uint32_t motionFrameBufferSize)
Create a Robot object to use G-Code, path motion, etc.
RSI::RapidCode::Cartesian::Vector3d::Z
double Z
The Z coordinate.
Definition: cartesianrobot.h:111
RSI::RapidCode::Cartesian::Robot::RobotDelete
static void RobotDelete(MotionController *controller, Robot *robot)
Delete a Robot.
RSI::RapidCode::Cartesian::GCodeWorkOffset::Machine
@ Machine
RSI::RapidCode::Cartesian::Pose::operator*
Vector3d operator*(const Vector3d &rhs) const
Transforms rhs by this. Rotates rhs by this, and adds this.position to it.
RSI::RapidCode::Cartesian::Quaternion::operator==
bool operator==(const Quaternion &rhs) const
Exact equality check. See equals for approximately equal to.
RSI::RapidCode::Cartesian::Robot::MotionCounterWait
bool MotionCounterWait(uint64_t motionWaitFor, uint64_t timeoutMilliseconds)=0
Waits for the counter to be greater than or euqal to the specified state.
RSI::RapidCode::Cartesian::Quaternion::Normalize
Quaternion & Normalize()
Normalizes this. Scales each component by 1 / magnitude.
RSI::RapidCode::Cartesian::Quaternion::Set
Quaternion & Set(double x, double y, double z)
Assigns the quaternion defined by the Euler angles into this. If you get unexpected values,...
RSI::RapidCode::Cartesian::Vector3d::operator-=
Vector3d & operator-=(const Vector3d &rhs)
Subtracts rhs from this. this.X-=rhs.X, this.Y-=rhs.Y, this.Z-=rhs.Z.
RSI::RapidCode::Cartesian::Robot::RobotCreate
static Robot * RobotCreate(MotionController *controller, MultiAxis *multiAxis, const char *const modelIdentifier)
This is an overloaded member function, provided for convenience. It differs from the above function o...
RSI::RapidCode::Cartesian::Robot::ActualPoseGet
Pose ActualPoseGet()=0
Get the actual pose of the robot from the trajectory exectutor.
RSI::RapidCode::Cartesian::Robot::PathLoadedMovesExecutionTimeGet
double PathLoadedMovesExecutionTimeGet()=0
Get total (seconds) duration of the planned motion that will happen when run is called.
RSI::RapidCode::Cartesian::Vector3d::operator-
Vector3d operator-(const Vector3d &rhs) const
Returns a copy of rhs subtracted from this. this.X-rhs.X, this.Y-rhs.Y, this.Z-rhs....
RSI::RapidCode::Cartesian::Quaternion::operator-
Quaternion operator-() const
Negates this quaternion (w, and v)
RSI::RapidCode::Cartesian::Vector3d::Normal
Vector3d Normal() const
Returns a normalized copy of this.
RSI::RapidCode::Cartesian::Robot::GcodeWorkOffsetValuesGet
const Pose & GcodeWorkOffsetValuesGet()=0
Gets the actual values of the currently applied offset.
RSI::RapidCode::Cartesian::Robot::PathStateWaitChange
bool PathStateWaitChange(PathState stateWaitChange, uint64_t timeoutMilliseconds)=0
Waits for the path state to not be the specified state.
RSI::RapidCode::Cartesian::Quaternion::Equals
bool Equals(const Quaternion &rhs, const double tolerance) const
Equality check within tolerance.
RSI::RapidCode::Cartesian::Robot::GcodeErrorLineNumberGet
int32_t GcodeErrorLineNumberGet()=0
Gets the line number of any errors in the G-Code syntax.
RSI::RapidCode::Cartesian::Robot::GcodeLineTextGet
const char *const GcodeLineTextGet(uint32_t lineNumber)=0
Gives you the text of a specific line from the last loaded program excluding all comments and with on...
RSI::RapidCode::Cartesian::Vector3d::Vector3d
Vector3d(double x, double y, double z)
Constructor. X=x, Y=y, Z=z.
RSI::RapidCode::Cartesian::Quaternion::Set
Quaternion & Set(const Vector3d &vec)
Assigns the quaternion defined by the Euler Angles into this.
RSI::RapidCode::Cartesian::Robot::GcodePlaneSet
void GcodePlaneSet(Plane plane)=0
Set the plane for arcs (same as calling G17/G18/G19)
RSI::RapidCode::Cartesian::Robot::ErrorMessageGet
const char *const ErrorMessageGet()=0
Get the text of any error in the Trajectory Executor.
RSI::RapidCode::Cartesian::Quaternion::SumSquares
double SumSquares() const
Gets the sum of the components squared (w*w+this.dot(this))
RSI::RapidCode::Cartesian::Robot::PathLinearScalingGet
double PathLinearScalingGet() const =0
Gets scaling between input to path motion and output of path motion to the kinematic model.
RSI::RapidCode::Cartesian::PathState::Uninitialized
@ Uninitialized
RSI::RapidCode::Cartesian::Vector3d::Vector3d
Vector3d(const Vector3d &rhs)
Constructor. X=rhs.X, Y=rhs.Y, Z=rhs.Z.
RSI::RapidCode::Cartesian::Robot::GcodeProgramGet
const char *const GcodeProgramGet()=0
Gets the raw file contents of the last loaded program.
RSI::RapidCode::MotionController
The MotionController object represents the RMP INtime soft motion controller.
Definition: rsi.h:746
RSI::RapidCode::Cartesian::PathMode::Absolute
@ Absolute
RSI::RapidCode::Cartesian::Robot::GlobalTranformGet
const Pose & GlobalTranformGet()=0
Gets the Pose (position and orientation) of the robot in the world. This is Visit our Topic Page for ...
RSI::RapidCode::Cartesian::Robot::PathPlaneGet
Plane PathPlaneGet()=0
Gets the plane some arcs will be forced to be on.
RSI::RapidCode::Cartesian::Quaternion::W
double W
W (real component) of Quaternion. Do not modify this unless you know what you are doing.
Definition: cartesianrobot.h:187
RSI::RapidCode::Cartesian::Robot::PathRun
void PathRun(bool blocking)=0
Run the loaded path lines/arcs. Default behavior is blocking. This is an overloaded member function,...
RSI::RapidCode::Cartesian::LinearUnits::None
@ None
RSI::RapidCode::Cartesian::Quaternion::Quaternion
Quaternion(double x, double y, double z)
Euler constructor.
RSI::RapidCode::Cartesian::Vector3d
Vector3d is used for three-dimensional points and vectors.
Definition: cartesianrobot.h:102
RSI
RSI::RapidCode::Cartesian::Quaternion::Normalized
Quaternion Normalized() const
Retunrs a normalized copy of this.
RSI::RapidCode::Cartesian::Vector3d::operator-
Vector3d operator-() const
Returns a copy of this with elements negated. -X, -Y, -Z.
RSI::RapidCode::Cartesian::Robot::ScaleFactorBetweenUnitsGet
const double ScaleFactorBetweenUnitsGet(LinearUnits from, LinearUnits to)=0
Gets scaling for scale factor needed to convert from -> to units by multiplying
RSI::RapidCode::Cartesian::Vector3d::operator+=
Vector3d & operator+=(const Vector3d &rhs)
Adds rhs to this. this.X+=rhs.X, this.Y+=rhs.Y, this.Z+=rhs.Z.
RSI::RapidCode::Cartesian::Quaternion::operator+=
Quaternion & operator+=(const Quaternion &rhs)
Addition assignment. Adds rhs into this. See operator+.
RSI::RapidCode::Cartesian::Robot::EndEffectorTransformGet
const Pose & EndEffectorTransformGet()=0
End of Robot to TCP.
RSI::RapidCode::Cartesian::Pose::Pose
Pose(const Vector3d &position)
Constructor from an existing position Vector3d.
RSI::RapidCode::Cartesian::Robot::GcodeAccelerationSet
void GcodeAccelerationSet(double acceleration)=0
Sets the target acceleration for the machine (units/minute^2). Should be set apprpriately based on yo...
RSI::RapidCode::Cartesian::Pose::operator*
Pose operator*(const Pose &rhs) const
Transforms rhs by this. Rotates rhs.position by this, adds this.position to it, and rotates rhs....
RSI::RapidCode::Cartesian::Robot::MotionDoneWait
uint64_t MotionDoneWait()=0
Waits for a move to complete.
RSI::RapidCode::Cartesian::Robot::PathStateGet
PathState PathStateGet()=0
Gets the path state of the Robot.
RSI::RapidCode::Cartesian::Quaternion::operator=
Quaternion & operator=(const Quaternion &rhs)
Assignment operator. Copies underlying.
RSI::RapidCode::Cartesian::Pose::Rotate
Pose Rotate(const Pose &rhs) const
Rotates rhs by the rotation component of this (ONLY the rotation component)
RSI::RapidCode::Cartesian::Quaternion::Set
Quaternion & Set(const Quaternion &rhs)
Assigns other into this.
RSI::RapidCode::Cartesian::Robot::PathClear
void PathClear()=0
Clears out all loaded lines and arcs from the path.
RSI::RapidCode::Cartesian::Robot::GcodeFeedRateSet
void GcodeFeedRateSet(double feedRate)=0
Sets the target feed rate for the machine (units/minute). Same as the "F" Command in a G-Code file.
RSI::RapidCode::Cartesian::Quaternion::Set
Quaternion & Set(double w, const Vector3d &vec)
Assigns other into this.
RSI::RapidCode::Cartesian::Vector3d::Normalize
Vector3d & Normalize()
Normalizes this.
RSI::RapidCode::Cartesian::Robot::IsRunning
bool IsRunning()=0
Returns whether or not the executor is executing motion.
RSI::RapidCode::Cartesian::Robot::PathPlannedPointsGet
uint64_t PathPlannedPointsGet(double *points, double *times, uint64_t startFrame, uint64_t frameCount)=0
Get points representing the planned motion in cartesian space that will happen when run is called.
RSI::RapidCode::Cartesian::Robot::MoveRelative
void MoveRelative(const Pose &relativePose)=0
Executes a point-to-point motion relative to the current end effector Pose.
RSI::RapidCode::Cartesian::Robot::GcodeUnitsSet
void GcodeUnitsSet(LinearUnits units)=0
Set the currently active unit (same as calling G20/G21)
RSI::RapidCode::Cartesian::Robot::PathUnitsGet
LinearUnits PathUnitsGet() const =0
Gets the units of path motion.
RSI::RapidCode::Cartesian::Quaternion::Inverse
Quaternion Inverse() const
Conjugate scaled by 1 / SumSquares()
RSI::RapidCode::Cartesian::Pose::Pose
Pose(const Vector3d position, const Quaternion &quaternion)
Constructor.
RSI::RapidCode::Cartesian::Robot::PathAccelerationGet
double PathAccelerationGet()=0
Gets the target acceleration for the machine (UserUnits/second^2).
RSI::RapidCode::Cartesian::Quaternion::Quaternion
Quaternion()
Default Constructor. W=1, X=Y=Z=0.
RSI::RapidCode::Cartesian::Robot::GcodeReload
void GcodeReload(int32_t startLine)=0
Reloads the lines from the last call to GcodeStringLoad() but only between start line and end of file...
RSI::RapidCode::Cartesian::LinearUnits
LinearUnits
Unit types. For G-code use.
Definition: cartesianrobot.h:85
RSI::RapidCode::Cartesian::Quaternion::FromAngleAxis
static Quaternion FromAngleAxis(double angle, const Vector3d &axis)
Creates a quaternion from an axis angle representation.
RSI::RapidCode::Cartesian::Robot::PathProgrammingModeSet
void PathProgrammingModeSet(PathMode mode)=0
Sets the programming mode (Relative/Absolute).
RSI::RapidCode::Cartesian::Plane::None
@ None
RSI::RapidCode::Cartesian::Robot::InverseKinematics
void InverseKinematics(Pose pose, double *outJoints, uint16_t outJointsSize)=0
Run the given pose through the current inverse kinematic model to see the joint positions the robot w...
RSI::RapidCode::Cartesian::Robot::GcodeStringLoad
bool GcodeStringLoad(const char *const text)=0
Interpret a string as contents of a G-Code file and prepare it for execution.
RSI::RapidCode::Cartesian::Robot::CommandPoseGet
Pose CommandPoseGet()=0
Get the commanded pose of the robot from the trajectory exectutor.
RSI::RapidCode::Cartesian::Vector3d::Cross
Vector3d Cross(const Vector3d &rhs) const
Cross Product. Vector.X = this.Y*rhs.z - this.Z*rhs.Y Vector.Y = this.Z*rhs.X - this....
RSI::RapidCode::Cartesian::Robot::PathVelocitySet
void PathVelocitySet(double velocity)=0
Sets the target linear cartesian velocity for each move (UserUnits/second).
RSI::RapidCode::Cartesian::Robot::PathProgrammingModeGet
PathMode PathProgrammingModeGet()=0
Gets the programming mode (Relative/Absolute).
RSI::RapidCode::Cartesian::Robot::Move
void Move(const Pose &absolutePose)=0
Executes a point-to-point absolute motion in cartesian space.
RSI::RapidCode::Cartesian::Pose::Pose
Pose(const Quaternion &quaternion)
Constructor.
RSI::RapidCode::Cartesian::Robot::MotionCounterWaitChange
bool MotionCounterWaitChange(uint64_t motionWaitChange, uint64_t timeoutMilliseconds)=0
Waits for the state to change off the specified counter value.
RSI::RapidCode::Cartesian::Robot::GcodeUnitsGet
LinearUnits GcodeUnitsGet()=0
Get the currently active unit as set by G20/G21.
RSI::RapidCode::Cartesian::Robot::JointsCommandPositionsGet
void JointsCommandPositionsGet(double *positions, uint16_t sz)=0
Get the commanded position of the axes in the underlying MultiAxis.
RSI::RapidCode::Cartesian::Robot::GcodeWorkOffsetSelect
void GcodeWorkOffsetSelect(GCodeWorkOffset offset)=0
Apply the offset saved under the specified location.
RSI::RapidCode::Cartesian::Robot::GlobalTranformSet
void GlobalTranformSet(const Pose &transform)=0
Sets the Pose (position and orientation) of the robot in the world. Use this when you want to program...
RSI::RapidCode::Cartesian::Quaternion::FromMatrix
static Quaternion FromMatrix(const double *const mat)
Matrix to Quaternion.
RSI::RapidCode::Cartesian::PathState
PathState
State of the Robot.
Definition: cartesianrobot.h:38
RSI::RapidCode::Cartesian::PathMode
PathMode
Motion types. For G-code use and Cartesian path motion.
Definition: cartesianrobot.h:51
RSI::RapidCode::Cartesian::Quaternion::Quaternion
Quaternion(double w, const Vector3d &v)
Constructor.
RSI::RapidCode::Cartesian::Robot::HasError
bool HasError()=0
Get the error state of the underlying Trajectory Executor.
RSI::RapidCode::Cartesian::Robot::PathAccelerationSet
void PathAccelerationSet(double acceleration)=0
Sets the target acceleration for the machine (UserUnits/second^2). Should be set apprpriately based o...
RSI::RapidCode::Cartesian::Pose::Pose
Pose()
Default constructor. Vector(0,0,0) Quaterion(1,0,0,0)
RSI::RapidCode::Cartesian::GCodeWorkOffset
GCodeWorkOffset
Register names for G-Code work offsets.
Definition: cartesianrobot.h:73
RSI::RapidCode::Cartesian::Robot::MotionCounterGet
uint64_t MotionCounterGet()=0
Gets the current motion counter of the Robot.
RSI::RapidCode::Cartesian::Vector3d::Set
Vector3d & Set(double x, double y, double z)
Sets X=x, Y=y, Z=z.
RSI::RapidCode::Cartesian::Pose::Equals
bool Equals(const Pose &rhs, double tolerance) const
Approximate equality check. Checks whether all components are within the tolerance of each other.