APIs, concepts, guides, and more
cartesianrobot.h
1#pragma once
2#ifndef _CARTESIANROBOT_H
3#define _CARTESIANROBOT_H
4
5#include "rsi.h" // HAS_CARTESIAN_ROBOT defined in rsi.h
6#if defined(HAS_CARTESIAN_ROBOT)
7#include <cmath>
8
9
10// doxygen requires these stacked namespaces, you CANNOT use RSI::RapidCode::Cartesian {
11namespace RSI
12{
13namespace RapidCode
14{
15
17namespace Cartesian
18{
21
23 enum class PathState
24 {
26 Idle,
27 Moving,
28 Stopping,
29 };
30
31 constexpr double pi = 3.1415926535897932384626433832795028841971693993751;
32
34 enum class PathMode
35 {
36 Absolute,
37 Relative,
38 };
39
42 {
43 Clockwise,
45 };
46
48 enum class Plane
49 {
50 None,
51 XY,
52 XZ,
53 YZ,
54 };
55
57 enum class GCodeWorkOffset {
58 Machine = 0,
59 G54 = 54,
60 G55 = 55,
61 G56 = 56,
62 G57 = 57,
63 G58 = 58,
64 G59 = 59,
65 };
66
68 enum class LinearUnits
69 {
70 None,
72 Inches,
73 Feet,
75 Meters,
76
77 };
78
87 enum class CartesianAxis
88 {
89 X = 0,
90 Y = 1,
91 Z = 2,
92 Roll = 3,
93 Pitch = 4,
94 Yaw = 5,
95 };
96
98
99 class Gcode; // forward declaration
100
101 template<typename ContainerClass>
102 class RSI_API RapidVectorIterator
103 {
104 public:
106 typedef typename ContainerClass::ValueType ValueType;
107
109 typedef typename ContainerClass::PointerType PointerType;
110
113 ValueType& operator*();
114
117 const ValueType& operator*() const;
118
121 RapidVectorIterator& operator++();
122
125 RapidVectorIterator operator++(int);
126
129 bool operator==(const RapidVectorIterator&) const;
130
133 bool operator!=(const RapidVectorIterator&) const;
134
136 RapidVectorIterator(PointerType pointerArgument, PointerType startArgument, const size_t containerSize);
137
139 RapidVectorIterator(const RapidVectorIterator&) = default;
140
142 RapidVectorIterator& operator=(const RapidVectorIterator&) = default;
143
145 RapidVectorIterator(RapidVectorIterator&&) noexcept = default;
146
148 RapidVectorIterator& operator=(RapidVectorIterator&&) noexcept = default;
149 private:
150 PointerType pointer;
151 PointerType start, end;
152 void VerifyPointer() const;
153 };
154
156 template<typename Type>
158
161
169 template<typename Type>
170 class RSI_API RapidVector
171 {
172 private:
173 // The pointer to the implementation.
175 public:
179 Type& operator[](const size_t index);
180
184 const Type& operator[](const size_t index) const;
185
189 Type& At(const size_t index);
190
194 const Type& At(const size_t index) const;
195
198 void PushBack(const Type& element);
199
202 const size_t Size() const;
203
205 void Clear();
206
208 void PopBack();
209
212 Type& Front();
213
216 const Type& Front() const;
217
220 Type& Back();
221
224 const Type& Back() const;
225
228
230 RapidVector(const size_t size);
231
234
237
240
243
245 RapidVector(RapidVector&& other) noexcept;
246
249
250 typedef typename Type* PointerType;
251 typedef typename Type ValueType;
252
254 typedef typename RapidVectorIterator<RapidVector> Iterator;
255
258 Iterator begin() noexcept;
259
262 Iterator begin() const noexcept;
263
267 Iterator end() const noexcept;
268 };
269
271 class RSI_API Vector3d
272 {
273 public:
275 double X;
276
278 double Y;
279
281 double Z;
282
285
287 Vector3d(double x, double y, double z);
288
290 Vector3d(const Vector3d& rightHandSide);
291
293 double Dot(const Vector3d& rightHandSide) const;
294
299 Vector3d Cross(const Vector3d& rightHandSide) const;
300
302 Vector3d& Set(double x, double y, double z);
303
305 Vector3d& Set(const Vector3d& rightHandSide);
306
309 bool Equals(const Vector3d& rightHandSide, const double tolerance) const;
310
312 double SumSquares() const;
313
315 double Length() const;
316
319
322
324 Vector3d& operator=(const Vector3d& rightHandSide);
326 bool operator==(const Vector3d& rightHandSide) const;
328 bool operator!=(const Vector3d& rightHandSide) const;
330 Vector3d operator*(double scalar) const;
332 Vector3d& operator*=(double scalar);
334 Vector3d operator+(const Vector3d& rightHandSide) const;
336 Vector3d& operator+=(const Vector3d& rightHandSide);
338 Vector3d operator-(const Vector3d& rightHandSide) const;
342 Vector3d& operator-=(const Vector3d& rightHandSide);
344 Vector3d operator/(double scalar) const;
346 Vector3d& operator/=(double scalar);
349 };
350
352 class RSI_API Quaternion
353 {
354 public:
357
359 double W;
360
363
369 Quaternion(double w, double x, double y, double z);
370
373 Quaternion(const Quaternion& otherQuaternion);
374
378 Quaternion(double w, const Vector3d& v);
379
384 Quaternion(double x, double y, double z);
385
388 Quaternion(const Vector3d& rpy);
389
393 bool operator==(const Quaternion& rightHandSide) const;
394
398 bool operator!=(const Quaternion& rightHandSide) const;
399
403 Quaternion& operator=(const Quaternion& rightHandSide);
404
408 Quaternion operator+(const Quaternion& rightHandSide) const;
409
413 Quaternion& operator+=(const Quaternion& rightHandSide);
414
418 Quaternion operator-(const Quaternion& rightHandSide) const;
419
423 Quaternion& operator-=(const Quaternion& rightHandSide);
424
428 Quaternion operator*(const Quaternion& rightHandSide) const;
429
433 Quaternion operator*(double s) const;
434
439
443 Vector3d operator*(const Vector3d& rightHandSide) const;
444
448
452 double Dot(const Quaternion& rightHandSide) const;
453
457 Quaternion& Set(const Quaternion& otherQuaternion);
458
465 Quaternion& Set(double w, double x, double y, double z);
466
471 Quaternion& Set(double w, const Vector3d& vec);
472
479 Quaternion& Set(double x, double y, double z);
480
484 Quaternion& Set(const Vector3d& vec);
485
490 bool Equals(const Quaternion& rightHandSide, const double tolerance) const;
491
495
499
500
503 double SumSquares() const;
504
507 double Magnitude() const;
508
512
516
517
518
523 static Quaternion FromAngleAxis(double angle, const Vector3d& axis);
524
528 void ToAngleAxis(double& outAngle, Vector3d& outAxis) const;
529
533
534
541 static Quaternion FromMatrix(const double* const matrix);
542
543
549 void ToMatrix(double* matrix) const;
550 };
551
553 class RSI_API Pose {
554 public:
557
560
563
566 Pose(const Pose& pose);
567
571 Pose(const Vector3d position, const Quaternion& quaternion);
572
575 Pose(const Vector3d& position);
576
581 Pose(double x, double y, double z);
582
585 Pose(const Quaternion& quaternion);
586
591 bool Equals(const Pose& rightHandSide, double tolerance) const;
592
596 Pose Rotate(const Pose& rightHandSide) const;
597
601 Vector3d operator*(const Vector3d& rightHandSide) const;
602
606 Pose operator*(const Pose& rightHandSide) const;
607
610 Pose Inverse() const;
611 };
612
614 class RSI_API RobotPosition
615 {
616 public:
619
622
628 RobotPosition(const Cartesian::Pose& newPose, const size_t numberOfFreeAxes = 0);
630 RobotPosition(const Cartesian::Pose& newPose, const RapidVector<double>& freeAxes);
636 void AxisValueSet(uint32_t index, double value);
638 double AxisValueGet(uint32_t index) const;
641 };
642
644 class RSI_API KinematicModel
645 {
646 public:
647 virtual const char* const NameGet() const = 0;
648 virtual uint32_t AxisCountGet() const = 0;
649
650 virtual LinearUnits UnitsGet() const = 0;
651
652 virtual bool IsConfigured() const = 0;
653
654 virtual const char* const* const ExpectedAxisNamesGet() const = 0;
655 virtual uint32_t ExpectedAxisNamesCountGet() const = 0;
656
657 virtual uint32_t FreeAxisCountGet() const = 0;
658
659 // input function
660 // inputPose requires this to be in robot centric coordinates.
661 // remove tool offset and robot base offset
662 virtual bool InverseKinematicsSolve(const Pose& inputPose, void* outputPointerSolutionVector) const = 0;
663
664 virtual bool InverseKinematicsSolve(const RobotPosition& inputPosition, void* vpSols) const = 0;
665
666 // ouput function
667 // inputJointPositions requires this to be in robot centric coordinates.
668 // apply tool offset and robot base offset
669 virtual void ForwardKinematicsSolve(const double* inputJointPositions, Pose& outputPose) const = 0;
670
671 virtual void ForwardKinematicsSolve(const double* inputJointPositions, RobotPosition& outputPosition) const = 0;
672
673 virtual bool IsArticulated() const = 0;
674 };
675
677 struct RSI_API ModelAxisMapping
678 {
679
680 public:
684
686 double Scaling;
687 static constexpr double DefaultScaling = 1.0;
688
690 double Offset;
691 static constexpr double DefaultOffset = 0.0;
692
694 static constexpr int MaxLabelSize = 16;
695 static constexpr int ExpectedLabelBufferSize = MaxLabelSize + 1;
696
700 char ExpectedLabel[ExpectedLabelBufferSize];
701
703 ModelAxisMapping(int jointIndex);
704
706 ModelAxisMapping(int jointIndex, double scaling, double offset, const char* const label);
707
708 // ModelAxisMapping Destructor.
709 virtual ~ModelAxisMapping() = default;
710 };
711
717 {
718 private:
719 struct Storage;
720 Storage* storage;
721
722 protected:
727 KinematicModelBuilder(const char* const label);
728
729 // Protect the copy constructor, for internal use only.
731
732
733 class ProtectedAccessor;
734 // Provide derived classes with protected access to certain members.
735 ProtectedAccessor ProtectedAccessGet();
736
737 public:
738 virtual ~KinematicModelBuilder();
739
740 // Delete the copy-assignment constructor.
741 KinematicModelBuilder& operator=(const KinematicModelBuilder&) = delete;
742
743 // Move constructors
745 KinematicModelBuilder& operator=(KinematicModelBuilder&&) noexcept;
746
751 virtual const KinematicModel& ModelBuild() = 0;
752
756 const KinematicModel& ModelGet();
757
760 void UnitsSet(LinearUnits units);
761
764 LinearUnits UnitsGet() const;
765
768 void LabelSet(const char* const label);
769
772 const char* const LabelGet() const;
773
775 void TransformFromKinematicOriginToRobotOriginSet(const Pose& KinematicOriginToRobotOrigin);
776
778 Pose TransformFromKinematicOriginToRobotOriginGet() const;
779
782 void FreeAxisAdd(const ModelAxisMapping& freeAxis);
783 };
784
799 struct RSI_API LinearJointMapping : public ModelAxisMapping
800 {
801 public:
805
806 LinearJointMapping(int jointIndex, CartesianAxis cartesianCoordinate);
807 LinearJointMapping(int jointIndex, CartesianAxis cartesianCoordinate, double scaling, double offset, const char* const label);
808 };
809
813 {
814 struct Storage;
815 Storage* storage;
816
817 public:
818
823 const KinematicModel& ModelBuild() override;
824
826 void JointAdd(const LinearJointMapping& joint);
827
830 LinearModelBuilder(const char* const label);
832
833 // Move constructors
834 LinearModelBuilder(LinearModelBuilder&& other) noexcept;
835 LinearModelBuilder& operator=(LinearModelBuilder&&) noexcept;
836
837 // Disable copy constructors
838 LinearModelBuilder(const LinearModelBuilder&) = delete;
839 LinearModelBuilder& operator=(const LinearModelBuilder&) = delete;
840 };
841
842 // UNSUPPORTED
843 class RSI_API ArticulatedModelBuilder : public KinematicModelBuilder
844 {
845 struct Storage;
846 Storage* storage;
847
848 public:
849 const KinematicModel& ModelBuild() override;
850 ArticulatedModelBuilder();
851 ~ArticulatedModelBuilder();
852
853 };
854
873 int32_t LineNumber;
874
879 const char* LineText;
880
890 };
891
904 class RSI_API GcodeCallback
905 {
906 public:
919 virtual void Execute(GcodeCallbackData* data) {};
920
921 virtual ~GcodeCallback() = default; // Virtual destructor for C++ base classes
922 };
923
927 class RSI_API Robot : public virtual RapidCodeObject
928 {
929 public:
930
935
949 static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, KinematicModelBuilder* builder, uint32_t motionFrameBufferSize);
950
955 static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier);
956
961 static Robot* RobotCreate(MotionController* controller, MultiAxis* multiAxis, const char* const modelIdentifier, uint32_t motionFrameBufferSize);
962
964 static void RobotDelete(MotionController* controller, Robot* robot);
965
967
968
973
975 static constexpr uint64_t TimeoutForever = 0xFFFFFFFFFFFFFFFF;
976
981 static constexpr uint32_t MotionFrameBufferSizeDefault = 256;
982
987 static constexpr uint32_t MotionFrameBufferSizeMinimum = 50;
989
990
995
1001
1006 virtual bool PathStateWait(PathState stateWaitFor, uint64_t timeoutMilliseconds) = 0;
1007
1012 virtual bool PathStateWaitChange(PathState stateWaitChange, uint64_t timeoutMilliseconds) = 0;
1013
1014
1019 virtual uint64_t MotionCounterGet() = 0;
1020
1025 virtual void MotionCounterWait(uint64_t motionCount, uint64_t timeoutMilliseconds) = 0;
1026
1028 virtual uint64_t SampleCounterGet() = 0;
1029
1034 virtual void SampleCounterWait(uint64_t sampleCount, uint64_t timeoutMilliseconds) = 0;
1035
1039 virtual bool IsRunning() = 0;
1040
1044 virtual bool PathIsRunning() = 0;
1045
1052 virtual bool MotionDoneGet() = 0;
1053
1060 virtual uint64_t MotionDoneWait() = 0;
1061
1069 virtual uint64_t MotionDoneWait(uint64_t timeoutMilliseconds) = 0;
1070
1076 virtual bool HasError() = 0;
1077
1082 virtual const char* const ErrorMessageGet() = 0;
1083
1085
1086
1090
1093 virtual void Resume() = 0;
1094
1097 virtual void Stop() = 0;
1098
1101 virtual void EStopAbort() = 0;
1102
1105 virtual void Abort() = 0;
1106
1110 virtual void AmpEnableSet(bool enable, int32_t ampActiveTimeoutMilliseconds) = 0;
1111
1118 virtual void EStop() = 0;
1119
1121 virtual void ClearFaults() = 0;
1122
1123
1124
1128
1134 virtual Pose ActualPoseGet() = 0;
1135
1142
1143
1150 virtual Pose ActualPoseGet(LinearUnits units) = 0;
1151
1159
1160
1166 virtual Pose CommandPoseGet() = 0;
1167
1174
1180 virtual Pose CommandPoseGet(LinearUnits units) = 0;
1181
1188
1194
1200
1206
1212
1218 virtual Pose ForwardKinematics(const RapidVector<double>& joints) = 0;
1219
1226
1230
1232 virtual const KinematicModel& ModelGet() = 0;
1233
1237 virtual void EndEffectorTransformSet(const Pose& transform) = 0;
1238
1240 virtual void EndEffectorTransformSet(const RobotPosition& transformPosition) = 0;
1241
1245 virtual void OriginTransformSet(const Pose& transform) = 0;
1247 virtual void OriginTransformSet(const RobotPosition& transformPosition) = 0;
1248
1252
1255 virtual const RobotPosition& OriginTransformGet() = 0;
1256
1257
1262 virtual const double ScaleFactorBetweenUnitsGet(LinearUnits from, LinearUnits to) = 0;
1263
1264
1268
1277 virtual void MoveAbsolute(const Pose& absolutePose) = 0;
1278
1279
1289 virtual void MoveRelative(const Pose& relativePose) = 0;
1290
1291
1296
1300 virtual void PathClear() = 0;
1301
1307 virtual void PathVelocitySet(double unitsPerSecond) = 0;
1308
1314 virtual double PathVelocityGet() = 0;
1315
1320 virtual void PathAccelerationSet(double unitsPerSecondSquared) = 0;
1321
1325
1327 virtual double PathAccelerationGet() = 0;
1328
1335 virtual void PathProgrammingModeSet(PathMode mode) = 0;
1336
1343
1350 virtual void PathArc(const Pose& target, double radius, RotationDirection direction) = 0;
1351
1358 virtual void PathArc(const RobotPosition& target, double radius, RotationDirection direction) = 0;
1359
1364 virtual void PathArc(const Pose& target, const Vector3d& center, RotationDirection direction) = 0;
1365
1370 virtual void PathArc(const RobotPosition& RobotPosition, const Vector3d& center, RotationDirection direction) = 0;
1371
1372
1373 // @brief Appends a line on the current path
1377 virtual void PathLine(const Pose& target) = 0;
1378
1379 // @brief Appends a line on the current path
1383 virtual void PathLine(const RobotPosition& position) = 0;
1384
1388 virtual void PathPlaneSet(Plane plane) = 0;
1389
1393 virtual Plane PathPlaneGet() = 0;
1394
1400 virtual void PathLinearScalingSet(double scaleFactor) = 0;
1401
1408 virtual double PathLinearScalingGet()const = 0;
1409
1414 virtual void PathUnitsSet(LinearUnits units) = 0;
1415
1419 virtual LinearUnits PathUnitsGet()const = 0;
1420
1421
1425 virtual double PathProcessLoadedMoves() = 0;
1426
1437 virtual RapidVector<RobotPosition> PathPlannedPositionsGet(uint64_t startFrame, uint64_t frameCount) = 0;
1438
1443 virtual double PathDurationGet() = 0;
1444
1450 virtual void Run() = 0;
1451
1468 virtual uint32_t MotionFrameBufferSizeGet()=0;
1469
1471
1474
1475 };
1476
1480 class RSI_API Gcode : public virtual RapidCodeObject
1481 {
1482 public:
1483
1488
1493 virtual void FeedRateSet(double programmingUnitsPerMinute) = 0;
1494
1500 virtual void AccelerationRateSet(double programmingUnitsPerMinuteSquared) = 0;
1501
1509
1513 virtual void Load(const char* const text) = 0;
1514
1518 virtual void LoadFile(const char* const file) = 0;
1519
1523 virtual void Run() = 0;
1524
1536 virtual uint64_t DoneWait(uint64_t timeoutMilliseconds) = 0;
1537
1539
1544
1549 virtual void ReloadLines(int32_t start = 0, int32_t end = -1) = 0;
1550
1553 virtual int32_t LineCountGet() = 0;
1554
1559 virtual const char* const ProgramGet() = 0;
1560
1568 virtual int32_t SyntaxErrorLineNumberGet() = 0;
1569
1577 virtual const char* const LineTextGet(uint32_t lineNumber) = 0;
1578
1585 virtual double FeedRateGet() = 0;
1586
1592 virtual double AccelerationRateGet() = 0;
1593
1598 virtual double DurationGet() = 0;
1599
1611 virtual void Cancel() = 0;
1612
1618 virtual RapidVector<RobotPosition> PlannedPositionsGet(uint64_t startFrame, uint64_t frameCount) = 0;
1619
1627 virtual void UnitsSet(LinearUnits units) = 0;
1628
1635 virtual LinearUnits UnitsGet() = 0;
1636
1643 virtual void WorkOffsetConfigure(GCodeWorkOffset workOffset, Vector3d offsetValues) = 0;
1644
1650 virtual void ActiveWorkOffsetSelect(GCodeWorkOffset workOffset) = 0;
1651
1658
1665
1667 virtual bool IsRunning() = 0;
1668
1671 virtual int32_t ExecutingLineNumberGet() = 0;
1672
1676 virtual void FreeAxisLetterSet(const char gcodeLetter, const int32_t freeAxisIndex) = 0;
1677
1679 virtual void CallbackRegister(Cartesian::GcodeCallback* callback) = 0;
1680
1682 };
1683
1685
1686} // Cartesian namespace
1687} // RapidCode namespace
1688} // RSI namespace
1689
1690#endif // defined(HAS_CARTESIAN_ROBOT)
1691#endif // _CARTESIANROBOT_H
void Cancel()=0
Cancel the currently running G-code program.
void ReloadLines(int32_t start=0, int32_t end=-1)=0
Reloads the lines from the last call to GcodeLoad() but only between start line and end of file....
uint64_t DoneWait(uint64_t timeoutMilliseconds)=0
Wait for the completion of G-code program execution with a timeout.
const RobotPosition & ActiveWorkOffsetValuesGet()=0
Gets the actual values of the currently applied offset.
const char *const LineTextGet(uint32_t lineNumber)=0
Gives you the text of a specific line from the last loaded program excluding all comments and with on...
void FreeAxisLetterSet(const char gcodeLetter, const int32_t freeAxisIndex)=0
Map a letter in a Gcode file to a free axis. It will be used to specify the free axis' motion.
GCodeWorkOffset ActiveWorkOffsetGet()=0
Gets the enum representing the location where the current offset is saved.
void ActiveWorkOffsetSelect(GCodeWorkOffset workOffset)=0
Apply the offset saved under the specified location.
int32_t LineCountGet()=0
Get the number of lines in the last loaded G-Code program.
int32_t SyntaxErrorLineNumberGet()=0
Gets the line number of any errors in the G-Code syntax. Or use the exception thrown by Load or LoadF...
const char *const ProgramGet()=0
Gets the raw file contents of the last loaded program.
int32_t ExecutingLineNumberGet()=0
Get the currently executing line number.
LinearUnits UnitsGet()=0
Get the currently active unit as set by G20/G21.
void AccelerationRateSet(double programmingUnitsPerMinuteSquared)=0
REQUIRED before Load() or LoadFile(). Sets the target acceleration for the machine (units/minute^2)....
double AccelerationRateGet()=0
Gets the target acceleration for the machine (GcodeUnitsGet()/minute^2).
void UnitsSet(LinearUnits units)=0
Set the currently active unit (same as calling G20/G21)
double DurationGet()=0
Get the time duration required to run the loaded G-Code program in seconds.
RapidVector< RobotPosition > PlannedPositionsGet(uint64_t startFrame, uint64_t frameCount)=0
Get RobotPositions representing the planned G-Code motion in cartesian space that will happen when ru...
double FeedRateGet()=0
Gets the target feed rate for the machine (GcodeUnitsGet()/minute).
void LoadFile(const char *const file)=0
Load a G-code program from a file.
void WorkOffsetConfigure(GCodeWorkOffset workOffset, Vector3d offsetValues)=0
Save an XYZ offset to be used with specified G-Code work offset.
void FeedRateSet(double programmingUnitsPerMinute)=0
REQUIRED before Load() or LoadFile() if your g-code does not have 'F' in its contents....
void Load(const char *const text)=0
Load a G-code program from a string.
void Run()=0
Run the loaded G-Code file (or string). The behavior is non-blocking. Use DoneWait() to block.
void Execute(GcodeCallbackData *data)
Executes the M-code callback.
Handles callbacks for M-codes within a G-code file.
Represents a G-code program executor. This class provides an interface to load and run G-code program...
void CallbackRegister(Cartesian::GcodeCallback *callback)=0
G-code callback register.
bool IsRunning()=0
Returns true if a Gcode program is executing, false otherwise.
The abstract class for Kinematic Model builders. Constructs a single Kinematic Model to use when crea...
KinematicModelBuilder(const char *const label)
The KinematicModelBuilder constructor is protected so that only derived classes can use it....
Describes the mathematical kinematic model of a robot.
const KinematicModel & ModelBuild() override
Construct the model using the information the builder currently has. The builder will maintain a refe...
LinearModelBuilder(const char *const label)
constructs a LinearModelBuilder instance.
The Builder for a linear kinematic model. Constructs a single Linear Kinematic Model to use when crea...
void JointAdd(const LinearJointMapping &joint)
adds a joint to the model using the configuration specified within the LinearJoint structure.
Pose operator*(const Pose &rightHandSide) const
Transforms rightHandSide by this. Rotates rightHandSide.position by this, adds this....
Vector3d operator*(const Vector3d &rightHandSide) const
Transforms rightHandSide by this. Rotates rightHandSide by this, and adds this.position to it.
Pose Inverse() const
Returns an inverted copy of this. this * this.inverse() = I.
Pose(const Pose &pose)
Constructor from an existing Pose.
Pose(const Quaternion &quaternion)
Constructor.
Pose Rotate(const Pose &rightHandSide) const
Rotates rightHandSide by the rotation component of this (ONLY the rotation component)
bool Equals(const Pose &rightHandSide, double tolerance) const
Approximate equality check. Checks whether all components are within the tolerance of each other.
Pose(const Vector3d position, const Quaternion &quaternion)
Constructor.
Pose(const Vector3d &position)
Constructor from an existing position Vector3d.
Pose(double x, double y, double z)
Constructor from X,Y,Z with an identity "no rotation" default Quaternion.
Vector3d Position
The position component of the Pose.
Quaternion Orientation
The orientation component of the Pose.
Pose()
Default constructor. Vector(0,0,0) Quaterion(1,0,0,0)
Vector3d operator*(const Vector3d &rightHandSide) const
Multiplication (vector rotation) operator. Rotates vector by this.
Quaternion & operator-=(const Quaternion &rightHandSide)
Subtraction assignment. Subtracts other from this. See operator-.
Quaternion & operator+=(const Quaternion &rightHandSide)
Addition assignment. Adds rightHandSide into this. See operator+.
Quaternion & Set(double w, const Vector3d &vec)
Assigns other into this.
Quaternion operator+(const Quaternion &rightHandSide) const
Adds two quaternions together. this.w + rightHandSide.w, this.v+rightHandSide.v.
Quaternion & operator=(const Quaternion &rightHandSide)
Assignment operator. Copies underlying.
Quaternion(double x, double y, double z)
Euler constructor.
Quaternion & operator*=(double s)
Multiplication (scalar) assignment operator. See operator*(double)
Quaternion Conjugate() const
Negates the imaginary component of the Quaternion.
Quaternion Inverse() const
Conjugate scaled by 1 / SumSquares()
Quaternion operator-(const Quaternion &rightHandSide) const
Subtraction operator. this.w - rightHandSide.w, this.v-rightHandSide.v.
Quaternion operator-() const
Negates this quaternion (w, and v)
Quaternion Normalized() const
Retunrs a normalized copy of this.
void ToAngleAxis(double &outAngle, Vector3d &outAxis) const
Gets the axis angle representation of this quaternion.
Quaternion & Set(const Quaternion &otherQuaternion)
Assigns other into this.
Quaternion & Set(double x, double y, double z)
Assigns the quaternion defined by the Euler angles into this. If you get unexpected values,...
bool operator==(const Quaternion &rightHandSide) const
Exact equality check. See equals for approximately equal to.
double Magnitude() const
Gets the magnitude of this. sqrt(SumSquares())
Vector3d ToEuler() const
Gets the Euler Angle representation of this quaternion.
Quaternion operator*(double s) const
Multiplication (scalar) operator. w*s, v*s.
static Quaternion FromAngleAxis(double angle, const Vector3d &axis)
Creates a quaternion from an axis angle representation.
Quaternion operator*(const Quaternion &rightHandSide) const
Multiplication (rotation) operator. Rotates other by this.
Quaternion(double w, const Vector3d &v)
Constructor.
bool operator!=(const Quaternion &rightHandSide) const
Exact inequality check. See equals for approximately equal to.
Quaternion(const Vector3d &rpy)
Euler Constructor (Alternate)
Quaternion & Normalize()
Normalizes this. Scales each component by 1 / magnitude.
Quaternion(const Quaternion &otherQuaternion)
Constructor.
double SumSquares() const
Gets the sum of the components squared (w*w+this.dot(this))
Quaternion(double w, double x, double y, double z)
Constructor.
double Dot(const Quaternion &rightHandSide) const
Returns the dot product of each element. w*rightHandSide.w+v.dot(rightHandSide.v)
Quaternion & Set(double w, double x, double y, double z)
Assigns each component into this.
Quaternion & Set(const Vector3d &vec)
Assigns the quaternion defined by the Euler Angles into this.
bool Equals(const Quaternion &rightHandSide, const double tolerance) const
Equality check within tolerance.
void ToMatrix(double *matrix) const
Quaternion To Matrix.
static Quaternion FromMatrix(const double *const matrix)
Matrix to Quaternion.
Quaternion for representing rotations.
double W
W (real component) of Quaternion. Do not modify this unless you know what you are doing.
Quaternion()
Default Constructor. W=1, X=Y=Z=0.
Vector3d V
Vector3d (imaginary) component of Quaternion. 1->x, 2->y, 3->z. Do not modify this unless you know wh...
const size_t Size() const
Returns the number of elements in the vector.
const Type & operator[](const size_t index) const
Returns a const reference to the element at the location specified by the index.
const Type & Back() const
Returns a const reference to the last element in the vector.
Iterator begin() noexcept
Returns a RapidVectorIterator to the first element of the vector. The naming convention follows STL c...
Type & Front()
Returns a reference to the first element in the vector.
const Type & Front() const
Returns a const reference to the first element in the vector.
const Type & At(const size_t index) const
Returns a const reference to the element at the location specified by the index.
void PushBack(const Type &element)
Appends the given element to the end of the vector.
Type & Back()
Returns a reference to the last element in the vector.
Type & operator[](const size_t index)
Returns a reference to the element at the location specified by the index.
Type & At(const size_t index)
Returns a reference to the element at the location specified by the index.
A wrapper class for the C++ STL vector class that aims to maintain application binary interface....
RapidVector(RapidVector &&other) noexcept
Move constructor. Replaces contents with those of the other RapidVector.
RapidVectorIterator< RapidVector > Iterator
The iterator type for this RapidVector.
~RapidVector()
Destructs the vector and deallocates used storage.
RapidVector & operator=(const RapidVector &other)
Copy assignment operator. Replaces contents with the contents of the other RapidVector.
RapidVector(const RapidVector &other)
Copy constructor. Constructs a RapidVector as a deep-copy.
RapidVector(RapidVectorImplementation< Type > *)
Constructs a RapidVector with the given implementation. Intended for RSI internal use only.
void Clear()
Erases all elements from the vector.
void PopBack()
Removes the last element from the container. Calling PopBack() on an empty container results in undef...
RapidVector(const size_t size)
Constructs a vector with the specified size.
RapidVector & operator=(RapidVector &&other) noexcept
Move assignment operator. Replaces contents with those of the other RapidVector. The original content...
RapidVector()
Default constructor that constructs an empty vector.
Forward declaration of the implementation class for RapidVector.
RapidVector< double > JointsActualPositionsGet()=0
Get the actual positions of the axes in the underlying MultiAxis.
bool MotionDoneGet()=0
Check to see if motion is done and settled.
double PathAccelerationGet()=0
Gets the target acceleration for the machine (UserUnits/second^2).
bool PathStateWait(PathState stateWaitFor, uint64_t timeoutMilliseconds)=0
Waits for a specific path state.
void OriginTransformSet(const Pose &transform)=0
Transform that defines the origin of the robot in the world. For example if you have a gantry that is...
bool PathStateWaitChange(PathState stateWaitChange, uint64_t timeoutMilliseconds)=0
Waits for the path state to not be the specified state.
RobotPosition ActualPositionGet()=0
Get the current actual position of the robot from the trajectory executor, in the Robot model's units...
void PathArc(const RobotPosition &target, double radius, RotationDirection direction)=0
Appends an arc on to the current on the PathPlaneGet() plane XY by default.
uint64_t MotionDoneWait()=0
Waits for a move to complete.
void Stop()=0
Stop an axis.
Pose CommandPoseGet(LinearUnits units)=0
Get the current commanded pose of the robot from the trajectory executor in the specified units.
double PathVelocityGet()=0
Gets the target velocity for the machine (UserUnits/second).
Pose CommandPoseGet()=0
Get the current commanded pose of the robot from the trajectory executor, in the Robot model's units.
RapidVector< RobotPosition > PathPlannedPositionsGet(uint64_t startFrame, uint64_t frameCount)=0
Get Positions (see RobotPosition) representing the planned motion in cartesian space that will happen...
uint32_t MotionFrameBufferSizeGet()=0
Gets the TrajectoryExecutor's buffer size in frames, one frame is used per RMP sample period.
void PathLine(const RobotPosition &position)=0
static Robot * RobotCreate(MotionController *controller, MultiAxis *multiAxis, const char *const modelIdentifier, uint32_t motionFrameBufferSize)
void PathVelocitySet(double unitsPerSecond)=0
Sets the target linear cartesian velocity for each move (UserUnits/second).
void SampleCounterWait(uint64_t sampleCount, uint64_t timeoutMilliseconds)=0
Waits for the sample counter of the Robot's main execution thread to incremented by the specified amo...
Plane PathPlaneGet()=0
Gets the plane some arcs will be forced to be on.
const char *const ErrorMessageGet()=0
Get the text of any error in the Trajectory Executor.
RobotPosition CommandPositionGet(LinearUnits units)=0
Get the current commanded position of the robot from the trajectory executor in the specified units.
uint64_t MotionDoneWait(uint64_t timeoutMilliseconds)=0
Waits for a move to complete.
bool IsRunning()=0
Returns whether or not the robot is in motion (from executor or other source). Can be used to determi...
void Abort()=0
Abort an axis.
bool HasError()=0
Get the error state of the underlying Trajectory Executor.
double PathProcessLoadedMoves()=0
Processes our loaded moves. Used internally and to check whether loaded moves are valid,...
void EndEffectorTransformSet(const Pose &transform)=0
Transform that defines the control point relative to the end end of the current kinematic model For e...
RobotPosition ActualPositionGet(LinearUnits units)=0
Get the current actual position of the robot from the trajectory executor in the specified units.
void PathPlaneSet(Plane plane)=0
Sets the plane for the purposes of ambiguous cases (arcs >= 180deg). Last set plane or XY plane is de...
double PathLinearScalingGet() const =0
Gets scaling between input to path motion and output of path motion to the kinematic model.
RapidVector< double > InverseKinematics(Pose pose)=0
Run the given pose through the current inverse kinematic model to see the joint positions the robot w...
Pose ActualPoseGet(LinearUnits units)=0
Get the current actual pose of the robot from the trajectory executor in the specified units.
PathMode PathProgrammingModeGet()=0
Gets the programming mode (Relative/Absolute).
static Robot * RobotCreate(MotionController *controller, MultiAxis *multiAxis, const char *const modelIdentifier)
RobotPosition ForwardKinematicsPosition(const RapidVector< double > &joints)=0
Get the Actual position of the robot from the the joint MultiAxis positions.
void PathLinearScalingSet(double scaleFactor)=0
Sets scaling between the input to path motion and output of path motion to the kinematic model.
static Robot * RobotCreate(MotionController *controller, MultiAxis *multiAxis, KinematicModelBuilder *builder, uint32_t motionFrameBufferSize)
Create a Robot object to use G-Code, path motion, etc.
const RobotPosition & OriginTransformGet()=0
Offset for the origin location.
void AmpEnableSet(bool enable, int32_t ampActiveTimeoutMilliseconds)=0
Enable all amplifiers.
void EStopAbort()=0
E-Stop, then abort an axis.
void PathAccelerationSet(double unitsPerSecondSquared)=0
Sets the target acceleration for the machine (UserUnits/second^2). Should be set appropriately based ...
LinearUnits PathUnitsGet() const =0
Gets the units of path motion.
bool PathIsRunning()=0
Returns whether or not a planned path is being executed. All G-Code gets converted to path....
const RobotPosition & EndEffectorTransformGet()=0
End of Robot to TCP.
void PathArc(const Pose &target, double radius, RotationDirection direction)=0
Appends an arc on to the current on the PathPlaneGet() plane XY by default.
void PathClear()=0
Clears out all loaded lines and arcs from the path. Part of the PathMotion method group.
RobotPosition CommandPositionGet()=0
Get the current commanded position of the robot from the trajectory executor, in the Robot model's un...
Pose ActualPoseGet()=0
Get the current actual pose of the robot from the trajectory executor, in the Robot model's units.
void PathUnitsSet(LinearUnits units)=0
Defines the units Cartesian Path Motion is in.
void PathLine(const Pose &target)=0
void MoveAbsolute(const Pose &absolutePose)=0
Executes a point-to-point absolute motion in cartesian space.
PathState PathStateGet()=0
Gets the path state of the Robot.
void EStop()=0
Commands a joint EStop and clears the loaded moves.
void MotionCounterWait(uint64_t motionCount, uint64_t timeoutMilliseconds)=0
Waits for the counter increment by the specified amount.
void MoveRelative(const Pose &relativePose)=0
Executes a point-to-point motion relative to the current end effector Pose.
RapidVector< double > JointsCommandPositionsGet()=0
Get the commanded positions of the axes in the underlying MultiAxis.
Pose ForwardKinematics(const RapidVector< double > &joints)=0
Get the Actual pose of the robot from the the joint MultiAxis positions.
void PathProgrammingModeSet(PathMode mode)=0
Sets the programming mode (Relative/Absolute).
void PathArc(const Pose &target, const Vector3d &center, RotationDirection direction)=0
Appends an arc on to the current path with end pose about a specific center point.
uint64_t MotionCounterGet()=0
Gets the current motion counter of the Robot.
const double ScaleFactorBetweenUnitsGet(LinearUnits from, LinearUnits to)=0
Gets scaling for scale factor needed to convert from -> to units by multiplying
void PathArc(const RobotPosition &RobotPosition, const Vector3d &center, RotationDirection direction)=0
Appends an arc on to the current path with end pose about a specific center point.
double PathDurationGet()=0
Get total (seconds) duration of the planned motion that will happen when run is called.
void Resume()=0
Resume an axis.
RapidVector< double > InverseKinematics(RobotPosition pose)=0
Run the given pose through the current inverse kinematic model to see the joint positions the robot w...
void Run()=0
Run the loaded path lines/arcs. The behavior is non-blocking. Use Robot.MotionDoneWait() to block.
Represents a collection of joints in Cartesian space with forward and inverse kinematics....
void ClearFaults()=0
Clears the MultiAxis fault then the Robot's error bit.
uint64_t SampleCounterGet()=0
Gets the current sample counter of the Robot's main execution thread.
void EndEffectorTransformSet(const RobotPosition &transformPosition)=0
Transform that defines the control point relative to the end end of the current kinematic model with ...
void OriginTransformSet(const RobotPosition &transformPosition)=0
Transform that defines the origin of the robot in the world as a RobotPosition.
const KinematicModel & ModelGet()=0
Get the model this robot was created with.
static void RobotDelete(MotionController *controller, Robot *robot)
Delete a Robot.
RSI::RapidCode::Cartesian::Gcode * Gcode
An object to load and run Gcode files.
A representation of the robot containing the Pose and the positions of the free axes.
RobotPosition(const Cartesian::Pose &newPose, const RapidVector< double > &freeAxes)
Copies the new pose into this->Pose. Makes a deep copy of the given RapidVector for this->FreeAxes.
RobotPosition()
Default constructor. Initializes all values to 0. Leaves the FreeAxes RapidVector empty.
Cartesian::RobotPosition operator*(const Cartesian::RobotPosition &otherPosition) const
multiplies this->Pose by otherPosition.Pose and adds otherPosition.FreeAxes to this->FreeAxes (since ...
double AxisValueGet(uint32_t index) const
Gets the freeAxis value at the specified index. Returns 0 if index is out of bounds of the FreeAxes.
RobotPosition(const Cartesian::Pose &newPose, const size_t numberOfFreeAxes=0)
Copies the new pose into this->Pose. Specified number of free axes initialized with zeroes for their ...
void AxisValueSet(uint32_t index, double value)
Sets the freeaxis value at specified index to the passed value. Does nothing if index is out of bound...
RobotPosition(const Cartesian::RobotPosition &otherPosition)
Copy constructor. Copies values of Pose and FreeAxes.
Cartesian::RobotPosition Inverse() const
Gets the inverse of the pose and free axes.
RapidVector< double > FreeAxes
The Robot's free axes. Will have elements added to it for each free axis in the robot.
Cartesian::RobotPosition operator*(const Cartesian::Pose &otherPose) const
multiplies this->Pose by pose. Does not modify this->FreeAxes.
Vector3d is used for three-dimensional points and vectors.
Vector3d Normal() const
Returns a normalized copy of this.
Vector3d operator*(double scalar) const
Returns a Vector3d scaled scaled by. X*scalar, Y*scalar, Z*scalar.
Vector3d & Set(double x, double y, double z)
Sets X=x, Y=y, Z=z.
Vector3d operator+(const Vector3d &rightHandSide) const
Returns a copy of rightHandSide added to this. this.X+rightHandSide.X, this.Y+rightHandSide....
Vector3d operator/(double scalar) const
Returns a Vector3d scaled scaled by. X/scalar, Y/scalar, Z/scalar.
Vector3d & Set(const Vector3d &rightHandSide)
Sets X=rightHandSide.X, Y=rightHandSide.Y, Z=rightHandSide.Z.
Vector3d operator-(const Vector3d &rightHandSide) const
Returns a copy of rightHandSide subtracted from this. this.X-rightHandSide.X, this....
Vector3d & operator*=(double scalar)
Scales each element by scalar. X*=scalar, Y*=scalar, Z*=scalar.
Vector3d operator-() const
Returns a copy of this with elements negated. -X, -Y, -Z.
double Length() const
Returns the length of the vector. Aka the magnitude =sqrt(SumSquares)
bool operator!=(const Vector3d &rightHandSide) const
Returns whether this is not exactly equal to rightHandSide.
bool operator==(const Vector3d &rightHandSide) const
Returns whether this is exactly equal to rightHandSide.
Vector3d & operator/=(double scalar)
Scales each element by scalar. X/=scalar, Y/=scalar, Z/=scalar.
double SumSquares() const
Returns the sum of squares. X*X+Y*Y+Z*Z.
Vector3d()
Default constructor. X=Y=Z=0.
Vector3d & operator-=(const Vector3d &rightHandSide)
Subtracts rightHandSide from this. this.X-=rightHandSide.X, this.Y-=rightHandSide....
Vector3d & operator+=(const Vector3d &rightHandSide)
Adds rightHandSide to this. this.X+=rightHandSide.X, this.Y+=rightHandSide.Y, this....
double Dot(const Vector3d &rightHandSide) const
Dot Product. this.X*rightHandSide.X + this.Y*rightHandSide.Y + this.Z*rightHandSide....
Vector3d Inverse() const
Returns an inverted copy of this. this * this.inverse() = I.
Vector3d Cross(const Vector3d &rightHandSide) const
Cross Product. Vector.X = this.Y*rightHandSide.z - this.Z*rightHandSide.Y Vector.Y = this....
Vector3d & operator=(const Vector3d &rightHandSide)
Sets this = rightHandSide.
Vector3d(double x, double y, double z)
Constructor. X=x, Y=y, Z=z.
bool Equals(const Vector3d &rightHandSide, const double tolerance) const
Checks whether this.X==rightHandSide.X && this.Y==rightHandSide.Y && this.Z==rightHandSide....
Vector3d & Normalize()
Normalizes this.
Vector3d(const Vector3d &rightHandSide)
Constructor. X=rightHandSide.X, Y=rightHandSide.Y, Z=rightHandSide.Z.
Represents the RMP soft motion controller. This class provides an interface to general controller con...
Definition rsi.h:815
Represents multiple axes of motion control, allows you to map two or more Axis objects together for e...
Definition rsi.h:10603
The RapidCode base class. All non-error objects are derived from this class.
Definition rsi.h:178
Represents the error details thrown as an exception by all RapidCode classes. This class contains an ...
Definition rsi.h:105
PathMode
Motion types. For G-code use and Cartesian path motion.
Plane
Rotational directions about and axis.
CartesianAxis
This enum specifies which Cartesian axis a LinearJointMapping maps a robot joint to.
PathState
State of the Robot.
@ Stopping
Motion was interrupted by a stop command or error on the MultiAxis. EStop time will be used....
@ Moving
A path motion is in progress.
@ Idle
The path motion is not moving or in progress. If the joints are moved directly, the PathState will be...
@ Uninitialized
Library is loading. Wait until it is idle.
RotationDirection
Rotational directions about an axis.
LinearUnits
Unit types. For G-code use.
GCodeWorkOffset
Register names for G-Code work offsets.
const char * LineText
The actual line content from the G-code file.
RsiError * UserError
Allows the user to set give error details to the G-Code execution thread what occurred during the exe...
int32_t LineNumber
The line number in the G-code file where the M-code is encountered.
Holds data for the G-code M-code callback mechanism.
Data for adding joint or free-axis mappings when building a kinematic model.
double Offset
The offset value that will be added to the scaled joint position.
int JointIndex
The index of the robot's joint to map (within an array of joint positions). Often this index will ref...
ModelAxisMapping(int jointIndex)
Constructor that sets the FointIndex and uses defaults for the rest of the data members.
double Scaling
The scaling value that the joint position will be multiplied by.
ModelAxisMapping(int jointIndex, double scaling, double offset, const char *const label)
Constructor that sets all data members in the struct.