├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── Notes.md ├── README.md ├── TODO.md ├── cmgmp ├── CMakeLists.txt ├── contacts │ ├── contact_constraints.cpp │ ├── contact_constraints.h │ ├── contact_kinematics.cpp │ ├── contact_kinematics.h │ ├── contact_mode_enumeration.cpp │ ├── contact_mode_enumeration.h │ ├── contact_utils.cpp │ └── contact_utils.h ├── dart_utils │ ├── dart_utils.cpp │ └── dart_utils.h ├── external │ ├── CMakeLists.txt │ ├── eigen3 │ │ ├── Eigen │ │ │ ├── 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 │ │ │ │ ├── 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 │ │ │ │ ├── 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 │ │ │ │ │ │ ├── ConjHelper.h │ │ │ │ │ │ └── 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 │ │ │ │ │ ├── MKL_support.h │ │ │ │ │ ├── Macros.h │ │ │ │ │ ├── Memory.h │ │ │ │ │ ├── Meta.h │ │ │ │ │ ├── NonMPL2.h │ │ │ │ │ ├── ReenableStupidWarnings.h │ │ │ │ │ ├── StaticAssert.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 │ │ │ │ ├── MatrixCwiseBinaryOps.h │ │ │ │ └── MatrixCwiseUnaryOps.h │ │ ├── signature_of_eigen3_matrix_library │ │ └── unsupported │ │ │ └── Eigen │ │ │ ├── AdolcForward │ │ │ ├── AlignedVector3 │ │ │ ├── ArpackSupport │ │ │ ├── AutoDiff │ │ │ ├── BVH │ │ │ ├── CXX11 │ │ │ ├── Tensor │ │ │ ├── TensorSymmetry │ │ │ ├── ThreadPool │ │ │ └── src │ │ │ │ ├── Tensor │ │ │ │ ├── Tensor.h │ │ │ │ ├── TensorArgMax.h │ │ │ │ ├── TensorAssign.h │ │ │ │ ├── TensorBase.h │ │ │ │ ├── TensorBroadcasting.h │ │ │ │ ├── TensorChipping.h │ │ │ │ ├── TensorConcatenation.h │ │ │ │ ├── TensorContraction.h │ │ │ │ ├── TensorContractionBlocking.h │ │ │ │ ├── TensorContractionCuda.h │ │ │ │ ├── TensorContractionMapper.h │ │ │ │ ├── TensorContractionThreadPool.h │ │ │ │ ├── TensorConversion.h │ │ │ │ ├── TensorConvolution.h │ │ │ │ ├── TensorCostModel.h │ │ │ │ ├── TensorCustomOp.h │ │ │ │ ├── TensorDevice.h │ │ │ │ ├── TensorDeviceCuda.h │ │ │ │ ├── TensorDeviceDefault.h │ │ │ │ ├── TensorDeviceSycl.h │ │ │ │ ├── TensorDeviceThreadPool.h │ │ │ │ ├── TensorDimensionList.h │ │ │ │ ├── TensorDimensions.h │ │ │ │ ├── TensorEvalTo.h │ │ │ │ ├── TensorEvaluator.h │ │ │ │ ├── TensorExecutor.h │ │ │ │ ├── TensorExpr.h │ │ │ │ ├── TensorFFT.h │ │ │ │ ├── TensorFixedSize.h │ │ │ │ ├── TensorForcedEval.h │ │ │ │ ├── TensorForwardDeclarations.h │ │ │ │ ├── TensorFunctors.h │ │ │ │ ├── TensorGenerator.h │ │ │ │ ├── TensorGlobalFunctions.h │ │ │ │ ├── TensorIO.h │ │ │ │ ├── TensorImagePatch.h │ │ │ │ ├── TensorIndexList.h │ │ │ │ ├── TensorInflation.h │ │ │ │ ├── TensorInitializer.h │ │ │ │ ├── TensorIntDiv.h │ │ │ │ ├── TensorLayoutSwap.h │ │ │ │ ├── TensorMacros.h │ │ │ │ ├── TensorMap.h │ │ │ │ ├── TensorMeta.h │ │ │ │ ├── TensorMorphing.h │ │ │ │ ├── TensorPadding.h │ │ │ │ ├── TensorPatch.h │ │ │ │ ├── TensorRandom.h │ │ │ │ ├── TensorReduction.h │ │ │ │ ├── TensorReductionCuda.h │ │ │ │ ├── TensorReductionSycl.h │ │ │ │ ├── TensorRef.h │ │ │ │ ├── TensorReverse.h │ │ │ │ ├── TensorScan.h │ │ │ │ ├── TensorShuffling.h │ │ │ │ ├── TensorStorage.h │ │ │ │ ├── TensorStriding.h │ │ │ │ ├── TensorSycl.h │ │ │ │ ├── TensorSyclConvertToDeviceExpression.h │ │ │ │ ├── TensorSyclExprConstructor.h │ │ │ │ ├── TensorSyclExtractAccessor.h │ │ │ │ ├── TensorSyclExtractFunctors.h │ │ │ │ ├── TensorSyclLeafCount.h │ │ │ │ ├── TensorSyclPlaceHolderExpr.h │ │ │ │ ├── TensorSyclRun.h │ │ │ │ ├── TensorSyclTuple.h │ │ │ │ ├── TensorTraits.h │ │ │ │ ├── TensorUInt128.h │ │ │ │ └── TensorVolumePatch.h │ │ │ │ ├── TensorSymmetry │ │ │ │ ├── DynamicSymmetry.h │ │ │ │ ├── StaticSymmetry.h │ │ │ │ ├── Symmetry.h │ │ │ │ └── util │ │ │ │ │ └── TemplateGroupTheory.h │ │ │ │ ├── ThreadPool │ │ │ │ ├── EventCount.h │ │ │ │ ├── NonBlockingThreadPool.h │ │ │ │ ├── RunQueue.h │ │ │ │ ├── SimpleThreadPool.h │ │ │ │ ├── ThreadEnvironment.h │ │ │ │ ├── ThreadLocal.h │ │ │ │ ├── ThreadPoolInterface.h │ │ │ │ └── ThreadYield.h │ │ │ │ └── util │ │ │ │ ├── CXX11Meta.h │ │ │ │ ├── CXX11Workarounds.h │ │ │ │ ├── EmulateArray.h │ │ │ │ ├── EmulateCXX11Meta.h │ │ │ │ └── MaxSizeVector.h │ │ │ ├── EulerAngles │ │ │ ├── FFT │ │ │ ├── IterativeSolvers │ │ │ ├── KroneckerProduct │ │ │ ├── LevenbergMarquardt │ │ │ ├── MPRealSupport │ │ │ ├── MatrixFunctions │ │ │ ├── MoreVectorization │ │ │ ├── NonLinearOptimization │ │ │ ├── NumericalDiff │ │ │ ├── OpenGLSupport │ │ │ ├── Polynomials │ │ │ ├── Skyline │ │ │ ├── SparseExtra │ │ │ ├── SpecialFunctions │ │ │ ├── Splines │ │ │ └── src │ │ │ ├── AutoDiff │ │ │ ├── AutoDiffJacobian.h │ │ │ ├── AutoDiffScalar.h │ │ │ └── AutoDiffVector.h │ │ │ ├── BVH │ │ │ ├── BVAlgorithms.h │ │ │ └── KdBVH.h │ │ │ ├── Eigenvalues │ │ │ └── ArpackSelfAdjointEigenSolver.h │ │ │ ├── EulerAngles │ │ │ ├── EulerAngles.h │ │ │ └── EulerSystem.h │ │ │ ├── FFT │ │ │ ├── ei_fftw_impl.h │ │ │ └── ei_kissfft_impl.h │ │ │ ├── IterativeSolvers │ │ │ ├── ConstrainedConjGrad.h │ │ │ ├── DGMRES.h │ │ │ ├── GMRES.h │ │ │ ├── IncompleteLU.h │ │ │ ├── IterationController.h │ │ │ ├── MINRES.h │ │ │ └── Scaling.h │ │ │ ├── KroneckerProduct │ │ │ └── KroneckerTensorProduct.h │ │ │ ├── LevenbergMarquardt │ │ │ ├── LMcovar.h │ │ │ ├── LMonestep.h │ │ │ ├── LMpar.h │ │ │ ├── LMqrsolv.h │ │ │ └── LevenbergMarquardt.h │ │ │ ├── MatrixFunctions │ │ │ ├── MatrixExponential.h │ │ │ ├── MatrixFunction.h │ │ │ ├── MatrixLogarithm.h │ │ │ ├── MatrixPower.h │ │ │ ├── MatrixSquareRoot.h │ │ │ └── StemFunction.h │ │ │ ├── MoreVectorization │ │ │ └── MathFunctions.h │ │ │ ├── NonLinearOptimization │ │ │ ├── HybridNonLinearSolver.h │ │ │ ├── LevenbergMarquardt.h │ │ │ ├── chkder.h │ │ │ ├── covar.h │ │ │ ├── dogleg.h │ │ │ ├── fdjac1.h │ │ │ ├── lmpar.h │ │ │ ├── qrsolv.h │ │ │ ├── r1mpyq.h │ │ │ ├── r1updt.h │ │ │ └── rwupdt.h │ │ │ ├── NumericalDiff │ │ │ └── NumericalDiff.h │ │ │ ├── Polynomials │ │ │ ├── Companion.h │ │ │ ├── PolynomialSolver.h │ │ │ └── PolynomialUtils.h │ │ │ ├── Skyline │ │ │ ├── SkylineInplaceLU.h │ │ │ ├── SkylineMatrix.h │ │ │ ├── SkylineMatrixBase.h │ │ │ ├── SkylineProduct.h │ │ │ ├── SkylineStorage.h │ │ │ └── SkylineUtil.h │ │ │ ├── SparseExtra │ │ │ ├── BlockOfDynamicSparseMatrix.h │ │ │ ├── BlockSparseMatrix.h │ │ │ ├── DynamicSparseMatrix.h │ │ │ ├── MarketIO.h │ │ │ ├── MatrixMarketIterator.h │ │ │ └── RandomSetter.h │ │ │ ├── SpecialFunctions │ │ │ ├── SpecialFunctionsArrayAPI.h │ │ │ ├── SpecialFunctionsFunctors.h │ │ │ ├── SpecialFunctionsHalf.h │ │ │ ├── SpecialFunctionsImpl.h │ │ │ ├── SpecialFunctionsPacketMath.h │ │ │ └── arch │ │ │ │ └── CUDA │ │ │ │ └── CudaSpecialFunctions.h │ │ │ └── Splines │ │ │ ├── Spline.h │ │ │ ├── SplineFitting.h │ │ │ └── SplineFwd.h │ └── modus │ │ ├── CMakeLists.txt │ │ ├── apps │ │ ├── CMakeLists.txt │ │ └── hesse_diagram.py │ │ ├── bench │ │ ├── CMakeLists.txt │ │ ├── bench_arrangements.cpp │ │ ├── bench_subspace.cpp │ │ └── profile_arrangements.cpp │ │ ├── cmake │ │ ├── FindGperftools.cmake │ │ └── FindQhull.cmake │ │ ├── data │ │ └── test │ │ │ ├── box-1.npz │ │ │ ├── box-2.npz │ │ │ ├── box-3.npz │ │ │ ├── box-4.npz │ │ │ ├── box-5.npz │ │ │ ├── cs_modes │ │ │ ├── box-1.npz │ │ │ ├── box-2.npz │ │ │ ├── box-3.npz │ │ │ ├── box-4.npz │ │ │ ├── box-5.npz │ │ │ └── box-box-1.npz │ │ │ ├── gen_test_data.py │ │ │ ├── modus.test_logging.xianyi-XPS-13-9350.xianyi.log.INFO.20210205-122151.16349 │ │ │ ├── modus.test_logging.xianyi-XPS-13-9350.xianyi.log.INFO.20210205-163624.26963 │ │ │ └── ss_modes │ │ │ ├── box-1.npz │ │ │ ├── box-2.npz │ │ │ ├── box-3.npz │ │ │ ├── box-4.npz │ │ │ └── box-5.npz │ │ ├── include │ │ └── modus │ │ │ ├── collision │ │ │ ├── collision.hpp │ │ │ └── contact.hpp │ │ │ ├── common │ │ │ ├── assert.hpp │ │ │ ├── backward.hpp │ │ │ ├── convex_hull.hpp │ │ │ ├── eigen.hpp │ │ │ ├── linear_algebra-impl.hpp │ │ │ ├── linear_algebra.hpp │ │ │ ├── logging.hpp │ │ │ ├── memory.hpp │ │ │ └── serialization.hpp │ │ │ ├── dynamics │ │ │ ├── actuator_dynamics.hpp │ │ │ ├── coefficient_of_friction.hpp │ │ │ ├── contact_dynamics.hpp │ │ │ ├── continuous_dynamics.hpp │ │ │ ├── dynamics.hpp │ │ │ ├── gravito_intertial_dynamics.hpp │ │ │ └── mass_matrix.hpp │ │ │ ├── examples │ │ │ ├── basic_collision.hpp │ │ │ ├── box.hpp │ │ │ ├── plane.hpp │ │ │ ├── se3_state.hpp │ │ │ └── sphere.hpp │ │ │ ├── geometry │ │ │ ├── bounds.hpp │ │ │ ├── box.hpp │ │ │ ├── cylinder.hpp │ │ │ ├── distance.hpp │ │ │ ├── edge.hpp │ │ │ ├── face.hpp │ │ │ ├── geometry.hpp │ │ │ ├── halfedge.hpp │ │ │ ├── halfedgemesh.hpp │ │ │ ├── icosphere.hpp │ │ │ ├── plane.hpp │ │ │ ├── ray.hpp │ │ │ └── vertex.hpp │ │ │ ├── graphics │ │ │ ├── glad.hpp │ │ │ ├── khrplatform.hpp │ │ │ └── opengl.hpp │ │ │ ├── integration │ │ │ ├── forward_euler.hpp │ │ │ └── integration.hpp │ │ │ ├── kinematics │ │ │ ├── contact_constraint.hpp │ │ │ ├── contact_kinematics.hpp │ │ │ ├── jacobian.hpp │ │ │ ├── jacobian_body.hpp │ │ │ ├── jacobian_floating.hpp │ │ │ ├── jacobian_geometric.hpp │ │ │ ├── jacobian_spatial.hpp │ │ │ └── kinematics.hpp │ │ │ ├── modes │ │ │ ├── constraints.hpp │ │ │ ├── enumerate.hpp │ │ │ ├── geometry │ │ │ │ ├── arrangements.hpp │ │ │ │ ├── convex_hull.hpp │ │ │ │ ├── halfspace_intersection.hpp │ │ │ │ ├── hyperplane.hpp │ │ │ │ ├── incidence_graph.hpp │ │ │ │ └── interior_point.hpp │ │ │ ├── graph.hpp │ │ │ ├── graph │ │ │ │ ├── contact_mode_graph.hpp │ │ │ │ ├── csmode_graph.hpp │ │ │ │ ├── incidence_graph.hpp │ │ │ │ ├── nested_graph.hpp │ │ │ │ ├── product_graph.hpp │ │ │ │ └── ssmode_graph.hpp │ │ │ ├── modus.hpp │ │ │ ├── preprocess.hpp │ │ │ └── preprocess │ │ │ │ ├── csmode_preprocessor.hpp │ │ │ │ ├── matroid_preprocessor.hpp │ │ │ │ ├── nullspace_preprocessor.hpp │ │ │ │ └── ssmode_preprocessor.hpp │ │ │ ├── qpcc │ │ │ ├── bounds.hpp │ │ │ ├── cost.hpp │ │ │ ├── qp.hpp │ │ │ ├── qpOASES.hpp │ │ │ ├── qpcc.hpp │ │ │ ├── solver.hpp │ │ │ └── solvers │ │ │ │ ├── MICP.hpp │ │ │ │ ├── SNOPT.hpp │ │ │ │ ├── adjacent_modes.hpp │ │ │ │ └── full_modes.hpp │ │ │ └── system │ │ │ ├── aspect.hpp │ │ │ ├── body.hpp │ │ │ ├── composite.hpp │ │ │ ├── detail │ │ │ └── composite.hpp │ │ │ ├── input.hpp │ │ │ ├── link.hpp │ │ │ ├── state.hpp │ │ │ ├── system.hpp │ │ │ └── transform.hpp │ │ └── src │ │ ├── modus_common │ │ ├── CMakeLists.txt │ │ ├── assert.cpp │ │ ├── backward.cpp │ │ ├── convex_hull.cpp │ │ ├── linear_algebra.cpp │ │ ├── logging.cpp │ │ └── serialization.cpp │ │ ├── modus_geometry │ │ ├── CMakeLists.txt │ │ ├── bounds.cpp │ │ ├── box.cpp │ │ ├── distance.cpp │ │ ├── edge.cpp │ │ ├── face.cpp │ │ ├── glad.cpp │ │ ├── halfedge.cpp │ │ ├── halfedgemesh.cpp │ │ ├── opengl.cpp │ │ ├── plane.cpp │ │ ├── rectangle.cpp │ │ ├── transforms.cpp │ │ └── vertex.cpp │ │ ├── modus_kinematics │ │ ├── CMakeLists.txt │ │ ├── contact_constraint.cpp │ │ ├── contact_kinematics.cpp │ │ ├── jacobian_body.cpp │ │ ├── jacobian_floating.cpp │ │ ├── jacobian_geometric.cpp │ │ ├── jacobian_spatial.cpp │ │ └── kinematics.cpp │ │ ├── modus_modes │ │ ├── CMakeLists.txt │ │ ├── constraints.cpp │ │ ├── enumerate_modes.cpp │ │ ├── geometry │ │ │ ├── arrangements.cpp │ │ │ ├── convex_hull.cpp │ │ │ ├── halfspace_intersection.cpp │ │ │ ├── incidence_graph.cpp │ │ │ └── interior_point.cpp │ │ ├── preprocess.cpp │ │ └── preprocess │ │ │ └── matroid_preprocessor.cpp │ │ └── modus_system │ │ ├── CMakeLists.txt │ │ ├── body.cpp │ │ ├── link.cpp │ │ ├── system.cpp │ │ └── transform.cpp ├── manipulators │ ├── DartDDHand.cpp │ ├── DartDDHand.h │ ├── DartDDHandFreeY.cpp │ ├── DartDDHandFreeY.h │ ├── DartKR5.cpp │ ├── DartKR5.h │ ├── DartManipulatorTemplate.h │ ├── DartParallelJawGripper.cpp │ ├── DartParallelJawGripper.h │ ├── DartPointManipulator.cpp │ ├── DartPointManipulator.h │ ├── DartURDFManipulatorTemplate.h │ ├── ManipulatorTemplate.h │ └── ddhand │ │ ├── handik.h │ │ ├── two_link_kinematics.cpp │ │ └── two_link_kinematics.h ├── search │ ├── planner_option.h │ ├── rrtplanner.cpp │ ├── rrtplanner.h │ ├── tree.cpp │ └── tree.h ├── search_hierarchical │ ├── hierarchical_planner.cpp │ ├── hierarchical_planner.h │ ├── hierarchical_tree.cpp │ ├── hierarchical_tree.h │ └── todo.md ├── utilities │ ├── eiquadprog.hpp │ ├── sample.cpp │ ├── sample.h │ ├── sample_grasp.cpp │ ├── sample_grasp.h │ ├── utilities.cpp │ └── utilities.h └── worlds │ ├── DartWorld.cpp │ ├── DartWorld.h │ ├── PlanningWorld.cpp │ ├── PlanningWorld.h │ └── WorldTemplate.h ├── data ├── grasp_sampling │ ├── BIG_SCREW.csv │ ├── BIG_SCREW_dense.csv │ ├── BIG_SCREW_grasps.csv │ ├── ball_compound_r_1.csv │ ├── ball_radius_1.csv │ ├── bookshelf_surface_points.csv │ ├── box_halflength_1.csv │ ├── box_halflength_1_ddhand.csv │ ├── box_halflength_1_grasps.csv │ ├── box_lx_1_ly_4_lz_4.csv │ ├── box_lx_1_ly_4_lz_4_ddhand.csv │ ├── box_lx_2_ly_4_lz_2.csv │ ├── cylindar_l_2_r_1.csv │ ├── peg_out_of_hole_surface_points.csv │ ├── russian_block_l_2.csv │ ├── russian_block_l_2_grasps.csv │ ├── small_bumper_scale_200.csv │ ├── socket_screw.csv │ ├── tetrahedron_1.csv │ └── torus_tri.csv ├── meshes │ ├── stl │ │ ├── ball_compound_r_1.stl │ │ ├── ball_radius_1.stl │ │ ├── box_halflength_1.stl │ │ ├── box_lx_1_ly_4_lz_4.stl │ │ ├── box_lx_2_ly_4_lz_2.stl │ │ ├── box_lx_2_ly_4_lz_4.stl │ │ ├── cylindar_l_2_r_1.stl │ │ ├── russian_block_l_2.stl │ │ ├── screw.stl │ │ └── tetrahedron_1.stl │ └── stl_scale_50 │ │ ├── BIG_SCREW.stl │ │ ├── ball_compound_r_1.stl │ │ ├── ball_radius_1.stl │ │ ├── box_halflength_1.stl │ │ ├── box_lx_1_ly_4_lz_4.stl │ │ ├── box_lx_2_ly_4_lz_2.stl │ │ ├── box_lx_2_ly_4_lz_4.stl │ │ ├── cylindar_l_2_r_1.stl │ │ ├── russian_block_l_2.stl │ │ ├── small_bumper_scale_200.stl │ │ ├── small_bumper_scale_2000.stl │ │ ├── small_bumper_scale_5000.stl │ │ ├── socket_screw.stl │ │ ├── tetrahedron_1.stl │ │ └── torus_tri.stl ├── robot_workspace │ ├── KR5_workspace.csv │ └── KR5_workspace_convhull.csv └── urdf │ └── KR5 │ ├── KR5 sixx R650.urdf │ ├── ground.urdf │ └── meshes │ ├── base_link.STL │ ├── bicep.STL │ ├── elbow.STL │ ├── forearm.STL │ ├── palm.STL │ ├── shoulder.STL │ └── wrist.STL ├── examples ├── CMakeLists.txt ├── KR5.cpp ├── bookshelf.cpp ├── ddhand_bookshelf.cpp ├── ddhand_flip.cpp ├── ddhand_slideup.cpp ├── flip_cube.cpp ├── peg_out_hole.cpp ├── pick_card.cpp └── push_along_walls.cpp └── tests ├── CMakeLists.txt ├── test_KR5.cpp ├── test_contact_mode.cpp ├── test_dartworld.cpp ├── test_ddhand.cpp └── test_utils.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | 35 | CMakeLists.txt.user 36 | CMakeCache.txt 37 | CMakeFiles 38 | CMakeScripts 39 | Testing 40 | Makefile 41 | cmake_install.cmake 42 | install_manifest.txt 43 | compile_commands.json 44 | CTestTestfile.cmake 45 | _deps 46 | 47 | 48 | [Bb][Uu][Ii][Ll][Dd]/ 49 | .vscode/settings.json -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmgmp/external/dartsim"] 2 | path = cmgmp/external/dartsim 3 | url = https://github.com/dartsim/dart.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.3) 2 | 3 | project(CMGMP) 4 | 5 | # test if we can find bullet through find_package 6 | find_package(Bullet COMPONENTS BulletMath BulletCollision MODULE QUIET) 7 | if (BULLET_FOUND) 8 | message("find bullet") 9 | else() 10 | message("cannot find bullet") 11 | endif() 12 | 13 | add_definitions(-DSRC_DIR="${PROJECT_SOURCE_DIR}") 14 | 15 | add_subdirectory(cmgmp) 16 | add_subdirectory(examples) 17 | add_subdirectory(tests) 18 | 19 | -------------------------------------------------------------------------------- /Notes.md: -------------------------------------------------------------------------------- 1 | Collision detection output is different for different collision detector in dart! You need different constraint projection if you switch from bullet collision detection to dart collision detection!!! -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | The following build process is tested on a clean Ubuntu 20.04 system. 2 | 3 | Install dependent libraries 4 | ``` 5 | sudo apt-get install build-essential cmake pkg-config git 6 | 7 | sudo apt-get install libglpk-dev libeigen3-dev libassimp-dev libccd-dev libfcl-dev libboost-regex-dev libboost-system-dev libopenscenegraph-dev libbullet-dev libtinyxml2-dev liburdfdom-dev libxi-dev libxmu-dev freeglut3-dev 8 | 9 | ``` 10 | install qhull 11 | 12 | ``` 13 | git clone https://github.com/qhull/qhull.git 14 | cd qhull 15 | cd build 16 | cmake .. 17 | make 18 | ctest 19 | sudo make install 20 | ``` 21 | 22 | Update the external libraries 23 | ``` 24 | git submodule update --init --recursive --progress 25 | ``` 26 | 27 | Use cmake to build this project in the project root folder 28 | 29 | ``` 30 | mkdir build 31 | cd build 32 | cmake .. 33 | make 34 | ``` 35 | 36 | Check out examples in the build folder 37 | 38 | ``` 39 | ./examples/[name of the example] 40 | ``` 41 | -------------------------------------------------------------------------------- /TODO.md: -------------------------------------------------------------------------------- 1 | [x] modus, get rid off ortools 2 | [] Test installation 3 | [] Test bullet and examples 4 | [] Move other examples to this repo 5 | 6 | -------------------------------------------------------------------------------- /cmgmp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(external) 2 | 3 | find_library(GLPKLIB glpk REQUIRED) 4 | 5 | set(cmgmp_required_libraries 6 | ${GLPKLIB} 7 | ) 8 | 9 | set(modus_libs modus_common modus_modes) 10 | 11 | set(dart_libs dart dart-utils-urdf dart-gui dart-collision-bullet) 12 | 13 | add_definitions(-DSRC_DIR="${PROJECT_SOURCE_DIR}") 14 | 15 | set(cmgmp_include_dirs 16 | "${PROJECT_SOURCE_DIR}" 17 | "${PROJECT_SOURCE_DIR}/cmgmp/external/eigen3" 18 | "${PROJECT_BINARY_DIR}/cmgmp/external/modus/include" 19 | "${PROJECT_BINARY_DIR}/cmgmp/external/dartsim" 20 | ) 21 | 22 | add_library(cmgmp-utils 23 | utilities/sample.cpp 24 | utilities/utilities.cpp 25 | utilities/sample_grasp.cpp ) 26 | 27 | target_include_directories(cmgmp-utils PRIVATE ${cmgmp_include_dirs}) 28 | target_link_libraries(cmgmp-utils PRIVATE ${cmgmp_required_libraries}) 29 | 30 | add_library(cmgmp-contact 31 | contacts/contact_kinematics.cpp 32 | contacts/contact_constraints.cpp 33 | contacts/contact_mode_enumeration.cpp 34 | contacts/contact_utils.cpp 35 | ) 36 | 37 | target_include_directories(cmgmp-contact PRIVATE ${cmgmp_include_dirs}) 38 | target_link_libraries(cmgmp-contact PRIVATE cmgmp-utils ${cmgmp_required_libraries} ${modus_libs}) 39 | 40 | add_library(cmgmp-world 41 | worlds/DartWorld.cpp 42 | worlds/PlanningWorld.cpp 43 | manipulators/DartPointManipulator.cpp 44 | # manipulators/DartParallelJawGripper.cpp 45 | manipulators/ddhand/two_link_kinematics.cpp 46 | manipulators/DartDDHandFreeY.cpp 47 | manipulators/DartDDHand.cpp 48 | manipulators/DartKR5.cpp 49 | dart_utils/dart_utils.cpp 50 | ) 51 | 52 | target_include_directories(cmgmp-world PRIVATE ${cmgmp_include_dirs}) 53 | target_link_libraries(cmgmp-world PRIVATE cmgmp-utils cmgmp-contact ${dart_libs} ${modus_libs}) 54 | 55 | add_library(cmgmp-search 56 | search/rrtplanner.cpp 57 | search/tree.cpp) 58 | 59 | target_include_directories(cmgmp-search PRIVATE ${cmgmp_include_dirs}) 60 | target_link_libraries(cmgmp-search PRIVATE cmgmp-utils cmgmp-contact cmgmp-world ${cmgmp_required_libraries} ${dart_libs} ${modus_libs}) 61 | -------------------------------------------------------------------------------- /cmgmp/contacts/contact_kinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | #include "../utilities/utilities.h" 4 | #endif 5 | 6 | #ifndef SAMPLE_H 7 | #define SAMPLE_H 8 | #include "../utilities/sample.h" 9 | #endif 10 | 11 | #include 12 | 13 | typedef Eigen::Matrix Vector7d; 14 | 15 | 16 | 17 | Matrix6d contact_jacobian(const Vector3d &position, const Vector3d &normal); 18 | 19 | Matrix4d pose2SE3(const Vector7d& x); 20 | Vector7d SE32pose(const Matrix4d& T); 21 | Vector6d compute_rbvel_body(const Vector7d& x, const Vector7d& x_goal); 22 | Vector6d compute_rbvel_spatial(const Vector7d& x, const Vector7d& x_goal); 23 | 24 | -------------------------------------------------------------------------------- /cmgmp/contacts/contact_mode_enumeration.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef UTILS_H 3 | #define UTILS_H 4 | #include "../utilities/utilities.h" 5 | #endif 6 | 7 | void cs_mode_enumeration(const MatrixXd& A, std::vector* cs_modes); 8 | 9 | void all_mode_enumeration(const MatrixXd& A, const MatrixXd& T, std::vector* all_modes); 10 | 11 | void ss_mode_enumeration(const MatrixXd& A, const MatrixXd& T, VectorXi cs_mode, std::vector* ss_modes); -------------------------------------------------------------------------------- /cmgmp/contacts/contact_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "contact_utils.h" 2 | #include "../utilities/sample_grasp.h" 3 | 4 | void surfacepoints_from_file(std::string fileToOpen, std::vector* pts){ 5 | MatrixXd data = load_points_from_csv(fileToOpen); 6 | int N = data.rows(); 7 | for (int i = 0; i < N; ++i) { 8 | ContactPoint pt((data.block(i,0,1,3)).transpose(), -(data.block(i,3,1,3)).transpose(), 0); 9 | pts->push_back(pt); 10 | } 11 | } -------------------------------------------------------------------------------- /cmgmp/contacts/contact_utils.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CONTACTKINEMATICS_H 3 | #define CONTACTKINEMATICS_H 4 | #include "contact_kinematics.h" 5 | #endif 6 | #ifndef CONTACTCONSTRAINTS_H 7 | #define CONTACTCONSTRAINTS_H 8 | #include "contact_constraints.h" 9 | #endif 10 | 11 | // #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | void surfacepoints_from_file(std::string fileToOpen, std::vector* pts); -------------------------------------------------------------------------------- /cmgmp/dart_utils/dart_utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #ifndef UTILS_H 6 | #define UTILS_H 7 | #include "../utilities/utilities.h" 8 | #endif 9 | 10 | using namespace dart::dynamics; 11 | using namespace dart::simulation; 12 | using namespace dart::collision; 13 | using namespace dart::common; 14 | 15 | void setSkeletonColor(const SkeletonPtr& object, const Eigen::Vector3d& color); 16 | 17 | void setBodyNodeColor(BodyNode* bn, const Eigen::Vector3d& color); 18 | 19 | SkeletonPtr createFreeBall(const std::string& name, double radius, 20 | const Eigen::Vector3d& color = Eigen::Vector3d(0.7,0.3,0.3)); 21 | 22 | SkeletonPtr createFreeBox(const std::string& name, const Eigen::Vector3d& dim, 23 | const Eigen::Vector3d& color = Eigen::Vector3d(0.7,0.3,0.3)); 24 | 25 | SkeletonPtr createFreeCylinder(const std::string& name, double radius, double height, 26 | const Eigen::Vector3d& color = Eigen::Vector3d(0.7,0.3,0.3)); 27 | 28 | SkeletonPtr createFixedBox(const std::string& name, const Eigen::Vector3d& dim, 29 | const Eigen::Vector3d& pos, const Eigen::Vector3d& color = Eigen::Vector3d(0.4,0.4,0.4)); 30 | 31 | SkeletonPtr createFreeObjectfromMesh(const std::string & name, const std::string & filePath, 32 | const Eigen::Vector3d& scale = Eigen::Vector3d(1,1,1)); 33 | 34 | SkeletonPtr createRobot(const std::string & name, const std::string & filePath); 35 | 36 | Vector7d pose6d_to_pose7d(const Eigen::Vector6d & object_pose_6d); 37 | 38 | Eigen::Vector6d pose7d_to_pose6d(const Vector7d & pose_7d); 39 | -------------------------------------------------------------------------------- /cmgmp/external/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(dartsim) 2 | add_subdirectory(modus) -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | #include "Jacobi" 13 | 14 | #include "src/Core/util/DisableStupidWarnings.h" 15 | 16 | /** \defgroup Cholesky_Module Cholesky module 17 | * 18 | * 19 | * 20 | * This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices. 21 | * Those decompositions are also accessible via the following methods: 22 | * - MatrixBase::llt() 23 | * - MatrixBase::ldlt() 24 | * - SelfAdjointView::llt() 25 | * - SelfAdjointView::ldlt() 26 | * 27 | * \code 28 | * #include 29 | * \endcode 30 | */ 31 | 32 | #include "src/Cholesky/LLT.h" 33 | #include "src/Cholesky/LDLT.h" 34 | #ifdef EIGEN_USE_LAPACKE 35 | #ifdef EIGEN_USE_MKL 36 | #include "mkl_lapacke.h" 37 | #else 38 | #include "src/misc/lapacke.h" 39 | #endif 40 | #include "src/Cholesky/LLT_LAPACKE.h" 41 | #endif 42 | 43 | #include "src/Core/util/ReenableStupidWarnings.h" 44 | 45 | #endif // EIGEN_CHOLESKY_MODULE_H 46 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 47 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/Eigen/Dense: -------------------------------------------------------------------------------- 1 | #include "Core" 2 | #include "LU" 3 | #include "Cholesky" 4 | #include "QR" 5 | #include "SVD" 6 | #include "Geometry" 7 | #include "Eigenvalues" 8 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/Eigen/Eigen: -------------------------------------------------------------------------------- 1 | #include "Dense" 2 | #include "Sparse" 3 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | #ifdef EIGEN_USE_MKL 49 | #include "mkl_lapacke.h" 50 | #else 51 | #include "src/misc/lapacke.h" 52 | #endif 53 | #include "src/Eigenvalues/RealSchur_LAPACKE.h" 54 | #include "src/Eigenvalues/ComplexSchur_LAPACKE.h" 55 | #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h" 56 | #endif 57 | 58 | #include "src/Core/util/ReenableStupidWarnings.h" 59 | 60 | #endif // EIGEN_EIGENVALUES_MODULE_H 61 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 62 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | #ifdef EIGEN_USE_MKL 32 | #include "mkl_lapacke.h" 33 | #else 34 | #include "src/misc/lapacke.h" 35 | #endif 36 | #include "src/LU/PartialPivLU_LAPACKE.h" 37 | #endif 38 | #include "src/LU/Determinant.h" 39 | #include "src/LU/InverseImpl.h" 40 | 41 | // Use the SSE optimized version whenever possible. At the moment the 42 | // SSE version doesn't compile when AVX is enabled 43 | #if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX 44 | #include "src/LU/arch/Inverse_SSE.h" 45 | #endif 46 | 47 | #include "src/Core/util/ReenableStupidWarnings.h" 48 | 49 | #endif // EIGEN_LU_MODULE_H 50 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 51 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | #ifdef EIGEN_USE_MKL 40 | #include "mkl_lapacke.h" 41 | #else 42 | #include "src/misc/lapacke.h" 43 | #endif 44 | #include "src/QR/HouseholderQR_LAPACKE.h" 45 | #include "src/QR/ColPivHouseholderQR_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_QR_MODULE_H 51 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 52 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | std::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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | #ifdef EIGEN_USE_MKL 41 | #include "mkl_lapacke.h" 42 | #else 43 | #include "src/misc/lapacke.h" 44 | #endif 45 | #include "src/SVD/JacobiSVD_LAPACKE.h" 46 | #endif 47 | 48 | #include "src/Core/util/ReenableStupidWarnings.h" 49 | 50 | #endif // EIGEN_SVD_MODULE_H 51 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 52 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | 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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::mul_assign_op()); 21 | return derived(); 22 | } 23 | 24 | template 25 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator+=(const Scalar& other) 26 | { 27 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::add_assign_op()); 28 | return derived(); 29 | } 30 | 31 | template 32 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& ArrayBase::operator-=(const Scalar& other) 33 | { 34 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::sub_assign_op()); 35 | return derived(); 36 | } 37 | 38 | template 39 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase::operator/=(const Scalar& other) 40 | { 41 | internal::call_assignment(this->derived(), PlainObject::Constant(rows(),cols(),other), internal::div_assign_op()); 42 | return derived(); 43 | } 44 | 45 | } // end namespace Eigen 46 | 47 | #endif // EIGEN_SELFCWISEBINARYOP_H 48 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/Eigen/src/Core/arch/Default/ConjHelper.h: -------------------------------------------------------------------------------- 1 | 2 | // This file is part of Eigen, a lightweight C++ template library 3 | // for linear algebra. 4 | // 5 | // Copyright (C) 2017 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_ARCH_CONJ_HELPER_H 12 | #define EIGEN_ARCH_CONJ_HELPER_H 13 | 14 | #define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL) \ 15 | template<> struct conj_helper { \ 16 | EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x, const PACKET_CPLX& y, const PACKET_CPLX& c) const \ 17 | { return padd(c, pmul(x,y)); } \ 18 | EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x, const PACKET_CPLX& y) const \ 19 | { return PACKET_CPLX(Eigen::internal::pmul(x, y.v)); } \ 20 | }; \ 21 | \ 22 | template<> struct conj_helper { \ 23 | EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x, const PACKET_REAL& y, const PACKET_CPLX& c) const \ 24 | { return padd(c, pmul(x,y)); } \ 25 | EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x, const PACKET_REAL& y) const \ 26 | { return PACKET_CPLX(Eigen::internal::pmul(x.v, y)); } \ 27 | }; 28 | 29 | #endif // EIGEN_ARCH_CONJ_HELPER_H 30 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | #ifndef EIGEN_VECTORIZE_AVX 18 | template <> 19 | struct type_casting_traits { 20 | enum { 21 | VectorizedCast = 1, 22 | SrcCoeffRatio = 1, 23 | TgtCoeffRatio = 1 24 | }; 25 | }; 26 | 27 | template <> 28 | struct type_casting_traits { 29 | enum { 30 | VectorizedCast = 1, 31 | SrcCoeffRatio = 1, 32 | TgtCoeffRatio = 1 33 | }; 34 | }; 35 | 36 | template <> 37 | struct type_casting_traits { 38 | enum { 39 | VectorizedCast = 1, 40 | SrcCoeffRatio = 2, 41 | TgtCoeffRatio = 1 42 | }; 43 | }; 44 | 45 | template <> 46 | struct type_casting_traits { 47 | enum { 48 | VectorizedCast = 1, 49 | SrcCoeffRatio = 1, 50 | TgtCoeffRatio = 2 51 | }; 52 | }; 53 | #endif 54 | 55 | template<> EIGEN_STRONG_INLINE Packet4i pcast(const Packet4f& a) { 56 | return _mm_cvttps_epi32(a); 57 | } 58 | 59 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet4i& a) { 60 | return _mm_cvtepi32_ps(a); 61 | } 62 | 63 | template<> EIGEN_STRONG_INLINE Packet4f pcast(const Packet2d& a, const Packet2d& b) { 64 | return _mm_shuffle_ps(_mm_cvtpd_ps(a), _mm_cvtpd_ps(b), (1 << 2) | (1 << 6)); 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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/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 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/ArpackSupport: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_ARPACKSUPPORT_MODULE_H 10 | #define EIGEN_ARPACKSUPPORT_MODULE_H 11 | 12 | #include 13 | 14 | #include 15 | 16 | /** \defgroup ArpackSupport_Module Arpack support module 17 | * 18 | * This module provides a wrapper to Arpack, a library for sparse eigenvalue decomposition. 19 | * 20 | * \code 21 | * #include 22 | * \endcode 23 | */ 24 | 25 | #include 26 | #include "src/Eigenvalues/ArpackSelfAdjointEigenSolver.h" 27 | 28 | #include 29 | 30 | #endif // EIGEN_ARPACKSUPPORT_MODULE_H 31 | /* vim: set filetype=cpp et sw=2 ts=2 ai: */ 32 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/AutoDiff: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 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_AUTODIFF_MODULE 11 | #define EIGEN_AUTODIFF_MODULE 12 | 13 | namespace Eigen { 14 | 15 | /** 16 | * \defgroup AutoDiff_Module Auto Diff module 17 | * 18 | * This module features forward automatic differentation via a simple 19 | * templated scalar type wrapper AutoDiffScalar. 20 | * 21 | * Warning : this should NOT be confused with numerical differentiation, which 22 | * is a different method and has its own module in Eigen : \ref NumericalDiff_Module. 23 | * 24 | * \code 25 | * #include 26 | * \endcode 27 | */ 28 | //@{ 29 | 30 | } 31 | 32 | #include "src/AutoDiff/AutoDiffScalar.h" 33 | // #include "src/AutoDiff/AutoDiffVector.h" 34 | #include "src/AutoDiff/AutoDiffJacobian.h" 35 | 36 | namespace Eigen { 37 | //@} 38 | } 39 | 40 | #endif // EIGEN_AUTODIFF_MODULE 41 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/TensorSymmetry: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2013 Christian Seiler 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_CXX11_TENSORSYMMETRY_MODULE 11 | #define EIGEN_CXX11_TENSORSYMMETRY_MODULE 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include "src/util/CXX11Meta.h" 18 | 19 | /** \defgroup CXX11_TensorSymmetry_Module Tensor Symmetry Module 20 | * 21 | * This module provides a classes that allow for the definition of 22 | * symmetries w.r.t. tensor indices. 23 | * 24 | * Including this module will implicitly include the Tensor module. 25 | * 26 | * \code 27 | * #include 28 | * \endcode 29 | */ 30 | 31 | #include "src/TensorSymmetry/util/TemplateGroupTheory.h" 32 | #include "src/TensorSymmetry/Symmetry.h" 33 | #include "src/TensorSymmetry/StaticSymmetry.h" 34 | #include "src/TensorSymmetry/DynamicSymmetry.h" 35 | 36 | #include 37 | 38 | #endif // EIGEN_CXX11_TENSORSYMMETRY_MODULE 39 | 40 | /* 41 | * kate: space-indent on; indent-width 2; mixedindent off; indent-mode cstyle; 42 | */ 43 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/ThreadPool: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 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_CXX11_THREADPOOL_MODULE 11 | #define EIGEN_CXX11_THREADPOOL_MODULE 12 | 13 | #include "../../../Eigen/Core" 14 | 15 | #include 16 | 17 | /** \defgroup CXX11_ThreadPool_Module C++11 ThreadPool Module 18 | * 19 | * This module provides 2 threadpool implementations 20 | * - a simple reference implementation 21 | * - a faster non blocking implementation 22 | * 23 | * This module requires C++11. 24 | * 25 | * \code 26 | * #include 27 | * \endcode 28 | */ 29 | 30 | 31 | // The code depends on CXX11, so only include the module if the 32 | // compiler supports it. 33 | #if __cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "src/util/CXX11Meta.h" 49 | #include "src/util/MaxSizeVector.h" 50 | 51 | #include "src/ThreadPool/ThreadLocal.h" 52 | #include "src/ThreadPool/ThreadYield.h" 53 | #include "src/ThreadPool/EventCount.h" 54 | #include "src/ThreadPool/RunQueue.h" 55 | #include "src/ThreadPool/ThreadPoolInterface.h" 56 | #include "src/ThreadPool/ThreadEnvironment.h" 57 | #include "src/ThreadPool/SimpleThreadPool.h" 58 | #include "src/ThreadPool/NonBlockingThreadPool.h" 59 | 60 | #endif 61 | 62 | #include 63 | 64 | #endif // EIGEN_CXX11_THREADPOOL_MODULE 65 | 66 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorContractionBlocking.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 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_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H 11 | #define EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H 12 | 13 | 14 | namespace Eigen { 15 | namespace internal { 16 | 17 | enum { 18 | ShardByRow = 0, 19 | ShardByCol = 1 20 | }; 21 | 22 | 23 | // Default Blocking Strategy 24 | template 25 | class TensorContractionBlocking { 26 | public: 27 | 28 | typedef typename LhsMapper::Scalar LhsScalar; 29 | typedef typename RhsMapper::Scalar RhsScalar; 30 | 31 | EIGEN_DEVICE_FUNC TensorContractionBlocking(Index k, Index m, Index n, Index num_threads = 1) : 32 | kc_(k), mc_(m), nc_(n) 33 | { 34 | if (ShardingType == ShardByCol) { 35 | computeProductBlockingSizes(kc_, mc_, nc_, num_threads); 36 | } 37 | else { 38 | computeProductBlockingSizes(kc_, nc_, mc_, num_threads); 39 | } 40 | } 41 | 42 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index kc() const { return kc_; } 43 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index mc() const { return mc_; } 44 | EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Index nc() const { return nc_; } 45 | 46 | private: 47 | Index kc_; 48 | Index mc_; 49 | Index nc_; 50 | }; 51 | 52 | 53 | } // end namespace internal 54 | } // end namespace Eigen 55 | 56 | #endif // EIGEN_CXX11_TENSOR_TENSOR_CONTRACTION_BLOCKING_H 57 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorGlobalFunctions.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_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H 11 | #define EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H 12 | 13 | namespace Eigen { 14 | 15 | /** \cpp11 \returns an expression of the coefficient-wise betainc(\a x, \a a, \a b) to the given tensors. 16 | * 17 | * This function computes the regularized incomplete beta function (integral). 18 | * 19 | */ 20 | template 21 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const 22 | TensorCwiseTernaryOp, 23 | const ADerived, const BDerived, const XDerived> 24 | betainc(const ADerived& a, const BDerived& b, const XDerived& x) { 25 | return TensorCwiseTernaryOp< 26 | internal::scalar_betainc_op, const ADerived, 27 | const BDerived, const XDerived>( 28 | a, b, x, internal::scalar_betainc_op()); 29 | } 30 | 31 | } // end namespace Eigen 32 | 33 | #endif // EIGEN_CXX11_TENSOR_TENSOR_GLOBAL_FUNCTIONS_H 34 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/src/Tensor/TensorMacros.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_CXX11_TENSOR_TENSOR_META_MACROS_H 11 | #define EIGEN_CXX11_TENSOR_TENSOR_META_MACROS_H 12 | 13 | 14 | /** use this macro in sfinae selection in templated functions 15 | * 16 | * template::value , int >::type = 0 18 | * > 19 | * void foo(){} 20 | * 21 | * becomes => 22 | * 23 | * template::value ) 25 | * > 26 | * void foo(){} 27 | */ 28 | 29 | // SFINAE requires variadic templates 30 | #ifndef __CUDACC__ 31 | #if EIGEN_HAS_VARIADIC_TEMPLATES 32 | // SFINAE doesn't work for gcc <= 4.7 33 | #ifdef EIGEN_COMP_GNUC 34 | #if EIGEN_GNUC_AT_LEAST(4,8) 35 | #define EIGEN_HAS_SFINAE 36 | #endif 37 | #else 38 | #define EIGEN_HAS_SFINAE 39 | #endif 40 | #endif 41 | #endif 42 | 43 | #define EIGEN_SFINAE_ENABLE_IF( __condition__ ) \ 44 | typename internal::enable_if< ( __condition__ ) , int >::type = 0 45 | 46 | 47 | #if EIGEN_HAS_CONSTEXPR 48 | #define EIGEN_CONSTEXPR constexpr 49 | #else 50 | #define EIGEN_CONSTEXPR 51 | #endif 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadEnvironment.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 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_CXX11_THREADPOOL_THREAD_ENVIRONMENT_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_ENVIRONMENT_H 12 | 13 | namespace Eigen { 14 | 15 | struct StlThreadEnvironment { 16 | struct Task { 17 | std::function f; 18 | }; 19 | 20 | // EnvThread constructor must start the thread, 21 | // destructor must join the thread. 22 | class EnvThread { 23 | public: 24 | EnvThread(std::function f) : thr_(std::move(f)) {} 25 | ~EnvThread() { thr_.join(); } 26 | 27 | private: 28 | std::thread thr_; 29 | }; 30 | 31 | EnvThread* CreateThread(std::function f) { return new EnvThread(std::move(f)); } 32 | Task CreateTask(std::function f) { return Task{std::move(f)}; } 33 | void ExecuteTask(const Task& t) { t.f(); } 34 | }; 35 | 36 | } // namespace Eigen 37 | 38 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_ENVIRONMENT_H 39 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadLocal.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 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_CXX11_THREADPOOL_THREAD_LOCAL_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_LOCAL_H 12 | 13 | // Try to come up with a portable implementation of thread local variables 14 | #if EIGEN_COMP_GNUC && EIGEN_GNUC_AT_MOST(4, 7) 15 | #define EIGEN_THREAD_LOCAL static __thread 16 | #elif EIGEN_COMP_CLANG 17 | #define EIGEN_THREAD_LOCAL static __thread 18 | #else 19 | #define EIGEN_THREAD_LOCAL static thread_local 20 | #endif 21 | 22 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_LOCAL_H 23 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadPoolInterface.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2014 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_CXX11_THREADPOOL_THREAD_POOL_INTERFACE_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_POOL_INTERFACE_H 12 | 13 | namespace Eigen { 14 | 15 | // This defines an interface that ThreadPoolDevice can take to use 16 | // custom thread pools underneath. 17 | class ThreadPoolInterface { 18 | public: 19 | virtual void Schedule(std::function fn) = 0; 20 | 21 | // Returns the number of threads in the pool. 22 | virtual int NumThreads() const = 0; 23 | 24 | // Returns a logical thread index between 0 and NumThreads() - 1 if called 25 | // from one of the threads in the pool. Returns -1 otherwise. 26 | virtual int CurrentThreadId() const = 0; 27 | 28 | virtual ~ThreadPoolInterface() {} 29 | }; 30 | 31 | } // namespace Eigen 32 | 33 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_POOL_INTERFACE_H 34 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/CXX11/src/ThreadPool/ThreadYield.h: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 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_CXX11_THREADPOOL_THREAD_YIELD_H 11 | #define EIGEN_CXX11_THREADPOOL_THREAD_YIELD_H 12 | 13 | // Try to come up with a portable way to yield 14 | #if EIGEN_COMP_GNUC && EIGEN_GNUC_AT_MOST(4, 7) 15 | #define EIGEN_THREAD_YIELD() sched_yield() 16 | #else 17 | #define EIGEN_THREAD_YIELD() std::this_thread::yield() 18 | #endif 19 | 20 | #endif // EIGEN_CXX11_THREADPOOL_THREAD_YIELD_H 21 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/EulerAngles: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2015 Tal Hadad 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_EULERANGLES_MODULE_H 11 | #define EIGEN_EULERANGLES_MODULE_H 12 | 13 | 14 | #include "Eigen/Core" 15 | #include "Eigen/Geometry" 16 | 17 | #include "Eigen/src/Core/util/DisableStupidWarnings.h" 18 | 19 | namespace Eigen { 20 | 21 | /** 22 | * \defgroup EulerAngles_Module EulerAngles module 23 | * \brief This module provides generic euler angles rotation. 24 | * 25 | * Euler angles are a way to represent 3D rotation. 26 | * 27 | * In order to use this module in your code, include this header: 28 | * \code 29 | * #include 30 | * \endcode 31 | * 32 | * See \ref EulerAngles for more information. 33 | * 34 | */ 35 | 36 | } 37 | 38 | #include "src/EulerAngles/EulerSystem.h" 39 | #include "src/EulerAngles/EulerAngles.h" 40 | 41 | #include "Eigen/src/Core/util/ReenableStupidWarnings.h" 42 | 43 | #endif // EIGEN_EULERANGLES_MODULE_H 44 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/IterativeSolvers: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 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_ITERATIVE_SOLVERS_MODULE_H 11 | #define EIGEN_ITERATIVE_SOLVERS_MODULE_H 12 | 13 | #include 14 | 15 | /** 16 | * \defgroup IterativeSolvers_Module Iterative solvers module 17 | * This module aims to provide various iterative linear and non linear solver algorithms. 18 | * It currently provides: 19 | * - a constrained conjugate gradient 20 | * - a Householder GMRES implementation 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | //@{ 26 | 27 | #ifndef EIGEN_MPL2_ONLY 28 | #include "src/IterativeSolvers/IterationController.h" 29 | #include "src/IterativeSolvers/ConstrainedConjGrad.h" 30 | #endif 31 | 32 | #include "src/IterativeSolvers/IncompleteLU.h" 33 | #include "../../Eigen/Jacobi" 34 | #include "../../Eigen/Householder" 35 | #include "src/IterativeSolvers/GMRES.h" 36 | #include "src/IterativeSolvers/DGMRES.h" 37 | //#include "src/IterativeSolvers/SSORPreconditioner.h" 38 | #include "src/IterativeSolvers/MINRES.h" 39 | 40 | //@} 41 | 42 | #endif // EIGEN_ITERATIVE_SOLVERS_MODULE_H 43 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/KroneckerProduct: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_KRONECKER_PRODUCT_MODULE_H 10 | #define EIGEN_KRONECKER_PRODUCT_MODULE_H 11 | 12 | #include "../../Eigen/Core" 13 | 14 | #include "../../Eigen/src/Core/util/DisableStupidWarnings.h" 15 | 16 | #include "../../Eigen/src/SparseCore/SparseUtil.h" 17 | 18 | namespace Eigen { 19 | 20 | /** 21 | * \defgroup KroneckerProduct_Module KroneckerProduct module 22 | * 23 | * This module contains an experimental Kronecker product implementation. 24 | * 25 | * \code 26 | * #include 27 | * \endcode 28 | */ 29 | 30 | } // namespace Eigen 31 | 32 | #include "src/KroneckerProduct/KroneckerTensorProduct.h" 33 | 34 | #include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" 35 | 36 | #endif // EIGEN_KRONECKER_PRODUCT_MODULE_H 37 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/LevenbergMarquardt: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Thomas Capricelli 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_LEVENBERGMARQUARDT_MODULE 11 | #define EIGEN_LEVENBERGMARQUARDT_MODULE 12 | 13 | // #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | /** 23 | * \defgroup LevenbergMarquardt_Module Levenberg-Marquardt module 24 | * 25 | * \code 26 | * #include 27 | * \endcode 28 | * 29 | * 30 | */ 31 | 32 | #include "Eigen/SparseCore" 33 | #ifndef EIGEN_PARSED_BY_DOXYGEN 34 | 35 | #include "src/LevenbergMarquardt/LMqrsolv.h" 36 | #include "src/LevenbergMarquardt/LMcovar.h" 37 | #include "src/LevenbergMarquardt/LMpar.h" 38 | 39 | #endif 40 | 41 | #include "src/LevenbergMarquardt/LevenbergMarquardt.h" 42 | #include "src/LevenbergMarquardt/LMonestep.h" 43 | 44 | 45 | #endif // EIGEN_LEVENBERGMARQUARDT_MODULE 46 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/MoreVectorization: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_MOREVECTORIZATION_MODULE_H 10 | #define EIGEN_MOREVECTORIZATION_MODULE_H 11 | 12 | #include 13 | 14 | namespace Eigen { 15 | 16 | /** 17 | * \defgroup MoreVectorization More vectorization module 18 | */ 19 | 20 | } 21 | 22 | #include "src/MoreVectorization/MathFunctions.h" 23 | 24 | #endif // EIGEN_MOREVECTORIZATION_MODULE_H 25 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/NumericalDiff: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2009 Thomas Capricelli 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_NUMERICALDIFF_MODULE 11 | #define EIGEN_NUMERICALDIFF_MODULE 12 | 13 | #include 14 | 15 | namespace Eigen { 16 | 17 | /** 18 | * \defgroup NumericalDiff_Module Numerical differentiation module 19 | * 20 | * \code 21 | * #include 22 | * \endcode 23 | * 24 | * See http://en.wikipedia.org/wiki/Numerical_differentiation 25 | * 26 | * Warning : this should NOT be confused with automatic differentiation, which 27 | * is a different method and has its own module in Eigen : \ref 28 | * AutoDiff_Module. 29 | * 30 | * Currently only "Forward" and "Central" schemes are implemented. Those 31 | * are basic methods, and there exist some more elaborated way of 32 | * computing such approximates. They are implemented using both 33 | * proprietary and free software, and usually requires linking to an 34 | * external library. It is very easy for you to write a functor 35 | * using such software, and the purpose is quite orthogonal to what we 36 | * want to achieve with Eigen. 37 | * 38 | * This is why we will not provide wrappers for every great numerical 39 | * differentiation software that exist, but should rather stick with those 40 | * basic ones, that still are useful for testing. 41 | * 42 | * Also, the \ref NonLinearOptimization_Module needs this in order to 43 | * provide full features compatibility with the original (c)minpack 44 | * package. 45 | * 46 | */ 47 | } 48 | 49 | //@{ 50 | 51 | #include "src/NumericalDiff/NumericalDiff.h" 52 | 53 | //@} 54 | 55 | 56 | #endif // EIGEN_NUMERICALDIFF_MODULE 57 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/Skyline: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef EIGEN_SKYLINE_MODULE_H 10 | #define EIGEN_SKYLINE_MODULE_H 11 | 12 | 13 | #include "Eigen/Core" 14 | 15 | #include "Eigen/src/Core/util/DisableStupidWarnings.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | /** 23 | * \defgroup Skyline_Module Skyline module 24 | * 25 | * 26 | * 27 | * 28 | */ 29 | 30 | #include "src/Skyline/SkylineUtil.h" 31 | #include "src/Skyline/SkylineMatrixBase.h" 32 | #include "src/Skyline/SkylineStorage.h" 33 | #include "src/Skyline/SkylineMatrix.h" 34 | #include "src/Skyline/SkylineInplaceLU.h" 35 | #include "src/Skyline/SkylineProduct.h" 36 | 37 | #include "Eigen/src/Core/util/ReenableStupidWarnings.h" 38 | 39 | #endif // EIGEN_SKYLINE_MODULE_H 40 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/SparseExtra: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2008-2009 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_EXTRA_MODULE_H 11 | #define EIGEN_SPARSE_EXTRA_MODULE_H 12 | 13 | #include "../../Eigen/Sparse" 14 | 15 | #include "../../Eigen/src/Core/util/DisableStupidWarnings.h" 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #ifdef EIGEN_GOOGLEHASH_SUPPORT 26 | #include 27 | #endif 28 | 29 | /** 30 | * \defgroup SparseExtra_Module SparseExtra module 31 | * 32 | * This module contains some experimental features extending the sparse module. 33 | * 34 | * \code 35 | * #include 36 | * \endcode 37 | */ 38 | 39 | 40 | #include "src/SparseExtra/DynamicSparseMatrix.h" 41 | #include "src/SparseExtra/BlockOfDynamicSparseMatrix.h" 42 | #include "src/SparseExtra/RandomSetter.h" 43 | 44 | #include "src/SparseExtra/MarketIO.h" 45 | 46 | #if !defined(_WIN32) 47 | #include 48 | #include "src/SparseExtra/MatrixMarketIterator.h" 49 | #endif 50 | 51 | #include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" 52 | 53 | #endif // EIGEN_SPARSE_EXTRA_MODULE_H 54 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/SpecialFunctions: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 2016 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_SPECIALFUNCTIONS_MODULE 11 | #define EIGEN_SPECIALFUNCTIONS_MODULE 12 | 13 | #include 14 | 15 | #include "../../Eigen/Core" 16 | 17 | #include "../../Eigen/src/Core/util/DisableStupidWarnings.h" 18 | 19 | namespace Eigen { 20 | 21 | /** 22 | * \defgroup SpecialFunctions_Module Special math functions module 23 | * 24 | * This module features additional coefficient-wise math functions available 25 | * within the numext:: namespace for the scalar version, and as method and/or free 26 | * functions of Array. Those include: 27 | * 28 | * - erf 29 | * - erfc 30 | * - lgamma 31 | * - igamma 32 | * - igammac 33 | * - digamma 34 | * - polygamma 35 | * - zeta 36 | * - betainc 37 | * 38 | * \code 39 | * #include 40 | * \endcode 41 | */ 42 | //@{ 43 | 44 | } 45 | 46 | #include "src/SpecialFunctions/SpecialFunctionsImpl.h" 47 | #include "src/SpecialFunctions/SpecialFunctionsPacketMath.h" 48 | #include "src/SpecialFunctions/SpecialFunctionsHalf.h" 49 | #include "src/SpecialFunctions/SpecialFunctionsFunctors.h" 50 | #include "src/SpecialFunctions/SpecialFunctionsArrayAPI.h" 51 | 52 | #if defined EIGEN_VECTORIZE_CUDA 53 | #include "src/SpecialFunctions/arch/CUDA/CudaSpecialFunctions.h" 54 | #endif 55 | 56 | namespace Eigen { 57 | //@} 58 | } 59 | 60 | 61 | #include "../../Eigen/src/Core/util/ReenableStupidWarnings.h" 62 | 63 | #endif // EIGEN_SPECIALFUNCTIONS_MODULE 64 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/Splines: -------------------------------------------------------------------------------- 1 | // This file is part of Eigen, a lightweight C++ template library 2 | // for linear algebra. 3 | // 4 | // Copyright (C) 20010-2011 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_SPLINES_MODULE_H 11 | #define EIGEN_SPLINES_MODULE_H 12 | 13 | namespace Eigen 14 | { 15 | /** 16 | * \defgroup Splines_Module Spline and spline fitting module 17 | * 18 | * This module provides a simple multi-dimensional spline class while 19 | * offering most basic functionality to fit a spline to point sets. 20 | * 21 | * \code 22 | * #include 23 | * \endcode 24 | */ 25 | } 26 | 27 | #include "src/Splines/SplineFwd.h" 28 | #include "src/Splines/Spline.h" 29 | #include "src/Splines/SplineFitting.h" 30 | 31 | #endif // EIGEN_SPLINES_MODULE_H 32 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/src/NonLinearOptimization/chkder.h: -------------------------------------------------------------------------------- 1 | #define chkder_log10e 0.43429448190325182765 2 | #define chkder_factor 100. 3 | 4 | namespace Eigen { 5 | 6 | namespace internal { 7 | 8 | template 9 | void chkder( 10 | const Matrix< Scalar, Dynamic, 1 > &x, 11 | const Matrix< Scalar, Dynamic, 1 > &fvec, 12 | const Matrix< Scalar, Dynamic, Dynamic > &fjac, 13 | Matrix< Scalar, Dynamic, 1 > &xp, 14 | const Matrix< Scalar, Dynamic, 1 > &fvecp, 15 | int mode, 16 | Matrix< Scalar, Dynamic, 1 > &err 17 | ) 18 | { 19 | using std::sqrt; 20 | using std::abs; 21 | using std::log; 22 | 23 | typedef DenseIndex Index; 24 | 25 | const Scalar eps = sqrt(NumTraits::epsilon()); 26 | const Scalar epsf = chkder_factor * NumTraits::epsilon(); 27 | const Scalar epslog = chkder_log10e * log(eps); 28 | Scalar temp; 29 | 30 | const Index m = fvec.size(), n = x.size(); 31 | 32 | if (mode != 2) { 33 | /* mode = 1. */ 34 | xp.resize(n); 35 | for (Index j = 0; j < n; ++j) { 36 | temp = eps * abs(x[j]); 37 | if (temp == 0.) 38 | temp = eps; 39 | xp[j] = x[j] + temp; 40 | } 41 | } 42 | else { 43 | /* mode = 2. */ 44 | err.setZero(m); 45 | for (Index j = 0; j < n; ++j) { 46 | temp = abs(x[j]); 47 | if (temp == 0.) 48 | temp = 1.; 49 | err += temp * fjac.col(j); 50 | } 51 | for (Index i = 0; i < m; ++i) { 52 | temp = 1.; 53 | if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i])) 54 | temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i])); 55 | err[i] = 1.; 56 | if (temp > NumTraits::epsilon() && temp < eps) 57 | err[i] = (chkder_log10e * log(temp) - epslog) / epslog; 58 | if (temp >= eps) 59 | err[i] = 0.; 60 | } 61 | } 62 | } 63 | 64 | } // end namespace internal 65 | 66 | } // end namespace Eigen 67 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/src/NonLinearOptimization/covar.h: -------------------------------------------------------------------------------- 1 | namespace Eigen { 2 | 3 | namespace internal { 4 | 5 | template 6 | void covar( 7 | Matrix< Scalar, Dynamic, Dynamic > &r, 8 | const VectorXi &ipvt, 9 | Scalar tol = std::sqrt(NumTraits::epsilon()) ) 10 | { 11 | using std::abs; 12 | typedef DenseIndex Index; 13 | 14 | /* Local variables */ 15 | Index i, j, k, l, ii, jj; 16 | bool sing; 17 | Scalar temp; 18 | 19 | /* Function Body */ 20 | const Index n = r.cols(); 21 | const Scalar tolr = tol * abs(r(0,0)); 22 | Matrix< Scalar, Dynamic, 1 > wa(n); 23 | eigen_assert(ipvt.size()==n); 24 | 25 | /* form the inverse of r in the full upper triangle of r. */ 26 | l = -1; 27 | for (k = 0; k < n; ++k) 28 | if (abs(r(k,k)) > tolr) { 29 | r(k,k) = 1. / r(k,k); 30 | for (j = 0; j <= k-1; ++j) { 31 | temp = r(k,k) * r(j,k); 32 | r(j,k) = 0.; 33 | r.col(k).head(j+1) -= r.col(j).head(j+1) * temp; 34 | } 35 | l = k; 36 | } 37 | 38 | /* form the full upper triangle of the inverse of (r transpose)*r */ 39 | /* in the full upper triangle of r. */ 40 | for (k = 0; k <= l; ++k) { 41 | for (j = 0; j <= k-1; ++j) 42 | r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k); 43 | r.col(k).head(k+1) *= r(k,k); 44 | } 45 | 46 | /* form the full lower triangle of the covariance matrix */ 47 | /* in the strict lower triangle of r and in wa. */ 48 | for (j = 0; j < n; ++j) { 49 | jj = ipvt[j]; 50 | sing = j > l; 51 | for (i = 0; i <= j; ++i) { 52 | if (sing) 53 | r(i,j) = 0.; 54 | ii = ipvt[i]; 55 | if (ii > jj) 56 | r(ii,jj) = r(i,j); 57 | if (ii < jj) 58 | r(jj,ii) = r(i,j); 59 | } 60 | wa[jj] = r(j,j); 61 | } 62 | 63 | /* symmetrize the covariance matrix in r. */ 64 | r.topLeftCorner(n,n).template triangularView() = r.topLeftCorner(n,n).transpose(); 65 | r.diagonal() = wa; 66 | } 67 | 68 | } // end namespace internal 69 | 70 | } // end namespace Eigen 71 | -------------------------------------------------------------------------------- /cmgmp/external/eigen3/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h: -------------------------------------------------------------------------------- 1 | namespace Eigen { 2 | 3 | namespace internal { 4 | 5 | // TODO : move this to GivensQR once there's such a thing in Eigen 6 | 7 | template 8 | void r1mpyq(DenseIndex m, DenseIndex n, Scalar *a, const std::vector > &v_givens, const std::vector > &w_givens) 9 | { 10 | typedef DenseIndex Index; 11 | 12 | /* apply the first set of givens rotations to a. */ 13 | for (Index j = n-2; j>=0; --j) 14 | for (Index i = 0; i 6 | void rwupdt( 7 | Matrix< Scalar, Dynamic, Dynamic > &r, 8 | const Matrix< Scalar, Dynamic, 1> &w, 9 | Matrix< Scalar, Dynamic, 1> &b, 10 | Scalar alpha) 11 | { 12 | typedef DenseIndex Index; 13 | 14 | const Index n = r.cols(); 15 | eigen_assert(r.rows()>=n); 16 | std::vector > givens(n); 17 | 18 | /* Local variables */ 19 | Scalar temp, rowj; 20 | 21 | /* Function Body */ 22 | for (Index j = 0; j < n; ++j) { 23 | rowj = w[j]; 24 | 25 | /* apply the previous transformations to */ 26 | /* r(i,j), i=0,1,...,j-1, and to w(j). */ 27 | for (Index i = 0; i < j; ++i) { 28 | temp = givens[i].c() * r(i,j) + givens[i].s() * rowj; 29 | rowj = -givens[i].s() * r(i,j) + givens[i].c() * rowj; 30 | r(i,j) = temp; 31 | } 32 | 33 | /* determine a givens rotation which eliminates w(j). */ 34 | givens[j].makeGivens(-r(j,j), rowj); 35 | 36 | if (rowj == 0.) 37 | continue; // givens[j] is identity 38 | 39 | /* apply the current transformation to r(j,j), b(j), and alpha. */ 40 | r(j,j) = givens[j].c() * r(j,j) + givens[j].s() * rowj; 41 | temp = givens[j].c() * b[j] + givens[j].s() * alpha; 42 | alpha = -givens[j].s() * b[j] + givens[j].c() * alpha; 43 | b[j] = temp; 44 | } 45 | } 46 | 47 | } // end namespace internal 48 | 49 | } // end namespace Eigen 50 | -------------------------------------------------------------------------------- /cmgmp/external/modus/apps/CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/apps/CMakeLists.txt -------------------------------------------------------------------------------- /cmgmp/external/modus/bench/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # set(CMAKE_EXE_LINKER_FLAGS "-Wl,-no-as-needed -pg") 2 | 3 | macro(add_benchmark TESTNAME) 4 | add_executable(${TESTNAME} ${ARGN}) 5 | add_test(NAME ${TESTNAME} COMMAND ${TESTNAME}) 6 | target_link_libraries(${TESTNAME} modus dw ${GPERFTOOLS_PROFILER}) 7 | endmacro() 8 | 9 | macro(add_cuda_benchmark TESTNAME) 10 | add_executable(${TESTNAME} ${ARGN}) 11 | # target_link_libraries(${TESTNAME} gtest gmock gtest_main dw) 12 | add_test(NAME ${TESTNAME} COMMAND ${TESTNAME}) 13 | # set_target_properties(${TESTNAME} PROPERTIES FOLDER test) 14 | set_target_properties(${TESTNAME} PROPERTIES CUDA_SEPARABLE_COMPILATION ON) 15 | target_link_libraries(${TESTNAME} PRIVATE mesh_geom dw) 16 | target_compile_options(${TESTNAME} PRIVATE $<$: 17 | --expt-relaxed-constexpr 18 | --relocatable-device-code=true 19 | >) 20 | endmacro() 21 | 22 | # add_cuda_benchmark(bench_query bench_query.cu) 23 | # add_cuda_benchmark(bench_bvh bench_bvh.cu) 24 | 25 | # add_benchmark(profile_arrangements profile_arrangements.cpp) 26 | add_benchmark(bench_subspace bench_subspace.cpp) 27 | add_benchmark(bench_arrangements bench_arrangements.cpp) -------------------------------------------------------------------------------- /cmgmp/external/modus/bench/bench_arrangements.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | // #include 5 | 6 | 7 | template 8 | inline bool is_aligned(void * ptr) noexcept { 9 | std::size_t max = 1u; 10 | return std::align(alignment, 1u, ptr, max); 11 | } 12 | 13 | int main(int argc, char* argv[]) { 14 | // google::InitGoogleLogging(argv[0]); 15 | 16 | // Set random seed. 17 | srand(0); 18 | 19 | // // Benchmark initial arrangements. 20 | // for (int n = 5; n < 14; n++) { 21 | // Eigen::MatrixXd A(n,n); 22 | // Eigen::VectorXd b(n); 23 | 24 | // A.setRandom(); 25 | // b.setRandom(); 26 | // auto start = std::chrono::high_resolution_clock::now(); 27 | // IncidenceGraphPtr I = initial_arrangement(A, b, 1e-8); 28 | // auto end = std::chrono::high_resolution_clock::now(); 29 | // } 30 | 31 | // Benchmark increment arrangements. 32 | for (int j = 0; j < 20; j++) 33 | { 34 | int n = 12; 35 | int d = 11; 36 | Eigen::MatrixXd A(d,d); 37 | Eigen::VectorXd b(d); 38 | A.setRandom(); 39 | b.setRandom(); 40 | 41 | IncidenceGraph* I = initial_arrangement(A, b, 1e-8); 42 | 43 | I->_num_arcs_created = 0; 44 | 45 | for (int i = d; i < n; i++) { 46 | Eigen::VectorXd a(d); 47 | Eigen::VectorXd b(1); 48 | a.setRandom(); 49 | b.setRandom(); 50 | increment_arrangement(a, b[0], I, 1e-8); 51 | } 52 | 53 | std::cout << "num arcs " << I->_num_arcs_created << std::endl; 54 | 55 | delete I; 56 | } 57 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/bench/bench_subspace.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | int main() { 7 | // Set random seed. 8 | srand(0); 9 | 10 | // Benchmark null space. 11 | for (int n = 1; n < 10; n++) { 12 | for (int m = 2; m < 10; m++) { 13 | Eigen::MatrixXd A(n, m); 14 | Eigen::MatrixXd image; 15 | Eigen::MatrixXd kernel; 16 | auto start = std::chrono::high_resolution_clock::now(); 17 | for (int k = 0; k < 1000; k++) { 18 | A.setRandom(); 19 | image_and_kernel_bases(A, image, kernel, 1e-8); 20 | } 21 | auto end = std::chrono::high_resolution_clock::now(); 22 | std::cout << n << " " << m << " " << 23 | std::chrono::duration_cast(end - start).count() / 1e6 / 1e3 24 | << " ms" << std::endl; 25 | } 26 | } 27 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/bench/profile_arrangements.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | int main() { 5 | int d = 8; 6 | Eigen::MatrixXd A(d, d); 7 | A.setIdentity(); 8 | Eigen::VectorXd b(d); 9 | 10 | IncidenceGraphPtr I = initial_arrangement(A, b, 1e-10); 11 | 12 | I.reset(); 13 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/box-1.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/box-1.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/box-2.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/box-2.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/box-3.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/box-3.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/box-4.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/box-4.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/box-5.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/box-5.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/cs_modes/box-1.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/cs_modes/box-1.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/cs_modes/box-2.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/cs_modes/box-2.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/cs_modes/box-3.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/cs_modes/box-3.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/cs_modes/box-4.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/cs_modes/box-4.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/cs_modes/box-5.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/cs_modes/box-5.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/cs_modes/box-box-1.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/cs_modes/box-box-1.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/ss_modes/box-1.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/ss_modes/box-1.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/ss_modes/box-2.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/ss_modes/box-2.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/ss_modes/box-3.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/ss_modes/box-3.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/ss_modes/box-4.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/ss_modes/box-4.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/data/test/ss_modes/box-5.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/data/test/ss_modes/box-5.npz -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/collision/collision.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace modus 7 | { 8 | 9 | class Collision : public Aspect { 10 | public: 11 | virtual Contacts GetContactPoints() = 0; 12 | }; 13 | MODUS_DEFINE_SHARED(Collision); 14 | 15 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/collision/contact.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace modus 11 | { 12 | 13 | struct Contact { 14 | // Public variables. 15 | // Body* body_A; 16 | // Body* body_B; 17 | // Link* link_A; 18 | // Link* link_B; 19 | int body_A; 20 | int body_B; 21 | int link_A; 22 | int link_B; 23 | Eigen::Vector3d point_A; 24 | Eigen::Vector3d point_B; 25 | Eigen::Vector3d contact_normal_B; 26 | double contact_distance; 27 | Eigen::Vector3d tangent_x; 28 | Eigen::Vector3d tangent_y; 29 | 30 | // For preprocessing use. 31 | int point_id_; 32 | int manifold_id_; 33 | double target_offset_; 34 | 35 | void SwapAB(); 36 | }; 37 | 38 | std::ostream& operator<<(std::ostream& os, const Contact& c); 39 | 40 | using ContactPtr = std::shared_ptr; 41 | using Contacts = std::vector; 42 | using ContactGroups = std::vector; 43 | using ContactGroupMap = std::map; 44 | 45 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/common/convex_hull.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | using Simplices = std::vector; 9 | 10 | using Points = std::vector; 11 | 12 | // Convex hull of input points as dxn matrix. 13 | Eigen::MatrixXi ConvexHull(const Eigen::MatrixXd& points, double eps); 14 | 15 | // Convex hull with std vector input scheme, will also reorient. 16 | Simplices ConvexHull(const Points& points, double eps); 17 | 18 | Simplices ConvertIncidenceMatrixToSimplices(const Eigen::MatrixXi& M); 19 | 20 | Simplices ReorderBoundary(const Simplices& simplices, const Points& points); 21 | 22 | Simplices ReorientHull(const Simplices& simplices, const Points& points); 23 | 24 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/common/eigen.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #define eigen_assert(x) \ 4 | if (!(x)) { MODUS_ASSERT((x)); } 5 | #include -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/common/memory.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #define MODUS_DEFINE_SHARED(X) using X ## Ptr = std::shared_ptr -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/dynamics/actuator_dynamics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | class ActuatorDynamics : public Aspect 9 | { 10 | public: 11 | virtual Eigen::MatrixXd GetActuatorMatrix() = 0; 12 | }; 13 | MODUS_DEFINE_SHARED(ActuatorDynamics); 14 | 15 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/dynamics/coefficient_of_friction.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace modus 7 | { 8 | 9 | class CoefficientOfFriction : public Aspect { 10 | protected: 11 | double mu_; 12 | 13 | public: 14 | CoefficientOfFriction(double mu) { mu_ = mu; } 15 | 16 | void SetMu(double mu) { mu_ = mu; } 17 | 18 | double GetMu() { return mu_; } 19 | }; 20 | using CoF = CoefficientOfFriction; 21 | MODUS_DEFINE_SHARED(CoefficientOfFriction); 22 | MODUS_DEFINE_SHARED(CoF); 23 | 24 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/dynamics/contact_dynamics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace modus 10 | { 11 | 12 | class ContactDynamics : public Aspect { 13 | protected: 14 | System* system_; 15 | State* state_; 16 | Collision* collision_; 17 | Kinematics* kinematics_; 18 | 19 | public: 20 | ContactDynamics(System* system); 21 | // ContactDynamics(Collision* collision, Kinematics* kinematics); 22 | 23 | size_t NumContactForces(); 24 | 25 | // Create polyhedral contact force generators. The generators are the ordered 26 | // counter-clockwise in the contact frame, i.e. they are +0, 0+, -0, 0-. 27 | Eigen::MatrixXd GetContactForceGeneratorMatrix(); 28 | 29 | // Get upper bounds for the contact forces given the input cs mode and ss 30 | // mode. The lower bounds are always zero. 31 | Eigen::VectorXd GetContactForceUpperBounds(const std::string& cs_mode, 32 | const std::string& ss_mode); 33 | 34 | // Create unit contact force directions in the contact frame for +0, 0+, -0, 35 | // 0- using the average coefficient of friction between the two contacting 36 | // links. 37 | Eigen::MatrixXd CreateContactForceRays(ContactPtr contact); 38 | 39 | // Create contact force generator for given force direction. 40 | Eigen::VectorXd CreateContactForceGenerator(ContactPtr contact, 41 | const Eigen::Vector3d& ray); 42 | }; 43 | MODUS_DEFINE_SHARED(ContactDynamics); 44 | 45 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/dynamics/continuous_dynamics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace modus 8 | { 9 | 10 | class ContinuousDynamics : public Aspect { 11 | protected: 12 | System* system_; 13 | 14 | public: 15 | ContinuousDynamics(System* system); 16 | 17 | Eigen::MatrixXd GetMassMatrix(); 18 | Eigen::MatrixXd GetCoriolisMatrix(); 19 | Eigen::VectorXd GetPotentialForces(); 20 | Eigen::MatrixXd GetActuatorMatrix(); 21 | }; 22 | MODUS_DEFINE_SHARED(ContinuousDynamics); 23 | 24 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/dynamics/dynamics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | 5 | namespace modus 6 | { 7 | 8 | class Dynamics { 9 | public: 10 | virtual size_t NumContactForces() = 0; 11 | virtual size_t NumActuators() = 0; 12 | 13 | virtual void GetDynamicEquationsOfMotion(Eigen::MatrixXd& eq_lhs, 14 | Eigen::VectorXd& eq_rhs) = 0; 15 | virtual void GetContactForceBounds(Eigen::VectorXd& lb_cf, 16 | Eigen::VectorXd& ub_cf) = 0; 17 | virtual void GetActuatorForceBounds(Eigen::VectorXd& lb_cf, 18 | Eigen::VectorXd& ub_cf) = 0; 19 | }; 20 | using DynamicsPtr = std::shared_ptr; 21 | 22 | 23 | class DynamicEquationsOfMotionConstraint { 24 | public: 25 | double h_; 26 | 27 | void SetTimestep(double h) { h_ = h; }; 28 | 29 | virtual size_t NumContactForces() = 0; 30 | virtual size_t NumActuators() = 0; 31 | virtual void GetDynamicEquationsOfMotion(Eigen::MatrixXd& eq_lhs, 32 | Eigen::VectorXd& eq_rhs) = 0; 33 | }; 34 | using DynamicEquationsOfMotionConstraintPtr = 35 | std::shared_ptr; 36 | using DynamicEoMConstraintPtr = DynamicEquationsOfMotionConstraintPtr; 37 | 38 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/dynamics/gravito_intertial_dynamics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | class GravitoIntertialDynamics : public Aspect 9 | { 10 | public: 11 | virtual Eigen::MatrixXd GetMassMatrix() = 0; 12 | virtual Eigen::MatrixXd GetCoriolisMatrix() = 0; 13 | virtual Eigen::VectorXd GetPotentialForces() = 0; 14 | }; 15 | MODUS_DEFINE_SHARED(GravitoIntertialDynamics); 16 | 17 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/dynamics/mass_matrix.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace Eigen { 7 | using Matrix6d = Matrix; 8 | } 9 | 10 | namespace modus 11 | { 12 | 13 | class MassMatrix : public Aspect 14 | { 15 | protected: 16 | double mass_; 17 | Eigen::Matrix3d inertia_; 18 | 19 | public: 20 | double GetMass() { return mass_; } 21 | 22 | Eigen::Matrix3d GetInertia() { return inertia_; } 23 | 24 | // TODO Move to CPP 25 | Eigen::Matrix6d GetMassMatrix() { 26 | Eigen::Matrix6d M; 27 | M.setIdentity(); 28 | M.topRows(3) *= mass_; 29 | M.bottomRightCorner(3, 3) = inertia_; 30 | return M; 31 | } 32 | }; 33 | MODUS_DEFINE_SHARED(MassMatrix); 34 | 35 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/examples/basic_collision.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | namespace modus { 8 | namespace examples { 9 | 10 | class BasicCollision : public Collision { 11 | protected: 12 | System* system_; 13 | double margin_; 14 | 15 | public: 16 | BasicCollision(System* system); 17 | 18 | void SetMargin(double margin) { margin_ = margin; } 19 | 20 | Contacts GetContactPoints() override; 21 | 22 | Contacts Collide(BoxGeometry* box, PlaneGeometry* plane); 23 | }; 24 | 25 | } 26 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/examples/box.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace modus { 11 | namespace examples { 12 | 13 | // Add basic 1-1-1 box to the system. This includes the box state + dynamics + 14 | // actuation. 15 | Body* AddBox(System* system, bool actuated=false); 16 | 17 | class BoxMassMatrix : public MassMatrix { 18 | public: 19 | BoxMassMatrix(double mass, double x=1.0, double y=1.0, double z=1.0); 20 | }; 21 | 22 | // Quasi-static dynamics of a box with respect to the body frame. 23 | class BoxQuasiStatics : public GravitoIntertialDynamics { 24 | protected: 25 | Body* box_; 26 | 27 | public: 28 | BoxQuasiStatics(Body* box); 29 | 30 | Eigen::MatrixXd GetMassMatrix() override; 31 | Eigen::MatrixXd GetCoriolisMatrix() override; 32 | Eigen::VectorXd GetPotentialForces() override; 33 | }; 34 | 35 | // 6-Dof controls for an actuated box. 36 | class BoxControls : public Input { 37 | public: 38 | Eigen::VectorXd u_; 39 | size_t first_input_index_; 40 | 41 | void SetFirstInputIndex(size_t first); 42 | 43 | size_t NumInputs() override; 44 | size_t GetFirstInputIndex() override; 45 | 46 | Eigen::VectorXd GetInputs() override; 47 | void SetInputs(const Eigen::VectorXd& u) override; 48 | }; 49 | 50 | // Actuated dynamics of a box with respect to the body frame. 51 | class BoxActuator: public ActuatorDynamics { 52 | public: 53 | BoxActuator(Body* box) {} 54 | 55 | Eigen::MatrixXd GetActuatorMatrix() override; 56 | }; 57 | 58 | } 59 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/examples/plane.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace modus { 8 | namespace examples { 9 | 10 | Body* AddPlane(System* system, const Eigen::Vector3d& normal, double offset); 11 | 12 | } 13 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/examples/se3_state.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | namespace modus { 10 | namespace examples { 11 | 12 | Body* AddSE3Body(System* system); 13 | 14 | // se3 twist coordinates, ordering is [x, y, z, w_x, w_y, w_z] 15 | class SE3State : public State { 16 | protected: 17 | Body* body_; 18 | size_t first_dof_index_; 19 | Eigen::VectorXd q_, qd_, qdd_; 20 | Sophus::SE3d g_; 21 | 22 | public: 23 | SE3State(Body* body); 24 | 25 | void SetFirstDofIndex(size_t first) { first_dof_index_ = first; } 26 | 27 | size_t NumDofs() override { return 6; } 28 | size_t GetFirstDofIndex() override { return first_dof_index_; } 29 | 30 | Eigen::VectorXd GetPositions() override; 31 | void SetPositions(const Eigen::VectorXd& q) override; 32 | 33 | Eigen::VectorXd GetVelocities() override { return qd_; } 34 | void SetVelocities(const Eigen::VectorXd& qd) override { qd_ = qd; } 35 | 36 | Eigen::VectorXd GetAccelerations() override { return qdd_; } 37 | void SetAccelerations(const Eigen::VectorXd& qdd) override { qdd_ = qdd; } 38 | }; 39 | 40 | } 41 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/examples/sphere.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/examples/sphere.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/bounds.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | namespace modus { 10 | 11 | class Bounds3 { 12 | public: 13 | Eigen::Vector3d pMin; 14 | Eigen::Vector3d pMax; 15 | 16 | Bounds3(); 17 | Bounds3(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2); 18 | 19 | Eigen::Vector3d diagonal(); 20 | Eigen::Vector3d center(); 21 | float surfaceArea(); 22 | float volume(); 23 | int maxExtent(); 24 | Eigen::Vector3d offset(const Eigen::Vector3d& p) const; 25 | 26 | bool intersectRay(RayPtr ray); 27 | float distance2(const Eigen::Vector3d& p); 28 | 29 | friend std::ostream& operator<<(std::ostream& os, const Bounds3& b); 30 | }; 31 | 32 | Bounds3 unionBB(const Bounds3& b0, const Bounds3& b1); 33 | 34 | Bounds3 unionBP(const Bounds3& b, const Eigen::Vector3d& p); 35 | 36 | typedef std::shared_ptr Bounds3Ptr; 37 | 38 | } 39 | 40 | -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/box.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace modus 5 | { 6 | 7 | class BoxGeometry : public HalfedgeMeshGeometry { 8 | protected: 9 | double x_, y_, z_; 10 | 11 | public: 12 | BoxGeometry(double x, double y, double z, Link* link); 13 | 14 | int GetType() override { return BOX; } 15 | }; 16 | MODUS_DEFINE_SHARED(BoxGeometry); 17 | 18 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/cylinder.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/geometry/cylinder.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/distance.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | 5 | namespace modus { 6 | 7 | float pointTriangleDist2(const Eigen::Vector3d& p, 8 | const Eigen::Vector3d& v0, 9 | const Eigen::Vector3d& v1, 10 | const Eigen::Vector3d& v2); 11 | 12 | float pointAABBDist2(const Eigen::Vector3d& p, 13 | const Eigen::Vector3d& b_min, 14 | const Eigen::Vector3d& b_max); 15 | 16 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/edge.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include 5 | 6 | namespace modus 7 | { 8 | 9 | class Edge { 10 | public: 11 | HalfedgePtr halfedge; 12 | int index; 13 | }; 14 | 15 | typedef std::shared_ptr EdgePtr; 16 | 17 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/face.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | // #include 5 | #include 6 | #include 7 | 8 | 9 | namespace modus 10 | { 11 | 12 | class Face { 13 | public: 14 | HalfedgePtr halfedge; 15 | bool boundary; 16 | int index; 17 | 18 | Face(bool isBoundary = false); 19 | 20 | Bounds3 worldBound(); 21 | float distance2(const Eigen::Vector3d& point); 22 | 23 | int degree(); 24 | double area(); 25 | Eigen::Vector3d normal(); 26 | bool isBoundary(); 27 | Eigen::Vector3d position(); 28 | void position(Eigen::Vector3d& pos); 29 | }; 30 | 31 | typedef std::shared_ptr FacePtr; 32 | 33 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/geometry.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | namespace modus { 10 | 11 | enum GeometryType { 12 | BOX = 1, 13 | CYLINDER = 2, 14 | CAPSULE = 3, 15 | SPHERE = 4, 16 | PLANE = 5, 17 | MESH = 6 18 | }; 19 | 20 | class Geometry : public Aspect { 21 | public: 22 | 23 | virtual int GetType() = 0; 24 | 25 | virtual Transform GetTransformWorld() = 0; 26 | 27 | // virtual Bounds3 worldBound() = 0; 28 | // virtual float distance2(const Eigen::Vector3f& point) = 0; 29 | // virtual Eigen::Vector3f position() = 0; 30 | }; 31 | MODUS_DEFINE_SHARED(Geometry); 32 | 33 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/halfedge.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | class Halfedge; 9 | class Vertex; 10 | class Edge; 11 | class Face; 12 | 13 | typedef std::shared_ptr HalfedgePtr; 14 | typedef std::shared_ptr VertexPtr; 15 | typedef std::shared_ptr EdgePtr; 16 | typedef std::shared_ptr FacePtr; 17 | 18 | class Halfedge { 19 | public: 20 | HalfedgePtr next; 21 | HalfedgePtr twin; 22 | VertexPtr vertex; 23 | EdgePtr edge; 24 | FacePtr face; 25 | int index; 26 | 27 | Halfedge(); 28 | 29 | bool isBoundary(); 30 | 31 | Eigen::Vector3d vector(); 32 | 33 | void setNeighbors(HalfedgePtr next, HalfedgePtr twin, VertexPtr vertex, 34 | EdgePtr edge, FacePtr face); 35 | }; 36 | 37 | typedef std::shared_ptr HalfedgePtr; 38 | 39 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/icosphere.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/plane.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace modus 7 | { 8 | 9 | class PlaneGeometry : public HalfedgeMeshGeometry { 10 | protected: 11 | Eigen::Vector3d normal_; 12 | double offset_; 13 | 14 | public: 15 | PlaneGeometry(const Eigen::Vector3d& normal, double offset); 16 | 17 | int GetType() override { return PLANE; } 18 | 19 | Eigen::Vector3d GetNormal() { return normal_; } 20 | double GetOffset() { return offset_; } 21 | }; 22 | MODUS_DEFINE_SHARED(PlaneGeometry); 23 | 24 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/ray.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace modus { 5 | 6 | class Ray { 7 | 8 | }; 9 | 10 | typedef std::shared_ptr RayPtr; 11 | 12 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/geometry/vertex.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | namespace modus 8 | { 9 | 10 | class Vertex { 11 | public: 12 | HalfedgePtr halfedge; 13 | Eigen::Vector3d position; 14 | int index; 15 | 16 | Vertex(); 17 | 18 | Eigen::Vector3d normal(); 19 | }; 20 | 21 | typedef std::shared_ptr VertexPtr; 22 | 23 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/graphics/opengl.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace modus { 8 | 9 | void loadOpenGL(); 10 | 11 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/integration/forward_euler.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace modus { 7 | 8 | class ForwardEuler : public Integration { 9 | protected: 10 | double timestep_; 11 | System* system_; 12 | 13 | public: 14 | ForwardEuler(System* system, double timestep=0.01); 15 | 16 | void SetTimestep(double h) { timestep_ = h; } 17 | 18 | void GetODE(Eigen::MatrixXd& lhs, Eigen::VectorXd& rhs) override; 19 | }; 20 | 21 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/integration/integration.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace modus 7 | { 8 | 9 | class Integration : public Aspect { 10 | public: 11 | virtual void GetODE(Eigen::MatrixXd& lhs, Eigen::VectorXd& rhs) = 0; 12 | }; 13 | MODUS_DEFINE_SHARED(Integration); 14 | 15 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/contact_constraint.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | namespace modus 8 | { 9 | 10 | struct ContactConstraint { 11 | enum Type { 12 | CC_NORMAL = 1, 13 | CC_TANGENT_X = 2, 14 | CC_TANGENT_Y = 3, 15 | }; 16 | 17 | // A contact constraint is of the form lb <= nx <= ub. 18 | Eigen::RowVectorXd n; 19 | double lb; 20 | double ub; 21 | 22 | // Type of constraint. 23 | Type type; 24 | 25 | // Source contact which generated the constraint. 26 | ContactPtr source; 27 | }; 28 | 29 | std::ostream& operator<<(std::ostream& os, const ContactConstraint& c); 30 | 31 | using ContactConstraintPtr = std::shared_ptr; 32 | using ContactConstraints = std::vector; 33 | 34 | std::ostream& operator<<(std::ostream& os, const ContactConstraints& C); 35 | 36 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/contact_kinematics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace modus 7 | { 8 | 9 | // This class computes kinematic contact constraints, a.k.a. normal and tangent 10 | // velocity constraints. 11 | class ContactKinematics : public Aspect { 12 | protected: 13 | System* system_; 14 | 15 | public: 16 | ContactKinematics(System* system); 17 | 18 | size_t NumNormalVelocityConstraints(); 19 | 20 | size_t NumTangentVelocityConstraints(); 21 | 22 | ContactConstraints GetNormalVelocityConstraints(const std::string& cs_mode, 23 | bool filter_contacts=false); 24 | 25 | ContactConstraints GetTangentVelocityConstraints(const std::string& cs_mode, 26 | const std::string& ss_mode, 27 | bool filter_contacts=false); 28 | }; 29 | MODUS_DEFINE_SHARED(ContactKinematics); 30 | 31 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/jacobian.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace modus 7 | { 8 | 9 | // Jacobian at a link. 10 | class Jacobian : public Aspect { 11 | public: 12 | /** 13 | * @brief Let d(x) be the position of a point in space along a direction, 14 | * where x is the body state. This function returns the gradient vector ∂d/∂x. 15 | * 16 | * @param point 17 | * @param direction 18 | * @return Eigen::RowVectorXd 19 | */ 20 | virtual Eigen::RowVectorXd 21 | GetGradientAtPointInDirection(const Eigen::Vector3d& point, 22 | const Eigen::Vector3d& direction) = 0; 23 | }; 24 | MODUS_DEFINE_SHARED(Jacobian); 25 | 26 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/jacobian_body.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | class BodyJacobian : public Jacobian { 9 | public: 10 | Link* link_; 11 | Eigen::MatrixXd body_; 12 | 13 | BodyJacobian(Link* link); 14 | 15 | void SetBodyJacobian(const Eigen::MatrixXd& body) { body_ = body; } 16 | 17 | Eigen::RowVectorXd 18 | GetGradientAtPointInDirection(const Eigen::Vector3d& point, 19 | const Eigen::Vector3d& direction) override; 20 | }; 21 | 22 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/jacobian_floating.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | // Body Jacobian for an unconstrained rigid body. 9 | class FloatingDofsJacobian : public Jacobian { 10 | public: 11 | Link* link_; 12 | 13 | FloatingDofsJacobian(Link* link); 14 | 15 | Eigen::RowVectorXd 16 | GetGradientAtPointInDirection(const Eigen::Vector3d& point, 17 | const Eigen::Vector3d& direction) override; 18 | }; 19 | 20 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/jacobian_geometric.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | /** 9 | * @brief A geometric jacobian. The linear jacobian is in world frame. The 10 | * angular jacobian 11 | * 12 | */ 13 | class GeometricJacobian : public Jacobian { 14 | public: 15 | Link* link_; 16 | 17 | Eigen::MatrixXd linear_; 18 | Eigen::MatrixXd rotational_; 19 | 20 | GeometricJacobian(Link* link); 21 | 22 | void SetLinearJacobian(const Eigen::MatrixXd& linear); 23 | void SetRotationalJacobian(const Eigen::MatrixXd& rotational); 24 | 25 | Eigen::RowVectorXd 26 | GetGradientAtPointInDirection(const Eigen::Vector3d& point, 27 | const Eigen::Vector3d& direction) override; 28 | }; 29 | 30 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/jacobian_spatial.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace modus 6 | { 7 | 8 | class SpatialJacobian : public Jacobian { 9 | public: 10 | Eigen::MatrixXd spatial_; 11 | 12 | void SetSpatialJacobian(const Eigen::MatrixXd& spatial) { spatial_ = spatial; } 13 | 14 | Eigen::RowVectorXd 15 | GetGradientAtPointInDirection(const Eigen::Vector3d& point, 16 | const Eigen::Vector3d& direction) override; 17 | }; 18 | 19 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/kinematics/kinematics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace modus 7 | { 8 | 9 | // Interface for multiple (possibly articulated) rigid body kinematics. 10 | class Kinematics : public Aspect { 11 | public: 12 | virtual Jacobian* GetJacobian(int body_id, int link_id) = 0; 13 | }; 14 | 15 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/geometry/convex_hull.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | /** 5 | * @brief Compute the convex hull of the input points. 6 | * 7 | * @param pts 8 | * @param eps 9 | * @return Eigen::MatrixXi vertex-facet incidence matrix 10 | */ 11 | Eigen::MatrixXi convex_hull(const Eigen::MatrixXd& pts, double eps); -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/geometry/halfspace_intersection.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | 5 | /** 6 | * @brief Compute the incidence graph of the halfspace intersection A - b<= 0. 7 | * 8 | * @param A 9 | * @param b 10 | * @param eps 11 | * @param int_pt 12 | * @return IncidenceGraph* 13 | */ 14 | IncidenceGraph* halfspace_intersection(const Eigen::MatrixXd& A, 15 | const Eigen::VectorXd& b, 16 | double eps, 17 | const Eigen::VectorXd* int_pt=nullptr); -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/geometry/hyperplane.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace modus 5 | { 6 | 7 | class Hyperplane { 8 | public: 9 | 10 | }; 11 | 12 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/geometry/interior_point.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | 5 | Eigen::VectorXd interior_point(const Eigen::MatrixXd& A, 6 | const Eigen::VectorXd& b, double eps); //, 7 | // const Eigen::MatrixXd* Aeq=nullptr, 8 | // const Eigen::VectorXd* beq=nullptr); 9 | 10 | namespace modus 11 | { 12 | /** 13 | * @brief Compute an interior point satisfying input cs modes. 14 | * 15 | * @param N n x d matrix of normal constraints 16 | * @param cs_mode contacting-separating mode 17 | * @param eps tolerance 18 | * @return Eigen::VectorXd 19 | */ 20 | Eigen::VectorXd InteriorPoint(const Eigen::MatrixXd& N, 21 | const std::string& cs_mode, 22 | double eps); 23 | 24 | /** 25 | * @brief Compute an interior point satisfying the input cs + ss modes. 26 | * 27 | * @param N n x d matrix of normal constraints 28 | * @param T nk x d matrix of tangent constraints 29 | * @param cs_mode contacting-separating mode 30 | * @param ss_mode sliding-sticking mode 31 | * @param eps tolerance 32 | * @return Eigen::VectorXd 33 | */ 34 | Eigen::VectorXd InteriorPoint(const Eigen::MatrixXd& N, 35 | const Eigen::MatrixXd& T, 36 | const std::string& cs_mode, 37 | const std::string& ss_mode, 38 | double eps); 39 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/graph.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace modus 4 | { 5 | 6 | class GraphInterface { 7 | 8 | }; 9 | 10 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/graph/contact_mode_graph.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace modus 7 | { 8 | 9 | class ContactMode; 10 | using ContactModePtr = std::shared_ptr; 11 | 12 | class ContactMode { 13 | public: 14 | std::string GetCSMode(); 15 | std::string GetSSMode(); 16 | std::vector AdjacentModes(); 17 | }; 18 | 19 | class ContactModeGraph { 20 | public: 21 | std::vector GetVertices(); 22 | ContactModePtr GetRoot(); 23 | }; 24 | 25 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/graph/csmode_graph.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/modes/graph/csmode_graph.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/graph/incidence_graph.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace modus 5 | { 6 | 7 | class NodeWrapper : public ContactMode { 8 | public: 9 | 10 | }; 11 | 12 | using NodeWrapperPtr = std::shared_ptr; 13 | 14 | class IncidenceGraphWrapper : public ContactModeGraph { 15 | public: 16 | 17 | }; 18 | 19 | using IncidenceGraphWrapperPtr = std::shared_ptr; 20 | 21 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/graph/nested_graph.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/modes/graph/nested_graph.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/graph/product_graph.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/modes/graph/product_graph.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/graph/ssmode_graph.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/modes/graph/ssmode_graph.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/modus.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/preprocess/csmode_preprocessor.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/modes/preprocess/csmode_preprocessor.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/preprocess/nullspace_preprocessor.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/modes/preprocess/nullspace_preprocessor.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/modes/preprocess/ssmode_preprocessor.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/modes/preprocess/ssmode_preprocessor.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/bounds.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace modus 7 | { 8 | 9 | class VariableBound { 10 | public: 11 | virtual void Apply(Eigen::VectorXd& lb, Eigen::VectorXd& ub) = 0; 12 | }; 13 | 14 | using VariableBoundPtr = std::shared_ptr; 15 | using VariableBounds = std::vector; 16 | 17 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/cost.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | namespace modus 9 | { 10 | 11 | // Quadratic cost: 12 | // 1/2xᵀHx + xᵀg 13 | class Cost { 14 | public: 15 | virtual void AddToObjective(Eigen::MatrixXd& H, Eigen::VectorXd& g) = 0; 16 | }; 17 | 18 | using CostPtr = std::shared_ptr; 19 | using Costs = std::vector; 20 | 21 | class VelocityCost : public Cost { 22 | protected: 23 | System* system_; 24 | State* state_; 25 | Eigen::VectorXd target_velocity_; 26 | double scale_; // 27 | 28 | public: 29 | VelocityCost(State* state, System* system, const Eigen::VectorXd& target); 30 | 31 | void SetScale(double scale) { scale_ = scale; } 32 | 33 | void AddToObjective(Eigen::MatrixXd& H, Eigen::VectorXd& g); 34 | }; 35 | 36 | class ContactForceCost : public Cost { 37 | protected: 38 | System* system_; 39 | double scale_; 40 | 41 | public: 42 | ContactForceCost(System* system); 43 | 44 | void SetScale(double scale) { scale_ = scale; } 45 | 46 | void AddToObjective(Eigen::MatrixXd& H, Eigen::VectorXd& g); 47 | }; 48 | 49 | class InputCost : public Cost { 50 | protected: 51 | System* system_; 52 | Input* input_; 53 | double scale_; 54 | 55 | public: 56 | InputCost(Input* input, System* system); 57 | 58 | void SetScale(double scale) { scale_ = scale; } 59 | 60 | void AddToObjective(Eigen::MatrixXd& H, Eigen::VectorXd& g); 61 | }; 62 | 63 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/qp.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace modus 7 | { 8 | 9 | // Quadratic program. 10 | struct QuadraticProgram { 11 | // Quadratic cost: 12 | // 1/2xᵀHx + xᵀg 13 | Eigen::MatrixXd H; 14 | Eigen::VectorXd g; 15 | 16 | // Constraint matrix and constraint bounds: 17 | // lbA ≤ Ax ≤ ubA 18 | Eigen::MatrixXd A; 19 | Eigen::VectorXd lbA; 20 | Eigen::VectorXd ubA; 21 | 22 | // Variable bounds: 23 | // lb ≤ x ≤ ub 24 | Eigen::VectorXd lb; 25 | Eigen::VectorXd ub; 26 | 27 | QuadraticProgram(int n_v, int n_c); 28 | }; 29 | 30 | using QP = QuadraticProgram; 31 | using QPPtr = std::shared_ptr; 32 | 33 | std::ostream& operator<<(std::ostream& os, const QP& qp); 34 | 35 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/qpOASES.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | namespace modus 9 | { 10 | 11 | class qpOASESSolver { 12 | public: 13 | using real_t = qpOASES::real_t; 14 | using MatrixXr = Eigen::Matrix; 15 | using VectorXr = Eigen::Matrix; 16 | using QProblemPtr = std::shared_ptr; 17 | 18 | MatrixXr H_; 19 | VectorXr g_; 20 | MatrixXr A_; 21 | VectorXr lb_; 22 | VectorXr ub_; 23 | VectorXr lbA_; 24 | VectorXr ubA_; 25 | VectorXr x_; 26 | QProblemPtr qp_solver_; 27 | 28 | Eigen::VectorXd GetSolution(); 29 | real_t GetCost(); 30 | void Solve(QPPtr qp); 31 | void InitQP(QPPtr qp); 32 | void UpdateVectors(QPPtr qp); 33 | }; 34 | 35 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/qpcc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace modus 11 | { 12 | 13 | // Quadratic program with contact constraints. 14 | // 15 | // Variable ordering is [q, q̇, τ, λ], where τ are the actuator forces and λ are 16 | // the contact forces. 17 | class QuadraticProgramWithContactConstraints { 18 | public: 19 | Costs costs_; 20 | VariableBounds bounds_; 21 | System* system_; 22 | QPPtr qp_; 23 | 24 | void SetSystem(System* system) { system_ = system; } 25 | 26 | void AddCost(CostPtr cost); 27 | 28 | void AddVariableBound(VariableBoundPtr bound); 29 | 30 | // Create quadratic program cost, constraint, and bound matrices. 31 | QPPtr CreateQuadraticProgram(); 32 | 33 | // Update quadratic program's bounds. 34 | QPPtr UpdateQuadraticProgramBounds(const std::string& cs_mode, 35 | const std::string& ss_mode); 36 | }; 37 | 38 | using QPCC = QuadraticProgramWithContactConstraints; 39 | 40 | using QPCCPtr = std::shared_ptr; 41 | 42 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/solver.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | namespace modus 10 | { 11 | 12 | class QPCCSolver { 13 | public: 14 | virtual void Solve(QPCCPtr qpcc) = 0; 15 | 16 | virtual Eigen::VectorXd GetSolution() = 0; 17 | }; 18 | 19 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/solvers/MICP.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/qpcc/solvers/MICP.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/solvers/SNOPT.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/include/modus/qpcc/solvers/SNOPT.hpp -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/solvers/adjacent_modes.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | 5 | namespace qpcc 6 | { 7 | 8 | class AdjacentModesSolver : public QPCCSolver { 9 | public: 10 | QPCCResultPtr Solve(QPCCProblemPtr problem, CSGraphPtr cs_graph, SSGraphMap ss_graphs, QPCCOptionsPtr options); 11 | }; 12 | 13 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/qpcc/solvers/full_modes.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace modus 4 | { 5 | 6 | class FullModeSolver : public QPCCSolver { 7 | public: 8 | size_t n_contacts_; 9 | std::vector modes_; 10 | 11 | void SetModes(const std::vector& modes, size_t n); 12 | 13 | void Solve(QPCCPtr qpcc); 14 | 15 | Eigen::VectorXd GetSolution(); 16 | }; 17 | 18 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/aspect.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | 5 | namespace modus 6 | { 7 | 8 | class Aspect : public std::enable_shared_from_this { 9 | public: 10 | virtual ~Aspect() = default; 11 | }; 12 | MODUS_DEFINE_SHARED(Aspect); 13 | 14 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/body.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace modus 11 | { 12 | 13 | class Body : public Composite { 14 | protected: 15 | int index_; 16 | 17 | public: 18 | std::vector links_; 19 | 20 | Body(); 21 | 22 | void SetIndex(int index) { index_ = index; } 23 | int GetIndex(); 24 | 25 | size_t NumLinks(); 26 | 27 | Link* GetRootLink(); 28 | Link* GetLink(int link_id); 29 | 30 | Link* CreateLink(); 31 | }; 32 | MODUS_DEFINE_SHARED(Body); 33 | 34 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/composite.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace modus 11 | { 12 | 13 | class Composite : public std::enable_shared_from_this { 14 | protected: 15 | using AspectMap = std::map >; 16 | AspectMap aspects_; 17 | 18 | public: 19 | virtual ~Composite() = default; 20 | Composite() = default; 21 | 22 | Composite(const Composite&) = delete; 23 | 24 | Composite(Composite&&) = delete; 25 | 26 | Composite& operator=(const Composite&) = delete; 27 | 28 | Composite& operator=(Composite&&) = delete; 29 | 30 | template 31 | bool Has() const; 32 | 33 | template 34 | T* Get(); 35 | 36 | // template 37 | // const T* Get() const; 38 | 39 | // template 40 | // T* Create(Args&&... args); 41 | 42 | template 43 | T* Create(T* aspect); 44 | }; 45 | MODUS_DEFINE_SHARED(Composite); 46 | 47 | } 48 | 49 | #include -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/detail/composite.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace modus 5 | { 6 | 7 | template 8 | bool Composite::Has() const 9 | { 10 | return (Get() != nullptr); 11 | } 12 | 13 | template 14 | T* Composite::Get() 15 | { 16 | AspectMap::iterator it = aspects_.find( typeid(T) ); 17 | if (aspects_.end() == it) { 18 | return nullptr; 19 | } 20 | return static_cast(it->second.get()); 21 | } 22 | 23 | // template 24 | // const T* Composite::Get() const 25 | // { 26 | // return const_cast(this)->Get(); 27 | // } 28 | 29 | // template 30 | // T* Composite::Create(Args&&... args) 31 | // { 32 | // T* aspect = new T(std::forward(args)...); 33 | // aspects_[typeid(B)] = std::unique_ptr(aspect); 34 | 35 | // return aspect; 36 | // } 37 | 38 | template 39 | T* Composite::Create(T* aspect) 40 | { 41 | // T* aspect = new T(std::forward(args)...); 42 | aspects_[typeid(B)] = std::unique_ptr(aspect); 43 | 44 | return aspect; 45 | } 46 | 47 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/input.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace modus 7 | { 8 | 9 | class Input : public Aspect {//, public std::enable_shared_from_this { 10 | public: 11 | virtual size_t NumInputs() = 0; 12 | virtual size_t GetFirstInputIndex() = 0; 13 | 14 | virtual Eigen::VectorXd GetInputs() = 0; 15 | virtual void SetInputs(const Eigen::VectorXd& u) = 0; 16 | }; 17 | MODUS_DEFINE_SHARED(Input); 18 | 19 | } 20 | -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/link.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | namespace modus 10 | { 11 | 12 | class Link : public Composite { 13 | protected: 14 | int index_; 15 | 16 | public: 17 | void SetIndex(int index) { index_ = index; } 18 | int GetIndex() { return index_; } 19 | }; 20 | MODUS_DEFINE_SHARED(Link); 21 | 22 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/state.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace modus 8 | { 9 | 10 | class State : public Aspect { //, public std::enable_shared_from_this { 11 | public: 12 | virtual size_t NumDofs() = 0; 13 | virtual size_t GetFirstDofIndex() = 0; 14 | 15 | virtual Eigen::VectorXd GetPositions() = 0; 16 | virtual void SetPositions(const Eigen::VectorXd& q) = 0; 17 | 18 | virtual Eigen::VectorXd GetVelocities() = 0; 19 | virtual void SetVelocities(const Eigen::VectorXd& qd) = 0; 20 | 21 | virtual Eigen::VectorXd GetAccelerations() = 0; 22 | virtual void SetAccelerations(const Eigen::VectorXd& qdd) = 0; 23 | }; 24 | MODUS_DEFINE_SHARED(State); 25 | 26 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/system.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | namespace modus 14 | { 15 | 16 | class System : public Composite { 17 | protected: 18 | std::vector bodies_; 19 | 20 | public: 21 | System(); 22 | 23 | size_t NumBodies(); 24 | 25 | Body* GetBody(int body_id); 26 | 27 | Body* CreateBody(); 28 | }; 29 | MODUS_DEFINE_SHARED(System); 30 | 31 | class SystemState : public State { 32 | protected: 33 | System* system_; 34 | 35 | public: 36 | SystemState(System* system); 37 | 38 | size_t NumDofs() override; 39 | size_t GetFirstDofIndex() override; 40 | 41 | Eigen::VectorXd GetPositions() override; 42 | void SetPositions(const Eigen::VectorXd& q) override; 43 | 44 | Eigen::VectorXd GetVelocities() override; 45 | void SetVelocities(const Eigen::VectorXd& qd) override; 46 | 47 | Eigen::VectorXd GetAccelerations() override; 48 | void SetAccelerations(const Eigen::VectorXd& qdd) override; 49 | }; 50 | MODUS_DEFINE_SHARED(SystemState); 51 | 52 | class SystemInput : public Input { 53 | protected: 54 | System* system_; 55 | 56 | public: 57 | SystemInput(System* system); 58 | 59 | size_t NumInputs() override; 60 | size_t GetFirstInputIndex() override; 61 | 62 | Eigen::VectorXd GetInputs() override; 63 | void SetInputs(const Eigen::VectorXd& u) override; 64 | }; 65 | MODUS_DEFINE_SHARED(SystemInput); 66 | 67 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/include/modus/system/transform.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | namespace modus 8 | { 9 | 10 | class Transform : public Aspect {//, public std::enable_shared_from_this { 11 | public: 12 | Eigen::Matrix3d rotation_; 13 | Eigen::Vector3d translation_; 14 | 15 | Transform(); 16 | 17 | void SetRotation(const Eigen::Matrix3d& rotation); 18 | Eigen::Matrix3d GetRotation(); 19 | 20 | void SetTranslation(const Eigen::Vector3d& translation); 21 | Eigen::Vector3d GetTranslation(); 22 | 23 | void Invert(); 24 | Transform Inverted(); 25 | }; 26 | MODUS_DEFINE_SHARED(Transform); 27 | 28 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | # Source files. 4 | set(MODUS_COMMON_SOURCE 5 | assert.cpp 6 | # backward.cpp 7 | # serialization.cpp 8 | convex_hull.cpp 9 | linear_algebra.cpp 10 | # logging.cpp 11 | ) 12 | 13 | add_library(modus_common SHARED ${MODUS_COMMON_SOURCE}) 14 | 15 | # Define headers for this library. PUBLIC headers are used for compiling the 16 | # library, and will be added to install paths. 17 | target_include_directories(modus_common 18 | PUBLIC 19 | $ 20 | $ 21 | ${EIGEN3_INCLUDE_DIR} 22 | ${QHULL_INCLUDE_DIR} 23 | ${CMAKE_INSTALL_PREFIX}/include 24 | ${PROJECT_SOURCE_DIR}/include 25 | ) 26 | 27 | # Add compiler requirements here. 28 | target_compile_features(modus_common PUBLIC cxx_std_17) 29 | 30 | # Set up assert behavior. 31 | if (MODUS_ASSERTS_ENABLED) 32 | target_compile_definitions(modus_common PUBLIC MODUS_ASSERTS_ENABLED) 33 | if (${MODUS_DEFAULT_ASSERT_LEVEL} STREQUAL "Debug") 34 | target_compile_definitions(modus_common PUBLIC MODUS_DEFAULT_ASSERT_LEVEL_DEBUG) 35 | elseif(${MODUS_DEFAULT_ASSERT_LEVEL} STREQUAL "Warning") 36 | target_compile_definitions(modus_common PUBLIC MODUS_DEFAULT_ASSERT_LEVEL_WARNING) 37 | elseif(${MODUS_DEFAULT_ASSERT_LEVEL} STREQUAL "Error") 38 | target_compile_definitions(modus_common PUBLIC MODUS_DEFAULT_ASSERT_LEVEL_ERROR) 39 | elseif(${MODUS_DEFAULT_ASSERT_LEVEL} STREQUAL "Fatal") 40 | target_compile_definitions(modus_common PUBLIC MODUS_DEFAULT_ASSERT_LEVEL_FATAL) 41 | endif() 42 | endif() 43 | 44 | # Link libraries. 45 | # target_link_libraries(modus_common gcc_s dw ${QHULLCPP_LIBRARY} ${QHULL_R_LIBRARY}) 46 | target_link_libraries(modus_common gcc_s ${QHULLCPP_LIBRARY} ${QHULL_R_LIBRARY}) 47 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_common/backward.cpp: -------------------------------------------------------------------------------- 1 | // Pick your poison. 2 | // 3 | // On GNU/Linux, you have few choices to get the most out of your stack trace. 4 | // 5 | // By default you get: 6 | // - object filename 7 | // - function name 8 | // 9 | // In order to add: 10 | // - source filename 11 | // - line and column numbers 12 | // - source code snippet (assuming the file is accessible) 13 | 14 | // Install one of the following library then uncomment one of the macro (or 15 | // better, add the detection of the lib and the macro definition in your build 16 | // system) 17 | 18 | // - apt-get install libdw-dev ... 19 | // - g++/clang++ -ldw ... 20 | #define BACKWARD_HAS_DW 1 21 | 22 | // - apt-get install binutils-dev ... 23 | // - g++/clang++ -lbfd ... 24 | // #define BACKWARD_HAS_BFD 1 25 | 26 | #include 27 | 28 | namespace backward { 29 | 30 | backward::SignalHandling sh; 31 | 32 | } // namespace backward 33 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_common/serialization.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/src/modus_common/serialization.cpp -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | # Source files. 4 | set(MODUS_GEOMETRY_SOURCE 5 | bounds.cpp 6 | distance.cpp 7 | edge.cpp 8 | vertex.cpp 9 | face.cpp 10 | halfedge.cpp 11 | halfedgemesh.cpp 12 | box.cpp 13 | plane.cpp 14 | ) 15 | 16 | add_library(modus_geometry SHARED ${MODUS_GEOMETRY_SOURCE}) 17 | 18 | target_include_directories(modus_geometry 19 | PUBLIC 20 | $ 21 | $ 22 | ${EIGEN3_INCLUDE_DIR} 23 | ${PROJECT_SOURCE_DIR}/include 24 | ) 25 | 26 | # Add compiler requirements here. 27 | target_compile_features(modus_geometry PUBLIC cxx_std_17) 28 | 29 | # Link libraries. 30 | target_link_libraries(modus_geometry modus_common) 31 | 32 | # set_property(TARGET modus_geometry PROPERTY POSITION_INDEPENDENT_CODE ON) 33 | 34 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/box.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | using namespace modus; 6 | 7 | modus::BoxGeometry::BoxGeometry(double x0, double y0, double z0, Link* link) 8 | : x_(x0), y_(y0), z_(z0), HalfedgeMeshGeometry(link) 9 | { 10 | // Create box vertices. 11 | std::vector vertices; 12 | std::vector X = {-x0/2, x0/2}; 13 | std::vector Y = {-y0/2, y0/2}; 14 | std::vector Z = {-z0/2, z0/2}; 15 | int k = 0; 16 | for (auto x : X) { 17 | for (auto y : Y) { 18 | for (auto z : Z) { 19 | vertices.push_back(Eigen::Vector3d(x, y, z)); 20 | } 21 | } 22 | } 23 | // Build from convex hull. 24 | BuildConvex(vertices); 25 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/edge.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/src/modus_geometry/edge.cpp -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/face.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | using namespace modus; 8 | 9 | Face::Face(bool isBoundary) : boundary(isBoundary) { 10 | this->halfedge = nullptr; 11 | } 12 | 13 | Bounds3 Face::worldBound() { 14 | const Eigen::Vector3d& p0 = halfedge->vertex->position; 15 | const Eigen::Vector3d& p1 = halfedge->next->vertex->position; 16 | const Eigen::Vector3d& p2 = halfedge->next->next->vertex->position; 17 | return unionBP(Bounds3(p0, p1), p2); 18 | } 19 | 20 | float Face::distance2(const Eigen::Vector3d& p) { 21 | const Eigen::Vector3d& p0 = halfedge->vertex->position; 22 | const Eigen::Vector3d& p1 = halfedge->next->vertex->position; 23 | const Eigen::Vector3d& p2 = halfedge->next->next->vertex->position; 24 | return pointTriangleDist2(p, p0, p1, p2); 25 | } 26 | 27 | int Face::degree() { 28 | return 0; 29 | } 30 | 31 | bool Face::isBoundary() { 32 | return boundary; 33 | } 34 | 35 | Eigen::Vector3d Face::position() { 36 | Eigen::Vector3d pos; 37 | position(pos); 38 | return pos; 39 | } 40 | 41 | void Face::position(Eigen::Vector3d& pos) { 42 | pos.setZero(); 43 | HalfedgePtr h = halfedge; 44 | int n = 0; 45 | while (true) { 46 | pos += h->vertex->position; 47 | h = h->next; 48 | n += 1; 49 | if (h == halfedge) { 50 | break; 51 | } 52 | } 53 | assert(n == 3); 54 | pos /= n; 55 | } 56 | 57 | Eigen::Vector3d Face::normal() { 58 | Eigen::Vector3d n; 59 | n.setZero(); 60 | HalfedgePtr h = halfedge; 61 | while (true) { 62 | const Eigen::Vector3d& pi = h->vertex->position; 63 | const Eigen::Vector3d& pj = h->next->vertex->position; 64 | n += pi.cross(pj); 65 | h = h->next; 66 | if (h == halfedge) { 67 | break; 68 | } 69 | } 70 | n.normalize(); 71 | return n; 72 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/halfedge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | using namespace modus; 7 | 8 | Halfedge::Halfedge() : index(0) { 9 | this->next = nullptr; 10 | this->twin = nullptr; 11 | this->vertex = nullptr; 12 | this->edge = nullptr; 13 | this->face = nullptr; 14 | } 15 | 16 | bool Halfedge::isBoundary() { 17 | return face->isBoundary(); 18 | } 19 | 20 | 21 | void Halfedge::setNeighbors(HalfedgePtr n, HalfedgePtr t, VertexPtr v, 22 | EdgePtr e, FacePtr f) { 23 | this->next = n; 24 | this->twin = t; 25 | this->vertex = v; 26 | this->edge = e; 27 | this->face = f; 28 | } 29 | 30 | Eigen::Vector3d Halfedge::vector() { 31 | return this->next->vertex->position - this->vertex->position; 32 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/opengl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | void modus::loadOpenGL() { 5 | if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress)) { 6 | std::cout << "Failed to initialize GLAD" << std::endl; 7 | } 8 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/plane.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace modus; 4 | 5 | modus::PlaneGeometry::PlaneGeometry(const Eigen::Vector3d& normal,double offset) 6 | : normal_(normal), offset_(offset) 7 | { 8 | } 9 | 10 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/transforms.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | void modus::triangleFrame(const Eigen::Vector3f& v0, 5 | const Eigen::Vector3f& v1, 6 | const Eigen::Vector3f& v2, 7 | Eigen::Matrix3f& R, 8 | Eigen::Vector3f& t) { 9 | // Translation. 10 | t = (v0 + v1 + v2)/3.0; 11 | // Rotation. 12 | Eigen::Vector3f x = (v1 - v0).normalized(); 13 | Eigen::Vector3f y = (v2 - v0).normalized(); 14 | Eigen::Vector3f z = x.cross(y).normalized(); 15 | y = z.cross(x).normalized(); 16 | R.col(0) = x; 17 | R.col(1) = y; 18 | R.col(2) = z; 19 | } 20 | 21 | void modus::inverse(Eigen::Matrix3f& R, Eigen::Vector3f& t) { 22 | R.transposeInPlace(); 23 | t = -R*t; 24 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_geometry/vertex.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace modus; 4 | 5 | Vertex::Vertex() { 6 | this->halfedge = nullptr; 7 | } 8 | 9 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | # Source files. 4 | set(MODUS_KINEMATICS_SOURCE 5 | contact_constraint.cpp 6 | contact_kinematics.cpp 7 | jacobian_body.cpp 8 | jacobian_floating.cpp 9 | jacobian_geometric.cpp 10 | jacobian_spatial.cpp 11 | ) 12 | 13 | add_library(modus_kinematics SHARED ${MODUS_KINEMATICS_SOURCE}) 14 | 15 | # Define headers for this library. PUBLIC headers are used for compiling the 16 | # library, and will be added to install paths. 17 | target_include_directories(modus_kinematics 18 | PUBLIC 19 | # $ 20 | # $ 21 | # ${EIGEN3_INCLUDE_DIR} 22 | # ${CMAKE_INSTALL_PREFIX}/include 23 | ${PROJECT_SOURCE_DIR}/include 24 | ) 25 | 26 | # Add compiler requirements here. 27 | target_compile_features(modus_kinematics PUBLIC cxx_std_17) 28 | 29 | # Link libraries. 30 | # target_link_libraries(modus_kinematics modus_common modus_system gcc_s dw) 31 | target_link_libraries(modus_kinematics modus_common modus_system gcc_s) 32 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_kinematics/contact_constraint.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | using namespace modus; 6 | 7 | std::ostream& modus::operator<<(std::ostream& os, const ContactConstraint& c) 8 | { 9 | std::string type; 10 | switch (c.type) { 11 | case ContactConstraint::CC_NORMAL: 12 | type = "n"; 13 | break; 14 | case ContactConstraint::CC_TANGENT_X: 15 | type = "x"; 16 | break; 17 | case ContactConstraint::CC_TANGENT_Y: 18 | type = "y"; 19 | break; 20 | default: 21 | MODUS_ASSERT(false); 22 | } 23 | os << "h: {" << std::setw(4) << c.lb << " <= " << PrettyMatrix(c.n,"",6) 24 | << " <= " << c.ub << "}," << " type: " << type << ", src: " 25 | << PrettyMatrix(c.source->point_A.transpose(),"",3);// << " " 26 | // << PrettyMatrix(c.direction.transpose(),"",3); 27 | return os; 28 | } 29 | 30 | std::ostream& modus::operator<<(std::ostream& os, const ContactConstraints& C) 31 | { 32 | for (size_t i = 0; i < C.size(); i++) { 33 | os << *C[i]; 34 | if (i < C.size() - 1) { 35 | os << std::endl; 36 | } 37 | } 38 | return os; 39 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_kinematics/jacobian_body.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace modus; 6 | 7 | 8 | modus::BodyJacobian:: BodyJacobian(Link* link) 9 | { 10 | link_ = link; 11 | } 12 | 13 | Eigen::RowVectorXd modus::BodyJacobian::GetGradientAtPointInDirection 14 | (const Eigen::Vector3d& point, const Eigen::Vector3d& direction) 15 | { 16 | // Get transform of link. 17 | Transform* T = link_->Get(); 18 | MODUS_ASSERT(T, "Error: link is missing Transform aspect"); 19 | 20 | // Convert the body jacobian into a distance gradient at the input point along 21 | // the input direction. First, transform the point and direction into the body 22 | // frame. Then use the twist to point velocity equation ω × p + v. 23 | Eigen::Matrix3d R_inv = T->GetRotation(); 24 | Eigen::Vector3d t_inv = T->GetTranslation(); 25 | InverseTransform(R_inv, t_inv); 26 | Eigen::Vector3d point_body = R_inv * point + t_inv; 27 | Eigen::Vector3d direction_body = R_inv * direction; 28 | Eigen::Matrix3d P = Adjoint(point_body); 29 | Eigen::MatrixXd V = body_.topRows(3) + P.transpose() * body_.bottomRows(3); 30 | return direction_body.transpose() * V; 31 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_kinematics/jacobian_floating.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace modus; 6 | 7 | 8 | modus:: FloatingDofsJacobian:: FloatingDofsJacobian(Link* link) 9 | { 10 | link_ = link; 11 | } 12 | 13 | Eigen::RowVectorXd modus:: FloatingDofsJacobian::GetGradientAtPointInDirection 14 | (const Eigen::Vector3d& point, const Eigen::Vector3d& direction) 15 | { 16 | // Get transform of link. 17 | Transform* T = link_->Get(); 18 | MODUS_ASSERT(T, "Error: link is missing Transform aspect"); 19 | 20 | // Use body twist as the Dofs. 21 | BodyJacobian J(link_); 22 | J.SetBodyJacobian(Eigen::MatrixXd::Identity(6,6)); 23 | return J.GetGradientAtPointInDirection(point, direction); 24 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_kinematics/jacobian_geometric.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace modus; 6 | 7 | 8 | modus::GeometricJacobian::GeometricJacobian(Link* link) { 9 | link_ = link; 10 | } 11 | 12 | void modus::GeometricJacobian::SetLinearJacobian(const Eigen::MatrixXd& linear) 13 | { 14 | linear_ = linear; 15 | } 16 | 17 | void modus::GeometricJacobian::SetRotationalJacobian(const Eigen::MatrixXd& rotational) 18 | { 19 | rotational_ = rotational; 20 | } 21 | 22 | Eigen::RowVectorXd modus::GeometricJacobian::GetGradientAtPointInDirection 23 | (const Eigen::Vector3d& point, const Eigen::Vector3d& direction) 24 | { 25 | // Compute the linear velocity jacobian at a point r offset from the center of 26 | // the frame t. It should satisfy the equation 27 | // 28 | // ̇r = ̇t(θ) + (r - t) × ω(θ) 29 | // 30 | // where ω is the angular velocity. 31 | Transform* T = link_->Get(); 32 | MODUS_ASSERT(T, "Error: link is missing Transform aspect"); 33 | Eigen::Vector3d t = T->GetTranslation(); 34 | Eigen::Vector3d dr = point - t; 35 | Eigen::Matrix3d dr_hat; 36 | dr_hat << 0, -dr.z(), dr.y(), 37 | dr.z(), 0, -dr.x(), 38 | -dr.y(), dr.x(), 0; 39 | Eigen::MatrixXd J = linear_ + dr_hat * rotational_; 40 | return direction.transpose() * J; 41 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_kinematics/jacobian_spatial.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace modus; 5 | 6 | 7 | Eigen::RowVectorXd modus::SpatialJacobian::GetGradientAtPointInDirection 8 | (const Eigen::Vector3d& point, const Eigen::Vector3d& direction) 9 | { 10 | // This function converts the spatial jacobian into a distance gradient at the 11 | // input point along the input direction. We do so using the twist to point 12 | // velocity equation ω × p + v. 13 | Eigen::Matrix3d P = Adjoint(point); 14 | Eigen::MatrixXd V = spatial_.topRows(3) + P.transpose() * spatial_.bottomRows(3); 15 | return direction.transpose() * V; 16 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_kinematics/kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_modes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | # Source files. 4 | set(MODUS_SOURCE 5 | enumerate_modes.cpp 6 | # preprocess.cpp 7 | # constraints.cpp 8 | 9 | # Preprocessing 10 | preprocess/matroid_preprocessor.cpp 11 | 12 | # Geometry 13 | geometry/arrangements.cpp 14 | geometry/incidence_graph.cpp 15 | # geometry/linear_algebra.cpp 16 | geometry/interior_point.cpp 17 | geometry/convex_hull.cpp 18 | geometry/halfspace_intersection.cpp 19 | ) 20 | 21 | add_library(modus_modes SHARED ${MODUS_SOURCE}) 22 | 23 | # Define headers for this library. PUBLIC headers are used for compiling the 24 | # library, and will be added to install paths. 25 | target_include_directories(modus_modes 26 | PUBLIC 27 | $ 28 | $ 29 | ${EIGEN3_INCLUDE_DIR} 30 | ${QHULL_INCLUDE_DIR} 31 | ${PROJECT_SOURCE_DIR}/include 32 | ) 33 | #${ORTOOLS_INCLUDE_DIR}) 34 | 35 | # Add compiler requirements here. 36 | target_compile_features(modus_modes PUBLIC cxx_std_17) 37 | 38 | # Link libraries. 39 | target_link_libraries(modus_modes modus_common modus_kinematics ${QHULLCPP_LIBRARY} ${QHULL_R_LIBRARY} )#ortools::ortools) 40 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | # Source files. 4 | set(MODUS_SYSTEM_SOURCE 5 | system.cpp 6 | body.cpp 7 | transform.cpp 8 | ) 9 | 10 | add_library(modus_system SHARED ${MODUS_SYSTEM_SOURCE}) 11 | 12 | target_include_directories(modus_system 13 | PUBLIC 14 | $ 15 | $ 16 | ${EIGEN3_INCLUDE_DIR} 17 | ${PROJECT_SOURCE_DIR}/include 18 | ) 19 | 20 | # Add compiler requirements here. 21 | target_compile_features(modus_system PUBLIC cxx_std_17) 22 | 23 | # Link libraries. 24 | target_link_libraries(modus_system modus_common) 25 | 26 | # set_property(TARGET modus_system PROPERTY POSITION_INDEPENDENT_CODE ON) 27 | -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_system/body.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace modus; 4 | 5 | 6 | modus::Body::Body() { 7 | // Add root link. 8 | CreateLink(); 9 | } 10 | 11 | int modus::Body::GetIndex() { 12 | return index_; 13 | } 14 | 15 | size_t modus::Body::NumLinks() { 16 | return links_.size(); 17 | } 18 | 19 | Link* modus::Body::GetRootLink() { 20 | return links_[0].get(); 21 | } 22 | 23 | Link* modus::Body::GetLink(int link_id) { 24 | return links_[link_id].get(); 25 | } 26 | 27 | Link* modus::Body::CreateLink() { 28 | std::unique_ptr link = std::make_unique(); 29 | link->SetIndex(links_.size()); 30 | Link* ret = link.get(); 31 | links_.push_back(std::move(link)); 32 | return ret; 33 | } -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_system/link.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/cmgmp/external/modus/src/modus_system/link.cpp -------------------------------------------------------------------------------- /cmgmp/external/modus/src/modus_system/transform.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | using namespace modus; 6 | 7 | modus::Transform::Transform() { 8 | rotation_.setIdentity(); 9 | translation_.setZero(); 10 | } 11 | 12 | void modus::Transform::SetRotation(const Eigen::Matrix3d& rotation) { 13 | rotation_ = rotation; 14 | } 15 | 16 | Eigen::Matrix3d modus::Transform::GetRotation() { 17 | return rotation_; 18 | } 19 | 20 | void modus::Transform::SetTranslation(const Eigen::Vector3d& translation) { 21 | translation_ = translation; 22 | } 23 | 24 | Eigen::Vector3d modus::Transform::GetTranslation() { 25 | return translation_; 26 | } 27 | 28 | void modus::Transform::Invert() { 29 | InverseTransform(rotation_, translation_); 30 | } -------------------------------------------------------------------------------- /cmgmp/manipulators/DartDDHandFreeY.h: -------------------------------------------------------------------------------- 1 | #include "DartDDHand.h" 2 | 3 | class DartDDHandFreeY: public DartDDHand { 4 | 5 | public: 6 | 7 | // config: p1, n1, p2, n2, y 8 | double max_dy = 3E-3; 9 | 10 | DartDDHandFreeY(int fingertype) : DartDDHand(fingertype) { 11 | } 12 | 13 | void setConfig(const VectorXd& config, const Vector7d& object_pose); 14 | 15 | bool resampleFingers(int n_on, const VectorXd& config, const Vector7d& object_pose, const std::vector& object_surface, 16 | VectorXd& new_config, std::vector* remain_fingertips); 17 | 18 | VectorXd getHandFrameConfig(const VectorXd& config, const Vector7d& object_pose); // return [p1, p2, y_world] 19 | }; -------------------------------------------------------------------------------- /cmgmp/manipulators/DartKR5.h: -------------------------------------------------------------------------------- 1 | #ifndef MANIPULATORS_DARTURDFMANIPULATORTEMP 2 | #define MANIPULATORS_DARTURDFMANIPULATORTEMP 3 | #include "DartURDFManipulatorTemplate.h" 4 | #endif 5 | 6 | class DartKR5: public virtual DartURDFManipulatorTemplate { 7 | 8 | public: 9 | // config: [p1, n1] in the object frame 10 | DartKR5(); 11 | DartKR5(const DartKR5& kr5): DartURDFManipulatorTemplate(kr5){} 12 | 13 | void setConfig(const VectorXd& config, const Vector7d& object_pose); 14 | 15 | void setupCollisionGroup(WorldPtr world); 16 | 17 | bool inverseKinematics(const VectorXd& mnp_config, const Vector7d& object_pose, VectorXd& result_joint_config); 18 | 19 | bool ifIKsolution(const VectorXd& mnp_config, const Vector7d& object_pose); 20 | 21 | void computeWorkspace(std::string save_to); 22 | 23 | void getFingertipsOnObject(const VectorXd& config, const Vector7d& object_pose, std::vector* fingertips); 24 | 25 | bool resampleFingers(int n_on, const VectorXd& config, const Vector7d& object_pose, const std::vector& object_surface, 26 | VectorXd& new_config, std::vector* remain_fingertips); 27 | 28 | void Fingertips2PointContacts(const std::vector& fingertips, 29 | std::vector* point_contacts); 30 | 31 | void applyJointPositions(const VectorXd& joint_config); 32 | 33 | private: 34 | 35 | Vector6d reset_joint_config; 36 | 37 | 38 | }; -------------------------------------------------------------------------------- /cmgmp/manipulators/DartManipulatorTemplate.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #ifndef UTILS_H 6 | #define UTILS_H 7 | #include "../utilities/utilities.h" 8 | #endif 9 | 10 | #ifndef MANIPULATORS_MANIPULATORTEMPLATE 11 | #define MANIPULATORS_MANIPULATORTEMPLATE 12 | #include "ManipulatorTemplate.h" 13 | #endif 14 | 15 | using namespace dart::dynamics; 16 | using namespace dart::simulation; 17 | using namespace dart::collision; 18 | using namespace dart::common; 19 | 20 | 21 | class DartManipulatorTemplate: public ManipulatorTemplate 22 | { 23 | 24 | public: 25 | std::vector bodies; 26 | std::shared_ptr mCollisionGroup = 0; 27 | bool ifCheckObjectCollision = false; 28 | 29 | DartManipulatorTemplate(){} 30 | DartManipulatorTemplate(const DartManipulatorTemplate& dmt): ManipulatorTemplate(dmt), bodies(dmt.bodies), 31 | mCollisionGroup(dmt.mCollisionGroup), 32 | ifCheckObjectCollision(dmt.ifCheckObjectCollision){} 33 | ~DartManipulatorTemplate(){} 34 | 35 | void addBody(const SkeletonPtr& body){ 36 | bodies.push_back(body); 37 | } 38 | 39 | virtual void setupCollisionGroup(WorldPtr world) = 0; 40 | 41 | virtual void setConfig(const VectorXd& config, const Vector7d& object_pose) = 0; 42 | 43 | 44 | }; 45 | -------------------------------------------------------------------------------- /cmgmp/manipulators/DartParallelJawGripper.h: -------------------------------------------------------------------------------- 1 | #ifndef MANIPULATORS_DARTMANIPULATORTEMPLATE 2 | #define MANIPULATORS_DARTMANIPULATORTEMPLATE 3 | #include "DartManipulatorTemplate.h" 4 | #endif 5 | 6 | #ifndef MANIPULATORS_MANIPULATORTEMPLATE 7 | #define MANIPULATORS_MANIPULATORTEMPLATE 8 | #include "ManipulatorTemplate.h" 9 | #endif 10 | 11 | #ifndef DART_UTILS 12 | #define DART_UTILS 13 | #include "../dart_utils/dart_utils.h" 14 | #endif 15 | 16 | class DartParallelJawGripper: public virtual DartManipulatorTemplate { 17 | 18 | public: 19 | // config: [p1, n1, p2, n2, n_finger] or [p1, n1, n_gripper, n_finger] 20 | 21 | double fingertip_radius = 0; 22 | double max_open_dist = 0; 23 | double finger_length = 0; 24 | 25 | DartParallelJawGripper(double max_d, double radius, double fl = 0); 26 | DartParallelJawGripper(const DartParallelJawGripper& dpm): DartManipulatorTemplate(dpm), fingertip_radius(dpm.fingertip_radius), max_open_dist(dpm.max_open_dist){} 27 | 28 | void setupCollisionGroup(WorldPtr world); 29 | 30 | void getFingertipsOnObject(const VectorXd& config, const Vector7d& object_pose, std::vector* fingertips); 31 | 32 | void setConfig(const VectorXd& config, const Vector7d& object_pose); 33 | 34 | bool resampleFingers(int n_on, const VectorXd& config, const Vector7d& object_pose, const std::vector& object_surface, 35 | VectorXd& new_config, std::vector* remain_fingertips); 36 | 37 | void Fingertips2PointContacts(const std::vector& fingertips, 38 | std::vector* point_contacts); 39 | 40 | 41 | bool ifIK(const VectorXd& config); 42 | 43 | bool ifIK(const std::vector& fingertips); 44 | 45 | 46 | 47 | 48 | }; -------------------------------------------------------------------------------- /cmgmp/manipulators/DartPointManipulator.h: -------------------------------------------------------------------------------- 1 | #ifndef MANIPULATORS_DARTMANIPULATORTEMPLATE 2 | #define MANIPULATORS_DARTMANIPULATORTEMPLATE 3 | #include "DartManipulatorTemplate.h" 4 | #endif 5 | 6 | #ifndef MANIPULATORS_MANIPULATORTEMPLATE 7 | #define MANIPULATORS_MANIPULATORTEMPLATE 8 | #include "ManipulatorTemplate.h" 9 | #endif 10 | 11 | #ifndef DART_UTILS 12 | #define DART_UTILS 13 | #include "../dart_utils/dart_utils.h" 14 | #endif 15 | 16 | class DartPointManipulator: public virtual DartManipulatorTemplate { 17 | 18 | public: 19 | // config: [p1, n1, p2, n2 ...] in the object frame 20 | 21 | double fingertip_radius = 0; 22 | 23 | bool is_patch_contact = false; 24 | 25 | DartPointManipulator(int n, double radius); 26 | DartPointManipulator(const DartPointManipulator& dpm): DartManipulatorTemplate(dpm), fingertip_radius(dpm.fingertip_radius){} 27 | 28 | void setupCollisionGroup(WorldPtr world); 29 | 30 | void getFingertipsOnObject(const VectorXd& config, const Vector7d& object_pose, std::vector* fingertips); 31 | 32 | void setConfig(const VectorXd& config, const Vector7d& object_pose); 33 | 34 | bool resampleFingers(int n_on, const VectorXd& config, const Vector7d& object_pose, const std::vector& object_surface, 35 | VectorXd& new_config, std::vector* remain_fingertips); 36 | 37 | void Fingertips2PointContacts(const std::vector& fingertips, 38 | std::vector* point_contacts); 39 | 40 | 41 | 42 | }; -------------------------------------------------------------------------------- /cmgmp/manipulators/DartURDFManipulatorTemplate.h: -------------------------------------------------------------------------------- 1 | #ifndef MANIPULATORS_DARTMANIPULATORTEMPLATE 2 | #define MANIPULATORS_DARTMANIPULATORTEMPLATE 3 | #include "DartManipulatorTemplate.h" 4 | #endif 5 | 6 | #ifndef DART_UTILS 7 | #define DART_UTILS 8 | #include "../dart_utils/dart_utils.h" 9 | #endif 10 | 11 | // template for loading manipulators from urdf files 12 | 13 | // set contact points 14 | // inverse kinematics 15 | // forward kinematics 16 | 17 | // 18 | 19 | class DartURDFManipulatorTemplate: public virtual DartManipulatorTemplate { 20 | 21 | public: 22 | // config should be defined wrt to the object 23 | int NumDofs = 0; 24 | InverseKinematicsPtr ik = 0; 25 | DartURDFManipulatorTemplate(){} 26 | DartURDFManipulatorTemplate(const DartURDFManipulatorTemplate& dumt): DartManipulatorTemplate(dumt){} 27 | 28 | void loadManipulator(const std::string & name, const std::string & filePath){ 29 | SkeletonPtr robot = createRobot(name, filePath); 30 | for(size_t i=0; i < robot->getNumJoints(); i++){ 31 | robot->getJoint(i)->setPositionLimitEnforced(true); 32 | } 33 | robot->enableSelfCollisionCheck(); 34 | this->addBody(robot); 35 | } 36 | 37 | virtual bool inverseKinematics(const VectorXd& mnp_config, const Vector7d& object_pose, VectorXd& result_joint_config) = 0; 38 | 39 | virtual void computeWorkspace(std::string save_to) = 0; 40 | 41 | }; -------------------------------------------------------------------------------- /cmgmp/manipulators/ManipulatorTemplate.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CONTACTCONSTRAINTS_H 3 | #define CONTACTCONSTRAINTS_H 4 | #include "../contacts/contact_constraints.h" 5 | #endif 6 | 7 | class ManipulatorTemplate 8 | { 9 | public: 10 | int n_pts = 0; 11 | 12 | // public: 13 | ManipulatorTemplate(){}; 14 | ~ManipulatorTemplate(){}; 15 | 16 | ManipulatorTemplate(const ManipulatorTemplate& mt): n_pts(mt.n_pts){} 17 | 18 | virtual void setConfig(const VectorXd& config, const Vector7d& object_pose) {}; 19 | 20 | virtual bool resampleFingers(int n_on, const VectorXd& config, const Vector7d& object_pose, const std::vector& object_surface, 21 | VectorXd& new_config, std::vector* remain_fingertips) =0; 22 | 23 | virtual void getFingertipsOnObject(const VectorXd& config, const Vector7d& object_pose, std::vector* fingertips) = 0; 24 | 25 | // contact model 26 | virtual void Fingertips2PointContacts(const std::vector& fingertips, std::vector* point_contacts) =0; 27 | 28 | virtual bool isSameConfig(const VectorXd& config0, const VectorXd& config1){ 29 | if (config0.size()!=config1.size()){ 30 | return false; 31 | } else { 32 | if ((config0 - config1).norm() < 1e-3){ 33 | return true; 34 | } else { 35 | return false; 36 | } 37 | } 38 | } 39 | 40 | int getNumberofFingertips(){ 41 | return this->n_pts; 42 | }; 43 | 44 | virtual bool ifIKsolution(const VectorXd& mnp_config, const Vector7d& object_pose){ 45 | return true; 46 | } 47 | 48 | }; -------------------------------------------------------------------------------- /cmgmp/manipulators/ddhand/two_link_kinematics.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define ZERO_SOLUTION 0 8 | #define ONE_SOLUTION 1 9 | #define TWO_SOLUTIONS 2 10 | #define INF_SOLUTION 3 11 | 12 | using namespace std; 13 | class TwoLinkKinematics { 14 | public: 15 | TwoLinkKinematics(double l1, double l2); 16 | Eigen::Vector2d fk(Eigen::Vector2d joints); 17 | std::tuple ik(Eigen::Vector2d pose); 18 | Eigen::Matrix2d jacobian(Eigen::Vector2d joints); 19 | bool inJointLimits(Eigen::Vector2d joints); 20 | private: 21 | double l1_, l2_; 22 | }; 23 | -------------------------------------------------------------------------------- /cmgmp/search/planner_option.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | #include "../utilities/utilities.h" 4 | #endif 5 | 6 | #define METHOD_QPCC 0 7 | #define METHOD_LCP 1 8 | #define METHOD_ALL 2 9 | #define METHOD_NONE 3 10 | 11 | class RRTPlannerOptions{ 12 | 13 | public: 14 | int method = METHOD_QPCC; 15 | int sampleSO3 = true; 16 | double goal_biased_prob = 0.8; 17 | int max_samples = 5; 18 | Vector3d rotation_sample_axis; 19 | 20 | RRTPlannerOptions(){}; 21 | RRTPlannerOptions(const RRTPlannerOptions& opts){ 22 | 23 | this->method = opts.method; 24 | this->sampleSO3 = opts.sampleSO3; 25 | this->goal_biased_prob = opts.goal_biased_prob; 26 | this->max_samples = opts.max_samples; 27 | this->rotation_sample_axis = opts.rotation_sample_axis; 28 | } 29 | 30 | }; -------------------------------------------------------------------------------- /cmgmp/search/tree.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #ifndef UTILS_H 7 | #define UTILS_H 8 | #include "../utilities/utilities.h" 9 | #endif 10 | 11 | #ifndef CONTACTCONSTRAINTS_H 12 | #define CONTACTCONSTRAINTS_H 13 | #include "../contacts/contact_constraints.h" 14 | #endif 15 | 16 | namespace CMGMP { 17 | struct Node { 18 | Vector7d config; // x, y, z, qx, qy, qz, qw 19 | int parent = -1; 20 | int edge = -1; // index of edge from parent to this 21 | bool is_extended_to_goal = false; 22 | // std::vector fingertips; 23 | VectorXd manipulator_config; 24 | std::vector envs; 25 | std::vector modes; 26 | // std::vector children; // index of children nodes 27 | // double cost = 0; 28 | Node(Vector7d data, VectorXd mnp_config){ 29 | config = data; 30 | manipulator_config = mnp_config; 31 | } 32 | }; 33 | 34 | struct Edge { 35 | VectorXi mode; 36 | bool manipulator_collide; 37 | std::vector path; 38 | Edge(VectorXi m, bool m_collide, std::vector& p){ 39 | mode = m; 40 | manipulator_collide = m_collide; 41 | path = p; 42 | } 43 | }; 44 | 45 | class Tree{ 46 | public: 47 | std::vector nodes; 48 | std::vector edges; 49 | double angle_weight; 50 | double translation_weight; 51 | Tree(const double& a_weight, const double& p_weight); 52 | double dist(const Vector7d& q1, const Vector7d& q2); 53 | int nearest_neighbor(const Vector7d& q); 54 | int nearest_unextended_to_goal(const Vector7d& q); 55 | // void nearest_neighbors(Vector7d q, std::vector* nearest_neighbors); 56 | void backtrack(int last_node_idx, std::vector* node_path); 57 | // void neighborhood(int node_idx, double radius, std::vector* neighbors); 58 | void add_node(Node* n, int parent, Edge* e); 59 | void initial_node(Node* n); 60 | // void remove_parent(int node_idx); 61 | // void set_parent(int parent, int child); 62 | }; 63 | } -------------------------------------------------------------------------------- /cmgmp/search_hierarchical/hierarchical_tree.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #ifndef UTILS_H 7 | #define UTILS_H 8 | #include "../utilities/utilities.h" 9 | #endif 10 | 11 | #ifndef CONTACTCONSTRAINTS_H 12 | #define CONTACTCONSTRAINTS_H 13 | #include "../contacts/contact_constraints.h" 14 | #endif 15 | 16 | #define QUASISTATIC 0 17 | #define QUASIDYNAMIC 1 18 | #define DYNAMIC 2 19 | 20 | 21 | class HNode { 22 | Vector7d config; // x, y, z, qx, qy, qz, qw 23 | int parent = -1; 24 | int edge = -1; // index of edge from parent to this 25 | 26 | bool is_extended_to_goal = false; 27 | 28 | bool is_alive = true; 29 | 30 | int dynamic_type = QUASISTATIC; 31 | 32 | std::vector envs; 33 | std::vector modes; 34 | 35 | HNode(Vector7d data){ 36 | config = data; 37 | } 38 | 39 | HNode(Vector7d data, int type){ 40 | config = data; 41 | dynamic_type = type; 42 | } 43 | }; 44 | 45 | class HEdge { 46 | VectorXi mode; 47 | bool manipulator_collide; 48 | std::vector path; 49 | HEdge(VectorXi m, bool m_collide, std::vector& p){ 50 | mode = m; 51 | manipulator_collide = m_collide; 52 | path = p; 53 | } 54 | }; 55 | 56 | class HTree{ 57 | public: 58 | std::vector nodes; 59 | std::vector edges; 60 | double angle_weight; 61 | double translation_weight; 62 | HTree(const double& a_weight, const double& p_weight); 63 | double dist(const Vector7d& q1, const Vector7d& q2); 64 | int nearest_neighbor(const Vector7d& q); 65 | int nearest_unextended_to_goal(const Vector7d& q); 66 | 67 | void backtrack(int last_node_idx, std::vector* node_path); 68 | 69 | void add_node(Node* n, int parent, Edge* e); 70 | void initial_node(Node* n); 71 | 72 | }; 73 | -------------------------------------------------------------------------------- /cmgmp/search_hierarchical/todo.md: -------------------------------------------------------------------------------- 1 | # content 2 | 3 | - [ ] generate finger plan 4 | - [x] Forward check quasistatic 5 | - [x] forward check quasidynamic 6 | - [ ] (later) forward check dynamic 7 | - [ ] add new nodes during forward check? 8 | - [ ] after plan generated 9 | - [ ] test some examples 10 | 11 | # code structure 12 | 13 | - [ ] reduce replicated functions 14 | 15 | # method improvement 16 | 17 | - [ ] any way to improve finger planning ? 18 | 19 | # enginneering 20 | 21 | - [ ] parallel 22 | 23 | -------------------------------------------------------------------------------- /cmgmp/utilities/sample.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | #include "utilities.h" 4 | #endif 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace Eigen; 11 | 12 | Vector3d sample_position(const Vector3d &ub, const Vector3d &lb); 13 | Quaterniond generate_unit_quaternion(); 14 | Quaterniond sample_rotation(const Vector3d& axis); 15 | Vector3d steer_position(const Vector3d &p0, const Eigen::Vector3d &p1, const double &length); 16 | Quaterniond steer_quaternion(const Quaterniond &q0, const Quaterniond &q1, const double &angle); 17 | 18 | void combinations(const std::vector &elems, int req_len, std::vector>* elem_combs); 19 | -------------------------------------------------------------------------------- /cmgmp/utilities/sample_grasp.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // void sample_points_box(double hl, double hw, double hh){ 6 | 7 | // } 8 | using namespace Eigen; 9 | void saveData(std::string fileName, MatrixXd matrix); 10 | MatrixXd openData(std::string fileToOpen); 11 | MatrixXd load_points_from_csv(std::string fileToOpen); -------------------------------------------------------------------------------- /cmgmp/worlds/WorldTemplate.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CONTACTCONSTRAINTS_H 3 | #define CONTACTCONSTRAINTS_H 4 | #include "../contacts/contact_constraints.h" 5 | #endif 6 | 7 | #ifndef MANIPULATORS_MANIPULATORTEMPLATE 8 | #define MANIPULATORS_MANIPULATORTEMPLATE 9 | #include "../manipulators/ManipulatorTemplate.h" 10 | #endif 11 | 12 | // wraps the simulation interface 13 | 14 | // the world consists of: the environment one movable object, one manipulator 15 | 16 | class WorldTemplate 17 | { 18 | public: 19 | 20 | WorldTemplate(){}; 21 | WorldTemplate(const WorldTemplate& wt){}; 22 | 23 | virtual void updateObjectPose(const Vector7d & object_pose) = 0; 24 | 25 | virtual void updateRobotConfig(const Eigen::VectorXd & robot_config) = 0; 26 | 27 | virtual bool isRobotCollide(const Eigen::VectorXd & robot_config) = 0; 28 | 29 | virtual void getObjectContacts(std::vector* pts) = 0; 30 | 31 | virtual void getObjectContacts(std::vector* pts, const Vector7d & object_pose) = 0; 32 | 33 | virtual void startWindow(int* pargc, char** argv) = 0; 34 | 35 | virtual void setPlaybackTrajectory(const std::vector& object_traj, const std::vector& robot_traj) = 0; 36 | 37 | virtual void setObjectTrajectory(const std::vector& object_traj) = 0; 38 | 39 | virtual void setSurfacePoints(const std::vector& pts) = 0; 40 | 41 | virtual ManipulatorTemplate* getRobot() = 0; 42 | 43 | }; -------------------------------------------------------------------------------- /data/grasp_sampling/bookshelf_surface_points.csv: -------------------------------------------------------------------------------- 1 | 0.5,-1.5,1,-1,0,0 2 | -0.5,-1.5,1,1,0,0 3 | 0,-1.5,2,0,0,-1 4 | 0,0,2,0,0,-1 -------------------------------------------------------------------------------- /data/grasp_sampling/peg_out_of_hole_surface_points.csv: -------------------------------------------------------------------------------- 1 | 0,1,1.5,0,-1,0 2 | 0,-1,1.5,0,1,0 3 | 0,1,1.75,0,-1,0 4 | 0,-1,1.75,0,1,0 5 | 1,0,1.5,-1,0,0 6 | -1,0,1.5,1,0,0 7 | 1,0,1.75,-1,0,0 8 | -1,0,1.75,1,0,0 9 | 0,0,2,0,0,-1 -------------------------------------------------------------------------------- /data/meshes/stl/ball_compound_r_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/ball_compound_r_1.stl -------------------------------------------------------------------------------- /data/meshes/stl/ball_radius_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/ball_radius_1.stl -------------------------------------------------------------------------------- /data/meshes/stl/box_halflength_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/box_halflength_1.stl -------------------------------------------------------------------------------- /data/meshes/stl/box_lx_1_ly_4_lz_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/box_lx_1_ly_4_lz_4.stl -------------------------------------------------------------------------------- /data/meshes/stl/box_lx_2_ly_4_lz_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/box_lx_2_ly_4_lz_2.stl -------------------------------------------------------------------------------- /data/meshes/stl/box_lx_2_ly_4_lz_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/box_lx_2_ly_4_lz_4.stl -------------------------------------------------------------------------------- /data/meshes/stl/cylindar_l_2_r_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/cylindar_l_2_r_1.stl -------------------------------------------------------------------------------- /data/meshes/stl/russian_block_l_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/russian_block_l_2.stl -------------------------------------------------------------------------------- /data/meshes/stl/screw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/screw.stl -------------------------------------------------------------------------------- /data/meshes/stl/tetrahedron_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl/tetrahedron_1.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/BIG_SCREW.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/BIG_SCREW.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/ball_compound_r_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/ball_compound_r_1.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/ball_radius_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/ball_radius_1.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/box_halflength_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/box_halflength_1.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/box_lx_1_ly_4_lz_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/box_lx_1_ly_4_lz_4.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/box_lx_2_ly_4_lz_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/box_lx_2_ly_4_lz_2.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/box_lx_2_ly_4_lz_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/box_lx_2_ly_4_lz_4.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/cylindar_l_2_r_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/cylindar_l_2_r_1.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/russian_block_l_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/russian_block_l_2.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/small_bumper_scale_200.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/small_bumper_scale_200.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/small_bumper_scale_2000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/small_bumper_scale_2000.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/small_bumper_scale_5000.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/small_bumper_scale_5000.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/socket_screw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/socket_screw.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/tetrahedron_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/tetrahedron_1.stl -------------------------------------------------------------------------------- /data/meshes/stl_scale_50/torus_tri.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/meshes/stl_scale_50/torus_tri.stl -------------------------------------------------------------------------------- /data/urdf/KR5/ground.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /data/urdf/KR5/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/urdf/KR5/meshes/base_link.STL -------------------------------------------------------------------------------- /data/urdf/KR5/meshes/bicep.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/urdf/KR5/meshes/bicep.STL -------------------------------------------------------------------------------- /data/urdf/KR5/meshes/elbow.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/urdf/KR5/meshes/elbow.STL -------------------------------------------------------------------------------- /data/urdf/KR5/meshes/forearm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/urdf/KR5/meshes/forearm.STL -------------------------------------------------------------------------------- /data/urdf/KR5/meshes/palm.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/urdf/KR5/meshes/palm.STL -------------------------------------------------------------------------------- /data/urdf/KR5/meshes/shoulder.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/urdf/KR5/meshes/shoulder.STL -------------------------------------------------------------------------------- /data/urdf/KR5/meshes/wrist.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XianyiCheng/CMGMP/7d4d1c7cb216d1028e29928864870f719c536484/data/urdf/KR5/meshes/wrist.STL -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(dart_libs dart dart-utils-urdf dart-gui) 2 | 3 | set(example_required_libraries 4 | cmgmp-utils cmgmp-contact cmgmp-world cmgmp-search 5 | ${dart_libs} 6 | ) 7 | 8 | set(example_include_dirs 9 | "${PROJECT_SOURCE_DIR}" 10 | "${PROJECT_SOURCE_DIR}/cmgmp/external/eigen3" 11 | "${PROJECT_SOURCE_DIR}/cmgmp/external/dartsim" 12 | ) 13 | 14 | set(example_link_dirs 15 | "${PROJECT_BINARY_DIR}" 16 | "${PROJECT_BINARY_DIR}/cmgmp/external/dartsim" 17 | ) 18 | 19 | 20 | 21 | 22 | include_directories(${example_include_dirs}) 23 | link_libraries(${example_required_libraries}) 24 | 25 | add_executable(kr5 KR5.cpp) 26 | add_executable(push_along_walls push_along_walls.cpp) 27 | add_executable(ddhand_bookself ddhand_bookshelf.cpp) 28 | add_executable(ddhand_flip ddhand_flip.cpp) 29 | add_executable(ddhand_slideup ddhand_slideup.cpp) 30 | add_executable(peg_out_of_hole peg_out_hole.cpp) 31 | add_executable(bookshelf bookshelf.cpp) 32 | add_executable(pick_card pick_card.cpp) 33 | add_executable(flip_cube flip_cube.cpp) 34 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(test_utils test_utils.cpp) 2 | add_executable(test_mode test_contact_mode.cpp) 3 | add_executable(test_dart test_dartworld.cpp) 4 | add_executable(test_kr5 test_KR5.cpp) 5 | add_executable(test_ddhand test_ddhand.cpp) 6 | 7 | set(example_required_libraries 8 | cmgmp-utils cmgmp-contact 9 | ) 10 | 11 | set(example_include_dirs 12 | "${PROJECT_SOURCE_DIR}" 13 | "${PROJECT_SOURCE_DIR}/cmgmp/external/eigen3" 14 | ) 15 | 16 | set(example_link_dirs 17 | "${PROJECT_BINARY_DIR}" 18 | "${PROJECT_BINARY_DIR}/cmgmp/external/dartsim" 19 | ) 20 | 21 | set(dart_libs dart dart-utils-urdf dart-gui) # dart-collision-bullet 22 | 23 | 24 | target_include_directories(test_utils PRIVATE ${example_include_dirs}) 25 | # target_link_directories(test_utils PRIVATE "${PROJECT_BINARY_DIR}") 26 | target_link_libraries(test_utils PRIVATE ${example_required_libraries}) 27 | 28 | target_include_directories(test_mode PRIVATE ${example_include_dirs}) 29 | target_link_libraries(test_mode PRIVATE ${example_required_libraries}) 30 | 31 | target_include_directories(test_dart PRIVATE ${example_include_dirs} "${PROJECT_SOURCE_DIR}/cmgmp/external/dartsim") 32 | target_link_libraries(test_dart PRIVATE ${example_required_libraries} cmgmp-world ${dart_libs}) 33 | 34 | target_include_directories(test_kr5 PRIVATE ${example_include_dirs} "${PROJECT_SOURCE_DIR}/cmgmp/external/dartsim") 35 | target_link_libraries(test_kr5 PRIVATE ${example_required_libraries} cmgmp-world ${dart_libs}) 36 | 37 | target_include_directories(test_ddhand PRIVATE ${example_include_dirs} "${PROJECT_SOURCE_DIR}/cmgmp/external/dartsim") 38 | target_link_libraries(test_ddhand PRIVATE ${example_required_libraries} cmgmp-world ${dart_libs}) 39 | -------------------------------------------------------------------------------- /tests/test_KR5.cpp: -------------------------------------------------------------------------------- 1 | // #include "cmgmp/search/rrtplanner.h" 2 | #include "cmgmp/utilities/sample_grasp.h" 3 | #include "cmgmp/contacts/contact_kinematics.h" 4 | 5 | #include "cmgmp/contacts/contact_utils.h" 6 | 7 | #include "cmgmp/manipulators/DartKR5.h" 8 | 9 | #include "cmgmp/worlds/DartWorld.h" 10 | // #include "cmgmp/worlds/PlanningWorld.h" 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | int main(int argc, char* argv[]) 17 | { 18 | DartWorld* my_world = new DartWorld(); 19 | 20 | 21 | SkeletonPtr object = createFreeBox("box_object", 0.1*Vector3d(1,1,1)); 22 | 23 | SkeletonPtr env1 = createFixedBox("ground", 0.1*2*Vector3d(10,10,1), 0.1*Vector3d(0,0,-10)); 24 | 25 | my_world->addObject(object); 26 | my_world->addEnvironmentComponent(env1); 27 | 28 | Vector7d x; 29 | x << 0,0,0,0,0,0,1; 30 | my_world->updateObjectPose(x); 31 | 32 | 33 | DartKR5* rpt = new DartKR5(); 34 | my_world->addRobot(rpt); 35 | 36 | // Position its base in a reasonable way 37 | Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); 38 | tf.translation() = Eigen::Vector3d(0, 0.0, 0.0); 39 | rpt->bodies[0]->getJoint(0)->setTransformFromParentBodyNode(tf); 40 | 41 | // std::vector pts; 42 | // rpt->computeWorkspace(&pts); 43 | 44 | // std::vector pts_world; 45 | VectorXd joint_v; 46 | VectorXd p(6); 47 | // p << 0.2,0.2,0.3,0,0,-1; 48 | // p << 0.7,0,0.025,0,0,-1; 49 | // p << 0.47,0,0.023*1.5,0,0,-1; 50 | // p << 0.482697,-0.025495,0.0082869,0,0,-1; 51 | p << 0.4,0,0.4,0,0,-1; 52 | 53 | 54 | 55 | 56 | 57 | bool iksolved = rpt->inverseKinematics(p, x, joint_v); 58 | std::cout << "if ik solved " << iksolved << ", joint values: " << joint_v.transpose() << std::endl; 59 | std::cout << "if robot collide" << my_world->isRobotCollide(p) << std::endl; 60 | // std::cout << "if robot collide: " << my_world->isRobotCollide(r); 61 | my_world->startWindow(&argc, argv); 62 | } -------------------------------------------------------------------------------- /tests/test_ddhand.cpp: -------------------------------------------------------------------------------- 1 | #include "cmgmp/search/rrtplanner.h" 2 | #include "cmgmp/utilities/sample_grasp.h" 3 | #include "cmgmp/contacts/contact_kinematics.h" 4 | 5 | #include "cmgmp/contacts/contact_utils.h" 6 | 7 | #include "cmgmp/manipulators/DartDDHand.h" 8 | 9 | #include "cmgmp/worlds/DartWorld.h" 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | int main(int argc, char* argv[]) 16 | { 17 | DartWorld* my_world = new DartWorld(); 18 | 19 | 20 | SkeletonPtr object = createFreeBox("box_object", 0.1*Vector3d(1,1,1)); 21 | 22 | SkeletonPtr env1 = createFixedBox("ground", 0.1*2*Vector3d(10,10,1), 0.1*Vector3d(0,0,-1)); 23 | 24 | my_world->addObject(object); 25 | my_world->addEnvironmentComponent(env1); 26 | 27 | Vector7d x; 28 | x << 0,0,0.05,0,0,0,1; 29 | my_world->updateObjectPose(x); 30 | 31 | 32 | DartDDHand* rpt = new DartDDHand(L_FINGER); 33 | my_world->addRobot(rpt); 34 | 35 | // Position its base in a reasonable way 36 | Vector7d handx; 37 | handx << -0.2,0,55E-3 + 28E-3,0,0,0,1; 38 | rpt->setHandFrameTransform(handx); 39 | 40 | 41 | for (double xx = 0.0; xx < 55E-3; xx += 1E-3){ 42 | for (double zz = 0.0; zz < 90E-3; zz += 1E-3){ 43 | bool if_hand_ik = rpt->hand_ik->is_inverse_kinematics(Vector2d(-xx, -zz), Vector2d(xx, -zz)); 44 | if (if_hand_ik){ 45 | 46 | std::cout << "hand ik true: [" << xx << ", " << zz << "]," << std::endl; 47 | } 48 | } 49 | } 50 | 51 | // bool if_hand_ik = rpt->hand_ik->is_inverse_kinematics(Vector2d(xx, zz), Vector2d(-xx, zz)); 52 | // std::cout << "if hand ik " << if_hand_ik << std::endl; 53 | 54 | my_world->startWindow(&argc, argv); 55 | } --------------------------------------------------------------------------------