├── .github └── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md ├── .gitignore ├── .gitmodules ├── .travis.yml ├── CMakeLists.txt ├── CONTRIBUTION.md ├── Doxyfile ├── LICENSE ├── README.md ├── aarch64.cmake ├── armhf.cmake ├── config ├── M3_params.yaml ├── m1_dynamic_params.cfg ├── m2p_params.yaml ├── x2.perspective ├── x2_dynamic_params.cfg ├── x2_params.yaml └── x2_ros_control.yaml ├── corc.cmake ├── doc ├── 1.GettingStarted │ ├── AdvancedSimulationAndHardwareTesting.md │ ├── GSBasicSimulation.md │ ├── GSM1DemoMachine.md │ ├── GSM2DemoMachine.md │ ├── GSM3DemoMachine.md │ ├── GettingStarted.md │ ├── InstallLinux.md │ ├── InstallROS.md │ ├── InstallWindows.md │ └── ROS2Application.md ├── 2.Hardware │ ├── BBUse.md │ ├── CANIMU.md │ ├── InputsList.md │ ├── ModifyingDevice.md │ └── USBCANadapters.md ├── 3.Software │ ├── CustomApplication.md │ ├── Flowchart.md │ ├── Logging.md │ └── NetworkCommunication.md ├── Diagrams │ ├── CORCStateMachineExecution.drawio │ └── CORC_flow_chart.drawio ├── am5729-beagleboneai-custom-can.dtb └── img │ ├── CANRJ45.png │ ├── CORCStateMachineExecutionDiagram.png │ ├── CORC_flow_chart.png │ ├── CORC_flow_chart.svg │ ├── DeviceHardwareModif.png │ ├── FLNLUnity.png │ ├── FLNLUnity.svg │ ├── GSExoSimSD.png │ ├── M1_robot.png │ ├── M2WithFrames.png │ ├── M3WithFrames.png │ ├── M3WithFrames.svg │ ├── exoTestMachine.png │ ├── x2_gui.png │ ├── x2_realtimeViz.png │ ├── x2_rviz.png │ └── x2_sin_rviz.gif ├── launch ├── m1_real.launch ├── x2_real.launch ├── x2_ros2.launch.py └── x2_sim.launch ├── lib └── Eigen │ ├── CMakeLists.txt │ ├── COPYING.BSD │ ├── COPYING.GPL │ ├── COPYING.LGPL │ ├── COPYING.MINPACK │ ├── COPYING.MPL2 │ ├── COPYING.README │ ├── CTestConfig.cmake │ ├── CTestCustom.cmake.in │ ├── Eigen │ ├── CMakeLists.txt │ ├── Cholesky │ ├── CholmodSupport │ ├── Core │ ├── Dense │ ├── Eigen │ ├── Eigenvalues │ ├── Geometry │ ├── Householder │ ├── IterativeLinearSolvers │ ├── Jacobi │ ├── LU │ ├── MetisSupport │ ├── OrderingMethods │ ├── PaStiXSupport │ ├── PardisoSupport │ ├── QR │ ├── QtAlignedMalloc │ ├── SPQRSupport │ ├── SVD │ ├── Sparse │ ├── SparseCholesky │ ├── SparseCore │ ├── SparseLU │ ├── SparseQR │ ├── StdDeque │ ├── StdList │ ├── StdVector │ ├── SuperLUSupport │ ├── UmfPackSupport │ └── src │ │ ├── Cholesky │ │ ├── LDLT.h │ │ ├── LLT.h │ │ └── LLT_LAPACKE.h │ │ ├── CholmodSupport │ │ └── CholmodSupport.h │ │ ├── Core │ │ ├── ArithmeticSequence.h │ │ ├── Array.h │ │ ├── ArrayBase.h │ │ ├── ArrayWrapper.h │ │ ├── Assign.h │ │ ├── AssignEvaluator.h │ │ ├── Assign_MKL.h │ │ ├── BandMatrix.h │ │ ├── Block.h │ │ ├── BooleanRedux.h │ │ ├── CommaInitializer.h │ │ ├── ConditionEstimator.h │ │ ├── CoreEvaluators.h │ │ ├── CoreIterators.h │ │ ├── CwiseBinaryOp.h │ │ ├── CwiseNullaryOp.h │ │ ├── CwiseTernaryOp.h │ │ ├── CwiseUnaryOp.h │ │ ├── CwiseUnaryView.h │ │ ├── DenseBase.h │ │ ├── DenseCoeffsBase.h │ │ ├── DenseStorage.h │ │ ├── Diagonal.h │ │ ├── DiagonalMatrix.h │ │ ├── DiagonalProduct.h │ │ ├── Dot.h │ │ ├── EigenBase.h │ │ ├── ForceAlignedAccess.h │ │ ├── Fuzzy.h │ │ ├── GeneralProduct.h │ │ ├── GenericPacketMath.h │ │ ├── GlobalFunctions.h │ │ ├── IO.h │ │ ├── IndexedView.h │ │ ├── Inverse.h │ │ ├── Map.h │ │ ├── MapBase.h │ │ ├── MathFunctions.h │ │ ├── MathFunctionsImpl.h │ │ ├── Matrix.h │ │ ├── MatrixBase.h │ │ ├── NestByValue.h │ │ ├── NoAlias.h │ │ ├── NumTraits.h │ │ ├── PermutationMatrix.h │ │ ├── PlainObjectBase.h │ │ ├── Product.h │ │ ├── ProductEvaluators.h │ │ ├── Random.h │ │ ├── Redux.h │ │ ├── Ref.h │ │ ├── Replicate.h │ │ ├── ReturnByValue.h │ │ ├── Reverse.h │ │ ├── Select.h │ │ ├── SelfAdjointView.h │ │ ├── SelfCwiseBinaryOp.h │ │ ├── Solve.h │ │ ├── SolveTriangular.h │ │ ├── SolverBase.h │ │ ├── StableNorm.h │ │ ├── Stride.h │ │ ├── Swap.h │ │ ├── Transpose.h │ │ ├── Transpositions.h │ │ ├── TriangularMatrix.h │ │ ├── VectorBlock.h │ │ ├── VectorwiseOp.h │ │ ├── Visitor.h │ │ ├── arch │ │ │ ├── AVX │ │ │ │ ├── Complex.h │ │ │ │ ├── MathFunctions.h │ │ │ │ ├── PacketMath.h │ │ │ │ └── TypeCasting.h │ │ │ ├── AVX512 │ │ │ │ ├── MathFunctions.h │ │ │ │ └── PacketMath.h │ │ │ ├── AltiVec │ │ │ │ ├── Complex.h │ │ │ │ ├── MathFunctions.h │ │ │ │ └── PacketMath.h │ │ │ ├── CUDA │ │ │ │ ├── Complex.h │ │ │ │ ├── Half.h │ │ │ │ ├── MathFunctions.h │ │ │ │ ├── PacketMath.h │ │ │ │ ├── PacketMathHalf.h │ │ │ │ └── TypeCasting.h │ │ │ ├── Default │ │ │ │ └── Settings.h │ │ │ ├── NEON │ │ │ │ ├── Complex.h │ │ │ │ ├── MathFunctions.h │ │ │ │ └── PacketMath.h │ │ │ ├── SSE │ │ │ │ ├── Complex.h │ │ │ │ ├── MathFunctions.h │ │ │ │ ├── PacketMath.h │ │ │ │ └── TypeCasting.h │ │ │ └── ZVector │ │ │ │ ├── Complex.h │ │ │ │ ├── MathFunctions.h │ │ │ │ └── PacketMath.h │ │ ├── functors │ │ │ ├── AssignmentFunctors.h │ │ │ ├── BinaryFunctors.h │ │ │ ├── NullaryFunctors.h │ │ │ ├── StlFunctors.h │ │ │ ├── TernaryFunctors.h │ │ │ └── UnaryFunctors.h │ │ ├── products │ │ │ ├── GeneralBlockPanelKernel.h │ │ │ ├── GeneralMatrixMatrix.h │ │ │ ├── GeneralMatrixMatrixTriangular.h │ │ │ ├── GeneralMatrixMatrixTriangular_BLAS.h │ │ │ ├── GeneralMatrixMatrix_BLAS.h │ │ │ ├── GeneralMatrixVector.h │ │ │ ├── GeneralMatrixVector_BLAS.h │ │ │ ├── Parallelizer.h │ │ │ ├── SelfadjointMatrixMatrix.h │ │ │ ├── SelfadjointMatrixMatrix_BLAS.h │ │ │ ├── SelfadjointMatrixVector.h │ │ │ ├── SelfadjointMatrixVector_BLAS.h │ │ │ ├── SelfadjointProduct.h │ │ │ ├── SelfadjointRank2Update.h │ │ │ ├── TriangularMatrixMatrix.h │ │ │ ├── TriangularMatrixMatrix_BLAS.h │ │ │ ├── TriangularMatrixVector.h │ │ │ ├── TriangularMatrixVector_BLAS.h │ │ │ ├── TriangularSolverMatrix.h │ │ │ ├── TriangularSolverMatrix_BLAS.h │ │ │ └── TriangularSolverVector.h │ │ └── util │ │ │ ├── BlasUtil.h │ │ │ ├── Constants.h │ │ │ ├── DisableStupidWarnings.h │ │ │ ├── ForwardDeclarations.h │ │ │ ├── IndexedViewHelper.h │ │ │ ├── IntegralConstant.h │ │ │ ├── MKL_support.h │ │ │ ├── Macros.h │ │ │ ├── Memory.h │ │ │ ├── Meta.h │ │ │ ├── NonMPL2.h │ │ │ ├── ReenableStupidWarnings.h │ │ │ ├── StaticAssert.h │ │ │ ├── SymbolicIndex.h │ │ │ └── XprHelper.h │ │ ├── Eigenvalues │ │ ├── ComplexEigenSolver.h │ │ ├── ComplexSchur.h │ │ ├── ComplexSchur_LAPACKE.h │ │ ├── EigenSolver.h │ │ ├── GeneralizedEigenSolver.h │ │ ├── GeneralizedSelfAdjointEigenSolver.h │ │ ├── HessenbergDecomposition.h │ │ ├── MatrixBaseEigenvalues.h │ │ ├── RealQZ.h │ │ ├── RealSchur.h │ │ ├── RealSchur_LAPACKE.h │ │ ├── SelfAdjointEigenSolver.h │ │ ├── SelfAdjointEigenSolver_LAPACKE.h │ │ └── Tridiagonalization.h │ │ ├── Geometry │ │ ├── AlignedBox.h │ │ ├── AngleAxis.h │ │ ├── EulerAngles.h │ │ ├── Homogeneous.h │ │ ├── Hyperplane.h │ │ ├── OrthoMethods.h │ │ ├── ParametrizedLine.h │ │ ├── Quaternion.h │ │ ├── Rotation2D.h │ │ ├── RotationBase.h │ │ ├── Scaling.h │ │ ├── Transform.h │ │ ├── Translation.h │ │ ├── Umeyama.h │ │ └── arch │ │ │ └── Geometry_SSE.h │ │ ├── Householder │ │ ├── BlockHouseholder.h │ │ ├── Householder.h │ │ └── HouseholderSequence.h │ │ ├── IterativeLinearSolvers │ │ ├── BasicPreconditioners.h │ │ ├── BiCGSTAB.h │ │ ├── ConjugateGradient.h │ │ ├── IncompleteCholesky.h │ │ ├── IncompleteLUT.h │ │ ├── IterativeSolverBase.h │ │ ├── LeastSquareConjugateGradient.h │ │ └── SolveWithGuess.h │ │ ├── Jacobi │ │ └── Jacobi.h │ │ ├── LU │ │ ├── Determinant.h │ │ ├── FullPivLU.h │ │ ├── InverseImpl.h │ │ ├── PartialPivLU.h │ │ ├── PartialPivLU_LAPACKE.h │ │ └── arch │ │ │ └── Inverse_SSE.h │ │ ├── MetisSupport │ │ └── MetisSupport.h │ │ ├── OrderingMethods │ │ ├── Amd.h │ │ ├── Eigen_Colamd.h │ │ └── Ordering.h │ │ ├── PaStiXSupport │ │ └── PaStiXSupport.h │ │ ├── PardisoSupport │ │ └── PardisoSupport.h │ │ ├── QR │ │ ├── ColPivHouseholderQR.h │ │ ├── ColPivHouseholderQR_LAPACKE.h │ │ ├── CompleteOrthogonalDecomposition.h │ │ ├── FullPivHouseholderQR.h │ │ ├── HouseholderQR.h │ │ └── HouseholderQR_LAPACKE.h │ │ ├── SPQRSupport │ │ └── SuiteSparseQRSupport.h │ │ ├── SVD │ │ ├── BDCSVD.h │ │ ├── JacobiSVD.h │ │ ├── JacobiSVD_LAPACKE.h │ │ ├── SVDBase.h │ │ └── UpperBidiagonalization.h │ │ ├── SparseCholesky │ │ ├── SimplicialCholesky.h │ │ └── SimplicialCholesky_impl.h │ │ ├── SparseCore │ │ ├── AmbiVector.h │ │ ├── CompressedStorage.h │ │ ├── ConservativeSparseSparseProduct.h │ │ ├── MappedSparseMatrix.h │ │ ├── SparseAssign.h │ │ ├── SparseBlock.h │ │ ├── SparseColEtree.h │ │ ├── SparseCompressedBase.h │ │ ├── SparseCwiseBinaryOp.h │ │ ├── SparseCwiseUnaryOp.h │ │ ├── SparseDenseProduct.h │ │ ├── SparseDiagonalProduct.h │ │ ├── SparseDot.h │ │ ├── SparseFuzzy.h │ │ ├── SparseMap.h │ │ ├── SparseMatrix.h │ │ ├── SparseMatrixBase.h │ │ ├── SparsePermutation.h │ │ ├── SparseProduct.h │ │ ├── SparseRedux.h │ │ ├── SparseRef.h │ │ ├── SparseSelfAdjointView.h │ │ ├── SparseSolverBase.h │ │ ├── SparseSparseProductWithPruning.h │ │ ├── SparseTranspose.h │ │ ├── SparseTriangularView.h │ │ ├── SparseUtil.h │ │ ├── SparseVector.h │ │ ├── SparseView.h │ │ └── TriangularSolver.h │ │ ├── SparseLU │ │ ├── SparseLU.h │ │ ├── SparseLUImpl.h │ │ ├── SparseLU_Memory.h │ │ ├── SparseLU_Structs.h │ │ ├── SparseLU_SupernodalMatrix.h │ │ ├── SparseLU_Utils.h │ │ ├── SparseLU_column_bmod.h │ │ ├── SparseLU_column_dfs.h │ │ ├── SparseLU_copy_to_ucol.h │ │ ├── SparseLU_gemm_kernel.h │ │ ├── SparseLU_heap_relax_snode.h │ │ ├── SparseLU_kernel_bmod.h │ │ ├── SparseLU_panel_bmod.h │ │ ├── SparseLU_panel_dfs.h │ │ ├── SparseLU_pivotL.h │ │ ├── SparseLU_pruneL.h │ │ └── SparseLU_relax_snode.h │ │ ├── SparseQR │ │ └── SparseQR.h │ │ ├── StlSupport │ │ ├── StdDeque.h │ │ ├── StdList.h │ │ ├── StdVector.h │ │ └── details.h │ │ ├── SuperLUSupport │ │ └── SuperLUSupport.h │ │ ├── UmfPackSupport │ │ └── UmfPackSupport.h │ │ ├── misc │ │ ├── Image.h │ │ ├── Kernel.h │ │ ├── RealSvd2x2.h │ │ ├── blas.h │ │ ├── lapack.h │ │ ├── lapacke.h │ │ └── lapacke_mangling.h │ │ └── plugins │ │ ├── ArrayCwiseBinaryOps.h │ │ ├── ArrayCwiseUnaryOps.h │ │ ├── BlockMethods.h │ │ ├── CommonCwiseBinaryOps.h │ │ ├── CommonCwiseUnaryOps.h │ │ ├── IndexedViewMethods.h │ │ ├── MatrixCwiseBinaryOps.h │ │ └── MatrixCwiseUnaryOps.h │ ├── INSTALL │ ├── README.md │ ├── eigen3.pc.in │ └── signature_of_eigen3_matrix_library ├── msg ├── X2Acceleration.msg ├── X2AccelerationMerge.msg └── X2Array.msg ├── package.ros1.xml ├── package.ros2.xml ├── script ├── createStateMachine.sh ├── initCAN0.sh ├── initCAN0CAN1.sh ├── initCAN1.sh ├── initVCAN.sh ├── rqt.sh └── uploadBB.sh └── src ├── apps ├── ExoTestMachine │ ├── TrajectoryGenerator │ │ ├── DummyTrajectoryGenerator.cpp │ │ └── DummyTrajectoryGenerator.h │ ├── app.cmake │ └── stateMachine │ │ ├── ExoTestMachine.cpp │ │ ├── ExoTestMachine.h │ │ └── states │ │ ├── ExoTestState.cpp │ │ ├── ExoTestState.h │ │ ├── InitState.cpp │ │ ├── InitState.h │ │ ├── Sitting.cpp │ │ ├── Sitting.h │ │ ├── SittingDwn.cpp │ │ ├── SittingDwn.h │ │ ├── Standing.cpp │ │ ├── Standing.h │ │ ├── StandingUp.cpp │ │ └── StandingUp.h ├── LoggingDevice │ ├── app.cmake │ └── stateMachine │ │ ├── LoggingDevice.cpp │ │ ├── LoggingDevice.h │ │ └── states │ │ ├── CalibrateState.cpp │ │ ├── CalibrateState.h │ │ ├── IdleState.cpp │ │ ├── IdleState.h │ │ ├── InitState.cpp │ │ ├── InitState.h │ │ ├── RecordState.cpp │ │ └── RecordState.h ├── M1DemoMachine │ ├── M1DemoMachine.cpp │ ├── M1DemoMachine.h │ ├── M1DemoStates.cpp │ ├── M1DemoStates.h │ └── app.cmake ├── M1DemoMachineROS │ ├── M1DemoMachineROS.cpp │ ├── M1DemoMachineROS.h │ ├── M1MachineROS.cpp │ ├── M1MachineROS.h │ ├── app.cmake │ └── states │ │ ├── MultiControllerState.cpp │ │ └── MultiControllerState.h ├── M2DemoMachine │ ├── M2DemoMachine.cpp │ ├── M2DemoMachine.h │ ├── M2DemoStates.cpp │ ├── M2DemoStates.h │ └── app.cmake ├── M2ProDemoMachine │ ├── M2ProDemoMachine.cpp │ ├── M2ProDemoMachine.h │ ├── M2ProDemoStates.cpp │ ├── M2ProDemoStates.h │ ├── README.md │ └── app.cmake ├── M3DemoMachine │ ├── M3DemoMachine.cpp │ ├── M3DemoMachine.h │ ├── M3DemoStates.cpp │ ├── M3DemoStates.h │ └── app.cmake ├── StateMachineTemplate │ ├── StateMachineTemplate.cpp │ ├── StateMachineTemplate.h │ ├── StatesTemplate.cpp │ ├── StatesTemplate.h │ └── app.cmake ├── X2DemoMachine │ ├── X2DemoMachine.cpp │ ├── X2DemoMachine.h │ ├── X2DemoMachineROS.cpp │ ├── X2DemoMachineROS.h │ ├── app.cmake │ └── states │ │ ├── X2DemoState.cpp │ │ └── X2DemoState.h └── X2ROS2DemoMachine │ ├── X2ROS2DemoMachine.cpp │ ├── X2ROS2DemoMachine.h │ ├── X2ROS2Node.cpp │ ├── X2ROS2Node.h │ ├── X2ROS2State.cpp │ ├── X2ROS2State.h │ └── app.cmake ├── core ├── CANopen │ ├── CANcomms │ │ ├── CO_comm_helpers.c │ │ ├── CO_comm_helpers.h │ │ ├── CO_command.c │ │ ├── CO_command.h │ │ ├── CO_master.c │ │ ├── CO_master.h │ │ ├── CO_time.c │ │ └── CO_time.h │ ├── CANopenNode │ │ ├── CANopen.c │ │ ├── CANopen.h │ │ ├── LICENSE │ │ ├── README.md │ │ ├── RPDO.cpp │ │ ├── RPDO.h │ │ ├── TPDO.cpp │ │ ├── TPDO.h │ │ └── stack │ │ │ ├── CO_Emergency.c │ │ │ ├── CO_Emergency.h │ │ │ ├── CO_HBconsumer.c │ │ │ ├── CO_HBconsumer.h │ │ │ ├── CO_NMT_Heartbeat.c │ │ │ ├── CO_NMT_Heartbeat.h │ │ │ ├── CO_PDO.c │ │ │ ├── CO_PDO.h │ │ │ ├── CO_SDO.c │ │ │ ├── CO_SDO.h │ │ │ ├── CO_SDOmaster.c │ │ │ ├── CO_SDOmaster.h │ │ │ ├── CO_SYNC.c │ │ │ ├── CO_SYNC.h │ │ │ ├── CO_trace.c │ │ │ ├── CO_trace.h │ │ │ ├── crc16-ccitt.c │ │ │ ├── crc16-ccitt.h │ │ │ └── socketCAN │ │ │ ├── CO_Linux_tasks.c │ │ │ ├── CO_Linux_tasks.h │ │ │ ├── CO_OD_storage.c │ │ │ ├── CO_OD_storage.h │ │ │ ├── CO_driver.c │ │ │ └── CO_driver.h │ └── objDict │ │ ├── CANopenSocket.eds │ │ ├── CANopenSocket.html │ │ ├── CO_OD.c │ │ ├── CO_OD.h │ │ └── _project.html ├── LogHelper.h ├── application.cpp ├── application.h ├── logging.cpp ├── logging.h ├── main.cpp ├── network │ └── FLNLHelper.h ├── robot │ ├── CANDevice.cpp │ ├── CANDevice.h │ ├── Drive.cpp │ ├── Drive.h │ ├── InputDevice.cpp │ ├── InputDevice.h │ ├── Joint.cpp │ ├── Joint.h │ ├── Robot.cpp │ └── Robot.h └── stateMachine │ ├── State.h │ ├── StateMachine.cpp │ └── StateMachine.h └── hardware ├── IO ├── CANIMU.cpp ├── CANIMU.h ├── FourierForceSensor.cpp ├── FourierForceSensor.h ├── FourierHandle.cpp ├── FourierHandle.h ├── HX711.cpp ├── HX711.h ├── Joystick.cpp ├── Joystick.h ├── Keyboard.cpp ├── Keyboard.h ├── RobotousRFT.cpp ├── RobotousRFT.h ├── TechnaidIMU.cpp ├── TechnaidIMU.h ├── iobb.c └── iobb.h ├── drives ├── CopleyDrive.cpp ├── CopleyDrive.h ├── KincoDrive.cpp └── KincoDrive.h └── platforms ├── LoggingRobot ├── LoggingRobot.cpp └── LoggingRobot.h ├── M1 ├── JointM1.cpp ├── JointM1.h ├── RobotM1.cpp └── RobotM1.h ├── M2 ├── JointM2.cpp ├── JointM2.h ├── RobotM2.cpp └── RobotM2.h ├── M2P ├── JointM2P.cpp ├── JointM2P.h ├── RobotM2P.cpp └── RobotM2P.h ├── M3 ├── JointM3.cpp ├── JointM3.h ├── RobotM3.cpp ├── RobotM3.h ├── SignalProcessing.cpp └── SignalProcessing.h └── X2 ├── X2Joint.cpp ├── X2Joint.h ├── X2Robot.cpp └── X2Robot.h /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve. Report bugs found while using the project 4 | title: "[BUG]" 5 | labels: bug 6 | assignees: "" 7 | --- 8 | 9 | 10 | 11 | ## Description 12 | 13 | 14 | 15 | ## Expected Behavior 16 | 17 | 18 | 19 | ## Actual Behavior 20 | 21 | 22 | 23 | ## Possible Fix 24 | 25 | 26 | 27 | ## Steps to Reproduce 28 | 29 | 30 | 31 | 32 | 1. 2. 3. 4. 33 | 34 | ## Context 35 | 36 | 37 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest features, propose improvements, discuss new ideas. 4 | title: "[FEATURE]" 5 | labels: enhancement 6 | assignees: "" 7 | --- 8 | 9 | 10 | 11 | ## Detailed Description 12 | 13 | 14 | 15 | ## Context 16 | 17 | 18 | 19 | 20 | ## Possible Implementation 21 | 22 | 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | *.iml 3 | CANopenSocket/canopend/X2_log.txt 4 | *.o 5 | .vs/ 6 | .vscode/ 7 | /.devcontainer/Dockerfile 8 | /.devcontainer/devcontainer.json 9 | /build/* 10 | bbbxc 11 | EXO_APP_2020 12 | cmake-build-debug/ 13 | .idea/ 14 | .history/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lib/FLNL"] 2 | path = lib/FLNL 3 | url = https://github.com/vcrocher/libFLNL.git 4 | [submodule "lib/yaml-cpp"] 5 | path = lib/yaml-cpp 6 | url = https://github.com/jbeder/yaml-cpp 7 | [submodule "lib/spdlog"] 8 | path = lib/spdlog 9 | url = https://github.com/gabime/spdlog -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This will run on Travis' 'new' container-based infrastructure 2 | sudo: false 3 | 4 | # Blacklist 5 | branches: 6 | only: 7 | - master 8 | 9 | # Install dependencies 10 | addons: 11 | apt: 12 | packages: 13 | - doxygen 14 | - graphviz 15 | 16 | # Build your code e.g. by calling make 17 | script: 18 | - doxygen Doxyfile 19 | 20 | # Generate and deploy documentation 21 | deploy: 22 | provider: pages 23 | skip_cleanup: true 24 | local_dir: html 25 | github_token: $GH_REPO_TOKEN 26 | on: 27 | branch: master 28 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.3) 2 | project(CORC C CXX) 3 | 4 | #To cross-compile for Arm (BeagleBone) use the armhf toolchain: 5 | # cmake -DCMAKE_TOOLCHAIN_FILE=../armhf.cmake .. 6 | # For aarch64 (e.g. Jetson): 7 | # cmake -DCMAKE_TOOLCHAIN_FILE=../aarch64.cmake .. 8 | 9 | ################################## USER FLAGS ################################## 10 | 11 | ### Select the application by setting the state machine app.cmake to use 12 | include(src/apps/ExoTestMachine/app.cmake) 13 | #include(src/apps/M1DemoMachine/app.cmake) 14 | #include(src/apps/M1DemoMachineROS/app.cmake) 15 | #include(src/apps/M2DemoMachine/app.cmake) 16 | #include(src/apps/M2ProDemoMachine/app.cmake) 17 | #include(src/apps/M3DemoMachine/app.cmake) 18 | #include(src/apps/X2DemoMachine/app.cmake) 19 | #include(src/apps/X2ROS2DemoMachine/app.cmake) 20 | #include(src/apps/LoggingDevice/app.cmake) 21 | #include(../myStateMachineApp/app.cmake) ## example only, need to be defined 22 | 23 | ## Comment to use actual hardware, uncomment for a nor robot (virtual) app 24 | set(NO_ROBOT ON) 25 | 26 | ## Select desired logging level (Options: TRACE, DEBUG, INFO, WARN, ERROR, CRITICAL, OFF) 27 | ## INFO is the recommended level in normal operation 28 | set(CORC_LOGGING_LEVEL DEBUG) 29 | 30 | ################################################################################ 31 | 32 | ## CORC internal cmake logic 33 | include(corc.cmake) 34 | -------------------------------------------------------------------------------- /aarch64.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.3) 2 | set(CMAKE_SYSTEM_NAME Linux) 3 | set(CMAKE_SYSTEM_PROCESSOR aarch64) 4 | set(CMAKE_C_COMPILER aarch64-linux-gnu-gcc) 5 | set(CMAKE_CXX_COMPILER aarch64-linux-gnu-g++) 6 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 7 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 8 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 9 | set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) -------------------------------------------------------------------------------- /armhf.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.3) 2 | set(CMAKE_SYSTEM_NAME Linux) 3 | set(CMAKE_SYSTEM_PROCESSOR arm) 4 | set(CMAKE_C_COMPILER arm-linux-gnueabihf-gcc) 5 | set(CMAKE_CXX_COMPILER arm-linux-gnueabihf-g++) 6 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 7 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 8 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 9 | set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) -------------------------------------------------------------------------------- /config/m1_dynamic_params.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "CORC" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("ff_ratio", double_t, 0, "Feedforward ratio", 0.7, 0, 1) # 0.6 9 | gen.add("kp", double_t, 0, "P", 1.2, 0, 12) # 3.0 10 | gen.add("ki", double_t, 0, "I", 0.05, 0, 1) # 0.7 11 | gen.add("kd", double_t, 0, "D", 0.02, 0, 10) # 0.01 12 | gen.add("tick_max", double_t, 0, "Reset interval (s)", 60, 0, 600) 13 | gen.add("spk", double_t, 0, "S_P", 0, 0, 50) 14 | 15 | interaction_enum = gen.enum([ 16 | gen.const("zero_calibration", int_t, 0, "controller 1"), 17 | gen.const("zero_torque", int_t, 1, "controller 1"), 18 | gen.const("follow_position", int_t, 2, "controller 2"), 19 | gen.const("follow_torque", int_t, 3, "controller 3"), 20 | gen.const("virtual_spring", int_t, 4, "controller 4"), 21 | gen.const("transperancy", int_t, 5, "controller 5"), 22 | gen.const("send_high", int_t, 11, "controller 11"), 23 | gen.const("send_low", int_t, 12, "controller 12"), 24 | gen.const("send_high_low", int_t, 13, "controller 13"),], 25 | "MultiState Controller Mode") 26 | 27 | gen.add("controller_mode", int_t, 1, "Controller Mode", 1, 0, 13, edit_method=interaction_enum) # 5 is the maximum value 28 | 29 | exit(gen.generate(PACKAGE, "CORC", "dynamic_params")) -------------------------------------------------------------------------------- /config/m2p_params.yaml: -------------------------------------------------------------------------------- 1 | M2Pro: 2 | 3 | -------------------------------------------------------------------------------- /config/x2_dynamic_params.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "CORC" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | #gen.add("desired_interaction_force", double_t, 1, "Desired Interaction Force", 0, -20, 20) 9 | 10 | enable = gen.add_group("A_Enable") 11 | enable.add("left_hip", bool_t, 2, "Enable Joint 0", False) 12 | enable.add("left_knee", bool_t, 3, "Enable Joint 1", False) 13 | enable.add("right_hip", bool_t, 4, "Enable Joint 2", False) 14 | enable.add("right_knee", bool_t, 5, "Enable Joint 3", False) 15 | 16 | sin = gen.add_group("B_Sine") 17 | sin.add("Amplitude", double_t, 1, "Amplitude (Nm or rad/s)", 0.0, 0, 10) 18 | sin.add("Period", double_t, 2, "Period (s)", 2.0, 0.2, 5) 19 | sin.add("offset", double_t, 3, "(Nm or rad/s)", 0, -5, 5) 20 | 21 | interactionTorque = gen.add_group("C_Interaction_Torque_Control_and_Estimation") 22 | interactionTorque.add("k_left_hip", double_t, 1, "Left Hip Virtual Mass", 0.0, 0, 5) 23 | interactionTorque.add("k_left_knee", double_t, 2, "Left Knee Virtual Mass", 0.0, 0, 5) 24 | interactionTorque.add("k_right_hip", double_t, 3, "Right Hip Virtual Mass", 0.0, 0, 5) 25 | interactionTorque.add("k_right_knee", double_t, 4, "Right Knee Virtual Mass", 0.0, 0, 5) 26 | 27 | interactionTorque.add("g_cutoff", double_t, 1, "Interaction Torque CutOff Freq", 40, 1, 150) 28 | 29 | acceleration = gen.add_group("D_Acceleration") 30 | 31 | acceleration.add("acc_deriv_cutoff", double_t, 6, "Acceleration Derivative CutOff Freq", 15, 0, 100) 32 | acceleration.add("backpack_deriv_cutoff", double_t, 6, "Acceleration Derivative CutOff Freq", 5, 0, 100) 33 | 34 | interaction_enum = gen.enum([ 35 | gen.const("no_controller", int_t, 0, "controller 0"), 36 | gen.const("zero_torque", int_t, 1, "controller 1"), 37 | gen.const("zero_velocity", int_t, 2, "controller 2"), 38 | gen.const("simplified_transparency", int_t, 3, "controller 3"), 39 | gen.const("sin_vel", int_t, 4, "controller 4"), 40 | gen.const("sin_torque", int_t, 5, "controller 5"),], 41 | "Controller Mode") 42 | 43 | gen.add("controller_mode", int_t, 0, "Controller Mode", 0, 0, 5, edit_method=interaction_enum) 44 | 45 | exit(gen.generate(PACKAGE, "CORC", "dynamic_params")) -------------------------------------------------------------------------------- /config/x2_params.yaml: -------------------------------------------------------------------------------- 1 | x2: # That ends with 0.9999 are not investigated yet 2 | m: [0.7, 2.14, 0.64, 0.47, 1.25, 10.3] # masses of upper thigh, lower thigh, upper shank, lower shank, foot, backpack [kg] todo 9.9 3 | l: [0.39, 0.4, 0.39, 0.4] # length of left thigh, left shank, right thigh, right shank [kg] 4 | s: [0.0811, 0.05564, 0.09080, 0.0562, 0.08834, 0.25, 0.1] # hip to upper thigh COM, knee to lower thigh COM, # todo calculate and fix last 2 parameters [m] 5 | # knee to upper shank COM, ankle to lower shank COM, ankle to foot COM, backpack COM to hip parallel to backpack link, backpack COM to hip vertical to backpack link [m] 6 | I: [0.002197, 0.005901, 0.002346, 0.0007941, 0.004441, 0.2] # mass moment of inertia of upper thigh, lower thigh, upper shank, lower shank, foot, backpack [kg.m^2] #todo calculate and fix backpack inertia 7 | G: [1.9, 1.9, 1.9, 1.9] # apparent rotor inertias (I_rotor*reduction_ratio^2) [kg. m^2] 8 | c0: [5.01, 3.26, 4.30, 4.45] # viscous fric constant of joints [N.s] [5.01, 3.26, 4.30, 4.45] 9 | c1: [4.58, 4.67, 3.25, 5.16] # coulomb friction const of joints [N.m] [4.58, 4.67, 3.25, 5.16] 10 | c2: [0, 0, 0, 0] # friction const related to sqrt of vel 11 | cuff_weights: [0, 0, 0, 0] # cuff Weights [N] not used 12 | force_sensor_scale_factor: [-0.324, -0.1326, 0.326, -0.137] # scale factor of force sensors [N/sensor output] [-0.41, -0.1326, 0.0, -0.1199] [-0.15, -0.1326, 0.15, -0.137] 13 | grf_sensor_scale_factor: [1, 1] 14 | grf_sensor_threshold: [100, 100] # Values higher than threshold will indicates contact with the ground 15 | max_torque: 100 # maximum allowable torque [Nm] If exceeds, robot will be disabled 16 | # max_power: 95 17 | max_velocity: 4.5 # maximum allowable speed [rad/s] If exceeds, robot will be disabled 18 | imu_distance: [1, 0.37, 0.29, 0.37] # distance of contact IMUs along the link from the previous joint # NOT USED 19 | joint_position_limits: # [deg] 20 | hip_max: 120 21 | hip_min: -40 22 | knee_max: 0 23 | knee_min: -120 24 | # technaid_imu: 25 | # can_channel: "can1" 26 | # serial_no: [596] # [519, 559, 596] 27 | # network_id: [141] # [135, 137, 141] 28 | # location: ['b'] # b for backpack, c for cuff/contact # ['c', 'c', 'b'] 29 | -------------------------------------------------------------------------------- /config/x2_ros_control.yaml: -------------------------------------------------------------------------------- 1 | x2: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 333 6 | 7 | # Backpack position Controller --------------------------------------- 8 | backpack_position_controller: 9 | type: position_controllers/JointPositionController 10 | joints: ['world_to_backpack'] 11 | 12 | # Position Controllers --------------------------------------- 13 | position_controller: 14 | type: position_controllers/JointGroupPositionController 15 | joints: ['left_hip_joint', 'left_knee_joint', 'right_hip_joint', 'right_knee_joint'] 16 | 17 | # Velocity Controllers --------------------------------------- 18 | velocity_controller: 19 | type: velocity_controllers/JointGroupVelocityController 20 | joints: ['left_hip_joint', 'left_knee_joint', 'right_hip_joint', 'right_knee_joint'] 21 | 22 | # Torque Controllers --------------------------------------- 23 | torque_controller: 24 | type: effort_controllers/JointGroupEffortController 25 | joints: ['left_hip_joint', 'left_knee_joint', 'right_hip_joint', 'right_knee_joint'] 26 | 27 | -------------------------------------------------------------------------------- /doc/1.GettingStarted/InstallROS.md: -------------------------------------------------------------------------------- 1 | ## Install ROS and Catkin tools 2 | We suggest installing either ROS Melodic or ROS Noetic depending on your Ubuntu version. While Melodic is compatible with 3 | Ubuntu 18.04, Noetic is compatible with Ubuntu 20.04. 4 | 5 | Update your Debian package index: 6 | ```bash 7 | $ sudo apt update 8 | ``` 9 | 10 | Install ROS Melodic or Noetic Desktop-Full: 11 | ```bash 12 | $ sudo apt install ros--desktop-full 13 | ``` 14 | 15 | Install Catkin tools for: 16 | ```bash 17 | $ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list' 18 | $ wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 19 | $ sudo apt-get update 20 | $ sudo apt-get install python-catkin-tools (For melodic) 21 | $ sudo apt-get install python3-catkin-tools (For noetic) 22 | ``` 23 | 24 | Install ros_control: 25 | ```bash 26 | $ sudo apt-get install ros--ros-control ros--ros-controllers 27 | ``` -------------------------------------------------------------------------------- /doc/1.GettingStarted/InstallWindows.md: -------------------------------------------------------------------------------- 1 | # [DEPRECATED: USING WINDOWS AS DEV SYSTEM IS NOT RECOMMENDED] Getting Started with Windows - Installation Instructions 2 | A compile CORC application can only run on a Linux machine, and as such, Windows can only be used as a development environment. Therefore, you must install the cross-compilation toolchains to compile for a Linux device. Here, we provide instructions only for the arm-linux-gnueabihf toolchain (which have been tested by the development team on various Beaglebone devices), however, cross compilation toolchains for other Linux devices may also be used. 3 | 4 | ## Git 5 | CORC is hosted on Github, and uses Git for its version control. Whilst technically not required (all the files can be downloaded from the Github repository), it is highly recommended that Git is used. 6 | 7 | On Windows, you have the following two options: 8 | 9 | - [Git for Windows](https://git-scm.com/download/win) - A command-line based utility 10 | - [Github Desktop](https://desktop.github.com/) - A GUI specifically for Github Integration 11 | 12 | ## Other Tools 13 | It is also useful to have a development environment. If you do not have a preferred environment, VSCode (https://code.visualstudio.com/) is popular and can be configured with a number of extensions which make development easier. 14 | 15 | ## Cross-compilation Tools 16 | Windows Tools chain for Beaglebone (which uses the arm-linux-gnueabihf) can be downloaded here: https://gnutoolchains.com/beaglebone/. Note that the tested environment is the `bone-debian-9.5-2018-10-07` Image with the `beaglebone-gcc6.3.0-r2.exe` toolchain. -------------------------------------------------------------------------------- /doc/1.GettingStarted/ROS2Application.md: -------------------------------------------------------------------------------- 1 | # ROS2 Application - Instructions 2 | 3 | Minimal instructions to compile and run a CORC application as a ROS2 node able to publish the robot state. 4 | 5 | These instructons have been tested with ROS2 Humble and ROS2 Jazzy and assumes you have an Ubuntu system with one of those versions installed (it may work with other ROS2 versions but this has not been tested). See [here](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) for ROS2 installation instructions. 6 | Testing the application also assumes you have an ExoMotus X2 Exoskeleton. 7 | 8 | 9 | > Note : these instructions are for ROS2. See [here](AdvancedSimulationAndHardwareTesting.md) for a ROS1 example. 10 | > Note : specific, detailed instructions for running ROS2 with the Armmotus EMU (as well as Python packages) are available [here](https://github.com/UniMelbHumanRoboticsLab/FourierEMU-CORC-ROS2). 11 | 12 | ## Compilation 13 | 14 | 15 | ```bash 16 | $ source /opt/ros/humble/setup.bash 17 | ``` 18 | 19 | In CORC root directory, rename ros2 package configuration file 20 | ```bash 21 | $ mv package.ros2.xml package.xml 22 | ``` 23 | 24 | 25 | Edit the CMAKEFileList.txt to select a ROS2 state machine: 26 | 27 | ``` 28 | ... 29 | ... 30 | #include(src/apps/X2DemoMachine/app.cmake) 31 | include(src/apps/X2ROS2DemoMachine/app.cmake) 32 | #include(src/apps/LoggingDevice/app.cmake) 33 | #include(../myStateMachineApp/app.cmake) ## example only, need to be defined 34 | 35 | # Comment to use actual hardware, uncomment for a nor robot (virtual) app 36 | set(NO_ROBOT OFF) 37 | 38 | ... 39 | ... 40 | ``` 41 | 42 | In the state machine folder, open the `app.cmake` and ensure it is configured to use ROS2: 43 | ``` 44 | ################################## USER FLAGS ################################## 45 | 46 | ## Which platform (robot) is the state machine using? 47 | ## this is the correspondig folder name in src/hardware/platform to use 48 | set(PLATFORM X2) 49 | 50 | ## Compile for ROS 2 51 | set(ROS 2) 52 | 53 | ################################################################################ 54 | ... 55 | 56 | ``` 57 | 58 | 59 | From the **CANOpenController parent folder**, compile the package: 60 | ```bash 61 | $ colcon build 62 | $ source install/setup.bash 63 | ``` 64 | 65 | 66 | ## Run the node 67 | ```bash 68 | $ ros2 run CORC X2ROS2DemoMachine_APP 69 | ``` 70 | 71 | Here CORC is the package name and the CORC app name is the executable name. 72 | 73 | See the content of the `src/apps/X2ROS2DemoMachine` folder to see how to interact with ROS2 from within CORC. 74 | -------------------------------------------------------------------------------- /doc/2.Hardware/InputsList.md: -------------------------------------------------------------------------------- 1 | # List of available sensors 2 | CORC [IO folder](../../src/hardware/IO/) contains a number of "drivers" for various types of sensors listed below. It also contains drivers for generic keyboards and joysticks which can easily be added to any platform. 3 | 4 | > Note that only some of them are used in supported platforms (others may have been used or are used in platforms not listed in CORC). 5 | 6 | 7 | ## Force sensors and load cells 8 | - Robotous RFT 6 axis force-torque sensors on CAN (tested with RFT80-6A01 sensors). See dedicated class [here](../../src/hardware/IO/RobotousRFT.h). 9 | - HX711 chip: a dedicated ADC and amplifier for load cells. See dedicated class [here](../../src/hardware/IO/HX711.h). 10 | 11 | ## IMUs 12 | - [Techniad IMU](https://www.technaid.com/products/inertial-measurement-unit-tech-imu-biomechanichs/): a driver for the CAN based IMU from Technaid is available [here](../../src/hardware/IO/TechnaidIMU.cpp). See also [X2Robot](../../src/hardware/platforms/X2/X2Robot.h) platform for an example of use. 13 | - A Generic driver for standalone IMUs on microcontrollers (tested with Xiao and Teensy with BNO055 IMU). See [dedicated documentation page](CANIMU.md) for details including links to microcontroller firmwares. 14 | 15 | ## Other IOs 16 | - BeagleBone DIOs: a copy of iobb library by shabaz is available in [IO folder](../../src/hardware/IO/) to handle BB GPIOs. 17 | - Fourier X2 handle/pendant: see [here](../../src/hardware/IO/FourierHandle.h). 18 | 19 | -------------------------------------------------------------------------------- /doc/2.Hardware/ModifyingDevice.md: -------------------------------------------------------------------------------- 1 | # Modifying your device 2 | 3 | By using CORC your are replacing the controller (both hadware and software) of your device to get full control on it. This thus involve some hardware modification such that the CORC controller (either a PC with USB-CAN or an SBC) get access to the internal CAN bus of the robot: 4 | 5 | ![Schematic of required hardware modification](../img/DeviceHardwareModif.png) 6 | Schematic of required hardware modifications. 7 | 8 | In a nutshell, you will need to: 9 | 10 | 1. Open your device to access the electronics; 11 | 2. Find an accessible CAN port to connect an RJ45 (ethernet) cable (either on a distributiuon board, a motor drive or another CAN peripheral with a spare RJ45 CAN port); 12 | 3. Run this cable outside, either adding or using a panel connector or just an accessible space; 13 | 4. Disconnect temporarilly the internal existing device controller: you do not want CORC to fight with the existing controller for the control of the device over the CAN bus; 14 | 5. Test CORC carefully, ensuring your Estop are functional and everything is safe; 15 | 6. Close the device. 16 | 17 | 18 | ## A great power... 19 | 20 | ... you know the drill. 21 | 22 | CORC allows you to entirely replace the controller of your machine and modify it the way you want. This potentially involves removing all safety and limits and create new risks. You should be aware of those risks (for your users, for yourself and for the device itself) and manage them appropriately. Process carefully following good practices. -------------------------------------------------------------------------------- /doc/2.Hardware/USBCANadapters.md: -------------------------------------------------------------------------------- 1 | # Note on USB-CAN adapters 2 | 3 | CORC can be used running on a Linux computer/laptop/SBC and using a USB-CAN adapter. Currently, CORC has been tested with two different USB-CAN adapters: 4 | 5 | - the [PCAN-USB](https://www.peak-system.com/PCAN-USB.199.0.html?&L=1) 6 | - the [inno-maker USB CAN Module](https://www.inno-maker.com/product/usb-can/) 7 | 8 | ## Recommendations 9 | 10 | We strongly recommend to use the inno-maker one: cheaper and has better performance natively. A simple case to be 3D printed is available to download [here](https://github.com/UniMelbHumanRoboticsLab/CORC-UI-Demo/blob/14f7208c58703624cce523ccba70a3ac1c9fef7e/innoMakerUSBCANEnclosure.zip). 11 | 12 | In case of the use of the PCAN device, you must check if a communication delay exists. In this case, use the following commands to reduce this delay (below the ms): 13 | 14 | - Edit `/etc/modprobe.d/pcan.conf` and add the line: `options pcan fast_fwd=1`. 15 | - Then reload the driver: 16 | ``` 17 | $sudo rmmod pcan 18 | $sudo modprobe pcan 19 | ``` 20 | -------------------------------------------------------------------------------- /doc/3.Software/Flowchart.md: -------------------------------------------------------------------------------- 1 | # Multi-threaded execution 2 | 3 | To run application tasks and network tasks (all CANopen activity and messaging) the software runs three parallel threads. Two threads to maintain up to date CANopen processes and a third running the applications program loop. A flowchart of a typical CORC implementation can be found below. 4 | 5 | ## CANopenNode threads 6 | 7 | Using the CANopenNode framework, CORC runs both a mainline thread for executing time-consuming CANopen operations (SDO messaging, Heartbeat, LSS, etc.) and a "realtime" thread for time-critical CANopen operations (PDO and SYNC messaging), running at a constant interval (typically 1ms). 8 | 9 | For more details on the implementation of CANopenNode please go to the projects GitHub and documentation [page](https://github.com/CANopenNode/CANopenNode). 10 | 11 | # Application thread 12 | 13 | The application loop speed can be altered by the developper, however, it must run slower than the CANopen Node rt loop. This ensures any PDO reliant commands to nodes from the application are processed and sent out on the bus. 14 | 15 | These values are set in `core\application.h`: 16 | ``` 17 | const float controlLoopPeriodInms = 2.; //!< Define the control loop period (in ms): the period of rt_control_thread loop (and so the app update rate). In [ms]. 18 | const float CANUpdateLoopPeriodInms = 1.0; //!< Define the CAN PDO processing period. SYNCH messages (and so actual PDO update) is set to twice this period (twice slower). In [ms]. 19 | const float activeWaitTimeInms = 0.5; //!< Define the active wait time (busy CPU) in the control thread to get more accurate timing. Typically between 10%-50% of controlLoopPeriod, 0 for no effect. In [ms]. 20 | ``` 21 | It is the responsability of the developper to ensure that the execution of its states (`during()`, `entry()` and `exit()` methods) can be executed during that time interval. A warning message is issued when a time overflow occurs. 22 | 23 | 24 | # Flowchart of a typical CORC implementation 25 | 26 | ![Flow Chart](../img/CORC_flow_chart.png) 27 | -------------------------------------------------------------------------------- /doc/am5729-beagleboneai-custom-can.dtb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/am5729-beagleboneai-custom-can.dtb -------------------------------------------------------------------------------- /doc/img/CANRJ45.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/CANRJ45.png -------------------------------------------------------------------------------- /doc/img/CORCStateMachineExecutionDiagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/CORCStateMachineExecutionDiagram.png -------------------------------------------------------------------------------- /doc/img/CORC_flow_chart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/CORC_flow_chart.png -------------------------------------------------------------------------------- /doc/img/DeviceHardwareModif.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/DeviceHardwareModif.png -------------------------------------------------------------------------------- /doc/img/FLNLUnity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/FLNLUnity.png -------------------------------------------------------------------------------- /doc/img/GSExoSimSD.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/GSExoSimSD.png -------------------------------------------------------------------------------- /doc/img/M1_robot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/M1_robot.png -------------------------------------------------------------------------------- /doc/img/M2WithFrames.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/M2WithFrames.png -------------------------------------------------------------------------------- /doc/img/M3WithFrames.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/M3WithFrames.png -------------------------------------------------------------------------------- /doc/img/exoTestMachine.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/exoTestMachine.png -------------------------------------------------------------------------------- /doc/img/x2_gui.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/x2_gui.png -------------------------------------------------------------------------------- /doc/img/x2_realtimeViz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/x2_realtimeViz.png -------------------------------------------------------------------------------- /doc/img/x2_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/x2_rviz.png -------------------------------------------------------------------------------- /doc/img/x2_sin_rviz.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UniMelbHumanRoboticsLab/CANOpenRobotController/7a4272390791cd90f0965830714e40004df0b104/doc/img/x2_sin_rviz.gif -------------------------------------------------------------------------------- /launch/m1_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/x2_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /launch/x2_ros2.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from xacro import process_file 4 | from ament_index_python import get_package_share_directory 5 | 6 | from launch import LaunchDescription 7 | from launch_ros.actions import Node 8 | 9 | def generate_launch_description(): 10 | # Launch description 11 | ld = LaunchDescription() 12 | 13 | # Package share path 14 | corc_path = get_package_share_directory("CORC") 15 | x2_description_path = get_package_share_directory("x2_description") 16 | 17 | # Package share paths 18 | exo_path = os.path.join(corc_path, "config", "x2_params.yaml") 19 | rviz_path = os.path.join(x2_description_path, "rviz", "x2.rviz") 20 | 21 | # Process XACRO 22 | urdf_xacro = process_file( 23 | os.path.join(x2_description_path, "urdf", "x2_fixed_base.urdf.xacro") 24 | ) 25 | 26 | # Nodes 27 | exo_node = Node( 28 | package="CORC", 29 | executable="X2ROS2DemoMachine_APP", 30 | arguments=["-can", "can0"], 31 | name="x2", 32 | output="screen", 33 | parameters=[ 34 | { "exo_path": exo_path } 35 | ] 36 | ) 37 | robot_state_node = Node( 38 | package="robot_state_publisher", 39 | executable="robot_state_publisher", 40 | name="robot_state_publisher", 41 | parameters=[ 42 | { "robot_description": urdf_xacro.toprettyxml(indent=' ') } 43 | ] 44 | ) 45 | rviz_node = Node( 46 | package="rviz2", 47 | executable="rviz2", 48 | arguments=["-d", rviz_path], 49 | name="rviz2" 50 | ) 51 | 52 | # Launch description actions 53 | ld.add_action(exo_node) 54 | ld.add_action(robot_state_node) 55 | ld.add_action(rviz_node) 56 | return ld 57 | -------------------------------------------------------------------------------- /lib/Eigen/COPYING.BSD: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Intel Corporation. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Intel Corporation nor the names of its contributors may 13 | be used to endorse or promote products derived from this software without 14 | specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 23 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ -------------------------------------------------------------------------------- /lib/Eigen/COPYING.MINPACK: -------------------------------------------------------------------------------- 1 | Minpack Copyright Notice (1999) University of Chicago. All rights reserved 2 | 3 | Redistribution and use in source and binary forms, with or 4 | without modification, are permitted provided that the 5 | following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above 8 | copyright notice, this list of conditions and the following 9 | disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above 12 | copyright notice, this list of conditions and the following 13 | disclaimer in the documentation and/or other materials 14 | provided with the distribution. 15 | 16 | 3. The end-user documentation included with the 17 | redistribution, if any, must include the following 18 | acknowledgment: 19 | 20 | "This product includes software developed by the 21 | University of Chicago, as Operator of Argonne National 22 | Laboratory. 23 | 24 | Alternately, this acknowledgment may appear in the software 25 | itself, if and wherever such third-party acknowledgments 26 | normally appear. 27 | 28 | 4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" 29 | WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE 30 | UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND 31 | THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR 32 | IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES 33 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE 34 | OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY 35 | OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR 36 | USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF 37 | THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) 38 | DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION 39 | UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL 40 | BE CORRECTED. 41 | 42 | 5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT 43 | HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF 44 | ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, 45 | INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF 46 | ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF 47 | PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER 48 | SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT 49 | (INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, 50 | EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE 51 | POSSIBILITY OF SUCH LOSS OR DAMAGES. 52 | 53 | -------------------------------------------------------------------------------- /lib/Eigen/COPYING.README: -------------------------------------------------------------------------------- 1 | Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: 2 | http://www.mozilla.org/MPL/2.0/ 3 | http://www.mozilla.org/MPL/2.0/FAQ.html 4 | 5 | Some files contain third-party code under BSD or LGPL licenses, whence the other 6 | COPYING.* files here. 7 | 8 | All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. 9 | For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. 10 | 11 | If you want to guarantee that the Eigen code that you are #including is licensed 12 | under the MPL2 and possibly more permissive licenses (like BSD), #define this 13 | preprocessor symbol: 14 | EIGEN_MPL2_ONLY 15 | For example, with most compilers, you could add this to your project CXXFLAGS: 16 | -DEIGEN_MPL2_ONLY 17 | This will cause a compilation error to be generated if you #include any code that is 18 | LGPL licensed. 19 | -------------------------------------------------------------------------------- /lib/Eigen/CTestConfig.cmake: -------------------------------------------------------------------------------- 1 | ## This file should be placed in the root directory of your project. 2 | ## Then modify the CMakeLists.txt file in the root directory of your 3 | ## project to incorporate the testing dashboard. 4 | ## # The following are required to uses Dart and the Cdash dashboard 5 | ## ENABLE_TESTING() 6 | ## INCLUDE(CTest) 7 | set(CTEST_PROJECT_NAME "Eigen") 8 | set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC") 9 | 10 | set(CTEST_DROP_METHOD "http") 11 | set(CTEST_DROP_SITE "manao.inria.fr") 12 | set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen") 13 | set(CTEST_DROP_SITE_CDASH TRUE) 14 | set(CTEST_PROJECT_SUBPROJECTS 15 | Official 16 | Unsupported 17 | ) 18 | -------------------------------------------------------------------------------- /lib/Eigen/CTestCustom.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "2000") 3 | set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS "2000") 4 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(RegexUtils) 2 | test_escape_string_as_regex() 3 | 4 | file(GLOB Eigen_directory_files "*") 5 | 6 | escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") 7 | 8 | foreach(f ${Eigen_directory_files}) 9 | if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src") 10 | list(APPEND Eigen_directory_files_to_install ${f}) 11 | endif() 12 | endforeach(f ${Eigen_directory_files}) 13 | 14 | install(FILES 15 | ${Eigen_directory_files_to_install} 16 | DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel 17 | ) 18 | 19 | install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h") 20 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Cholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLESKY_MODULE_H 9 | #define EIGEN_CHOLESKY_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Cholesky_Module Cholesky module 16 | * 17 | * 18 | * 19 | * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. 20 | * Those decompositions are also accessible via the following methods: 21 | * - MatrixBase::llt() 22 | * - MatrixBase::ldlt() 23 | * - SelfAdjointView::llt() 24 | * - SelfAdjointView::ldlt() 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | */ 30 | 31 | #include "src/Cholesky/LLT.h" 32 | #include "src/Cholesky/LDLT.h" 33 | #ifdef EIGEN_USE_LAPACKE 34 | #include "src/misc/lapacke.h" 35 | #include "src/Cholesky/LLT_LAPACKE.h" 36 | #endif 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_CHOLESKY_MODULE_H 41 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 42 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/CholmodSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_CHOLMODSUPPORT_MODULE_H 9 | #define EIGEN_CHOLMODSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup CholmodSupport_Module CholmodSupport module 21 | * 22 | * This module provides an interface to the Cholmod library which is part of the suitesparse package. 23 | * It provides the two following main factorization classes: 24 | * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. 25 | * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). 26 | * 27 | * For the sake of completeness, this module also propose the two following classes: 28 | * - class CholmodSimplicialLLT 29 | * - class CholmodSimplicialLDLT 30 | * Note that these classes does not bring any particular advantage compared to the built-in 31 | * SimplicialLLT and SimplicialLDLT factorization classes. 32 | * 33 | * \code 34 | * #include 35 | * \endcode 36 | * 37 | * In order to use this module, the cholmod headers must be accessible from the include paths, and your binary must be linked to the cholmod library and its dependencies. 38 | * The dependencies depend on how cholmod has been compiled. 39 | * For a cmake based project, you can use our FindCholmod.cmake module to help you in this task. 40 | * 41 | */ 42 | 43 | #include "src/CholmodSupport/CholmodSupport.h" 44 | 45 | #include "src/Core/util/ReenableStupidWarnings.h" 46 | 47 | #endif // EIGEN_CHOLMODSUPPORT_MODULE_H 48 | 49 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | #include "Sparse" 3 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Eigenvalues: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_EIGENVALUES_MODULE_H 9 | #define EIGEN_EIGENVALUES_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | #include "LU" 19 | #include "Geometry" 20 | 21 | /** \defgroup Eigenvalues_Module Eigenvalues module 22 | * 23 | * 24 | * 25 | * This module mainly provides various eigenvalue solvers. 26 | * This module also provides some MatrixBase methods, including: 27 | * - MatrixBase::eigenvalues(), 28 | * - MatrixBase::operatorNorm() 29 | * 30 | * \code 31 | * #include 32 | * \endcode 33 | */ 34 | 35 | #include "src/misc/RealSvd2x2.h" 36 | #include "src/Eigenvalues/Tridiagonalization.h" 37 | #include "src/Eigenvalues/RealSchur.h" 38 | #include "src/Eigenvalues/EigenSolver.h" 39 | #include "src/Eigenvalues/SelfAdjointEigenSolver.h" 40 | #include "src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h" 41 | #include "src/Eigenvalues/HessenbergDecomposition.h" 42 | #include "src/Eigenvalues/ComplexSchur.h" 43 | #include "src/Eigenvalues/ComplexEigenSolver.h" 44 | #include "src/Eigenvalues/RealQZ.h" 45 | #include "src/Eigenvalues/GeneralizedEigenSolver.h" 46 | #include "src/Eigenvalues/MatrixBaseEigenvalues.h" 47 | #ifdef EIGEN_USE_LAPACKE 48 | #include "src/misc/lapacke.h" 49 | #include "src/Eigenvalues/RealSchur_LAPACKE.h" 50 | #include "src/Eigenvalues/ComplexSchur_LAPACKE.h" 51 | #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" 52 | #endif 53 | 54 | #include "src/Core/util/ReenableStupidWarnings.h" 55 | 56 | #endif // EIGEN_EIGENVALUES_MODULE_H 57 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 58 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Geometry: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_GEOMETRY_MODULE_H 9 | #define EIGEN_GEOMETRY_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SVD" 16 | #include "LU" 17 | #include 18 | 19 | /** \defgroup Geometry_Module Geometry module 20 | * 21 | * This module provides support for: 22 | * - fixed-size homogeneous transformations 23 | * - translation, scaling, 2D and 3D rotations 24 | * - \link Quaternion quaternions \endlink 25 | * - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3) 26 | * - orthognal vector generation (\ref MatrixBase::unitOrthogonal) 27 | * - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink 28 | * - \link AlignedBox axis aligned bounding boxes \endlink 29 | * - \link umeyama least-square transformation fitting \endlink 30 | * 31 | * \code 32 | * #include 33 | * \endcode 34 | */ 35 | 36 | #include "src/Geometry/OrthoMethods.h" 37 | #include "src/Geometry/EulerAngles.h" 38 | 39 | #include "src/Geometry/Homogeneous.h" 40 | #include "src/Geometry/RotationBase.h" 41 | #include "src/Geometry/Rotation2D.h" 42 | #include "src/Geometry/Quaternion.h" 43 | #include "src/Geometry/AngleAxis.h" 44 | #include "src/Geometry/Transform.h" 45 | #include "src/Geometry/Translation.h" 46 | #include "src/Geometry/Scaling.h" 47 | #include "src/Geometry/Hyperplane.h" 48 | #include "src/Geometry/ParametrizedLine.h" 49 | #include "src/Geometry/AlignedBox.h" 50 | #include "src/Geometry/Umeyama.h" 51 | 52 | // Use the SSE optimized version whenever possible. At the moment the 53 | // SSE version doesn't compile when AVX is enabled 54 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 55 | #include "src/Geometry/arch/Geometry_SSE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_GEOMETRY_MODULE_H 61 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 62 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Householder: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_HOUSEHOLDER_MODULE_H 9 | #define EIGEN_HOUSEHOLDER_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Householder_Module Householder module 16 | * This module provides Householder transformations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | */ 22 | 23 | #include "src/Householder/Householder.h" 24 | #include "src/Householder/HouseholderSequence.h" 25 | #include "src/Householder/BlockHouseholder.h" 26 | 27 | #include "src/Core/util/ReenableStupidWarnings.h" 28 | 29 | #endif // EIGEN_HOUSEHOLDER_MODULE_H 30 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 31 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/IterativeLinearSolvers: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 9 | #define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** 17 | * \defgroup IterativeLinearSolvers_Module IterativeLinearSolvers module 18 | * 19 | * This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse. 20 | * Those solvers are accessible via the following classes: 21 | * - ConjugateGradient for selfadjoint (hermitian) matrices, 22 | * - LeastSquaresConjugateGradient for rectangular least-square problems, 23 | * - BiCGSTAB for general square matrices. 24 | * 25 | * These iterative solvers are associated with some preconditioners: 26 | * - IdentityPreconditioner - not really useful 27 | * - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices. 28 | * - IncompleteLUT - incomplete LU factorization with dual thresholding 29 | * 30 | * Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport. 31 | * 32 | \code 33 | #include 34 | \endcode 35 | */ 36 | 37 | #include "src/IterativeLinearSolvers/SolveWithGuess.h" 38 | #include "src/IterativeLinearSolvers/IterativeSolverBase.h" 39 | #include "src/IterativeLinearSolvers/BasicPreconditioners.h" 40 | #include "src/IterativeLinearSolvers/ConjugateGradient.h" 41 | #include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h" 42 | #include "src/IterativeLinearSolvers/BiCGSTAB.h" 43 | #include "src/IterativeLinearSolvers/IncompleteLUT.h" 44 | #include "src/IterativeLinearSolvers/IncompleteCholesky.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_ITERATIVELINEARSOLVERS_MODULE_H 49 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Jacobi: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_JACOBI_MODULE_H 9 | #define EIGEN_JACOBI_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup Jacobi_Module Jacobi module 16 | * This module provides Jacobi and Givens rotations. 17 | * 18 | * \code 19 | * #include 20 | * \endcode 21 | * 22 | * In addition to listed classes, it defines the two following MatrixBase methods to apply a Jacobi or Givens rotation: 23 | * - MatrixBase::applyOnTheLeft() 24 | * - MatrixBase::applyOnTheRight(). 25 | */ 26 | 27 | #include "src/Jacobi/Jacobi.h" 28 | 29 | #include "src/Core/util/ReenableStupidWarnings.h" 30 | 31 | #endif // EIGEN_JACOBI_MODULE_H 32 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 33 | 34 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/LU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_LU_MODULE_H 9 | #define EIGEN_LU_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup LU_Module LU module 16 | * This module includes %LU decomposition and related notions such as matrix inversion and determinant. 17 | * This module defines the following MatrixBase methods: 18 | * - MatrixBase::inverse() 19 | * - MatrixBase::determinant() 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | 26 | #include "src/misc/Kernel.h" 27 | #include "src/misc/Image.h" 28 | #include "src/LU/FullPivLU.h" 29 | #include "src/LU/PartialPivLU.h" 30 | #ifdef EIGEN_USE_LAPACKE 31 | #include "src/misc/lapacke.h" 32 | #include "src/LU/PartialPivLU_LAPACKE.h" 33 | #endif 34 | #include "src/LU/Determinant.h" 35 | #include "src/LU/InverseImpl.h" 36 | 37 | // Use the SSE optimized version whenever possible. At the moment the 38 | // SSE version doesn't compile when AVX is enabled 39 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 40 | #include "src/LU/arch/Inverse_SSE.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_LU_MODULE_H 46 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 47 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/MetisSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_METISSUPPORT_MODULE_H 9 | #define EIGEN_METISSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | 20 | /** \ingroup Support_modules 21 | * \defgroup MetisSupport_Module MetisSupport module 22 | * 23 | * \code 24 | * #include 25 | * \endcode 26 | * This module defines an interface to the METIS reordering package (http://glaros.dtc.umn.edu/gkhome/views/metis). 27 | * It can be used just as any other built-in method as explained in \link OrderingMethods_Module here. \endlink 28 | */ 29 | 30 | 31 | #include "src/MetisSupport/MetisSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_METISSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/OrderingMethods: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_ORDERINGMETHODS_MODULE_H 9 | #define EIGEN_ORDERINGMETHODS_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** 16 | * \defgroup OrderingMethods_Module OrderingMethods module 17 | * 18 | * This module is currently for internal use only 19 | * 20 | * It defines various built-in and external ordering methods for sparse matrices. 21 | * They are typically used to reduce the number of elements during 22 | * the sparse matrix decomposition (LLT, LU, QR). 23 | * Precisely, in a preprocessing step, a permutation matrix P is computed using 24 | * those ordering methods and applied to the columns of the matrix. 25 | * Using for instance the sparse Cholesky decomposition, it is expected that 26 | * the nonzeros elements in LLT(A*P) will be much smaller than that in LLT(A). 27 | * 28 | * 29 | * Usage : 30 | * \code 31 | * #include 32 | * \endcode 33 | * 34 | * A simple usage is as a template parameter in the sparse decomposition classes : 35 | * 36 | * \code 37 | * SparseLU > solver; 38 | * \endcode 39 | * 40 | * \code 41 | * SparseQR > solver; 42 | * \endcode 43 | * 44 | * It is possible as well to call directly a particular ordering method for your own purpose, 45 | * \code 46 | * AMDOrdering ordering; 47 | * PermutationMatrix perm; 48 | * SparseMatrix A; 49 | * //Fill the matrix ... 50 | * 51 | * ordering(A, perm); // Call AMD 52 | * \endcode 53 | * 54 | * \note Some of these methods (like AMD or METIS), need the sparsity pattern 55 | * of the input matrix to be symmetric. When the matrix is structurally unsymmetric, 56 | * Eigen computes internally the pattern of \f$A^T*A\f$ before calling the method. 57 | * If your matrix is already symmetric (at leat in structure), you can avoid that 58 | * by calling the method with a SelfAdjointView type. 59 | * 60 | * \code 61 | * // Call the ordering on the pattern of the lower triangular matrix A 62 | * ordering(A.selfadjointView(), perm); 63 | * \endcode 64 | */ 65 | 66 | #ifndef EIGEN_MPL2_ONLY 67 | #include "src/OrderingMethods/Amd.h" 68 | #endif 69 | 70 | #include "src/OrderingMethods/Ordering.h" 71 | #include "src/Core/util/ReenableStupidWarnings.h" 72 | 73 | #endif // EIGEN_ORDERINGMETHODS_MODULE_H 74 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/PaStiXSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PASTIXSUPPORT_MODULE_H 9 | #define EIGEN_PASTIXSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | #include 18 | } 19 | 20 | #ifdef complex 21 | #undef complex 22 | #endif 23 | 24 | /** \ingroup Support_modules 25 | * \defgroup PaStiXSupport_Module PaStiXSupport module 26 | * 27 | * This module provides an interface to the PaSTiX library. 28 | * PaSTiX is a general \b supernodal, \b parallel and \b opensource sparse solver. 29 | * It provides the two following main factorization classes: 30 | * - class PastixLLT : a supernodal, parallel LLt Cholesky factorization. 31 | * - class PastixLDLT: a supernodal, parallel LDLt Cholesky factorization. 32 | * - class PastixLU : a supernodal, parallel LU factorization (optimized for a symmetric pattern). 33 | * 34 | * \code 35 | * #include 36 | * \endcode 37 | * 38 | * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies. 39 | * The dependencies depend on how PaSTiX has been compiled. 40 | * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task. 41 | * 42 | */ 43 | 44 | #include "src/PaStiXSupport/PaStiXSupport.h" 45 | 46 | #include "src/Core/util/ReenableStupidWarnings.h" 47 | 48 | #endif // EIGEN_PASTIXSUPPORT_MODULE_H 49 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/PardisoSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_PARDISOSUPPORT_MODULE_H 9 | #define EIGEN_PARDISOSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup PardisoSupport_Module PardisoSupport module 19 | * 20 | * This module brings support for the Intel(R) MKL PARDISO direct sparse solvers. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the MKL headers must be accessible from the include paths, and your binary must be linked to the MKL library and its dependencies. 27 | * See this \ref TopicUsingIntelMKL "page" for more information on MKL-Eigen integration. 28 | * 29 | */ 30 | 31 | #include "src/PardisoSupport/PardisoSupport.h" 32 | 33 | #include "src/Core/util/ReenableStupidWarnings.h" 34 | 35 | #endif // EIGEN_PARDISOSUPPORT_MODULE_H 36 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/QR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QR_MODULE_H 9 | #define EIGEN_QR_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "Cholesky" 16 | #include "Jacobi" 17 | #include "Householder" 18 | 19 | /** \defgroup QR_Module QR module 20 | * 21 | * 22 | * 23 | * This module provides various QR decompositions 24 | * This module also provides some MatrixBase methods, including: 25 | * - MatrixBase::householderQr() 26 | * - MatrixBase::colPivHouseholderQr() 27 | * - MatrixBase::fullPivHouseholderQr() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/QR/HouseholderQR.h" 35 | #include "src/QR/FullPivHouseholderQR.h" 36 | #include "src/QR/ColPivHouseholderQR.h" 37 | #include "src/QR/CompleteOrthogonalDecomposition.h" 38 | #ifdef EIGEN_USE_LAPACKE 39 | #include "src/misc/lapacke.h" 40 | #include "src/QR/HouseholderQR_LAPACKE.h" 41 | #include "src/QR/ColPivHouseholderQR_LAPACKE.h" 42 | #endif 43 | 44 | #include "src/Core/util/ReenableStupidWarnings.h" 45 | 46 | #endif // EIGEN_QR_MODULE_H 47 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 48 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/QtAlignedMalloc: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_QTMALLOC_MODULE_H 9 | #define EIGEN_QTMALLOC_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #if (!EIGEN_MALLOC_ALREADY_ALIGNED) 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | void *qMalloc(std::size_t size) 18 | { 19 | return Eigen::internal::aligned_malloc(size); 20 | } 21 | 22 | void qFree(void *ptr) 23 | { 24 | Eigen::internal::aligned_free(ptr); 25 | } 26 | 27 | void *qRealloc(void *ptr, std::size_t size) 28 | { 29 | void* newPtr = Eigen::internal::aligned_malloc(size); 30 | memcpy(newPtr, ptr, size); 31 | Eigen::internal::aligned_free(ptr); 32 | return newPtr; 33 | } 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | 39 | #endif // EIGEN_QTMALLOC_MODULE_H 40 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 41 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/SPQRSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPQRSUPPORT_MODULE_H 9 | #define EIGEN_SPQRSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include "SuiteSparseQR.hpp" 16 | 17 | /** \ingroup Support_modules 18 | * \defgroup SPQRSupport_Module SuiteSparseQR module 19 | * 20 | * This module provides an interface to the SPQR library, which is part of the suitesparse package. 21 | * 22 | * \code 23 | * #include 24 | * \endcode 25 | * 26 | * In order to use this module, the SPQR headers must be accessible from the include paths, and your binary must be linked to the SPQR library and its dependencies (Cholmod, AMD, COLAMD,...). 27 | * For a cmake based project, you can use our FindSPQR.cmake and FindCholmod.Cmake modules 28 | * 29 | */ 30 | 31 | #include "src/CholmodSupport/CholmodSupport.h" 32 | #include "src/SPQRSupport/SuiteSparseQRSupport.h" 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/SVD: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SVD_MODULE_H 9 | #define EIGEN_SVD_MODULE_H 10 | 11 | #include "QR" 12 | #include "Householder" 13 | #include "Jacobi" 14 | 15 | #include "src/Core/util/DisableStupidWarnings.h" 16 | 17 | /** \defgroup SVD_Module SVD module 18 | * 19 | * 20 | * 21 | * This module provides SVD decomposition for matrices (both real and complex). 22 | * Two decomposition algorithms are provided: 23 | * - JacobiSVD implementing two-sided Jacobi iterations is numerically very accurate, fast for small matrices, but very slow for larger ones. 24 | * - BDCSVD implementing a recursive divide & conquer strategy on top of an upper-bidiagonalization which remains fast for large problems. 25 | * These decompositions are accessible via the respective classes and following MatrixBase methods: 26 | * - MatrixBase::jacobiSvd() 27 | * - MatrixBase::bdcSvd() 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | */ 33 | 34 | #include "src/misc/RealSvd2x2.h" 35 | #include "src/SVD/UpperBidiagonalization.h" 36 | #include "src/SVD/SVDBase.h" 37 | #include "src/SVD/JacobiSVD.h" 38 | #include "src/SVD/BDCSVD.h" 39 | #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT) 40 | #include "src/misc/lapacke.h" 41 | #include "src/SVD/JacobiSVD_LAPACKE.h" 42 | #endif 43 | 44 | #include "src/Core/util/ReenableStupidWarnings.h" 45 | 46 | #endif // EIGEN_SVD_MODULE_H 47 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 48 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/Sparse: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSE_MODULE_H 9 | #define EIGEN_SPARSE_MODULE_H 10 | 11 | /** \defgroup Sparse_Module Sparse meta-module 12 | * 13 | * Meta-module including all related modules: 14 | * - \ref SparseCore_Module 15 | * - \ref OrderingMethods_Module 16 | * - \ref SparseCholesky_Module 17 | * - \ref SparseLU_Module 18 | * - \ref SparseQR_Module 19 | * - \ref IterativeLinearSolvers_Module 20 | * 21 | \code 22 | #include 23 | \endcode 24 | */ 25 | 26 | #include "SparseCore" 27 | #include "OrderingMethods" 28 | #ifndef EIGEN_MPL2_ONLY 29 | #include "SparseCholesky" 30 | #endif 31 | #include "SparseLU" 32 | #include "SparseQR" 33 | #include "IterativeLinearSolvers" 34 | 35 | #endif // EIGEN_SPARSE_MODULE_H 36 | 37 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/SparseCholesky: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2013 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSECHOLESKY_MODULE_H 11 | #define EIGEN_SPARSECHOLESKY_MODULE_H 12 | 13 | #include "SparseCore" 14 | #include "OrderingMethods" 15 | 16 | #include "src/Core/util/DisableStupidWarnings.h" 17 | 18 | /** 19 | * \defgroup SparseCholesky_Module SparseCholesky module 20 | * 21 | * This module currently provides two variants of the direct sparse Cholesky decomposition for selfadjoint (hermitian) matrices. 22 | * Those decompositions are accessible via the following classes: 23 | * - SimplicialLLt, 24 | * - SimplicialLDLt 25 | * 26 | * Such problems can also be solved using the ConjugateGradient solver from the IterativeLinearSolvers module. 27 | * 28 | * \code 29 | * #include 30 | * \endcode 31 | */ 32 | 33 | #ifdef EIGEN_MPL2_ONLY 34 | #error The SparseCholesky module has nothing to offer in MPL2 only mode 35 | #endif 36 | 37 | #include "src/SparseCholesky/SimplicialCholesky.h" 38 | 39 | #ifndef EIGEN_MPL2_ONLY 40 | #include "src/SparseCholesky/SimplicialCholesky_impl.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_SPARSECHOLESKY_MODULE_H 46 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/SparseCore: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSECORE_MODULE_H 9 | #define EIGEN_SPARSECORE_MODULE_H 10 | 11 | #include "Core" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | /** 22 | * \defgroup SparseCore_Module SparseCore module 23 | * 24 | * This module provides a sparse matrix representation, and basic associated matrix manipulations 25 | * and operations. 26 | * 27 | * See the \ref TutorialSparse "Sparse tutorial" 28 | * 29 | * \code 30 | * #include 31 | * \endcode 32 | * 33 | * This module depends on: Core. 34 | */ 35 | 36 | #include "src/SparseCore/SparseUtil.h" 37 | #include "src/SparseCore/SparseMatrixBase.h" 38 | #include "src/SparseCore/SparseAssign.h" 39 | #include "src/SparseCore/CompressedStorage.h" 40 | #include "src/SparseCore/AmbiVector.h" 41 | #include "src/SparseCore/SparseCompressedBase.h" 42 | #include "src/SparseCore/SparseMatrix.h" 43 | #include "src/SparseCore/SparseMap.h" 44 | #include "src/SparseCore/MappedSparseMatrix.h" 45 | #include "src/SparseCore/SparseVector.h" 46 | #include "src/SparseCore/SparseRef.h" 47 | #include "src/SparseCore/SparseCwiseUnaryOp.h" 48 | #include "src/SparseCore/SparseCwiseBinaryOp.h" 49 | #include "src/SparseCore/SparseTranspose.h" 50 | #include "src/SparseCore/SparseBlock.h" 51 | #include "src/SparseCore/SparseDot.h" 52 | #include "src/SparseCore/SparseRedux.h" 53 | #include "src/SparseCore/SparseView.h" 54 | #include "src/SparseCore/SparseDiagonalProduct.h" 55 | #include "src/SparseCore/ConservativeSparseSparseProduct.h" 56 | #include "src/SparseCore/SparseSparseProductWithPruning.h" 57 | #include "src/SparseCore/SparseProduct.h" 58 | #include "src/SparseCore/SparseDenseProduct.h" 59 | #include "src/SparseCore/SparseSelfAdjointView.h" 60 | #include "src/SparseCore/SparseTriangularView.h" 61 | #include "src/SparseCore/TriangularSolver.h" 62 | #include "src/SparseCore/SparsePermutation.h" 63 | #include "src/SparseCore/SparseFuzzy.h" 64 | #include "src/SparseCore/SparseSolverBase.h" 65 | 66 | #include "src/Core/util/ReenableStupidWarnings.h" 67 | 68 | #endif // EIGEN_SPARSECORE_MODULE_H 69 | 70 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/SparseLU: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // Copyright (C) 2012 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_SPARSELU_MODULE_H 12 | #define EIGEN_SPARSELU_MODULE_H 13 | 14 | #include "SparseCore" 15 | 16 | /** 17 | * \defgroup SparseLU_Module SparseLU module 18 | * This module defines a supernodal factorization of general sparse matrices. 19 | * The code is fully optimized for supernode-panel updates with specialized kernels. 20 | * Please, see the documentation of the SparseLU class for more details. 21 | */ 22 | 23 | // Ordering interface 24 | #include "OrderingMethods" 25 | 26 | #include "src/SparseLU/SparseLU_gemm_kernel.h" 27 | 28 | #include "src/SparseLU/SparseLU_Structs.h" 29 | #include "src/SparseLU/SparseLU_SupernodalMatrix.h" 30 | #include "src/SparseLU/SparseLUImpl.h" 31 | #include "src/SparseCore/SparseColEtree.h" 32 | #include "src/SparseLU/SparseLU_Memory.h" 33 | #include "src/SparseLU/SparseLU_heap_relax_snode.h" 34 | #include "src/SparseLU/SparseLU_relax_snode.h" 35 | #include "src/SparseLU/SparseLU_pivotL.h" 36 | #include "src/SparseLU/SparseLU_panel_dfs.h" 37 | #include "src/SparseLU/SparseLU_kernel_bmod.h" 38 | #include "src/SparseLU/SparseLU_panel_bmod.h" 39 | #include "src/SparseLU/SparseLU_column_dfs.h" 40 | #include "src/SparseLU/SparseLU_column_bmod.h" 41 | #include "src/SparseLU/SparseLU_copy_to_ucol.h" 42 | #include "src/SparseLU/SparseLU_pruneL.h" 43 | #include "src/SparseLU/SparseLU_Utils.h" 44 | #include "src/SparseLU/SparseLU.h" 45 | 46 | #endif // EIGEN_SPARSELU_MODULE_H 47 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/SparseQR: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SPARSEQR_MODULE_H 9 | #define EIGEN_SPARSEQR_MODULE_H 10 | 11 | #include "SparseCore" 12 | #include "OrderingMethods" 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | /** \defgroup SparseQR_Module SparseQR module 16 | * \brief Provides QR decomposition for sparse matrices 17 | * 18 | * This module provides a simplicial version of the left-looking Sparse QR decomposition. 19 | * The columns of the input matrix should be reordered to limit the fill-in during the 20 | * decomposition. Built-in methods (COLAMD, AMD) or external methods (METIS) can be used to this end. 21 | * See the \link OrderingMethods_Module OrderingMethods\endlink module for the list 22 | * of built-in and external ordering methods. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | * 28 | * 29 | */ 30 | 31 | #include "OrderingMethods" 32 | #include "src/SparseCore/SparseColEtree.h" 33 | #include "src/SparseQR/SparseQR.h" 34 | 35 | #include "src/Core/util/ReenableStupidWarnings.h" 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/StdDeque: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDDEQUE_MODULE_H 12 | #define EIGEN_STDDEQUE_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdDeque.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDDEQUE_MODULE_H 28 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/StdList: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Hauke Heibel 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_STDLIST_MODULE_H 11 | #define EIGEN_STDLIST_MODULE_H 12 | 13 | #include "Core" 14 | #include 15 | 16 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 17 | 18 | #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) 19 | 20 | #else 21 | 22 | #include "src/StlSupport/StdList.h" 23 | 24 | #endif 25 | 26 | #endif // EIGEN_STDLIST_MODULE_H 27 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/StdVector: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Gael Guennebaud 5 | // Copyright (C) 2009 Hauke Heibel 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_STDVECTOR_MODULE_H 12 | #define EIGEN_STDVECTOR_MODULE_H 13 | 14 | #include "Core" 15 | #include 16 | 17 | #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */ 18 | 19 | #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...) 20 | 21 | #else 22 | 23 | #include "src/StlSupport/StdVector.h" 24 | 25 | #endif 26 | 27 | #endif // EIGEN_STDVECTOR_MODULE_H 28 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/SuperLUSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_SUPERLUSUPPORT_MODULE_H 9 | #define EIGEN_SUPERLUSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | #ifdef EMPTY 16 | #define EIGEN_EMPTY_WAS_ALREADY_DEFINED 17 | #endif 18 | 19 | typedef int int_t; 20 | #include 21 | #include 22 | #include 23 | 24 | // slu_util.h defines a preprocessor token named EMPTY which is really polluting, 25 | // so we remove it in favor of a SUPERLU_EMPTY token. 26 | // If EMPTY was already defined then we don't undef it. 27 | 28 | #if defined(EIGEN_EMPTY_WAS_ALREADY_DEFINED) 29 | # undef EIGEN_EMPTY_WAS_ALREADY_DEFINED 30 | #elif defined(EMPTY) 31 | # undef EMPTY 32 | #endif 33 | 34 | #define SUPERLU_EMPTY (-1) 35 | 36 | namespace Eigen { struct SluMatrix; } 37 | 38 | /** \ingroup Support_modules 39 | * \defgroup SuperLUSupport_Module SuperLUSupport module 40 | * 41 | * This module provides an interface to the SuperLU library. 42 | * It provides the following factorization class: 43 | * - class SuperLU: a supernodal sequential LU factorization. 44 | * - class SuperILU: a supernodal sequential incomplete LU factorization (to be used as a preconditioner for iterative methods). 45 | * 46 | * \warning This wrapper requires at least versions 4.0 of SuperLU. The 3.x versions are not supported. 47 | * 48 | * \warning When including this module, you have to use SUPERLU_EMPTY instead of EMPTY which is no longer defined because it is too polluting. 49 | * 50 | * \code 51 | * #include 52 | * \endcode 53 | * 54 | * In order to use this module, the superlu headers must be accessible from the include paths, and your binary must be linked to the superlu library and its dependencies. 55 | * The dependencies depend on how superlu has been compiled. 56 | * For a cmake based project, you can use our FindSuperLU.cmake module to help you in this task. 57 | * 58 | */ 59 | 60 | #include "src/SuperLUSupport/SuperLUSupport.h" 61 | 62 | #include "src/Core/util/ReenableStupidWarnings.h" 63 | 64 | #endif // EIGEN_SUPERLUSUPPORT_MODULE_H 65 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/UmfPackSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // This Source Code Form is subject to the terms of the Mozilla 5 | // Public License v. 2.0. If a copy of the MPL was not distributed 6 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 7 | 8 | #ifndef EIGEN_UMFPACKSUPPORT_MODULE_H 9 | #define EIGEN_UMFPACKSUPPORT_MODULE_H 10 | 11 | #include "SparseCore" 12 | 13 | #include "src/Core/util/DisableStupidWarnings.h" 14 | 15 | extern "C" { 16 | #include 17 | } 18 | 19 | /** \ingroup Support_modules 20 | * \defgroup UmfPackSupport_Module UmfPackSupport module 21 | * 22 | * This module provides an interface to the UmfPack library which is part of the suitesparse package. 23 | * It provides the following factorization class: 24 | * - class UmfPackLU: a multifrontal sequential LU factorization. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | * 30 | * In order to use this module, the umfpack headers must be accessible from the include paths, and your binary must be linked to the umfpack library and its dependencies. 31 | * The dependencies depend on how umfpack has been compiled. 32 | * For a cmake based project, you can use our FindUmfPack.cmake module to help you in this task. 33 | * 34 | */ 35 | 36 | #include "src/UmfPackSupport/UmfPackSupport.h" 37 | 38 | #include "src/Core/util/ReenableStupidWarnings.h" 39 | 40 | #endif // EIGEN_UMFPACKSUPPORT_MODULE_H 41 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/DiagonalProduct.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008 Gael Guennebaud 5 | // Copyright (C) 2007-2009 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_DIAGONALPRODUCT_H 12 | #define EIGEN_DIAGONALPRODUCT_H 13 | 14 | namespace Eigen { 15 | 16 | /** \returns the diagonal matrix product of \c *this by the diagonal matrix \a diagonal. 17 | */ 18 | template 19 | template 20 | EIGEN_DEVICE_FUNC inline const Product 21 | MatrixBase::operator*(const DiagonalBase &a_diagonal) const 22 | { 23 | return Product(derived(),a_diagonal.derived()); 24 | } 25 | 26 | } // end namespace Eigen 27 | 28 | #endif // EIGEN_DIAGONALPRODUCT_H 29 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/MathFunctionsImpl.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 Pedro Gonnet (pedro.gonnet@gmail.com) 5 | // Copyright (C) 2016 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_MATHFUNCTIONSIMPL_H 12 | #define EIGEN_MATHFUNCTIONSIMPL_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | /** \internal \returns the hyperbolic tan of \a a (coeff-wise) 19 | Doesn't do anything fancy, just a 13/6-degree rational interpolant which 20 | is accurate up to a couple of ulp in the range [-9, 9], outside of which 21 | the tanh(x) = +/-1. 22 | 23 | This implementation works on both scalars and packets. 24 | */ 25 | template 26 | T generic_fast_tanh_float(const T& a_x) 27 | { 28 | // Clamp the inputs to the range [-9, 9] since anything outside 29 | // this range is +/-1.0f in single-precision. 30 | const T plus_9 = pset1(9.f); 31 | const T minus_9 = pset1(-9.f); 32 | const T x = pmax(pmin(a_x, plus_9), minus_9); 33 | // The monomial coefficients of the numerator polynomial (odd). 34 | const T alpha_1 = pset1(4.89352455891786e-03f); 35 | const T alpha_3 = pset1(6.37261928875436e-04f); 36 | const T alpha_5 = pset1(1.48572235717979e-05f); 37 | const T alpha_7 = pset1(5.12229709037114e-08f); 38 | const T alpha_9 = pset1(-8.60467152213735e-11f); 39 | const T alpha_11 = pset1(2.00018790482477e-13f); 40 | const T alpha_13 = pset1(-2.76076847742355e-16f); 41 | 42 | // The monomial coefficients of the denominator polynomial (even). 43 | const T beta_0 = pset1(4.89352518554385e-03f); 44 | const T beta_2 = pset1(2.26843463243900e-03f); 45 | const T beta_4 = pset1(1.18534705686654e-04f); 46 | const T beta_6 = pset1(1.19825839466702e-06f); 47 | 48 | // Since the polynomials are odd/even, we need x^2. 49 | const T x2 = pmul(x, x); 50 | 51 | // Evaluate the numerator polynomial p. 52 | T p = pmadd(x2, alpha_13, alpha_11); 53 | p = pmadd(x2, p, alpha_9); 54 | p = pmadd(x2, p, alpha_7); 55 | p = pmadd(x2, p, alpha_5); 56 | p = pmadd(x2, p, alpha_3); 57 | p = pmadd(x2, p, alpha_1); 58 | p = pmul(x, p); 59 | 60 | // Evaluate the denominator polynomial p. 61 | T q = pmadd(x2, beta_6, beta_4); 62 | q = pmadd(x2, q, beta_2); 63 | q = pmadd(x2, q, beta_0); 64 | 65 | // Divide the numerator by the denominator. 66 | return pdiv(p, q); 67 | } 68 | 69 | } // end namespace internal 70 | 71 | } // end namespace Eigen 72 | 73 | #endif // EIGEN_MATHFUNCTIONSIMPL_H 74 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SELFCWISEBINARYOP_H 11 | #define EIGEN_SELFCWISEBINARYOP_H 12 | 13 | namespace Eigen { 14 | 15 | // TODO generalize the scalar type of 'other' 16 | 17 | template 18 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator*=(const Scalar& other) 19 | { 20 | typedef typename Derived::PlainObject PlainObject; 21 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op()); 22 | return derived(); 23 | } 24 | 25 | template 26 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const Scalar& other) 27 | { 28 | typedef typename Derived::PlainObject PlainObject; 29 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op()); 30 | return derived(); 31 | } 32 | 33 | template 34 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const Scalar& other) 35 | { 36 | typedef typename Derived::PlainObject PlainObject; 37 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op()); 38 | return derived(); 39 | } 40 | 41 | template 42 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator/=(const Scalar& other) 43 | { 44 | typedef typename Derived::PlainObject PlainObject; 45 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op()); 46 | return derived(); 47 | } 48 | 49 | } // end namespace Eigen 50 | 51 | #endif // EIGEN_SELFCWISEBINARYOP_H 52 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/arch/AVX/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_AVX_H 11 | #define EIGEN_TYPE_CASTING_AVX_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | // For now we use SSE to handle integers, so we can't use AVX instructions to cast 18 | // from int to float 19 | template <> 20 | struct type_casting_traits { 21 | enum { 22 | VectorizedCast = 0, 23 | SrcCoeffRatio = 1, 24 | TgtCoeffRatio = 1 25 | }; 26 | }; 27 | 28 | template <> 29 | struct type_casting_traits { 30 | enum { 31 | VectorizedCast = 0, 32 | SrcCoeffRatio = 1, 33 | TgtCoeffRatio = 1 34 | }; 35 | }; 36 | 37 | 38 | 39 | template<> EIGEN_STRONG_INLINE Packet8i pcast(const Packet8f& a) { 40 | return _mm256_cvtps_epi32(a); 41 | } 42 | 43 | template<> EIGEN_STRONG_INLINE Packet8f pcast(const Packet8i& a) { 44 | return _mm256_cvtepi32_ps(a); 45 | } 46 | 47 | } // end namespace internal 48 | 49 | } // end namespace Eigen 50 | 51 | #endif // EIGEN_TYPE_CASTING_AVX_H 52 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/arch/Default/Settings.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2010 Gael Guennebaud 5 | // Copyright (C) 2006-2008 Benoit Jacob 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | 12 | /* All the parameters defined in this file can be specialized in the 13 | * architecture specific files, and/or by the user. 14 | * More to come... */ 15 | 16 | #ifndef EIGEN_DEFAULT_SETTINGS_H 17 | #define EIGEN_DEFAULT_SETTINGS_H 18 | 19 | /** Defines the maximal loop size to enable meta unrolling of loops. 20 | * Note that the value here is expressed in Eigen's own notion of "number of FLOPS", 21 | * it does not correspond to the number of iterations or the number of instructions 22 | */ 23 | #ifndef EIGEN_UNROLLING_LIMIT 24 | #define EIGEN_UNROLLING_LIMIT 100 25 | #endif 26 | 27 | /** Defines the threshold between a "small" and a "large" matrix. 28 | * This threshold is mainly used to select the proper product implementation. 29 | */ 30 | #ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 31 | #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8 32 | #endif 33 | 34 | /** Defines the maximal width of the blocks used in the triangular product and solver 35 | * for vectors (level 2 blas xTRMV and xTRSV). The default is 8. 36 | */ 37 | #ifndef EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 38 | #define EIGEN_TUNE_TRIANGULAR_PANEL_WIDTH 8 39 | #endif 40 | 41 | 42 | /** Defines the default number of registers available for that architecture. 43 | * Currently it must be 8 or 16. Other values will fail. 44 | */ 45 | #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 46 | #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 8 47 | #endif 48 | 49 | #endif // EIGEN_DEFAULT_SETTINGS_H 50 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/arch/SSE/TypeCasting.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Benoit Steiner 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TYPE_CASTING_SSE_H 11 | #define EIGEN_TYPE_CASTING_SSE_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | template <> 18 | struct type_casting_traits { 19 | enum { 20 | VectorizedCast = 1, 21 | SrcCoeffRatio = 1, 22 | TgtCoeffRatio = 1 23 | }; 24 | }; 25 | 26 | template<> EIGEN_STRONG_INLINE Packet4i pcast(const Packet4f& a) { 27 | return _mm_cvttps_epi32(a); 28 | } 29 | 30 | 31 | template <> 32 | struct type_casting_traits { 33 | enum { 34 | VectorizedCast = 1, 35 | SrcCoeffRatio = 1, 36 | TgtCoeffRatio = 1 37 | }; 38 | }; 39 | 40 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet4i& a) { 41 | return _mm_cvtepi32_ps(a); 42 | } 43 | 44 | 45 | template <> 46 | struct type_casting_traits { 47 | enum { 48 | VectorizedCast = 1, 49 | SrcCoeffRatio = 2, 50 | TgtCoeffRatio = 1 51 | }; 52 | }; 53 | 54 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet2d& a, const Packet2d& b) { 55 | return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6)); 56 | } 57 | 58 | template <> 59 | struct type_casting_traits { 60 | enum { 61 | VectorizedCast = 1, 62 | SrcCoeffRatio = 1, 63 | TgtCoeffRatio = 2 64 | }; 65 | }; 66 | 67 | template<> EIGEN_STRONG_INLINE Packet2d pcast(const Packet4f& a) { 68 | // Simply discard the second half of the input 69 | return _mm_cvtps_pd(a); 70 | } 71 | 72 | 73 | } // end namespace internal 74 | 75 | } // end namespace Eigen 76 | 77 | #endif // EIGEN_TYPE_CASTING_SSE_H 78 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/functors/TernaryFunctors.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 Eugene Brevdo 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_TERNARY_FUNCTORS_H 11 | #define EIGEN_TERNARY_FUNCTORS_H 12 | 13 | namespace Eigen { 14 | 15 | namespace internal { 16 | 17 | //---------- associative ternary functors ---------- 18 | 19 | 20 | 21 | } // end namespace internal 22 | 23 | } // end namespace Eigen 24 | 25 | #endif // EIGEN_TERNARY_FUNCTORS_H 26 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/util/NonMPL2.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_MPL2_ONLY 2 | #error Including non-MPL2 code in EIGEN_MPL2_ONLY mode 3 | #endif 4 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h: -------------------------------------------------------------------------------- 1 | #ifdef EIGEN_WARNINGS_DISABLED 2 | #undef EIGEN_WARNINGS_DISABLED 3 | 4 | #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS 5 | #ifdef _MSC_VER 6 | #pragma warning( pop ) 7 | #elif defined __INTEL_COMPILER 8 | #pragma warning pop 9 | #elif defined __clang__ 10 | #pragma clang diagnostic pop 11 | #elif defined __GNUC__ && __GNUC__>=6 12 | #pragma GCC diagnostic pop 13 | #endif 14 | 15 | #if defined __NVCC__ 16 | // Don't reenable the diagnostic messages, as it turns out these messages need 17 | // to be disabled at the point of the template instantiation (i.e the user code) 18 | // otherwise they'll be triggered by nvcc. 19 | // #pragma diag_default code_is_unreachable 20 | // #pragma diag_default initialization_not_reachable 21 | // #pragma diag_default 2651 22 | // #pragma diag_default 2653 23 | #endif 24 | 25 | #endif 26 | 27 | #endif // EIGEN_WARNINGS_DISABLED 28 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_MAPPED_SPARSEMATRIX_H 11 | #define EIGEN_MAPPED_SPARSEMATRIX_H 12 | 13 | namespace Eigen { 14 | 15 | /** \deprecated Use Map > 16 | * \class MappedSparseMatrix 17 | * 18 | * \brief Sparse matrix 19 | * 20 | * \param _Scalar the scalar type, i.e. the type of the coefficients 21 | * 22 | * See http://www.netlib.org/linalg/html_templates/node91.html for details on the storage scheme. 23 | * 24 | */ 25 | namespace internal { 26 | template 27 | struct traits > : traits > 28 | {}; 29 | } // end namespace internal 30 | 31 | template 32 | class MappedSparseMatrix 33 | : public Map > 34 | { 35 | typedef Map > Base; 36 | 37 | public: 38 | 39 | typedef typename Base::StorageIndex StorageIndex; 40 | typedef typename Base::Scalar Scalar; 41 | 42 | inline MappedSparseMatrix(Index rows, Index cols, Index nnz, StorageIndex* outerIndexPtr, StorageIndex* innerIndexPtr, Scalar* valuePtr, StorageIndex* innerNonZeroPtr = 0) 43 | : Base(rows, cols, nnz, outerIndexPtr, innerIndexPtr, valuePtr, innerNonZeroPtr) 44 | {} 45 | 46 | /** Empty destructor */ 47 | inline ~MappedSparseMatrix() {} 48 | }; 49 | 50 | namespace internal { 51 | 52 | template 53 | struct evaluator > 54 | : evaluator > > 55 | { 56 | typedef MappedSparseMatrix<_Scalar,_Options,_StorageIndex> XprType; 57 | typedef evaluator > Base; 58 | 59 | evaluator() : Base() {} 60 | explicit evaluator(const XprType &mat) : Base(mat) {} 61 | }; 62 | 63 | } 64 | 65 | } // end namespace Eigen 66 | 67 | #endif // EIGEN_MAPPED_SPARSEMATRIX_H 68 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/SparseCore/SparseFuzzy.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSE_FUZZY_H 11 | #define EIGEN_SPARSE_FUZZY_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | template 17 | bool SparseMatrixBase::isApprox(const SparseMatrixBase& other, const RealScalar &prec) const 18 | { 19 | const typename internal::nested_eval::type actualA(derived()); 20 | typename internal::conditional::type, 22 | const PlainObject>::type actualB(other.derived()); 23 | 24 | return (actualA - actualB).squaredNorm() <= prec * prec * numext::mini(actualA.squaredNorm(), actualB.squaredNorm()); 25 | } 26 | 27 | } // end namespace Eigen 28 | 29 | #endif // EIGEN_SPARSE_FUZZY_H 30 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/SparseCore/SparseRedux.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2014 Gael Guennebaud 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | #ifndef EIGEN_SPARSEREDUX_H 11 | #define EIGEN_SPARSEREDUX_H 12 | 13 | namespace Eigen { 14 | 15 | template 16 | typename internal::traits::Scalar 17 | SparseMatrixBase::sum() const 18 | { 19 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 20 | Scalar res(0); 21 | internal::evaluator thisEval(derived()); 22 | for (Index j=0; j::InnerIterator iter(thisEval,j); iter; ++iter) 24 | res += iter.value(); 25 | return res; 26 | } 27 | 28 | template 29 | typename internal::traits >::Scalar 30 | SparseMatrix<_Scalar,_Options,_Index>::sum() const 31 | { 32 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 33 | if(this->isCompressed()) 34 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 35 | else 36 | return Base::sum(); 37 | } 38 | 39 | template 40 | typename internal::traits >::Scalar 41 | SparseVector<_Scalar,_Options,_Index>::sum() const 42 | { 43 | eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); 44 | return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); 45 | } 46 | 47 | } // end namespace Eigen 48 | 49 | #endif // EIGEN_SPARSEREDUX_H 50 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/SparseLU/SparseLU_Utils.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2012 Désiré Nuentsa-Wakam 5 | // 6 | // This Source Code Form is subject to the terms of the Mozilla 7 | // Public License v. 2.0. If a copy of the MPL was not distributed 8 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 | 10 | 11 | #ifndef EIGEN_SPARSELU_UTILS_H 12 | #define EIGEN_SPARSELU_UTILS_H 13 | 14 | namespace Eigen { 15 | namespace internal { 16 | 17 | /** 18 | * \brief Count Nonzero elements in the factors 19 | */ 20 | template 21 | void SparseLUImpl::countnz(const Index n, Index& nnzL, Index& nnzU, GlobalLU_t& glu) 22 | { 23 | nnzL = 0; 24 | nnzU = (glu.xusub)(n); 25 | Index nsuper = (glu.supno)(n); 26 | Index jlen; 27 | Index i, j, fsupc; 28 | if (n <= 0 ) return; 29 | // For each supernode 30 | for (i = 0; i <= nsuper; i++) 31 | { 32 | fsupc = glu.xsup(i); 33 | jlen = glu.xlsub(fsupc+1) - glu.xlsub(fsupc); 34 | 35 | for (j = fsupc; j < glu.xsup(i+1); j++) 36 | { 37 | nnzL += jlen; 38 | nnzU += j - fsupc + 1; 39 | jlen--; 40 | } 41 | } 42 | } 43 | 44 | /** 45 | * \brief Fix up the data storage lsub for L-subscripts. 46 | * 47 | * It removes the subscripts sets for structural pruning, 48 | * and applies permutation to the remaining subscripts 49 | * 50 | */ 51 | template 52 | void SparseLUImpl::fixupL(const Index n, const IndexVector& perm_r, GlobalLU_t& glu) 53 | { 54 | Index fsupc, i, j, k, jstart; 55 | 56 | StorageIndex nextl = 0; 57 | Index nsuper = (glu.supno)(n); 58 | 59 | // For each supernode 60 | for (i = 0; i <= nsuper; i++) 61 | { 62 | fsupc = glu.xsup(i); 63 | jstart = glu.xlsub(fsupc); 64 | glu.xlsub(fsupc) = nextl; 65 | for (j = jstart; j < glu.xlsub(fsupc + 1); j++) 66 | { 67 | glu.lsub(nextl) = perm_r(glu.lsub(j)); // Now indexed into P*A 68 | nextl++; 69 | } 70 | for (k = fsupc+1; k < glu.xsup(i+1); k++) 71 | glu.xlsub(k) = nextl; // other columns in supernode i 72 | } 73 | 74 | glu.xlsub(n) = nextl; 75 | } 76 | 77 | } // end namespace internal 78 | 79 | } // end namespace Eigen 80 | #endif // EIGEN_SPARSELU_UTILS_H 81 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/misc/RealSvd2x2.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009-2010 Benoit Jacob 5 | // Copyright (C) 2013-2016 Gael Guennebaud 6 | // 7 | // This Source Code Form is subject to the terms of the Mozilla 8 | // Public License v. 2.0. If a copy of the MPL was not distributed 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 | 11 | #ifndef EIGEN_REALSVD2X2_H 12 | #define EIGEN_REALSVD2X2_H 13 | 14 | namespace Eigen { 15 | 16 | namespace internal { 17 | 18 | template 19 | void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, 20 | JacobiRotation *j_left, 21 | JacobiRotation *j_right) 22 | { 23 | using std::sqrt; 24 | using std::abs; 25 | Matrix m; 26 | m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)), 27 | numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q)); 28 | JacobiRotation rot1; 29 | RealScalar t = m.coeff(0,0) + m.coeff(1,1); 30 | RealScalar d = m.coeff(1,0) - m.coeff(0,1); 31 | 32 | if(abs(d) < (std::numeric_limits::min)()) 33 | { 34 | rot1.s() = RealScalar(0); 35 | rot1.c() = RealScalar(1); 36 | } 37 | else 38 | { 39 | // If d!=0, then t/d cannot overflow because the magnitude of the 40 | // entries forming d are not too small compared to the ones forming t. 41 | RealScalar u = t / d; 42 | RealScalar tmp = sqrt(RealScalar(1) + numext::abs2(u)); 43 | rot1.s() = RealScalar(1) / tmp; 44 | rot1.c() = u / tmp; 45 | } 46 | m.applyOnTheLeft(0,1,rot1); 47 | j_right->makeJacobi(m,0,1); 48 | *j_left = rot1 * j_right->transpose(); 49 | } 50 | 51 | } // end namespace internal 52 | 53 | } // end namespace Eigen 54 | 55 | #endif // EIGEN_REALSVD2X2_H 56 | -------------------------------------------------------------------------------- /lib/Eigen/Eigen/src/misc/lapacke_mangling.h: -------------------------------------------------------------------------------- 1 | #ifndef LAPACK_HEADER_INCLUDED 2 | #define LAPACK_HEADER_INCLUDED 3 | 4 | #ifndef LAPACK_GLOBAL 5 | #if defined(LAPACK_GLOBAL_PATTERN_LC) || defined(ADD_) 6 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 7 | #elif defined(LAPACK_GLOBAL_PATTERN_UC) || defined(UPPER) 8 | #define LAPACK_GLOBAL(lcname,UCNAME) UCNAME 9 | #elif defined(LAPACK_GLOBAL_PATTERN_MC) || defined(NOCHANGE) 10 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname 11 | #else 12 | #define LAPACK_GLOBAL(lcname,UCNAME) lcname##_ 13 | #endif 14 | #endif 15 | 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /lib/Eigen/INSTALL: -------------------------------------------------------------------------------- 1 | Installation instructions for Eigen 2 | *********************************** 3 | 4 | Explanation before starting 5 | *************************** 6 | 7 | Eigen consists only of header files, hence there is nothing to compile 8 | before you can use it. Moreover, these header files do not depend on your 9 | platform, they are the same for everybody. 10 | 11 | Method 1. Installing without using CMake 12 | **************************************** 13 | 14 | You can use right away the headers in the Eigen/ subdirectory. In order 15 | to install, just copy this Eigen/ subdirectory to your favorite location. 16 | If you also want the unsupported features, copy the unsupported/ 17 | subdirectory too. 18 | 19 | Method 2. Installing using CMake 20 | ******************************** 21 | 22 | Let's call this directory 'source_dir' (where this INSTALL file is). 23 | Before starting, create another directory which we will call 'build_dir'. 24 | 25 | Do: 26 | 27 | cd build_dir 28 | cmake source_dir 29 | make install 30 | 31 | The "make install" step may require administrator privileges. 32 | 33 | You can adjust the installation destination (the "prefix") 34 | by passing the -DCMAKE_INSTALL_PREFIX=myprefix option to cmake, as is 35 | explained in the message that cmake prints at the end. 36 | -------------------------------------------------------------------------------- /lib/Eigen/README.md: -------------------------------------------------------------------------------- 1 | **Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.** 2 | 3 | For more information go to http://eigen.tuxfamily.org/. 4 | -------------------------------------------------------------------------------- /lib/Eigen/eigen3.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=${prefix} 3 | 4 | Name: Eigen3 5 | Description: A C++ template library for linear algebra: vectors, matrices, and related algorithms 6 | Requires: 7 | Version: @EIGEN_VERSION_NUMBER@ 8 | Libs: 9 | Cflags: -I${prefix}/@INCLUDE_INSTALL_DIR@ 10 | -------------------------------------------------------------------------------- /lib/Eigen/signature_of_eigen3_matrix_library: -------------------------------------------------------------------------------- 1 | This file is just there as a signature to help identify directories containing Eigen3. When writing a script looking for Eigen3, just look for this file. This is especially useful to help disambiguate with Eigen2... 2 | -------------------------------------------------------------------------------- /msg/X2Acceleration.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string[] name 4 | float64[] estimatedAcceleration 5 | float64[] desiredAcceleration 6 | float64[] commandedAcceleration 7 | float64[] errorAcceleration 8 | float64[] sumErrorAcceleration 9 | -------------------------------------------------------------------------------- /msg/X2AccelerationMerge.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string[] name 4 | float64[] jointAccByDerivative 5 | float64[] filteredJointAccByDerivative 6 | float64[] jointAccByIMU 7 | float64[] filteredJointAccByImu 8 | float64[] filteredBias 9 | float64[] mergedJointAcc 10 | -------------------------------------------------------------------------------- /msg/X2Array.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string[] name 4 | float32[] data -------------------------------------------------------------------------------- /script/createStateMachine.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script intended to quickly create a new CORC state machine from a simple template. 3 | # It creates a state machine in a separate folder and create the minimal cpp and headers files 4 | # for state machine and states derived classes. 5 | # It does not edit the CMakeFileList which should be edited manually to compile the new state machine. 6 | # 7 | # vcrocher - Unimelb - 16/04/2025 8 | 9 | read -p "Enter the new state machine name in CamelCase (e.g. EMUDemo): " name 10 | read -p "Enter the name of platform folder to use (folder name, e.g. M2, M3...): " platform_folder 11 | read -p "Enter the name of platform class to use (folder name, e.g. RobotM3...): " platform_name 12 | 13 | state_machine_name="${name}Machine" 14 | states_name="${name}States" 15 | 16 | echo "Creating ${state_machine_name}.cpp/h with states in ${states_name}.cpp/h and platform ${platform_name} (${platform_folder})". 17 | 18 | 19 | ## Create relevant folder 20 | echo "Creating folder ${state_machine_name} ..." 21 | parent_path=$( cd "$(dirname "${BASH_SOURCE[0]}")" ; pwd -P ) 22 | cd "$parent_path" 23 | cd ../../ 24 | cp -r CANOpenRobotController/src/apps/StateMachineTemplate "./${state_machine_name}" 25 | cd "${state_machine_name}" 26 | 27 | 28 | 29 | #Rename files 30 | echo "Renaming files..." 31 | mv StateMachineTemplate.h "${state_machine_name}.h" #old school because rename too not convenient and not necessarily installed 32 | mv StateMachineTemplate.cpp "${state_machine_name}.cpp" 33 | mv StatesTemplate.h "${states_name}.h" 34 | mv StatesTemplate.cpp "${states_name}.cpp" 35 | 36 | 37 | #Templates replacement in files 38 | echo "Editing files..." 39 | sed -i "s/StateMachineTemplate/${state_machine_name}/g" * 40 | sed -i "s/StatesTemplate/${states_name}/g" * 41 | sed -i "s/PlatformName/${platform_name}/g" * 42 | sed -i "s/PLATFORM_FOLDER/${platform_folder}/g" app.cmake 43 | sed -i "s/STATEMACHINETEMPLATE/${state_machine_name^^}/g" * 44 | sed -i "s/STATESTEMPLATE/${name^^}/g" * 45 | 46 | echo "$(pwd):" 47 | echo $(ls) 48 | echo "Done" 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /script/initCAN0.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #initialisation of CAN interface on BBAI 4 | #assumes proper configuration on CAN0 (see https://stackoverflow.com/questions/62207737/beaglebone-ai-how-to-setup-can-bus) 5 | 6 | echo "Enabling CAN0" 7 | sudo ip link set can0 up type can bitrate 1000000 8 | sudo ifconfig can0 up 9 | #sudo ifconfig can0 txqueuelen 1000 10 | 11 | #echo "can0 up. Dumping (ctrl+c to close):" 12 | #candump -c -t z can0,080~111111 #Filter out 080 sync messages -------------------------------------------------------------------------------- /script/initCAN0CAN1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #initialisation of CAN interfaces on BB 4 | 5 | echo "Enabling CAN0" 6 | sudo ip link set can0 up type can bitrate 1000000 7 | sudo ifconfig can0 up 8 | 9 | echo "Enabling CAN1" 10 | sudo ip link set can1 up type can bitrate 1000000 11 | sudo ifconfig can1 up 12 | 13 | #echo "can0 up. Dumping (ctrl+c to close):" 14 | #candump -c -t z can0,080~111111 #Filter out 080 sync messages 15 | -------------------------------------------------------------------------------- /script/initCAN1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "Configure CAN1 pins" 4 | sudo config-pin p9.24 can 5 | sudo config-pin p9.26 can 6 | echo "Enabling CAN1" 7 | sudo ip link set can1 up type can bitrate 1000000 8 | sudo ifconfig can1 up 9 | #sudo ifconfig can1 txqueuelen 1000 10 | 11 | #echo "can0 up. Dumping (ctrl+c to close):" 12 | #candump -c -t z can1,080~111111 #Filter out 080 sync messages -------------------------------------------------------------------------------- /script/initVCAN.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "Enabling VCAN on vcan0" 4 | sudo modprobe vcan 5 | sudo ip link add dev vcan0 type vcan 6 | sudo ip link set up vcan0 7 | #echo "starting candump of vcan0" 8 | #candump vcan0 -------------------------------------------------------------------------------- /script/rqt.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # There is a problem with RQT. Can't run it as a ros node. THerefore it is run as a bash code. 4 | 5 | rqt --force-discover -------------------------------------------------------------------------------- /script/uploadBB.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #Upload app and init scripts to BB target 4 | #using default IP / users parameters 5 | 6 | SSH_USER="debian" 7 | BUILD_FOLDER="build/" 8 | INIT_SCRIPTS="*.sh" 9 | SCRIPT_FOLDER="script/" 10 | CONFIG_FOLDER="config/" 11 | 12 | #use default BB address (dependent on OS used) 13 | if [[ "$OSTYPE" == "cygwin" ]] || [[ "$OSTYPE" == "msys" ]] || [[ "$OSTYPE" == "win32" ]]; then 14 | #Any flavour of Windows (Cygwin, Mingw...) 15 | SSH_IP_ADDR="192.168.7.2" 16 | else 17 | #Any other case (Linux, Unix, OSX...) 18 | SSH_IP_ADDR="192.168.7.2" 19 | fi 20 | 21 | #Check if BB is connected 22 | ping -W 1 -c 1 $SSH_IP_ADDR >/dev/null 2>&1 23 | if [ $? -ne 0 ] ; then 24 | echo "Nothing connected on ${SSH_IP_ADDR}. Exiting." 25 | exit 26 | fi 27 | 28 | #set in root folder 29 | parent_path=$( cd "$(dirname "${BASH_SOURCE[0]}")" ; pwd -P ) 30 | cd "$parent_path"/.. 31 | 32 | 33 | echo "## Create remote folder (~/CANOpenRobotController/):" 34 | echo -n "ssh -q $SSH_USER@$SSH_IP_ADDR "mkdir -p ~/CANOpenRobotController/" ... " 35 | ssh -q $SSH_USER@$SSH_IP_ADDR "mkdir -p ~/CANOpenRobotController/" 36 | echo "done." 37 | 38 | echo "" 39 | echo "## Copy scripts:" 40 | echo -n "rsync -chaz -e 'ssh -q' ${SCRIPT_FOLDER} $SSH_USER@$SSH_IP_ADDR:~/CANOpenRobotController/${SCRIPT_FOLDER} ... " 41 | rsync -chaz -e 'ssh -q' ${SCRIPT_FOLDER} $SSH_USER@$SSH_IP_ADDR:~/CANOpenRobotController/${SCRIPT_FOLDER} 42 | echo "done." 43 | 44 | echo "" 45 | echo "## Copy config files:" 46 | echo -n "rsync -chaz -e 'ssh -q' ${CONFIG_FOLDER} $SSH_USER@$SSH_IP_ADDR:~/CANOpenRobotController/${CONFIG_FOLDER} ... " 47 | rsync -chaz -e 'ssh -q' ${CONFIG_FOLDER} $SSH_USER@$SSH_IP_ADDR:~/CANOpenRobotController/${CONFIG_FOLDER} 48 | echo "done." 49 | 50 | echo "" 51 | echo "## Copy APP(s):" 52 | echo -n "rsync -chaz -e 'ssh -q' --include='*APP' --include='*APP_NOROBOT' --exclude='*' ${BUILD_FOLDER} $SSH_USER@$SSH_IP_ADDR:~/CANOpenRobotController/${BUILD_FOLDER} ... " 53 | rsync -chaz -e 'ssh -q' --include='*APP' --include='*APP_NOROBOT' --exclude='*' ${BUILD_FOLDER} $SSH_USER@$SSH_IP_ADDR:~/CANOpenRobotController/${BUILD_FOLDER} 54 | echo "done." 55 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/TrajectoryGenerator/DummyTrajectoryGenerator.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file DummyTrajectoryGenerator.cpp 3 | * @author Justin Fong 4 | * @brief 5 | * @version 0.1 6 | * @date 2020-05-04 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #include "DummyTrajectoryGenerator.h" 13 | 14 | 15 | double sitting[6] = {deg2rad(95), -deg2rad(90), deg2rad(95), -deg2rad(90), 0, 0}; 16 | double standing[6] = {0, 0, 0, 0, 0, 0}; 17 | Eigen::VectorXd startPos(6); 18 | 19 | DummyTrajectoryGenerator::DummyTrajectoryGenerator(int NumOfJoints) { 20 | double sitPos[6] = {deg2rad(95), -deg2rad(90), deg2rad(95), -deg2rad(90), 0, 0}; 21 | double standPos[6] = {0, 0, 0, 0, 0, 0}; 22 | 23 | numJoints = NumOfJoints; 24 | for (int i = 0; i 56 | */ 57 | 58 | Eigen::VectorXd DummyTrajectoryGenerator::getSetPoint(double time) { 59 | double progress = time / trajTime; 60 | Eigen::VectorXd angles(numJoints); 61 | 62 | if (currTraj == SIT) { 63 | for (int i = 0; i < numJoints; i++) { 64 | 65 | if (progress > 1) { 66 | angles(i) = sitting[i]; 67 | } else { 68 | angles(i) = startPos[i] + progress * (sitting[i] - startPos[i]); 69 | } 70 | } 71 | } else { 72 | for (int i = 0; i < numJoints; i++) { 73 | if (progress > 1) { 74 | angles(i) = standing[i]; 75 | } else { 76 | angles(i) = startPos[i] + progress * (standing[i] - startPos[i]); 77 | } 78 | } 79 | } 80 | lastProgress = progress; 81 | 82 | return angles; 83 | } 84 | 85 | bool DummyTrajectoryGenerator::isTrajectoryFinished() { 86 | if (lastProgress > 1.0) { 87 | return true; 88 | } else { 89 | return false; 90 | } 91 | } 92 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/TrajectoryGenerator/DummyTrajectoryGenerator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file DummyTrajectoryGenerator.h 3 | * \author Justin Fong 4 | * \brief A trajectory generator to be used for testing purposes 5 | * \version 0.1 6 | * \date 2020-05-04 7 | * 8 | * \copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #ifndef DUMMYTRAJECTORYGENERATOR_H_INCLUDED 13 | #define DUMMYTRAJECTORYGENERATOR_H_INCLUDED 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "LogHelper.h" 22 | 23 | 24 | #define deg2rad(deg) ((deg)*M_PI / 180.0) 25 | #define rad2deg(rad) ((rad)*180.0 / M_PI) 26 | 27 | /** 28 | * \brief Enum containing possible trajectory types for DummyTrajectoryGenerator 29 | * 30 | */ 31 | enum Trajectory { 32 | SIT = 0, 33 | STAND = 1, 34 | }; 35 | 36 | /** 37 | * \brief Example Implementation of TrajectoryGenerator. Includes only two trajectories (Sit-to-Stand and Stand-to-sit) for an Exoskeleton 38 | * 39 | */ 40 | class DummyTrajectoryGenerator { 41 | private: 42 | std::vector endPoints; 43 | Trajectory currTraj = SIT; 44 | double trajTime = 2; 45 | int numJoints = 4; 46 | double lastProgress = 0; 47 | 48 | /** Parameters associated with Trajectory Progression */ 49 | double currTrajProgress = 0; 50 | timespec prevTime; 51 | 52 | public: 53 | DummyTrajectoryGenerator(int NumOfJoints); 54 | /** 55 | * \brief Implementation of the initialiseTrajectory method in TrajectoryGenerator 56 | * 57 | * \return true 58 | * \return false 59 | */ 60 | bool initialiseTrajectory(); 61 | 62 | /** 63 | * \brief Initialise the Trajectory Generator object variables with a traj type (enum defined type) 64 | * and length of time the trajectory will take. 65 | * 66 | */ 67 | bool initialiseTrajectory(Trajectory traj, double time, Eigen::VectorXd &startPos_); 68 | 69 | /** 70 | * \brief Implementation of the getSetPoint method in TrajectoryGenerator 71 | * \param time The time corresponding to the point. 72 | * 73 | * \return vector 74 | */ 75 | Eigen::VectorXd getSetPoint(double time); 76 | 77 | /** 78 | * \brief Check if the trajectory has been completed based on last elapsed time 79 | * 80 | * \return true if trajectory has been completed 81 | * \return false if trajectory has not been completed 82 | */ 83 | bool isTrajectoryFinished(); 84 | }; 85 | #endif 86 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM X2) 6 | 7 | ## Conditional use of Fourier AIOS actuators library 8 | ##TODO: Not functional: needed for some internal source files now: to condition 9 | set(WITH_FOURIER_AIOS TRUE) 10 | 11 | ################################################################################ 12 | 13 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 14 | 15 | ## StateMachine name is (and should be!) current folder name 16 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 17 | ## And its relative path to the root folder is: 18 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 19 | 20 | ################################################################################ 21 | 22 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/ExoTestMachine.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file ExoTestMachine.h 3 | * \author William Campbell 4 | * \version 0.1 5 | * \date 2019-09-24 6 | * \copyright Copyright (c) 2020 7 | * 8 | * /brief The ExoTestMachine class represents an example implementation of an exoskeleton state machine 9 | * with five states. Initialisation, sitting, standing, standing up and sitting down. The test machine 10 | * is made as example for developers to structure their specific use cases with. 11 | * For more detail on the architecture and mechanics of the state machine class see:https://embeded.readthedocs.io/en/latest/StaeMachines/. 12 | * 13 | * State transition Diagram. 14 | * 15 | * startExo startStand 16 | * initState +-----> sitting +---------> standingUp 17 | * ^ + 18 | * EndTraj | | EndTraj 19 | * | | 20 | * + | 21 | * sittingDwn <---------+ standing 22 | * startSit 23 | * 24 | * Version 0.1 25 | * Date: 07/04/2020 26 | * 27 | */ 28 | #ifndef EXO_SM_H 29 | #define EXO_SM_H 30 | 31 | #include "ExoTestState.h" 32 | #include "StateMachine.h" 33 | #include "X2Robot.h" 34 | 35 | // State Classes 36 | #include "InitState.h" 37 | #include "Sitting.h" 38 | #include "SittingDwn.h" 39 | #include "Standing.h" 40 | #include "StandingUp.h" 41 | 42 | /** 43 | * @brief Example implementation of a StateMachine for the ExoRobot class. States should implemented ExoTestState 44 | * 45 | */ 46 | class ExoTestMachine : public StateMachine { 47 | public: 48 | //bool running = false; 49 | /** 50 | * \todo Pilot Parameters would be set in constructor here 51 | * 52 | */ 53 | ExoTestMachine(); 54 | void init(); 55 | void end(); 56 | 57 | State *gettCurState(); 58 | 59 | bool trajComplete; 60 | DummyTrajectoryGenerator *trajectoryGenerator; 61 | 62 | X2Robot *robot() { return static_cast(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 63 | }; 64 | 65 | #endif /*EXO_SM_H*/ 66 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/ExoTestState.cpp: -------------------------------------------------------------------------------- 1 | #include "ExoTestState.h" 2 | 3 | ExoTestState::ExoTestState(X2Robot *exo, DummyTrajectoryGenerator *tg, const char *name) : State(name), robot(exo), trajectoryGenerator(tg){}; -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/ExoTestState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file ExoTestState.h 3 | * /author Justin Fong 4 | * /brief Virtual Class to include all required classes for ExoTestStates 5 | * /version 0.2 6 | * /date 2020-11-3 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #ifndef EXOTESTSTATE_H_DEF 13 | #define EXOTESTSTATE_H_DEF 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include "DummyTrajectoryGenerator.h" 20 | #include "State.h" 21 | #include "X2Robot.h" 22 | 23 | /** 24 | * \brief Example Implementation of State Class. Used with ExoTestMachine 25 | * 26 | * Note: This is used to ensure that all states here have an ExoRobot, and a DummyTrajectoryGenerator (as opposed to more generic Robot and TrajectoryGenerator) 27 | * 28 | */ 29 | class ExoTestState : public State { 30 | protected: 31 | /** 32 | * \todo Might be good to make these Const 33 | * 34 | */ 35 | X2Robot *robot; /*resetErrors(); 14 | } 15 | void InitState::during(void) { 16 | } 17 | void InitState::exit(void) { 18 | robot->initPositionControl(); 19 | robot->setPosControlContinuousProfile(true); 20 | spdlog::info("InitState Exited"); 21 | } 22 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/InitState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file InitState.h 3 | * /author Justin Fong 4 | * /brief Concrete implementation of ExoTestState 5 | * /version 0.2 6 | * /date 2020-11-3 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | #ifndef INITSTATE_H_INCLUDED 12 | #define INITSTATE_H_INCLUDED 13 | 14 | #include "ExoTestState.h" 15 | 16 | /** 17 | * \brief Initialisation State for the ExoTestMachine (implementing ExoTestState) 18 | * 19 | * State holds until event triggers its exit, and runs initPositionControl on exit 20 | * Control of transition is independent of this class and is defined in ExoTestMachine. 21 | * 22 | */ 23 | class InitState : public ExoTestState { 24 | public: 25 | void entry(void); 26 | void during(void); 27 | void exit(void); 28 | InitState(X2Robot *exo, DummyTrajectoryGenerator *tg, const char *name = "ExoTest Init State") : ExoTestState(exo, tg, name){}; 29 | }; 30 | 31 | #endif -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/Sitting.cpp: -------------------------------------------------------------------------------- 1 | ////////// STATE //////////////////// 2 | //------- Sitting ------------///// 3 | //////////////////////////////////// 4 | #include "Sitting.h" 5 | void Sitting::entry() { 6 | spdlog::info("Sitting State Entered"); 7 | //READ TIME OF MAIN 8 | std::cout << "=======================" << std::endl 9 | << " HIT W to begin standing up" << std::endl 10 | << "=======================" << std::endl 11 | << std::endl; 12 | } 13 | void Sitting::during() { 14 | } 15 | void Sitting::exit() { 16 | spdlog::info("Sitting State Exited"); 17 | } -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/Sitting.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file Sitting.h 3 | * /author Justin Fong 4 | * /version 0.2 5 | * /date 2020-11-3 6 | * 7 | * @copyright Copyright (c) 2020 8 | * 9 | */ 10 | 11 | #ifndef SITTING_H_INCLUDED 12 | #define SITTING_H_INCLUDED 13 | 14 | #include 15 | 16 | #include "ExoTestState.h" 17 | 18 | /** 19 | * @brief State for the ExoTestMachine (implementing ExoTestState) - representing when the exoskeleton is sitting down (stationary) 20 | * 21 | * State machines enters this state when the sitting down trajectory is finished, and waits here for input 22 | */ 23 | class Sitting : public ExoTestState { 24 | public: 25 | void entry(void); 26 | void during(void); 27 | void exit(void); 28 | Sitting(X2Robot *exo, DummyTrajectoryGenerator *tg, const char *name = "ExoTest Sitting State") : ExoTestState(exo, tg, name){}; 29 | }; 30 | 31 | #endif -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/SittingDwn.cpp: -------------------------------------------------------------------------------- 1 | ////////// STATE //////////////////// 2 | //------- Sitting Down ------------///// 3 | //////////////////////////////////// 4 | #include "SittingDwn.h" 5 | void SittingDwn::entry(void) { 6 | spdlog::info("Sitting Down State Entered "); 7 | std::cout << "===================" << std::endl 8 | << " GREEN -> SIT DOWN " << std::endl 9 | << "===================" << std::endl; 10 | trajectoryGenerator->initialiseTrajectory(SIT, 2, robot->getPosition()); 11 | currTrajProgress = 0; 12 | clock_gettime(CLOCK_MONOTONIC, &prevTime); 13 | } 14 | void SittingDwn::during(void) { 15 | timespec currTime; 16 | clock_gettime(CLOCK_MONOTONIC, &currTime); 17 | 18 | double elapsedSec = currTime.tv_sec - prevTime.tv_sec + (currTime.tv_nsec - prevTime.tv_nsec) / 1e9; 19 | prevTime = currTime; 20 | 21 | //if (robot->keyboard->getA() ) { 22 | currTrajProgress += elapsedSec; 23 | robot->setPosition(trajectoryGenerator->getSetPoint(currTrajProgress)); 24 | //} 25 | } 26 | void SittingDwn::exit(void) { 27 | spdlog::info("Sitting Down State Exited "); 28 | } 29 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/SittingDwn.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file SittingDwn.h 3 | * \author Justin Fong 4 | * /version 0.2 5 | * /date 2020-11-3 6 | * 7 | * \copyright Copyright (c) 2020 8 | * 9 | */ 10 | 11 | #ifndef SITTINGDWN_H_INCLUDED 12 | #define SITTINGDWN_H_INCLUDED 13 | #include "ExoTestState.h" 14 | 15 | /** 16 | * \brief State for the ExoTestMachine (implementing ExoTestState) - representing when the exo is sitting down (moving) 17 | * 18 | * Starts the Sitting Down trajectory on entry, executes in during, and exits when trajectory is complete 19 | */ 20 | class SittingDwn : public ExoTestState { 21 | private: 22 | /** 23 | * Parameters associated with progression through a trajectory 24 | */ 25 | double currTrajProgress = 0; 26 | timespec prevTime; 27 | 28 | public: 29 | /** 30 | * \brief Prepare Robot and Trajectory Generator objects to tigger a sit motion 31 | * loads SIT paramaters into the Trajectory Generator object and runs robot startNewTrajectory function. 32 | * 33 | */ 34 | void 35 | entry(void); 36 | /** 37 | * \brief run the robot objectsmoveThroughtrajectory function using the loaded trajectory 38 | * dictated by the state machines Trajectory Generator object. 39 | * 40 | */ 41 | void during(void); 42 | void exit(void); 43 | SittingDwn(X2Robot *exo, DummyTrajectoryGenerator *tg, const char *name = "ExoTest Sitting Down State") : ExoTestState(exo, tg, name){}; 44 | }; 45 | 46 | #endif -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/Standing.cpp: -------------------------------------------------------------------------------- 1 | ////////// STATE //////////////////// 2 | //------- Standing ------------///// 3 | //////////////////////////////////// 4 | #include "Standing.h" 5 | 6 | void Standing::entry(void) { 7 | spdlog::info("Standing State Entered"); 8 | std::cout 9 | << "======================" << std::endl 10 | << " HIT W -> Sit DOWN" << std::endl 11 | << "======================" << std::endl; 12 | } 13 | 14 | void Standing::during(void) { 15 | } 16 | 17 | void Standing::exit(void) { 18 | spdlog::info("Standing State Exited"); 19 | } 20 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/Standing.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file Standing.h 3 | * /author Justin Fong 4 | * /version 0.2 5 | * /date 2020-11-3 6 | * 7 | * @copyright Copyright (c) 2020 8 | * 9 | */ 10 | 11 | #ifndef STANDING_H_INCLUDED 12 | #define STANDING_H_INCLUDED 13 | 14 | #include "ExoTestState.h" 15 | 16 | /** 17 | * @brief State for the ExoTestMachine (implementing ExoTestState) - representing when the exoskeleton is standing up (stationary) 18 | * 19 | * State machines enters this state when the standing up trajectory is finished, and waits here for input 20 | */ 21 | class Standing : public ExoTestState { 22 | public: 23 | void entry(void); 24 | void during(void); 25 | void exit(void); 26 | Standing(X2Robot *exo, DummyTrajectoryGenerator *tg, const char *name = "ExoTest Standing State") : ExoTestState(exo, tg, name){}; 27 | }; 28 | 29 | #endif -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/StandingUp.cpp: -------------------------------------------------------------------------------- 1 | #include "StandingUp.h" 2 | 3 | // Negative bending control machine 4 | void StandingUp::entry(void) { 5 | spdlog::info("Standing Up State Entered"); 6 | std::cout << "===================" << std::endl 7 | << " GREEN -> STAND UP" << std::endl 8 | << "===================" << std::endl; 9 | trajectoryGenerator->initialiseTrajectory(STAND, 2, robot->getPosition()); 10 | currTrajProgress = 0; 11 | clock_gettime(CLOCK_MONOTONIC, &prevTime); 12 | } 13 | 14 | void StandingUp::during(void) { 15 | timespec currTime; 16 | clock_gettime(CLOCK_MONOTONIC, &currTime); 17 | 18 | double elapsedSec = currTime.tv_sec - prevTime.tv_sec + (currTime.tv_nsec - prevTime.tv_nsec) / 1e9; 19 | prevTime = currTime; 20 | 21 | /** 22 | * /todo - Check if the GO button on the robot is pressed 23 | * 24 | */ 25 | //if (robot->keyboard->getA() ) { 26 | currTrajProgress += elapsedSec; 27 | robot->setPosition(trajectoryGenerator->getSetPoint(currTrajProgress)); 28 | //} 29 | } 30 | void StandingUp::exit(void) { 31 | spdlog::info("Standing Up State Exited"); 32 | } 33 | -------------------------------------------------------------------------------- /src/apps/ExoTestMachine/stateMachine/states/StandingUp.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file StandingUp.h 3 | * /author Justin Fong 4 | * /version 0.2 5 | * /date 2020-11-3 6 | * 7 | * @copyright Copyright (c) 2020 8 | * 9 | */ 10 | 11 | #ifndef STANDINGUP_H_INCLUDED 12 | #define STANDINGUP_H_INCLUDED 13 | 14 | #include "ExoTestState.h" 15 | 16 | /** 17 | * @brief State for the ExoTestMachine (implementing ExoTestState) - representing when the exo is standing up (moving) 18 | * 19 | * Starts the Standing Up trajectory on entry, executes in during, and exits when trajectory is complete 20 | */ 21 | class StandingUp : public ExoTestState { 22 | private: 23 | /** 24 | * Parameters associated with progression through a trajectory 25 | */ 26 | double currTrajProgress = 0; 27 | timespec prevTime; 28 | 29 | public: 30 | /** 31 | * \brief Prepare Robot and Trajectory Generator objects to tigger a stand motion 32 | * loads STAND paramaters into the Trajectory Generator object and runs robot startNewTrajectory function. 33 | * 34 | */ 35 | void entry(void); 36 | /** 37 | * \brief run the robot objects moveThroughtrajecoty function using the loaded trajectory 38 | * dictated by the state machines Trajectory Generator object. 39 | * 40 | */ 41 | void during(void); 42 | void exit(void); 43 | StandingUp(X2Robot *exo, DummyTrajectoryGenerator *tg, const char *name = "ExoTest Standing Up State") : ExoTestState(exo, tg, name){}; 44 | }; 45 | 46 | #endif -------------------------------------------------------------------------------- /src/apps/LoggingDevice/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM LoggingRobot) 6 | 7 | ################################################################################ 8 | 9 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 10 | 11 | ## StateMachine name is (and should be!) current folder name 12 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 13 | ## And its relative path to the root folder is: 14 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 15 | 16 | ################################################################################ 17 | 18 | -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/LoggingDevice.cpp: -------------------------------------------------------------------------------- 1 | #include "LoggingDevice.h" 2 | 3 | 4 | 5 | //////////////////////////////////////////////////////////////// 6 | // Transitions-------------------------------------------------- 7 | /////////////////////////////////////////////////////////////// 8 | bool isAPressed(StateMachine & sm) { 9 | LoggingDevice & SM = static_cast(sm); 10 | spdlog::trace("IsAPressed"); 11 | if (SM.robot()->keyboard->getA() == true) { 12 | 13 | return true; 14 | } 15 | return false; 16 | } 17 | bool isSPressed(StateMachine & sm) { 18 | LoggingDevice & SM = static_cast(sm); 19 | if (SM.robot()->keyboard->getS() == true) { 20 | return true; 21 | } 22 | return false; 23 | } 24 | 25 | bool isCalibrationFinished(StateMachine & sm) { 26 | LoggingDevice & SM = static_cast(sm); 27 | if (SM.state("calibrateState")->getCurrReading() < NUM_CALIBRATE_READINGS) { 28 | return false; 29 | } 30 | return true; 31 | } 32 | 33 | 34 | LoggingDevice::LoggingDevice() { 35 | setRobot(std::make_unique()); 36 | 37 | // States 38 | addState("initState", std::make_shared(robot())); 39 | addState("idleState", std::make_shared(robot())); 40 | addState("calibrateState", std::make_shared(robot())); 41 | addState("recordState", std::make_shared(robot())); 42 | 43 | // Transitions 44 | addTransition("initState", isAPressed, "idleState"); 45 | addTransition("idleState", isAPressed, "calibrateState"); 46 | 47 | addTransition("calibrateState", isCalibrationFinished, "idleState"); 48 | addTransition("idleState", isSPressed, "recordState"); 49 | addTransition("recordState", isSPressed, "idleState"); 50 | 51 | //Initialize the state machine with first state of the designed state machine, using baseclass function. 52 | setInitState("initState"); 53 | } 54 | 55 | void LoggingDevice::init() { 56 | spdlog::info("LoggingDevice::init()"); 57 | robot()->initialise(); 58 | 59 | // Initialising the data logger 60 | logHelper.initLogger("test_logger", "logs/testLog.csv", LogFormat::CSV, true); 61 | logHelper.add(runningTime(), "time"); 62 | logHelper.add(robot()->getCrutchReadings(), "CrutchReadings"); 63 | //logHelper.add(robot()->getMotorPositions(), "MotorPositions"); 64 | //logHelper.add(robot()->getMotorVelocities(), "MotorVelocities"); 65 | //logHelper.add(robot()->getMotorTorques(), "MotorTorques"); 66 | //logHelper.add(robot()->getGoButton(), "GoButton"); 67 | //logHelper.add(robot()->getCurrentState(), "CurrentState"); 68 | //logHelper.add(robot()->getCurrentMovement(), "CurrentMovement"); 69 | } 70 | -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/LoggingDevice.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file LoggingDevice.h 3 | * \author William Campbell 4 | * \version 0.1 5 | * \date 2019-09-24 6 | * \copyright Copyright (c) 2020 7 | * 8 | * 9 | */ 10 | #ifndef LOGGINGDEVICE_SM_H 11 | #define LOGGINGDEVICE_SM_H 12 | 13 | #include "StateMachine.h" 14 | #include "LoggingRobot.h" 15 | 16 | // State Classes 17 | #include "InitState.h" 18 | #include "IdleState.h" 19 | #include "CalibrateState.h" 20 | #include "RecordState.h" 21 | 22 | /** 23 | * @brief Example implementation of a StateMachine for the ExoRobot class. States should implemented ExoTestState 24 | * 25 | */ 26 | class LoggingDevice : public StateMachine { 27 | public: 28 | /** 29 | * \todo Pilot Parameters would be set in constructor here 30 | * 31 | */ 32 | LoggingDevice(); 33 | void init(); 34 | 35 | bool trajComplete; 36 | 37 | LoggingRobot *robot() { return static_cast(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 38 | }; 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/CalibrateState.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * /file CalibrateState.cpp 3 | * /author Justin Fong 4 | * /brief Calibration - takes sensors and offsets sensors by NUM_CALIBRATE_READINGS second average 5 | * /version 0.1 6 | * /date 2021-1-21 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #include "CalibrateState.h" 13 | 14 | CalibrateState::CalibrateState(LoggingRobot *robot, const char *name) : State(name), robot(robot) { 15 | spdlog::info("CalibrateState Created"); 16 | }; 17 | void CalibrateState::entry(void) { 18 | spdlog::info("CalibrateState entry"); 19 | spdlog::info("Calibrating...."); 20 | 21 | Eigen::VectorXd force = robot->getCrutchReadings(); 22 | readings = Eigen::ArrayXXd::Zero(NUM_CALIBRATE_READINGS, force.size()); 23 | 24 | //robot->zeroForcePlate(); 25 | robot->startCrutchSensors(); 26 | currReading =0; 27 | }; 28 | 29 | void CalibrateState::during(void){ 30 | // Collect data and save 31 | if (currReading< NUM_CALIBRATE_READINGS){ 32 | //Eigen::VectorXd force = robot->getCrutchReadings(); 33 | readings.row(currReading) = robot->getCrutchReadings(); 34 | } 35 | currReading = currReading+1; 36 | }; 37 | 38 | void CalibrateState::exit(void) { 39 | // Take average of the matrices 40 | Eigen::VectorXd offsets = Eigen::VectorXd::Zero(readings.cols()); 41 | 42 | // Set offsets for crutches 43 | for (int i = 0; i < readings.cols(); i++) { 44 | offsets[i] = readings.col(i).sum()/NUM_CALIBRATE_READINGS; 45 | spdlog::debug("Crutch Offset {}", offsets[i]); 46 | } 47 | spdlog::info("Crutch Sensor Calibration Complete"); 48 | 49 | for (int i = 0; i < readings.cols()/6; i++){ 50 | if (offsets.segment(i*6, 6).isApprox(Eigen::VectorXd::Zero(6))){ 51 | spdlog::warn("Crutches may not be connected"); 52 | } 53 | } 54 | 55 | robot->setCrutchOffsets(offsets); 56 | robot->stopCrutchSensors(); 57 | 58 | spdlog::info("CalibrateState Exit"); 59 | }; 60 | 61 | int CalibrateState::getCurrReading(void) { 62 | return currReading; 63 | }; -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/CalibrateState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file IdleState.h 3 | * /author Justin Fong 4 | * /brief Calibration - takes sensors and offsets sensors by NUM_CALIBRATE_READINGS average 5 | * /version 0.1 6 | * /date 2021-1-21 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #ifndef LR_CALIBRATESTATE_H 13 | #define LR_CALIBRATESTATE_H 14 | 15 | #include "LoggingRobot.h" 16 | #include "State.h" 17 | 18 | #define NUM_CALIBRATE_READINGS 200 19 | 20 | class CalibrateState : public State { 21 | private: 22 | Eigen::ArrayXXd readings; 23 | Eigen::ArrayXXi strainReadings; 24 | 25 | int currReading = 0; 26 | 27 | public : 28 | LoggingRobot * robot; 29 | 30 | CalibrateState(LoggingRobot *robot, const char *name = ""); 31 | 32 | void entry(void); 33 | void during(void); 34 | void exit(void); 35 | 36 | int getCurrReading(); 37 | }; 38 | #endif -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/IdleState.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * /file IdleState.cpp 3 | * /author Justin Fong 4 | * /brief Nothing happens in this state, just waiting for action 5 | * /version 0.1 6 | * /date 2021-1-21 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #include "IdleState.h" 13 | 14 | IdleState::IdleState(LoggingRobot *robot, const char *name) : State(name), robot(robot) { 15 | spdlog::info("IdleState Created"); 16 | }; 17 | void IdleState::entry(void) { 18 | spdlog::info("IdleState entry"); 19 | spdlog::info("To Zero: A = Crutches"); 20 | spdlog::info("S to start logging"); 21 | }; 22 | 23 | void IdleState::during(void){ 24 | // Do nothing 25 | }; 26 | 27 | void IdleState::exit(void) { 28 | spdlog::info("IdleState Exit"); 29 | }; 30 | -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/IdleState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file IdleState.h 3 | * /author Justin Fong 4 | * /brief Idle state - logger is doing nothing 5 | * /version 0.1 6 | * /date 2021-1-21 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #ifndef LR_IDLESTATE_H 13 | #define LR_IDLESTATE_H 14 | 15 | #include "LoggingRobot.h" 16 | #include "State.h" 17 | 18 | class IdleState : public State { 19 | public: 20 | LoggingRobot *robot; 21 | IdleState(LoggingRobot *robot, const char *name = ""); 22 | 23 | void entry(void); 24 | void during(void); 25 | void exit(void); 26 | }; 27 | #endif -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/InitState.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * /file InitState.cpp 3 | * /author Justin Fong 4 | * /brief Virtual Class to include all required classes for Logging Robot 5 | * /version 0.1 6 | * /date 2020-12-1 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #include "InitState.h" 13 | 14 | InitState::InitState(LoggingRobot *robot, const char *name) : State(name), robot(robot) { 15 | spdlog::info("InitState Created"); 16 | }; 17 | void InitState::entry(void) { 18 | spdlog::info("InitState entry"); 19 | }; 20 | 21 | void InitState::during(void){ 22 | //Eigen::VectorXd force = robot->getCrutchReadings(); 23 | //spdlog::info("Fx1: {}, Fy1: {}, Fz1: {}, Fx2: {}, Fy2: {}, Fz2: {}", force[0], force[1], force[2], force[3], force[4], force[5]); 24 | }; 25 | 26 | void InitState::exit(void) { 27 | spdlog::info("InitState Exit"); 28 | }; -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/InitState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file InitState.h 3 | * /author Justin Fong 4 | * /brief Virtual Class to include all required classes for Logging Robot 5 | * /version 0.1 6 | * /date 2020-12-1 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #ifndef LR_INITSTATE_H 13 | #define LR_INITSTATE_H 14 | 15 | #include "LoggingRobot.h" 16 | #include "State.h" 17 | 18 | extern CO_NMT_reset_cmd_t reset_local; 19 | 20 | class InitState : public State { 21 | public: 22 | LoggingRobot *robot; 23 | InitState(LoggingRobot *robot, const char *name = ""); 24 | 25 | void entry(void); 26 | void during(void); 27 | void exit(void); 28 | }; 29 | #endif -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/RecordState.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * /file RecordState.cpp 3 | * /author Justin Fong 4 | * /brief Just records data 5 | * /version 0.1 6 | * /date 2021-1-21 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #include "RecordState.h" 13 | 14 | RecordState::RecordState(LoggingRobot *robot, const char *name) : State(name), robot(robot) { 15 | spdlog::info("RecordState Created"); 16 | }; 17 | void RecordState::entry(void) { 18 | spdlog::info("RecordState entry"); 19 | spdlog::info("S to Stop"); 20 | lastCrutchForce = robot->getCrutchReadings(); 21 | 22 | robot->startSensors(); 23 | }; 24 | 25 | void RecordState::during(void){ 26 | Eigen::VectorXd crutchForce = robot->getCrutchReadings(); 27 | 28 | // Check if some sensors are not responding properly every one second 29 | if(ticker % 100 == 99){ 30 | bool ok =true; 31 | if (lastCrutchForce.isApprox(crutchForce)){ 32 | spdlog::error("Crutches Not Updating"); 33 | ok = false; 34 | } 35 | if (ok) { 36 | spdlog::info("Recording..."); 37 | } 38 | lastCrutchForce = crutchForce; 39 | } 40 | ticker++; 41 | 42 | }; 43 | 44 | void RecordState::exit(void) { 45 | robot->stopSensors(); 46 | spdlog::info("RecordState Exit"); 47 | }; 48 | -------------------------------------------------------------------------------- /src/apps/LoggingDevice/stateMachine/states/RecordState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file IdleState.h 3 | * /author Justin Fong 4 | * /brief Idle state - logger is doing nothing 5 | * /version 0.1 6 | * /date 2021-1-21 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #ifndef LR_RECORDSTATE_H 13 | #define LR_RECORDSTATE_H 14 | 15 | #include "LoggingRobot.h" 16 | #include "State.h" 17 | 18 | class RecordState : public State { 19 | private: 20 | int ticker = 0; 21 | Eigen::VectorXd lastCrutchForce; 22 | 23 | public: 24 | LoggingRobot *robot; 25 | RecordState(LoggingRobot *robot, const char *name = ""); 26 | 27 | void entry(void); 28 | void during(void); 29 | void exit(void); 30 | }; 31 | #endif 32 | -------------------------------------------------------------------------------- /src/apps/M1DemoMachine/M1DemoMachine.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file M1DemoMachine.h 3 | * \author Yue Wen adapted from Vincent Crocher 4 | * \version 0.2 5 | * \date 2022-10-26 6 | * \copyright Copyright (c) 2020 - 2022 7 | * 8 | * /brief The M1DemoMachine class represents an example implementation of an M1 state machine. 9 | * 10 | */ 11 | #ifndef M1_SM_H 12 | #define M1_SM_H 13 | 14 | #include "RobotM1.h" 15 | #include "StateMachine.h" 16 | 17 | // State Classes 18 | #include "M1DemoStates.h" 19 | 20 | /** 21 | * @brief Example implementation of a StateMachine for the M1Robot class. States should implemented M1DemoState 22 | * 23 | */ 24 | class M1DemoMachine : public StateMachine { 25 | public: 26 | /** 27 | * \todo Pilot Parameters would be set in constructor here 28 | * 29 | */ 30 | M1DemoMachine(); 31 | ~M1DemoMachine(); 32 | void init(); 33 | 34 | RobotM1 *robot() { return static_cast(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 35 | }; 36 | 37 | #endif /*M1_SM_H*/ 38 | -------------------------------------------------------------------------------- /src/apps/M1DemoMachine/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM M1) 6 | 7 | ################################################################################ 8 | 9 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 10 | 11 | ## StateMachine name is (and should be!) current folder name 12 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 13 | ## And its relative path to the root folder is: 14 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 15 | 16 | ################################################################################ -------------------------------------------------------------------------------- /src/apps/M1DemoMachineROS/M1DemoMachineROS.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file M1DemoMachineROS.h 3 | * \author Emek Baris Kucuktabak 4 | * \version 0.2 5 | * \date 2022-10-25 6 | * \copyright Copyright (c) 2020 7 | * 8 | * /brief The M1DemoMachineROS class represents an example implementation of a stateMachine in multi-robot control setting 9 | * 10 | */ 11 | #ifndef M1_SM_H 12 | #define M1_SM_H 13 | 14 | #include "RobotM1.h" 15 | #include "StateMachine.h" 16 | 17 | // State Classes 18 | #include "states/MultiControllerState.h" 19 | 20 | #include "M1MachineROS.h" 21 | 22 | /** 23 | * @brief Example implementation of a StateMachine for the M1Robot class. States should implemented M1DemoState 24 | * 25 | */ 26 | class M1DemoMachineROS : public StateMachine { 27 | public: 28 | /** 29 | * \todo Pilot Parameters would be set in constructor here 30 | * 31 | */ 32 | M1DemoMachineROS(int argc, char *argv[]); 33 | ~M1DemoMachineROS(); 34 | void init(); 35 | 36 | void hwStateUpdate(); 37 | 38 | RobotM1 *robot() { return static_cast(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 39 | protected: 40 | M1MachineROS *M1MachineRos_; /* 17 | #include 18 | #include 19 | #include 20 | 21 | #include "RobotM1.h" 22 | #include "ros/ros.h" // This state machine requires ROS 23 | 24 | class M1MachineROS { 25 | public: 26 | M1MachineROS(RobotM1 *robot); 27 | ~M1MachineROS(); 28 | 29 | void update(void); 30 | void publishJointStates(void); 31 | void publishInteractionForces(void); 32 | void initialize(); 33 | void setNodeHandle(ros::NodeHandle& nodeHandle); 34 | 35 | ros::NodeHandle* nodeHandle_; 36 | 37 | Eigen::VectorXd jointPositionCommand_, jointVelocityCommand_, jointTorqueCommand_; 38 | Eigen::VectorXd interactionTorqueCommand_; 39 | 40 | private: 41 | // Subscriber and callback func for joint command subscription 42 | ros::Subscriber jointCommandSubscriber_; 43 | void jointCommandCallback(const sensor_msgs::JointState &msg); 44 | 45 | // Subscriber and callback func for interaction torque subscription 46 | ros::Subscriber interactionTorqueCommandSubscriber_; 47 | void interactionTorqueCommandCallback(const std_msgs::Float64MultiArray &msg); 48 | 49 | // Publisher and message for joint state publication 50 | ros::Publisher jointStatePublisher_; 51 | sensor_msgs::JointState jointStateMsg_; 52 | 53 | // Publisher and message for interaction wrench publication 54 | ros::Publisher interactionWrenchPublisher_; 55 | geometry_msgs::WrenchStamped interactionWrenchMsg_; 56 | RobotM1 *robot_; 57 | }; 58 | 59 | #endif //SRC_M1MachineROS_H 60 | -------------------------------------------------------------------------------- /src/apps/M1DemoMachineROS/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM M1) 6 | 7 | ## Compile for ROS 1 8 | set(ROS 1) 9 | 10 | ################################################################################ 11 | 12 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 13 | 14 | ## StateMachine name is (and should be!) current folder name 15 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 16 | ## And its relative path to the root folder is: 17 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 18 | 19 | ################################################################################ -------------------------------------------------------------------------------- /src/apps/M1DemoMachineROS/states/MultiControllerState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file MultiControllerState.h 3 | * \author Emek Baris Kucuktabak 4 | * \version 0.1 5 | * \date 2020-11-09 6 | * \copyright Copyright (c) 2020 7 | * 8 | * 9 | */ 10 | 11 | #ifndef SRC_MULTICONTROLLERSTATE_H 12 | #define SRC_MULTICONTROLLERSTATE_H 13 | 14 | #include "State.h" 15 | #include "RobotM1.h" 16 | #include "M1MachineROS.h" 17 | 18 | // dynamic reconfigure 19 | #include 20 | #include 21 | 22 | /** 23 | * \brief A multi purpose state with different controllers implemented 24 | * 25 | * 26 | */ 27 | class MultiControllerState : public State { 28 | RobotM1 *robot_; 29 | M1MachineROS *M1MachineRos_; 30 | 31 | public: 32 | void entry(void); 33 | void during(void); 34 | void exit(void); 35 | MultiControllerState(RobotM1 *exo, M1MachineROS *M1MachineRos, const char *name = "") : 36 | State(name), robot_(exo), M1MachineRos_(M1MachineRos){}; 37 | 38 | int cali_stage; 39 | int cali_velocity; 40 | 41 | // FOR TRANSPERANCY EXPERIMENTS 42 | 43 | double kp_; 44 | double kd_; 45 | double ki_; 46 | double tick_max_; 47 | double spk_; 48 | double ffRatio_; 49 | int controller_mode_; 50 | 51 | double control_freq; 52 | int current_mode; 53 | double torque_error_last_time_step = 0; 54 | 55 | double error; 56 | double delta_error; 57 | double integral_error; 58 | 59 | double spring_tor; 60 | double tick_count; 61 | 62 | Eigen::VectorXd q; //positive dorsi flexion 63 | Eigen::VectorXd dq; 64 | Eigen::VectorXd tau; 65 | Eigen::VectorXd tau_s; 66 | Eigen::VectorXd tau_cmd; 67 | 68 | int digitalInValue_; 69 | int digitalOutValue_; 70 | 71 | double alpha_q; 72 | double alpha_tau; 73 | double q_pre; 74 | double tau_pre; 75 | double cut_off; 76 | double tau_raw; 77 | double tau_filtered; 78 | double q_raw; 79 | double q_filtered; 80 | 81 | private: 82 | // dynamic reconfigure server and callback 83 | dynamic_reconfigure::Server server_; 84 | void dynReconfCallback(CORC::dynamic_paramsConfig &config, uint32_t level); 85 | 86 | std::chrono::steady_clock::time_point time0; 87 | 88 | protected: 89 | struct timespec initTime; /*(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 36 | 37 | std::shared_ptr UIserver = nullptr; //!< Pointer to communication server 38 | }; 39 | 40 | #endif /*M2_SM_H*/ 41 | -------------------------------------------------------------------------------- /src/apps/M2DemoMachine/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM M2) 6 | 7 | ## State machine uses FLNL for network communication 8 | set(USE_FLNL TRUE) 9 | 10 | ################################################################################ 11 | 12 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 13 | 14 | ## StateMachine name is (and should be!) current folder name 15 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 16 | ## And its relative path to the root folder is: 17 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 18 | 19 | ################################################################################ 20 | -------------------------------------------------------------------------------- /src/apps/M2ProDemoMachine/M2ProDemoMachine.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file M2ProDemoMachine.h 3 | * \author Vincent Crocher, Hao Yu, Zebin Huang 4 | * /brief The M2ProDemoMachine class represents an example implementation of an M2Pro state machine. 5 | * \version 0.1 6 | * \date 2024-05-14 7 | * 8 | * \copyright Copyright (c) 2024 - 2025 9 | * 10 | */ 11 | #ifndef M2_SM_H 12 | #define M2_SM_H 13 | 14 | #include "RobotM2P.h" 15 | #include "StateMachine.h" 16 | #include "FLNLHelper.h" 17 | 18 | // State Classes 19 | #include "M2ProDemoStates.h" 20 | 21 | /** 22 | * @brief Example implementation of a StateMachine for the M2Robot class. States should implemented M2DemoState 23 | * 24 | */ 25 | class M2ProDemoMachine : public StateMachine { 26 | 27 | public: 28 | M2ProDemoMachine(); 29 | ~M2ProDemoMachine(); 30 | void init(); 31 | void end(); 32 | 33 | void hwStateUpdate(); 34 | 35 | RobotM2P *robot() { return static_cast(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 36 | 37 | std::shared_ptr UIserver = nullptr; //!< Pointer to communication server 38 | }; 39 | 40 | #endif /*M2_SM_H*/ 41 | -------------------------------------------------------------------------------- /src/apps/M2ProDemoMachine/README.md: -------------------------------------------------------------------------------- 1 | # set up the network 2 | ```bash 3 | ifconfig 4 | UIserver = std::make_shared(*robot(), "192.168.1.11") 5 | ``` 6 | 7 | ## build the demo app 8 | if no build folder 9 | ```bash 10 | mkdir build && cd build && cmake .. 11 | make -j8 12 | ``` 13 | 14 | if existing a build folder 15 | ```bash 16 | rm -r build && mkdir build && cd build && cmake ../CMakeLists.txt 17 | make -j8 18 | ``` 19 | 20 | ## run the demo app 21 | Initialize the CAN device (No need to repeat this step, unless you restart your PC): 22 | 23 | ```bash 24 | sudo ../script/initCAN0.sh # or initCAN1.sh 25 | ``` 26 | 27 | Run the compiled executable file: 28 | ```bash 29 | sudo ./M2ProDemoMachine_APP 30 | ``` 31 | 32 | ## check the configuration of the device CAN port 33 | ```bash 34 | ifconfig -a 35 | ``` -------------------------------------------------------------------------------- /src/apps/M2ProDemoMachine/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM M2P) 6 | 7 | ## State machine uses FLNL for network communication 8 | set(USE_FLNL TRUE) 9 | 10 | ################################################################################ 11 | 12 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 13 | 14 | ## StateMachine name is (and should be!) current folder name 15 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 16 | ## And its relative path to the root folder is: 17 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 18 | 19 | ################################################################################ 20 | -------------------------------------------------------------------------------- /src/apps/M3DemoMachine/M3DemoMachine.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file M3DemoMachine.h 3 | * \author Vincent Crocher 4 | * /brief The M3DemoMachine class represents an example implementation of an M3 state machine. 5 | * \version 0.3 6 | * \date 2021-12-05 7 | * 8 | * \copyright Copyright (c) 2020 - 2021 9 | * 10 | */ 11 | #ifndef M3_SM_H 12 | #define M3_SM_H 13 | 14 | 15 | #include "StateMachine.h" 16 | #include "RobotM3.h" 17 | #include "FLNLHelper.h" 18 | 19 | // State Classes 20 | #include "M3DemoStates.h" 21 | 22 | 23 | /** 24 | * @brief Example implementation of a StateMachine for the M3Robot class. States should implemented M3DemoState 25 | * 26 | */ 27 | class M3DemoMachine : public StateMachine { 28 | 29 | public: 30 | M3DemoMachine(); 31 | ~M3DemoMachine(); 32 | void init(); 33 | void end(); 34 | 35 | void hwStateUpdate(); 36 | 37 | RobotM3 *robot() { return static_cast(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 38 | 39 | std::shared_ptr UIserver = nullptr; //!< Pointer to communication server 40 | }; 41 | 42 | #endif /*M3_SM_H*/ 43 | -------------------------------------------------------------------------------- /src/apps/M3DemoMachine/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM M3) 6 | 7 | ## State machine uses FLNL for network communication 8 | set(USE_FLNL TRUE) 9 | 10 | ################################################################################ 11 | 12 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 13 | 14 | ## StateMachine name is (and should be!) current folder name 15 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 16 | ## And its relative path to the root folder is: 17 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 18 | 19 | ################################################################################ 20 | -------------------------------------------------------------------------------- /src/apps/StateMachineTemplate/StateMachineTemplate.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file StateMachineTemplate.h 3 | * \author Vincent Crocher 4 | * \brief The StateMachineTemplate class represents an example implementation of a state machine. 5 | * \version 0.1 6 | * \date 2025-04-14 7 | * 8 | * \copyright Copyright (c) 2025 9 | * 10 | */ 11 | #ifndef STATEMACHINETEMPLATE_H 12 | #define STATEMACHINETEMPLATE_H 13 | 14 | 15 | #include "StateMachine.h" 16 | #include "PlatformName.h" 17 | #include "FLNLHelper.h" 18 | 19 | // State Classes 20 | #include "StatesTemplate.h" 21 | 22 | 23 | /** 24 | * Example implementation of a StateMachine class. States should implemented StatesTemplate 25 | * 26 | */ 27 | class StateMachineTemplate : public StateMachine { 28 | 29 | public: 30 | StateMachineTemplate(); 31 | ~StateMachineTemplate(); 32 | void init(); 33 | void end(); 34 | 35 | void hwStateUpdate(); 36 | 37 | PlatformName *robot() { return static_cast(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 38 | 39 | std::shared_ptr UIserver = nullptr; //!< Pointer to communication server 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/apps/StateMachineTemplate/StatesTemplate.cpp: -------------------------------------------------------------------------------- 1 | #include "StatesTemplate.h" 2 | #include "StateMachineTemplate.h" 3 | 4 | using namespace std; 5 | 6 | 7 | void CalibState::entry(void) { 8 | calibDone=false; 9 | stop_reached_time = .0; 10 | at_stop = false; 11 | 12 | robot->decalibrate(); 13 | robot->initTorqueControl(); 14 | robot->printJointStatus(); 15 | std::cout << "Calibrating (keep clear)..." << std::flush; 16 | } 17 | //Move slowly on each joint until max force detected 18 | void CalibState::during(void) { 19 | //TODO 20 | } 21 | void CalibState::exit(void) { 22 | //TODO 23 | //apply zero force/torque 24 | } 25 | 26 | 27 | void StandbyState::entry(void) { 28 | } 29 | void StandbyState::during(void) { 30 | //Apply corresponding force 31 | //TODO 32 | //robot->setJointTorque(tau); 33 | 34 | //Keyboard inputs 35 | if(robot->keyboard->getS()) { 36 | //TODO 37 | } 38 | if(robot->keyboard->getW()) { 39 | //TODO 40 | } 41 | 42 | //Regular display status 43 | if(iterations()%1000==1) { 44 | robot->printJointStatus(); 45 | robot->printStatus(); 46 | } 47 | } 48 | void StandbyState::exit(void) { 49 | //TODO 50 | //apply zero force/torque 51 | } 52 | 53 | -------------------------------------------------------------------------------- /src/apps/StateMachineTemplate/StatesTemplate.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file StatesTemplate.h 3 | * \author Vincent Crocher 4 | * \version 0.1 5 | * \date 2025-04-14 6 | * 7 | * \copyright Copyright (c) 2025 8 | * 9 | */ 10 | 11 | #ifndef STATESTEMPLATE_H 12 | #define STATESTEMPLATE_H 13 | 14 | #include "State.h" 15 | #include "PlatformName.h" 16 | 17 | 18 | class StateMachineTemplate; 19 | 20 | /** 21 | * \brief Generic state type including a pointer to PlatformName 22 | * 23 | */ 24 | class PlatformNameState : public State { 25 | protected: 26 | PlatformName * robot; //!< Pointer to state machines robot object 27 | 28 | PlatformNameState(PlatformName* _robot, const char *name = NULL): State(name), robot(_robot){spdlog::debug("Created PlatformNameState {}", name);}; 29 | }; 30 | 31 | 32 | class StandbyState : public PlatformNameState { 33 | 34 | public: 35 | StandbyState(PlatformName * _robot, const char *name = "PlatformName Standby"):PlatformNameState(_robot, name){}; 36 | 37 | void entry(void); 38 | void during(void); 39 | void exit(void); 40 | }; 41 | 42 | 43 | 44 | /** 45 | * \brief Position calibration example. Go to stops of robot at constant torque for absolute position calibration. 46 | * 47 | */ 48 | class CalibState : public PlatformNameState { 49 | 50 | public: 51 | CalibState(PlatformName * _robot, const char *name = "PlatformName Standby"):PlatformNameState(_robot, name){}; 52 | 53 | void entry(void); 54 | void during(void); 55 | void exit(void); 56 | 57 | bool isCalibDone() {return calibDone;} 58 | 59 | private: 60 | double stop_reached_time; 61 | bool at_stop; 62 | bool calibDone=false; 63 | 64 | }; 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /src/apps/StateMachineTemplate/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM PLATFORM_FOLDER) 6 | 7 | ## State machine uses FLNL for network communication 8 | set(USE_FLNL TRUE) 9 | 10 | ################################################################################ 11 | 12 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 13 | 14 | ## StateMachine name is (and should be!) current folder name 15 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 16 | ## And its relative path to the root folder is: 17 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 18 | 19 | ################################################################################ 20 | -------------------------------------------------------------------------------- /src/apps/X2DemoMachine/X2DemoMachine.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file X2DemoMachine.h 3 | * \author Emek Baris KUcukabak 4 | * \version 0.1 5 | * \date 2020-07-06 6 | * \copyright Copyright (c) 2020 7 | * 8 | * /brief The X2DemoMachine class represents an example implementation of an X2 state machine. 9 | * 10 | */ 11 | 12 | #ifndef SRC_X2DEMOMACHINE_H 13 | #define SRC_X2DEMOMACHINE_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "X2Robot.h" 23 | #include "StateMachine.h" 24 | 25 | //// State Classes 26 | #include "states/X2DemoState.h" 27 | 28 | #include "X2DemoMachineROS.h" 29 | 30 | // Logger 31 | #include "LogHelper.h" 32 | 33 | class X2DemoMachine : public StateMachine { 34 | 35 | public: 36 | X2DemoMachine(int argc, char *argv[]); 37 | 38 | X2DemoMachineROS *x2DemoMachineRos_; /*(_robot.get()); } //!< Robot getter with specialised type (lifetime is managed by Base StateMachine) 49 | 50 | private: 51 | 52 | std::string robotName_; // robot name(obtained from node name) 53 | 54 | std::chrono::steady_clock::time_point time0; // initial time that machine started 55 | double time; // time passed after time0 in [s] 56 | 57 | }; 58 | 59 | 60 | #endif //SRC_X2DEMOMACHINE_H 61 | -------------------------------------------------------------------------------- /src/apps/X2DemoMachine/X2DemoMachineROS.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file X2DemoMachineROS.h 3 | * /author Emek Baris Kucuktabak 4 | * /brief ROS part of the X2DemoMachine 5 | * /version 0.1 6 | * /date 2020-07-06 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #ifndef SRC_X2DEMOMACHINEROS_H 13 | #define SRC_X2DEMOMACHINEROS_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "X2Robot.h" 27 | #include "X2DemoState.h" 28 | #include "ros/ros.h" // This state machine requires ROS 29 | 30 | class X2DemoMachineROS { 31 | public: 32 | X2DemoMachineROS(X2Robot *robot, std::shared_ptr x2DemoState, ros::NodeHandle& nodeHandle); 33 | ~X2DemoMachineROS(); 34 | 35 | void update(void); 36 | void publishJointStates(void); 37 | void publishInteractionForces(void); 38 | void publishGroundReactionForces(void); 39 | void initialize(); 40 | void setNodeHandle(ros::NodeHandle& nodeHandle); 41 | ros::NodeHandle& getNodeHandle(); 42 | 43 | Eigen::VectorXd interactionForceCommand_; 44 | 45 | private: 46 | ros::Publisher jointStatePublisher_; 47 | ros::Publisher interactionForcePublisher_; 48 | ros::Publisher groundReactionForcePublisher_[X2_NUM_GRF_SENSORS]; 49 | 50 | ros::ServiceServer calibrateForceSensorsService_; 51 | ros::ServiceServer startHomingService_; 52 | ros::ServiceServer emergencyStopService_; 53 | ros::ServiceServer imuCalibrationService_; 54 | 55 | sensor_msgs::JointState jointStateMsg_; 56 | CORC::X2Array interactionForceMsg_; 57 | geometry_msgs::WrenchStamped groundReactionForceMsgArray_[X2_NUM_GRF_SENSORS]; 58 | 59 | std::string grfFramesArray_[X2_NUM_GRF_SENSORS] = {"left_lower_shank", "right_lower_shank"}; 60 | 61 | X2Robot *robot_; 62 | std::shared_ptr x2DemoState_; 63 | 64 | bool calibrateForceSensorsCallback(std_srvs::Trigger::Request& req, 65 | std_srvs::Trigger::Response& res); 66 | 67 | bool startHomingCallback(std_srvs::Trigger::Request& req, 68 | std_srvs::Trigger::Response& res); 69 | 70 | bool emergencyStopCallback(std_srvs::Trigger::Request& req, 71 | std_srvs::Trigger::Response& res); 72 | 73 | bool calibrateIMUCallback(std_srvs::Trigger::Request& req, 74 | std_srvs::Trigger::Response& res); 75 | 76 | ros::NodeHandle* nodeHandle_; 77 | }; 78 | 79 | #endif //SRC_X2DEMOMACHINEROS_H 80 | -------------------------------------------------------------------------------- /src/apps/X2DemoMachine/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM X2) 6 | 7 | ## Compile for ROS 1 8 | set(ROS 1) 9 | 10 | ################################################################################ 11 | 12 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 13 | 14 | ## StateMachine name is (and should be!) current folder name 15 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 16 | ## And its relative path to the root folder is: 17 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 18 | 19 | ################################################################################ 20 | 21 | -------------------------------------------------------------------------------- /src/apps/X2DemoMachine/states/X2DemoState.h: -------------------------------------------------------------------------------- 1 | /** 2 | * /file X2DemoState.h 3 | * /author Emek Baris Kucuktabak 4 | * /brief Concrete implementation of DemoState 5 | * /version 1.1 6 | * /date 2022-02-22 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | #ifndef SRC_X2DEMOSTATE_H 12 | #define SRC_X2DEMOSTATE_H 13 | 14 | #include "State.h" 15 | #include "X2Robot.h" 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // dynamic reconfigure 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | /** 32 | * \brief Demo State for the X2DemoMachine 33 | * 34 | * 35 | */ 36 | class X2DemoState : public State { 37 | 38 | protected: 39 | X2Robot *robot_; 40 | 41 | public: 42 | void entry(void); 43 | void during(void); 44 | void exit(void); 45 | X2DemoState(X2Robot *exo, const char *name = "X2DemoState"); 46 | 47 | Eigen::VectorXd& getDesiredJointTorques(); 48 | Eigen::VectorXd& getDesiredJointVelocities(); 49 | 50 | int controller_mode_; 51 | 52 | Eigen::VectorXd enableJoints; 53 | 54 | 55 | private: 56 | dynamic_reconfigure::Server server_; 57 | void dynReconfCallback(CORC::dynamic_paramsConfig &config, uint32_t level); 58 | 59 | double t_step_ = 0.003; // 0.003 todo: get from main 60 | 61 | std::chrono::steady_clock::time_point time0; 62 | Eigen::VectorXd desiredJointTorques_; 63 | Eigen::VectorXd desiredJointVelocities_; 64 | 65 | Eigen::VectorXd kTransperancy_; 66 | double amplitude_, period_, offset_; 67 | 68 | }; 69 | 70 | #endif -------------------------------------------------------------------------------- /src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.cpp: -------------------------------------------------------------------------------- 1 | #include "X2ROS2DemoMachine.h" 2 | 3 | X2ROS2DemoMachine::X2ROS2DemoMachine(int argc, char **argv) : StateMachine() 4 | { 5 | // Create and set X2Robot object 6 | setRobot(std::make_unique()); 7 | 8 | // Configure ROS2 initialisation options and disable SIGINT capture (handled by CORC) 9 | rclcpp::InitOptions ros_init = rclcpp::InitOptions(); 10 | ros_init.shutdown_on_signal = false; 11 | rclcpp::init(argc, argv, ros_init); 12 | 13 | // Create the ROS2 node and pass a reference to the X2 Robot object 14 | m_Node = std::make_shared("x2", get_robot()); 15 | 16 | // Add some state that also holds reference to the ROS2 node 17 | addState("state", std::make_shared(get_robot(), get_node())); 18 | 19 | // Set the initial state 20 | setInitState("state"); 21 | } 22 | 23 | void 24 | X2ROS2DemoMachine::end() 25 | { 26 | spdlog::info("State machine end"); 27 | } 28 | 29 | void 30 | X2ROS2DemoMachine::init() 31 | { 32 | get_robot()->initialiseNetwork(); 33 | } 34 | 35 | void 36 | X2ROS2DemoMachine::hwStateUpdate() 37 | { 38 | // Update robot parameters 39 | get_robot()->updateRobot(); 40 | // Call custom publish method for publishing joint states of X2 Robot 41 | get_node()->publish_joint_states(); 42 | // Allow for the ROS2 node to execute callbacks (e.g., subscriptions) 43 | rclcpp::spin_some(get_node()->get_interface()); 44 | } 45 | 46 | bool 47 | X2ROS2DemoMachine::configureMasterPDOs() 48 | { 49 | return get_robot()->configureMasterPDOs(); 50 | } 51 | 52 | X2Robot * 53 | X2ROS2DemoMachine::get_robot() 54 | { 55 | return static_cast(_robot.get()); 56 | } 57 | 58 | const std::shared_ptr & 59 | X2ROS2DemoMachine::get_node() 60 | { 61 | return m_Node; 62 | } 63 | -------------------------------------------------------------------------------- /src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file X2ROS2DemoMachine.h 3 | * \author Benjamin von Snarski 4 | * \version 0.1 5 | * \date 2022-10-24 6 | * \copyright Copyright (c) 2022 7 | * \brief An example CORC state machine implementing the ROS2 client library. 8 | */ 9 | #ifndef SRC_X2ROS2DEMOMACHINE_H 10 | #define SRC_X2ROS2DEMOMACHINE_H 11 | 12 | #include "X2Robot.h" 13 | #include "X2ROS2Node.h" 14 | #include "X2ROS2State.h" 15 | #include "StateMachine.h" 16 | 17 | class X2ROS2DemoMachine : public StateMachine 18 | { 19 | public: 20 | X2ROS2DemoMachine(int argc, char **argv); 21 | 22 | void end() override; 23 | void init() override; 24 | void hwStateUpdate() override; 25 | bool configureMasterPDOs() override; 26 | 27 | X2Robot * get_robot(); 28 | const std::shared_ptr & get_node(); 29 | 30 | private: 31 | std::shared_ptr m_Node; 32 | }; 33 | 34 | #endif//SRC_X2ROS2DEMOMACHINE_H -------------------------------------------------------------------------------- /src/apps/X2ROS2DemoMachine/X2ROS2Node.cpp: -------------------------------------------------------------------------------- 1 | #include "X2ROS2Node.h" 2 | 3 | X2ROS2Node::X2ROS2Node(const std::string &name, X2Robot *robot) 4 | : Node(name), m_Robot(robot) 5 | { 6 | // Create an example subscription 7 | m_Sub = create_subscription( 8 | "example_topic", 10, std::bind(&X2ROS2Node::string_callback, this, _1) 9 | ); 10 | // Create a joint state publisher 11 | m_Pub = create_publisher( 12 | "joint_states", 10 13 | ); 14 | } 15 | 16 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr 17 | X2ROS2Node::get_interface() 18 | { 19 | // Must have this method to return base interface for spinning 20 | return this->get_node_base_interface(); 21 | } 22 | 23 | void 24 | X2ROS2Node::string_callback(const std_msgs::msg::String::SharedPtr msg) 25 | { 26 | // Example callback for subscription 27 | spdlog::info("Received ({}) in string callback", msg->data); 28 | } 29 | 30 | void 31 | X2ROS2Node::publish_joint_states() 32 | { 33 | // Instantiate joint state message 34 | sensor_msgs::msg::JointState msg; 35 | 36 | // Assign current header time stamp 37 | msg.header.stamp = this->now(); 38 | 39 | // Use this naming scheme for robot state publisher to recognise 40 | msg.name = { 41 | "left_hip_joint", 42 | "left_knee_joint", 43 | "right_hip_joint", 44 | "right_knee_joint", 45 | "world_to_backpack" 46 | }; 47 | 48 | // Copy position, velocity and toroque from X2 Robot 49 | msg.position.assign( 50 | m_Robot->getPosition().data(), 51 | m_Robot->getPosition().data() + m_Robot->getPosition().size() 52 | ); 53 | msg.velocity.assign( 54 | m_Robot->getVelocity().data(), 55 | m_Robot->getVelocity().data() + m_Robot->getVelocity().size() 56 | ); 57 | msg.effort.assign( 58 | m_Robot->getTorque().data(), 59 | m_Robot->getTorque().data() + m_Robot->getTorque().size() 60 | ); 61 | 62 | // Add backpack angle to positions for visualisation (RViz) 63 | msg.position.push_back(m_Robot->getBackPackAngleOnMedianPlane() - M_PI_2); 64 | 65 | // Publish the joint state message 66 | m_Pub->publish(msg); 67 | } -------------------------------------------------------------------------------- /src/apps/X2ROS2DemoMachine/X2ROS2Node.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file X2ROS2Node.h 3 | * \author Benjamin von Snarski 4 | * \version 0.1 5 | * \date 2022-10-24 6 | * \copyright Copyright (c) 2022 7 | * \brief An example ROS2 node that also holds a reference to the robot object. 8 | */ 9 | #ifndef SRC_X2ROS2NODE_H 10 | #define SRC_X2ROS2NODE_H 11 | 12 | #include "X2Robot.h" 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | using std::placeholders::_1; 19 | 20 | class X2ROS2Node : public rclcpp::Node 21 | { 22 | public: 23 | X2ROS2Node(const std::string &name, X2Robot *robot); 24 | 25 | void string_callback(const std_msgs::msg::String::SharedPtr msg); 26 | void publish_joint_states(); 27 | 28 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_interface(); 29 | 30 | private: 31 | X2Robot *m_Robot; 32 | 33 | rclcpp::Subscription::SharedPtr m_Sub; 34 | rclcpp::Publisher::SharedPtr m_Pub; 35 | }; 36 | 37 | #endif//SRC_X2ROS2NODE_H -------------------------------------------------------------------------------- /src/apps/X2ROS2DemoMachine/X2ROS2State.cpp: -------------------------------------------------------------------------------- 1 | #include "X2ROS2State.h" 2 | 3 | X2ROS2State::X2ROS2State(X2Robot *robot, const std::shared_ptr node) 4 | : m_Robot(robot), m_Node(node) 5 | { 6 | } 7 | 8 | void 9 | X2ROS2State::exit() 10 | { 11 | spdlog::info("Exited X2ROS2State"); 12 | } 13 | 14 | void 15 | X2ROS2State::entry() 16 | { 17 | spdlog::info("Entered X2ROS2State"); 18 | } 19 | 20 | void 21 | X2ROS2State::during() 22 | { 23 | } -------------------------------------------------------------------------------- /src/apps/X2ROS2DemoMachine/X2ROS2State.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file X2ROS2State.h 3 | * \author Benjamin von Snarski 4 | * \version 0.1 5 | * \date 2022-10-24 6 | * \copyright Copyright (c) 2022 7 | * \brief An example state that publishes the current X2 robot joint states and subscribes to an example topic. 8 | * Also holds reference to the ROS2 node. 9 | */ 10 | #ifndef SRC_X2ROS2STATE_H 11 | #define SRC_X2ROS2STATE_H 12 | 13 | #include "State.h" 14 | #include "X2Robot.h" 15 | #include "X2ROS2Node.h" 16 | 17 | class X2ROS2State : public State 18 | { 19 | public: 20 | X2ROS2State(X2Robot *robot, const std::shared_ptr node); 21 | 22 | void exit() override; 23 | void entry() override; 24 | void during() override; 25 | 26 | private: 27 | X2Robot *m_Robot; 28 | const std::shared_ptr m_Node; 29 | }; 30 | 31 | #endif//SRC_X2ROS2STATE_H -------------------------------------------------------------------------------- /src/apps/X2ROS2DemoMachine/app.cmake: -------------------------------------------------------------------------------- 1 | ################################## USER FLAGS ################################## 2 | 3 | ## Which platform (robot) is the state machine using? 4 | ## this is the correspondig folder name in src/hardware/platform to use 5 | set(PLATFORM X2) 6 | 7 | ## Compile for ROS 2 8 | set(ROS 2) 9 | 10 | ################################################################################ 11 | 12 | ################## AUTOMATED PATH AND NAME DEFINITION ########################## 13 | 14 | ## StateMachine name is (and should be!) current folder name 15 | get_filename_component(STATE_MACHINE_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) 16 | ## And its relative path to the root folder is: 17 | file(RELATIVE_PATH STATE_MACHINE_PATH ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_LIST_DIR}/) 18 | 19 | ################################################################################ 20 | 21 | -------------------------------------------------------------------------------- /src/core/CANopen/CANcomms/CO_time.h: -------------------------------------------------------------------------------- 1 | /** 2 | * CANopen time interface. 3 | * 4 | * @file CO_time.h 5 | * @author Janez Paternoster 6 | * @copyright 2016 Janez Paternoster 7 | * 8 | * This file is part of CANopenSocket, a Linux implementation of CANopen 9 | * stack with master functionality. Project home page is 10 | * . CANopenSocket is based 11 | * on CANopenNode: . 12 | * 13 | * CANopenSocket is free and open source software: you can redistribute 14 | * it and/or modify it under the terms of the GNU General Public License 15 | * as published by the Free Software Foundation, either version 2 of the 16 | * License, or (at your option) any later version. 17 | * 18 | * This program is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with this program. If not, see . 25 | */ 26 | 27 | #ifndef CO_TIME_H 28 | #define CO_TIME_H 29 | 30 | #include // Must be included by CO_SDO.h due to typedefs being here. 31 | 32 | #include "CO_SDO.h" 33 | 34 | /** 35 | * @brief Time object, usable for timestamping - Defined in CANOpen code 36 | */ 37 | typedef struct { 38 | uint64_t *epochTimeBaseMs; /**< From CO_time_init(). */ 39 | uint32_t *epochTimeOffsetMs; /**< From CO_time_init(). */ 40 | } CO_time_t; 41 | 42 | /** 43 | * Initialize time object. 44 | * 45 | * Function must be called in the communication reset section. 46 | * 47 | * @param tm This object will be initialized. 48 | * @param SDO SDO server object. 49 | * @param epochTimeBaseMs Pointer to milliseconds since Unix Epoch (1.1.1970, 00:00:00, UTC) - base. 50 | * @param epochTimeOffsetMs pointer to offset. Set to 0 at program startup and increments since then. 51 | * @param idx_OD_time Index in Object Dictionary. 52 | */ 53 | void CO_time_init( 54 | CO_time_t *tm, 55 | CO_SDO_t *SDO, 56 | uint64_t *epochTimeBaseMs, 57 | uint32_t *epochTimeOffsetMs, 58 | uint16_t idx_OD_time); 59 | 60 | /** 61 | * Process time object. 62 | * 63 | * Function must be called cyclically in 1ms intervals. 64 | * 65 | * @param tm This object. 66 | */ 67 | void CO_time_process(CO_time_t *tm); 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /src/core/CANopen/CANopenNode/RPDO.cpp: -------------------------------------------------------------------------------- 1 | #include "RPDO.h" 2 | 3 | RPDO::RPDO(UNSIGNED32 COBID, UNSIGNED8 transmissionType, void *dataEntry[], UNSIGNED16 dataSize[], UNSIGNED8 numMappedObjects) { 4 | myCOBID = COBID; 5 | commParam.COB_IDUsedByRPDO = COBID; 6 | commParam.transmissionType = transmissionType; 7 | 8 | lengthData = numMappedObjects; 9 | mappingParam.numberOfMappedObjects = numMappedObjects; 10 | 11 | // Required to do pointer indexing because of the stupid way OD_RPDOMappingParameter_t is defined. 12 | uint32_t *pMap = &mappingParam.mappedObject1; 13 | for (int i = 0; i < numMappedObjects; i++) { 14 | // Change the dataRecord pointers 15 | dataRecord[i+1].pData = dataEntry[i]; 16 | dataRecord[i + 1].length = dataSize[i]; 17 | 18 | // Change the mapping parameter to include subindex and data size (in bits now...) 19 | *pMap = 0x00000100*(i+1) + 0x08*dataSize[i]; 20 | pMap++; 21 | } 22 | 23 | // Then set up the OD 24 | CO_setRPDO(&commParam, &mappingParam, commRecord, dataRecord, mappingRecord); 25 | 26 | } 27 | 28 | UNSIGNED32 RPDO::getCOBID(){ 29 | return myCOBID; 30 | } 31 | -------------------------------------------------------------------------------- /src/core/CANopen/CANopenNode/TPDO.cpp: -------------------------------------------------------------------------------- 1 | #include "TPDO.h" 2 | 3 | TPDO::TPDO(UNSIGNED32 COBID, UNSIGNED8 transmissionType, void *dataEntry[], UNSIGNED16 dataSize[], UNSIGNED8 numMappedObjects) { 4 | myCOBID = COBID; 5 | commParam.COB_IDUsedByTPDO = COBID; 6 | commParam.transmissionType = transmissionType; 7 | 8 | lengthData = numMappedObjects; 9 | mappingParam.numberOfMappedObjects = numMappedObjects; 10 | 11 | // Required to do pointer indexing because of the stupid way OD_RPDOMappingParameter_t is defined. 12 | uint32_t *pMap = &mappingParam.mappedObject1; 13 | for (int i = 0; i < numMappedObjects; i++) { 14 | // Change the dataRecord pointers 15 | dataRecord[i+1].pData = dataEntry[i]; 16 | dataRecord[i + 1].length = dataSize[i]; 17 | 18 | // Change the mapping parameter to include subindex and data size (in bits now...) 19 | *pMap = 0x00000100*(i+1) + 0x08*dataSize[i]; 20 | pMap++; 21 | } 22 | 23 | // Then set up the OD 24 | CO_setTPDO(&commParam, &mappingParam, commRecord, dataRecord, mappingRecord); 25 | } 26 | 27 | UNSIGNED32 TPDO::getCOBID() { 28 | return myCOBID; 29 | } 30 | -------------------------------------------------------------------------------- /src/core/application.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file application.c 3 | * \author William Campbell, Justin Fong, Vincent Crocher 4 | * \version 0.3 5 | * \date 2021-12-18 6 | * \copyright Copyright (c) 2020 - 2021 7 | * 8 | * \brief Application interface of CORC. Based on CANopenSocket. 9 | * 10 | */ 11 | #include "application.h" 12 | 13 | //Select state machine to use for this application (can be set in cmake) 14 | #ifndef STATE_MACHINE_TYPE 15 | #error "State Machine Type not defined" 16 | #endif 17 | 18 | std::unique_ptr stateMachine; 19 | 20 | /******************** RUNS BEFORE CO_init() ********************/ 21 | void app_communicationReset(int argc, char *argv[]) { 22 | #if ROS > 0 23 | stateMachine = std::make_unique(argc, argv); 24 | #else 25 | stateMachine = std::make_unique(); 26 | #endif 27 | stateMachine->configureMasterPDOs(); 28 | } 29 | 30 | /******************** Runs at the Start of rt_control_thread********************/ 31 | void app_programStart(void) { 32 | spdlog::info("CORC Start application"); 33 | 34 | #ifdef NOROBOT 35 | spdlog::info("Running in NOROBOT (virtual) mode."); 36 | #endif // NOROBOT 37 | spdlog::info("Application thread running at {}Hz.", (int)(1000./(float)controlLoopPeriodInms)); 38 | stateMachine->init(); 39 | stateMachine->activate(); 40 | } 41 | 42 | /******************** Runs in low priority thread ********************/ 43 | void app_programAsync(uint16_t timer1msDiffy) { 44 | } 45 | 46 | /******************** Runs in rt_control_thread ********************/ 47 | void app_programControlLoop(void) { 48 | std::chrono::steady_clock::time_point _t0 = std::chrono::steady_clock::now(); 49 | 50 | //StateMachine execution 51 | if (stateMachine->running()) { 52 | stateMachine->update(); 53 | } 54 | 55 | //Warn if time overflow (this is the effective used time, normally lower than the allocated time period) 56 | double dt = (std::chrono::duration_cast( 57 | std::chrono::steady_clock::now() - _t0).count()) / 1e6; 58 | if(dt>controlLoopPeriodInms/1000.) 59 | spdlog::warn("Applicaton thread time overflow: {}ms (>{}ms) !", dt*1000., controlLoopPeriodInms); 60 | } 61 | 62 | /******************** Runs at the End of rt_control_thread********************/ 63 | void app_programEnd(void) { 64 | stateMachine->end(); 65 | stateMachine.reset(); //Explicit delete of the state machine to answer deletion on time 66 | spdlog::info("CORC End application"); 67 | } 68 | -------------------------------------------------------------------------------- /src/core/logging.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file logging.cpp 3 | * \author Vincent Crocher 4 | * \brief Main logging function used to log trace, debug and error outputs 5 | * \version 0.3 6 | * \date 2020-10-30 7 | * \copyright Copyright (c) 2020 8 | * 9 | */ 10 | 11 | #include "logging.h" 12 | 13 | 14 | void init_logging(const char * filename) 15 | { 16 | //Needed on top of the MACRO definition to be effective 17 | switch(SPDLOG_ACTIVE_LEVEL) 18 | { 19 | case SPDLOG_LEVEL_TRACE: 20 | spdlog::set_level(spdlog::level::trace); 21 | break; 22 | 23 | case SPDLOG_LEVEL_DEBUG: 24 | spdlog::set_level(spdlog::level::debug); 25 | break; 26 | 27 | case SPDLOG_LEVEL_INFO: 28 | spdlog::set_level(spdlog::level::info); 29 | break; 30 | 31 | case SPDLOG_LEVEL_WARN: 32 | spdlog::set_level(spdlog::level::warn); 33 | break; 34 | 35 | case SPDLOG_LEVEL_ERROR: 36 | spdlog::set_level(spdlog::level::err); 37 | break; 38 | 39 | case SPDLOG_LEVEL_CRITICAL: 40 | spdlog::set_level(spdlog::level::critical); 41 | break; 42 | 43 | case SPDLOG_LEVEL_OFF: 44 | spdlog::set_level(spdlog::level::off); 45 | break; 46 | 47 | default: 48 | spdlog::set_level(spdlog::level::off); 49 | } 50 | //Create two logging sinks: one file (rotating, max 50M0), one console (with colors) 51 | std::vector sinks; 52 | sinks.push_back(std::make_shared()); 53 | sinks.push_back(std::make_shared(filename, 1024 * 1024 * 50, 1)); 54 | //Register this two sinks logger as main logger 55 | std::shared_ptr main_logger = std::make_shared("CORC", begin(sinks), end(sinks)); 56 | spdlog::initialize_logger(main_logger); 57 | spdlog::set_default_logger(main_logger); 58 | 59 | 60 | spdlog::info("==============================================="); 61 | spdlog::info("============ Start logging session ============"); 62 | } 63 | -------------------------------------------------------------------------------- /src/core/logging.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file logging.h 3 | * \author Vincent Crocher 4 | * \brief Main logging function used to log trace, debug and error outputs 5 | * \version 0.3 6 | * \date 2020-10-30 7 | * \copyright Copyright (c) 2020 8 | * 9 | */ 10 | #ifndef LOGGING_H_INCLUDED 11 | #define LOGGING_H_INCLUDED 12 | 13 | //Desired logging level can be defined within CMakeLists.txt 14 | 15 | //For file logging (and console output) use either the following functions: 16 | //spdlog::trace("Test trace {}", my_value); 17 | //spdlog::debug("Test debug {}", my_value); 18 | //... 19 | //spdlog::critical("Test debug {}", my_value); 20 | 21 | //Or the following macros 22 | //SPDLOG_LOGGER_TRACE(main_logger, "Test trace"); 23 | //SPDLOG_LOGGER_DEBUG(main_logger, "Test debug"); 24 | //SPDLOG_LOGGER_INFO(main_logger, "Test info"); 25 | //SPDLOG_LOGGER_WARN(main_logger, "Test warning"); 26 | //SPDLOG_LOGGER_ERROR(main_logger, "Test error"); 27 | //SPDLOG_LOGGER_CRITICAL(main_logger, "Test critical"); 28 | 29 | #include "spdlog/spdlog.h" 30 | #include "spdlog/sinks/rotating_file_sink.h" // support for rotating file logging 31 | #include "spdlog/sinks/stdout_color_sinks.h" 32 | 33 | void init_logging(const char * filename = "logs/CORC.log"); 34 | 35 | //Development use to log loop timing 36 | //#define TIMING_LOG 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/core/robot/CANDevice.cpp: -------------------------------------------------------------------------------- 1 | // To do -------------------------------------------------------------------------------- /src/core/robot/CANDevice.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file CANDevice.h 3 | * \author Justin Fong 4 | * \brief Generic CANDevice Class 5 | * 6 | * \version 0.1 7 | * \date 2021-01-12 8 | * \version 0.1 9 | * 10 | * \copyright Copyright (c) 2021 11 | * 12 | */ 13 | 14 | 15 | 16 | // TODO - THIS IS NOT FINISHED YET, OR USED. 17 | 18 | 19 | #ifndef CANDEVICE_H_INCLUDED 20 | #define CANDEVICE_H_INCLUDED 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include "logging.h" 29 | 30 | /** 31 | * @ingroup Robot 32 | * \brief Abstract class describing a CANOpen Device 33 | * 34 | */ 35 | class CANDevice { 36 | protected: 37 | /** 38 | * \brief The CAN Node ID used to address this particular device on the CAN bus 39 | * 40 | */ 41 | int NodeID; 42 | 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /src/core/robot/InputDevice.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "InputDevice.h" 3 | 4 | InputDevice::InputDevice() { 5 | //empty 6 | } 7 | 8 | InputDevice::~InputDevice() { 9 | // Does nothing 10 | } -------------------------------------------------------------------------------- /src/core/robot/InputDevice.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file InputDevice.h 3 | * \author William Campbell 4 | * \brief The Input class is a abstract class which represents an input device. 5 | * The Update function is called in a main program to query the devices input and 6 | * update any memory representation of the device implemented. 7 | * For example the keyboard implementation checks for key presses and fills key memory 8 | * with a boolean value when pressed. See Keyboard for further detail. 9 | * \version 0.1 10 | * \date 2020-04-09 11 | * \version 0.1 12 | * \copyright Copyright (c) 2020 13 | */ 14 | 15 | #ifndef InputDevice_H_INCLUDED 16 | #define InputDevice_H_INCLUDED 17 | #include 18 | 19 | #include "logging.h" 20 | #include "RPDO.h" 21 | #include "TPDO.h" 22 | 23 | /** 24 | * @ingroup Robot 25 | * @brief Abstract class representing any Input device (sensor, joystick..) to be used in a Robot object 26 | * 27 | */ 28 | class InputDevice { 29 | private: 30 | public: 31 | InputDevice(); 32 | virtual ~InputDevice(); 33 | /** 34 | * @brief pure virtual method to be implemented by specific input devices and used by the virtual robot object to update the 35 | * current state of the robotic systems input device. 36 | * 37 | */ 38 | virtual void updateInput() = 0; 39 | 40 | virtual bool configureMasterPDOs() =0; 41 | }; 42 | #endif 43 | -------------------------------------------------------------------------------- /src/hardware/IO/FourierForceSensor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file FourierForceSensor.h 3 | * \author Emek Baris Kucuktabak, Vincent Crocher 4 | * \brief A class for the Fourier force sensors (ASU) on CAN bus (but not CANOpen compatible or standard) 5 | * \version 0.1 6 | * \date 2021-02-13 7 | * 8 | * \copyright Copyright (c) 2021 9 | * 10 | */ 11 | 12 | #ifndef FOURIER_FORCE_SENSOR_H 13 | #define FOURIER_FORCE_SENSOR_H 14 | 15 | #include "InputDevice.h" 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | class FourierForceSensor : public InputDevice { 22 | 23 | public: 24 | /** 25 | * Construct a new FourierForceSensor object 26 | * 27 | */ 28 | FourierForceSensor(int sensor_can_node_ID, double scale_factor = 1.0, double calib_time = 2.0); 29 | 30 | /** 31 | * Configure Master (controller) side PDO for force sensor reading. 32 | * 33 | */ 34 | bool configureMasterPDOs(); 35 | 36 | /** 37 | * Updates the force readings from last updated PDO and applying zeroing and scaling. 38 | * 39 | */ 40 | void updateInput(); 41 | 42 | /** 43 | * Zero the force sensor. When called, sets the sensor value to 0 by measuring over a short period of time (Blocking). 44 | * 45 | * \return bool success of calibration 46 | */ 47 | bool calibrate(double calib_time = -1); 48 | 49 | /** 50 | * Return true if calibraated (zeroed). 51 | */ 52 | bool isCalibrated() { return calibrated; } 53 | 54 | /** 55 | * Returns the lastest updated sensor reading in N. 56 | * 57 | */ 58 | double getForce(); 59 | 60 | /** 61 | * send SDO command to shift the measurement to a value around 1500. 62 | * 63 | * \return bool success of internal calibration 64 | */ 65 | bool sendInternalCalibrateSDOMessage(); 66 | 67 | protected: 68 | virtual double sensorValueToNewton(int sensorValue); 69 | 70 | private: 71 | int sensorNodeID; 72 | double scaleFactor; 73 | RPDO *rpdo; 74 | INTEGER32 rawData[2] = {0}; 75 | double forceReading; //!< Store latest updated sensor reading (in N) 76 | double calibrationOffset; //!< Sensor offset from calibration/zeroing (raw value) 77 | bool calibrated; 78 | double calibrationTime; //!< Zeroing time (in s). 79 | 80 | std::chrono::steady_clock::time_point time0; 81 | }; 82 | 83 | 84 | #endif //FOURIER_FORCE_SENSOR_H 85 | -------------------------------------------------------------------------------- /src/hardware/IO/FourierHandle.cpp: -------------------------------------------------------------------------------- 1 | #include "FourierHandle.h" 2 | 3 | FourierHandle::FourierHandle(int sensor_can_node_ID): InputDevice(), 4 | sensorNodeID_(sensor_can_node_ID){ 5 | 6 | buttonValues_ = Eigen::VectorXd::Zero(4); 7 | } 8 | 9 | bool FourierHandle::configureMasterPDOs() { 10 | UNSIGNED16 dataSize[4] = {1, 1, 1, 1}; 11 | void *dataEntry[4] = {(void *)&rawData_[0], 12 | (void *)&rawData_[1], 13 | (void *)&rawData_[2], 14 | (void *)&rawData_[3],}; 15 | 16 | rpdo_ = new RPDO(0x180+sensorNodeID_, 0xff, dataEntry, dataSize, 4); 17 | 18 | return true; 19 | } 20 | 21 | void FourierHandle::updateInput() { 22 | 23 | for(int i = 0; i<4; i++){ 24 | buttonValues_[i] = rawData_[i]; 25 | } 26 | } 27 | 28 | Eigen::VectorXd& FourierHandle::getButtonValues() { 29 | return buttonValues_; 30 | } -------------------------------------------------------------------------------- /src/hardware/IO/FourierHandle.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file FourierHandle.h 3 | * \author Emek Baris Kucuktabak 4 | * \brief A class for the Fourier button handles 5 | * \version 0.1 6 | * \date 2021-04-09 7 | * 8 | * \copyright Copyright (c) 2021 9 | * 10 | */ 11 | 12 | #ifndef SRC_FOURIERHANDLE_H 13 | #define SRC_FOURIERHANDLE_H 14 | 15 | #include "InputDevice.h" 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | enum ButtonColor { 23 | RED = 0, 24 | BLUE = 1, 25 | YELLOW = 2, 26 | GREEN = 3 27 | }; 28 | 29 | class FourierHandle : public InputDevice { 30 | 31 | public: 32 | /** 33 | * Construct a new FourierHandle object 34 | * 35 | */ 36 | FourierHandle(int sensor_can_node_ID); 37 | 38 | /** 39 | * Configure Master (controller) side PDO for button value reading. 40 | * 41 | */ 42 | bool configureMasterPDOs(); 43 | 44 | /** 45 | * Updates the button readings from last updated PDO 46 | * 47 | */ 48 | void updateInput(); 49 | 50 | /** 51 | * Returns the lastest updated button reading. 52 | * 53 | */ 54 | Eigen::VectorXd& getButtonValues(); 55 | 56 | private: 57 | int sensorNodeID_; 58 | RPDO *rpdo_; 59 | INTEGER32 rawData_[4] = {0}; 60 | Eigen::VectorXd buttonValues_; // Values of red, blue, yellow, and green, respectively. if pressed 1, else 0 61 | 62 | }; 63 | 64 | 65 | #endif //SRC_FOURIERHANDLE_H 66 | -------------------------------------------------------------------------------- /src/hardware/IO/Joystick.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * 4 | * \file Joystick.h 5 | * \author Vincent Crocher (Mostly stollen from Jason White (https://gist.github.com/jasonwhite/)) 6 | * \version 0.1 7 | * \date 2020-07-02 8 | * \copyright Copyright (c) 2020 9 | * 10 | * \brief Joystick as Input device. 11 | * 12 | */ 13 | 14 | #ifndef JOYSTICK_H 15 | #define JOYSTICK_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | #include "InputDevice.h" 24 | 25 | 26 | /** 27 | * Current state of an axis. 28 | */ 29 | struct axis_state { 30 | short x, y; 31 | }; 32 | 33 | #define MAX_NB_STICKS 5 34 | #define STICK_MAX_VALUE 65535.0 35 | #define MAX_NB_BUTTONS 20 36 | 37 | /** 38 | * \brief Joystick support class. Mostly stollen from Jason White (https://gist.github.com/jasonwhite/) 39 | */ 40 | class Joystick : public InputDevice 41 | { 42 | public: 43 | /** 44 | * \brief Standard joystick on /dev/input/js#id# 45 | * \param id: the joystick id (#id#), default is 0 46 | * 47 | */ 48 | Joystick(int id=0); 49 | ~Joystick(); 50 | 51 | void updateInput(); 52 | 53 | bool isButtonPressed(int id) {return button[id];} //!< True if button currently pressed (at last call of updateInput()) 54 | int isButtonTransition(int id) {return button_transition[id];} //!< Return +1 if pressing transition detected, -1 if releasing one, 0 otherwise. Reseted every call of updateInput() ( every loop ) 55 | double getAxis(int axis_id) { 56 | int stick = axis_id / 2; 57 | if (axis_id % 2 == 0) 58 | return axes[stick].x/STICK_MAX_VALUE; 59 | else 60 | return axes[stick].y/STICK_MAX_VALUE; 61 | } 62 | 63 | /** 64 | * \brief Does nothing as there are none here 65 | * 66 | */ 67 | bool configureMasterPDOs(){return true;}; 68 | 69 | protected: 70 | private: 71 | bool initialised; 72 | char device[50]; 73 | int js; 74 | struct js_event event; 75 | struct axis_state axes[MAX_NB_STICKS] = {0}; 76 | size_t axis; 77 | bool button[MAX_NB_BUTTONS] = {false}; 78 | int8_t button_transition[MAX_NB_BUTTONS] = {0}; 79 | }; 80 | 81 | #endif // JOYSTICK_H 82 | -------------------------------------------------------------------------------- /src/hardware/platforms/M2/JointM2.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file JointM2.h 3 | * \author Vincent Crocher 4 | * \brief An M2 actuated joint 5 | * \version 0.2 6 | * \date 2020-12-09 7 | * 8 | * \copyright Copyright (c) 2020 9 | * 10 | */ 11 | #ifndef JOINTM2_H_INCLUDED 12 | #define JOINTM2_H_INCLUDED 13 | 14 | #include 15 | #include 16 | 17 | #include "Joint.h" 18 | #include "KincoDrive.h" 19 | 20 | /** 21 | * \brief M2 actuated joints, using Kinoc drives. 22 | * 23 | */ 24 | class JointM2 : public Joint { 25 | private: 26 | const short int sign; 27 | const double qMin, qMax, dqMin, dqMax, tauMin, tauMax; 28 | int encoderCounts = 10000; //Encoder counts per turn 29 | double reductionRatio = 166; 30 | 31 | double Ipeak = 45.0; //Kinco FD123 peak current 32 | double motorTorqueConstant = 0.132; //SMC60S-0020 motor torque constant 33 | 34 | double driveUnitToJointPosition(int driveValue) { return sign * driveValue * (2. * M_PI) / (double)encoderCounts / reductionRatio; }; 35 | int jointPositionToDriveUnit(double jointValue) { return sign * jointValue / (2. * M_PI) * (double)encoderCounts * reductionRatio; }; 36 | double driveUnitToJointVelocity(int driveValue) { return sign * driveValue * (2. * M_PI) / 60. / 512. / (double)encoderCounts * 1875 / reductionRatio; }; 37 | int jointVelocityToDriveUnit(double jointValue) { return sign * jointValue / (2. * M_PI) * 60. * 512. * (double)encoderCounts / 1875 * reductionRatio; }; 38 | double driveUnitToJointTorque(int driveValue) { return sign * driveValue / Ipeak / 1.414 * motorTorqueConstant * reductionRatio; }; 39 | int jointTorqueToDriveUnit(double jointValue) { return sign * jointValue * Ipeak * 1.414 / motorTorqueConstant / reductionRatio; }; 40 | 41 | /** 42 | * \brief motor drive position control profile paramaters, user defined. 43 | * 44 | */ 45 | motorProfile posControlMotorProfile{4000000, 240000, 240000}; 46 | 47 | public: 48 | JointM2(int jointID, double q_min, double q_max, short int sign_ = 1, double dq_min = 0, double dq_max = 0, double tau_min = 0, double tau_max = 0, KincoDrive *drive = NULL, const std::string& name=""); 49 | ~JointM2(); 50 | /** 51 | * \brief Cehck if current velocity and torque are within limits. 52 | * 53 | * \return OUTSIDE_LIMITS if outside the limits (!), SUCCESS otherwise 54 | */ 55 | setMovementReturnCode_t safetyCheck(); 56 | 57 | setMovementReturnCode_t setPosition(double qd); 58 | setMovementReturnCode_t setVelocity(double dqd); 59 | setMovementReturnCode_t setTorque(double taud); 60 | 61 | bool initNetwork(); 62 | }; 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /src/hardware/platforms/M2P/JointM2P.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file JointM2P.h 3 | * \author Vincent Crocher, Hao Yu, Zebin Huang 4 | * \brief An M2 M2Pro actuated joint 5 | * \version 0.1 6 | * \date 2024-05-14 7 | * 8 | * \copyright Copyright (c) 2024 - 2025 9 | * 10 | */ 11 | #ifndef JointM2P_H_INCLUDED 12 | #define JointM2P_H_INCLUDED 13 | 14 | #include 15 | #include 16 | 17 | #include "Joint.h" 18 | #include "KincoDrive.h" 19 | 20 | /** 21 | * \brief M2 actuated joints, using Kinoc drives. 22 | * 23 | */ 24 | class JointM2P : public Joint { 25 | private: 26 | const short int sign; 27 | const short int ratio; 28 | const double qMin, qMax, dqMin, dqMax, tauMin, tauMax; 29 | int encoderCounts = 65536; //Encoder counts per turn, M2 is 10000 30 | double reductionRatio = ratio; // for x axis, this is 30, for y axis, this is 50 31 | 32 | double Ipeak = 48.0; //Kinco FD124S peak current 33 | double motorTorqueConstant = 0.11; //SMC40S-0010-30MAK-5DSU motor torque constant 34 | 35 | double driveUnitToJointPosition(int driveValue) { return sign * driveValue * (2. * M_PI) / (double)encoderCounts / reductionRatio; }; 36 | int jointPositionToDriveUnit(double jointValue) { return sign * jointValue / (2. * M_PI) * (double)encoderCounts * reductionRatio; }; 37 | double driveUnitToJointVelocity(int driveValue) { return sign * driveValue * (2. * M_PI) / 60. / 512. / (double)encoderCounts * 1875 / reductionRatio; }; 38 | int jointVelocityToDriveUnit(double jointValue) { return sign * jointValue / (2. * M_PI) * 60. * 512. * (double)encoderCounts / 1875 * reductionRatio; }; 39 | double driveUnitToJointTorque(int driveValue) { return sign * driveValue / Ipeak / 1.414 * motorTorqueConstant * reductionRatio; }; 40 | int jointTorqueToDriveUnit(double jointValue) { return sign * jointValue * Ipeak * 1.414 / motorTorqueConstant / reductionRatio; }; 41 | 42 | /** 43 | * \brief motor drive position control profile paramaters, user defined. 44 | * 45 | */ 46 | motorProfile posControlMotorProfile{4000000, 240000, 240000}; 47 | 48 | public: 49 | JointM2P(int jointID, int ratio, double q_min, double q_max, short int sign_ = 1, double dq_min = 0, double dq_max = 0, double tau_min = 0, double tau_max = 0, KincoDrive *drive = NULL, const std::string& name=""); 50 | ~JointM2P(); 51 | /** 52 | * \brief Cehck if current velocity and torque are within limits. 53 | * 54 | * \return OUTSIDE_LIMITS if outside the limits (!), SUCCESS otherwise 55 | */ 56 | setMovementReturnCode_t safetyCheck(); 57 | 58 | setMovementReturnCode_t setPosition(double qd); 59 | setMovementReturnCode_t setVelocity(double dqd); 60 | setMovementReturnCode_t setTorque(double taud); 61 | 62 | bool initNetwork(); 63 | }; 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /src/hardware/platforms/M3/JointM3.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file JointM3.h 3 | * \author Vincent Crocher 4 | * \brief An M3 actuated joint 5 | * \version 0.2 6 | * \date 2020-07-27 7 | * 8 | * \copyright Copyright (c) 2020 9 | * 10 | */ 11 | #ifndef JOINTM3_H_INCLUDED 12 | #define JOINTM3_H_INCLUDED 13 | 14 | #include 15 | #include 16 | 17 | #include "Joint.h" 18 | #include "KincoDrive.h" 19 | 20 | /** 21 | * \brief M3 actuated joints, using Kinoc drives. 22 | * 23 | */ 24 | class JointM3 : public Joint { 25 | private: 26 | const short int sign; 27 | const double qMin, qMax, dqMin, dqMax, tauMin, tauMax; 28 | int encoderCounts = 10000; //Encoder counts per turn 29 | double reductionRatio = 22.; 30 | 31 | double Ipeak; //!< Drive max current (used in troque conversion) 32 | double motorTorqueConstant; //!< Motor torque constant 33 | 34 | double driveUnitToJointPosition(int driveValue) { return sign * driveValue * (2. * M_PI) / (double)encoderCounts / reductionRatio; }; 35 | int jointPositionToDriveUnit(double jointValue) { return sign * jointValue / (2. * M_PI) * (double)encoderCounts * reductionRatio; }; 36 | double driveUnitToJointVelocity(int driveValue) { return sign * driveValue * (2. * M_PI) / 60. / 512. / (double)encoderCounts * 1875 / reductionRatio; }; 37 | int jointVelocityToDriveUnit(double jointValue) { return sign * jointValue / (2. * M_PI) * 60. * 512. * (double)encoderCounts / 1875 * reductionRatio; }; 38 | double driveUnitToJointTorque(int driveValue) { return sign * driveValue / Ipeak / 1.414 * motorTorqueConstant * reductionRatio; }; 39 | int jointTorqueToDriveUnit(double jointValue) { return sign * jointValue * Ipeak * 1.414 / motorTorqueConstant / reductionRatio; }; 40 | 41 | /** 42 | * \brief motor drive position control profile paramaters, user defined. 43 | * 44 | */ 45 | motorProfile posControlMotorProfile{4000000, 240000, 240000}; 46 | 47 | public: 48 | JointM3(int jointID, double q_min, double q_max, short int sign_ = 1, double dq_min = 0, double dq_max = 0, double tau_min = 0, double tau_max = 0, double i_peak = 45.0 /*Kinco FD123 peak current*/ , double motorTorqueConstant_ = 0.132 /*SMC series constant*/, KincoDrive *drive = NULL, const std::string& name=""); 49 | ~JointM3(); 50 | /** 51 | * \brief Cehck if current velocity and torque are within limits. 52 | * 53 | * \return OUTSIDE_LIMITS if outside the limits (!), SUCCESS otherwise 54 | */ 55 | setMovementReturnCode_t safetyCheck(); 56 | 57 | setMovementReturnCode_t setPosition(double qd); 58 | setMovementReturnCode_t setVelocity(double dqd); 59 | setMovementReturnCode_t setTorque(double taud); 60 | 61 | bool initNetwork(); 62 | }; 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /src/hardware/platforms/X2/X2Joint.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file X2Joint.cpp 3 | * @author Justin Fong 4 | * @brief 5 | * @version 0.1 6 | * @date 2020-04-09 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | #include "X2Joint.h" 12 | 13 | #include 14 | 15 | 16 | X2Joint::X2Joint(int jointID, double jointMin, double jointMax, JointDrivePairs jdp, Drive *drive) : Joint(jointID, jointMin, jointMax, drive) { 17 | spdlog::debug("Joint Created, JOINT ID: {}", this->id); 18 | // Do nothing else 19 | JDSlope = (jdp.drivePosB - jdp.drivePosA) / (jdp.jointPosB - jdp.jointPosA); 20 | JDIntercept = jdp.drivePosA - JDSlope * jdp.jointPosA; 21 | } 22 | 23 | int X2Joint::jointPositionToDriveUnit(double jointPosition) { 24 | return JDSlope * jointPosition + JDIntercept; 25 | } 26 | 27 | double X2Joint::driveUnitToJointPosition(int driveValue) { 28 | return (driveValue - JDIntercept) / JDSlope; 29 | } 30 | 31 | int X2Joint::jointVelocityToDriveUnit(double jointVelocity) { 32 | return (JDSlope * jointVelocity) * 10; 33 | } 34 | 35 | double X2Joint::driveUnitToJointVelocity(int driveValue) { 36 | return ((driveValue) / (JDSlope * 10)); 37 | } 38 | 39 | int X2Joint::jointTorqueToDriveUnit(double jointTorque) { 40 | int signSlope = (JDSlope > 0) ? 1 : -1; 41 | return signSlope * jointTorque / (MOTOR_RATED_TORQUE * REDUCTION_RATIO / 1000.0); 42 | } 43 | 44 | double X2Joint::driveUnitToJointTorque(int driveValue) { 45 | int signSlope = (JDSlope > 0) ? 1 : -1; 46 | return signSlope * driveValue * (MOTOR_RATED_TORQUE * REDUCTION_RATIO / 1000.0); 47 | } 48 | 49 | bool X2Joint::initNetwork() { 50 | spdlog::debug("Joint::initNetwork()"); 51 | drive->start(); 52 | if (drive->initPDOs()) { 53 | return true; 54 | } else { 55 | return false; 56 | } 57 | } 58 | double X2Joint::getPosition() { 59 | return position; 60 | } 61 | double X2Joint::getVelocity() { 62 | return velocity; 63 | } 64 | double X2Joint::getTorque() { 65 | return torque; 66 | } 67 | 68 | void X2Joint::setPositionOffset(double offset) { 69 | ((CopleyDrive *)drive)->setPositionOffset(jointPositionToDriveUnit(-offset)); 70 | } 71 | --------------------------------------------------------------------------------