├── .gitignore ├── CHANGES ├── CMakeLists.txt ├── CMakeModules ├── FindCBLAS.cmake ├── FindIPOPT.cmake ├── FindLAPACKLite.cmake ├── FindOpenMP.cmake ├── FindQHULL.cmake ├── FindQPOASES.cmake ├── FindRavelin.cmake └── FindSuperLU.cmake ├── README ├── README.md ├── cmake_uninstall.cmake.in ├── docs ├── Moby ├── XML-structure.tex ├── fixed.nb ├── install.tex ├── prismatic.nb ├── revolute.nb └── universal.nb ├── example ├── README ├── bouncing-ball │ └── bouncing-ball.xml ├── contact-constrained-pendulum │ ├── contact-constrained-pendulum-coldet-plugin.cpp │ ├── contact-constrained-pendulum-init.cpp │ └── contact-constrained-pendulum.xml ├── ellipse │ ├── ellipse-init.cpp │ ├── ellipse-plane-coldet-plugin.cpp │ ├── ellipse.obj │ └── ellipse2D.xml ├── events │ └── rotating-box.xml ├── fixed-joint │ └── fixed-articulated-table.xml ├── gears │ └── pendulum-gears.xml ├── joint-limits │ ├── chain.xml │ ├── limit-double-pendulum.xml │ └── limit-pendulum.xml ├── mrobot │ ├── controller.cpp │ ├── mrobot.pacer │ ├── mrobot.sdf │ └── mrobot.xml ├── parts-feeder │ ├── CMakeLists.txt │ ├── feeder.cpp │ └── feeder.xml ├── passive-walker │ ├── coldet-plugin.cpp │ ├── init.cpp │ ├── walker.sdf │ └── walker.xml ├── planar-joint │ └── constrained.xml ├── plugin-template.cpp ├── polyhedra │ ├── spinning-box-frictional.xml │ └── spinning-box-frictionless.xml ├── reduced-coords │ ├── chain.xml │ ├── double-pendulum.xml │ ├── pendulum-gears-impact.xml │ ├── pendulum-plugin.cpp │ └── pendulum.xml ├── regress.cpp ├── relative-visualization │ └── box-plane.xml ├── rimless-wheel │ ├── coldet-plugin.cpp │ ├── init.cpp │ ├── params.h │ └── wheel.xml ├── rolling-torus │ └── torus.xml ├── simple-contact │ ├── box-plugin.cpp │ ├── cylinder.xml │ ├── ramp.xml │ ├── simplest.xml │ ├── spinning-box-frictional.xml │ └── spinning-box-frictionless.xml ├── sims-in-code │ ├── block.cpp │ ├── doublependulum.cpp │ ├── linearactuator.cpp │ ├── pendulum.cpp │ ├── pushpendulum.cpp │ └── viewer.h ├── stacks │ ├── sphere-stack.xml │ ├── stack.xml │ ├── stack2.xml │ └── stack3.xml ├── tare │ └── pendulum.xml ├── ur10 │ ├── block.obj │ ├── controller.cpp │ ├── meshes │ │ ├── Base.obj │ │ ├── Base.obj.mtl │ │ ├── Forearm.obj │ │ ├── SCHUNK_MPG-80_BASE.obj │ │ ├── SCHUNK_MPG-80_LEFT_FINGER.obj │ │ ├── SCHUNK_MPG-80_RIGHT_FINGER.obj │ │ ├── Shoulder.obj │ │ ├── UpperArm.obj │ │ ├── UpperArm.obj.mtl │ │ ├── Wrist1.obj │ │ ├── Wrist2.obj │ │ ├── Wrist3.obj │ │ ├── centered.Base.obj │ │ ├── centered.Shoulder.obj │ │ ├── schunk_base.obj │ │ ├── schunk_left_finger.obj │ │ └── schunk_right_finger.obj │ ├── model.config │ ├── model.sdf │ ├── schunk_mpg_80 │ │ ├── cvio.dat │ │ ├── driver.out-00000000-0.001000.mtl │ │ ├── driver.out-00000000-0.001000.obj │ │ ├── meshes │ │ │ └── visual │ │ │ │ ├── SCHUNK_MPG-80_BASE.dae │ │ │ │ ├── SCHUNK_MPG-80_LEFT_FINGER.dae │ │ │ │ └── SCHUNK_MPG-80_RIGHT_FINGER.dae │ │ ├── model.config │ │ ├── model.sdf │ │ └── schunk.xml │ ├── ur10.xml │ └── ur10 │ │ ├── meshes │ │ ├── collision │ │ │ ├── Base.dae │ │ │ ├── Forearm.dae │ │ │ ├── Shoulder.dae │ │ │ ├── UpperArm.dae │ │ │ ├── Wrist1.dae │ │ │ ├── Wrist2.dae │ │ │ └── Wrist3.dae │ │ └── visual │ │ │ ├── Base.dae │ │ │ ├── Forearm.dae │ │ │ ├── Shoulder.dae │ │ │ ├── UpperArm.dae │ │ │ ├── Wrist1.dae │ │ │ ├── Wrist2.dae │ │ │ └── Wrist3.dae │ │ ├── model.config │ │ ├── ref_ur10.sdf │ │ └── ur10.sdf └── urdf │ ├── pendulum-urdf.xml │ └── pendulum.urdf ├── include └── Moby │ ├── AABB.h │ ├── AABB.inl │ ├── ADF.h │ ├── ArticulatedBody.h │ ├── ArticulatedBody.inl │ ├── BV.h │ ├── BV.inl │ ├── Base.h │ ├── BoundingSphere.h │ ├── BoundingSphere.inl │ ├── BoxPrimitive.h │ ├── C2ACCD.h │ ├── C2ACCD.inl │ ├── CCD.h │ ├── CCD.inl │ ├── CP.h │ ├── CSG.h │ ├── CollisionDetection.h │ ├── CollisionDetection.inl │ ├── CollisionGeometry.h │ ├── CompGeom.h │ ├── CompGeom.inl │ ├── ConePrimitive.h │ ├── Constants.h │ ├── ConstraintSimulator.h │ ├── ConstraintStabilization.h │ ├── Contact.inl │ ├── ContactParameters.h │ ├── ControlledBody.h │ ├── CylinderPrimitive.h │ ├── DampingForce.h │ ├── DegenerateTriangleException.h │ ├── Dissipation.h │ ├── DummyBV.h │ ├── EventDrivenSimulator.h │ ├── EventDrivenSimulator.inl │ ├── FastThreadable.h │ ├── FixedJoint.h │ ├── GJK.h │ ├── GaussianMixture.h │ ├── Gears.h │ ├── GravityForce.h │ ├── HashClasses.h │ ├── HeightmapPrimitive.h │ ├── ImpactConstraintHandler.h │ ├── ImpactToleranceException.h │ ├── IndexedTetra.h │ ├── IndexedTetraArray.h │ ├── IndexedTetraArray.inl │ ├── IndexedTri.h │ ├── IndexedTriArray.h │ ├── IndexedTriArray.inl │ ├── InvalidIndexException.h │ ├── InvalidStateException.h │ ├── InvalidVelocityException.h │ ├── Joint.h │ ├── LCP.h │ ├── LCPSolverException.h │ ├── LCP_IPOPT.h │ ├── LP.h │ ├── Log.h │ ├── MCArticulatedBody.h │ ├── MissizeException.h │ ├── NQP_IPOPT.h │ ├── Node.h │ ├── NonsquareMatrixException.h │ ├── NumericalException.h │ ├── OBB.h │ ├── OBB.inl │ ├── OSGGroupWrapper.h │ ├── PairwiseDistInfo.h │ ├── PenaltyConstraintHandler.h │ ├── PlanarJoint.h │ ├── Plane.h │ ├── PlanePrimitive.h │ ├── PolyhedralPrimitive.h │ ├── PolyhedralPrimitive.inl │ ├── Polyhedron.h │ ├── Polyhedron.inl │ ├── Primitive.h │ ├── Primitive.inl │ ├── PrismaticJoint.h │ ├── QLCPD.h │ ├── QLCPD.inl │ ├── QP.h │ ├── QP.inl │ ├── RCArticulatedBody.h │ ├── RCArticulatedBodyInvDynAlgo.h │ ├── RecurrentForce.h │ ├── RevoluteJoint.h │ ├── RigidBody.h │ ├── RigidBody.inl │ ├── SDFReader.h │ ├── SSL.h │ ├── SSL.inl │ ├── SSR.h │ ├── SSR.inl │ ├── ScrewJoint.h │ ├── SignedDistDot.h │ ├── Simulator.h │ ├── Simulator.inl │ ├── SingularException.h │ ├── SparseJacobian.h │ ├── SpherePrimitive.h │ ├── SphericalJoint.h │ ├── StokesDragForce.h │ ├── SustainedUnilateralConstraintHandler.h │ ├── SustainedUnilateralConstraintProblemData.h │ ├── SustainedUnilateralConstraintSolveFailException.h │ ├── TessellatedPolyhedron.h │ ├── TessellatedPolyhedron.inl │ ├── Tetrahedron.h │ ├── ThickTriangle.h │ ├── TimeSteppingSimulator.h │ ├── TorusPrimitive.h │ ├── Triangle.h │ ├── TriangleMeshPrimitive.h │ ├── TriangleMeshPrimitive.inl │ ├── Types.h │ ├── URDFReader.h │ ├── UndefinedAxisException.h │ ├── UnilateralConstraint.h │ ├── UnilateralConstraint.inl │ ├── UnilateralConstraintProblemData.h │ ├── UniversalJoint.h │ ├── Visualizable.h │ ├── XMLReader.h │ ├── XMLTree.h │ ├── XMLWriter.h │ ├── insertion_sort │ ├── permute.h │ ├── qpOASES.h │ ├── qpOASES.inl │ └── select ├── programs ├── adjust-center.cpp ├── center.cpp ├── compare-trajs.cpp ├── conv-decomp.cpp ├── convexify.cpp ├── driver.cpp ├── main.cpp ├── objwrl.cpp ├── regress.cpp ├── render-movie-simple.sh ├── render-movie.sh ├── render.cpp └── view.cpp ├── regress ├── absolute-coords.setup ├── bouncing-ball.setup ├── contact-constrained-pendulum.dat ├── contact-constrained-pendulum.setup ├── cylinder.setup ├── fixed-articulated-table.dat ├── fixed-articulated-table.setup ├── gears.setup ├── joint-limits.setup ├── pendulum-urdf.setup ├── planar-joint.setup ├── ramp.setup ├── reduced-coords.setup ├── regenerate-regression-data ├── regression-test ├── rimless-wheel.dat ├── rimless-wheel.setup ├── rolling-torus.setup ├── sitting-box.dat ├── sphere-stack.dat ├── spinning-boxes.setup └── stacks.setup ├── src ├── AABB.cpp ├── ADF.cpp ├── ArticulatedBody.cpp ├── BV.cpp ├── Base.cpp ├── BoundingSphere.cpp ├── BoxPrimitive.cpp ├── BulirschStoerIntegrator.cpp ├── C2ACCD.cpp ├── CCD.cpp ├── CP.cpp ├── CSG.cpp ├── CollisionDetection.cpp ├── CollisionGeometry.cpp ├── Color.h ├── CompGeom.cpp ├── ConePrimitive.cpp ├── ConstraintSimulator.cpp ├── ConstraintStabilization.cpp ├── ContactParameters.cpp ├── ControlledBody.cpp ├── CylinderPrimitive.cpp ├── DampingForce.cpp ├── Dissipation.cpp ├── EulerIntegrator.cpp ├── EventDrivenSimulator.cpp ├── FixedJoint.cpp ├── GJK.cpp ├── GaussianMixture.cpp ├── Gears.cpp ├── GravityForce.cpp ├── HeightmapPrimitive.cpp ├── ImpactConstraintHandler.cpp ├── ImpactConstraintHandlerLCP.cpp ├── ImpactConstraintHandlerNQP.cpp ├── ImpactConstraintHandlerQP.cpp ├── IndexedTetraArray.cpp ├── IndexedTriArray.cpp ├── Integrator.cpp ├── Joint.cpp ├── LCP.cpp ├── LCP_IPOPT.cpp ├── LP.cpp ├── Log.cpp ├── MCArticulatedBody.cpp ├── NQP_IPOPT.cpp ├── OBB.cpp ├── ODEPACKIntegrator.cpp ├── OSGGroupWrapper.cpp ├── OsgTorus.cpp ├── OsgTorus.h ├── PenaltyConstraintHandler.cpp ├── PlanarJoint.cpp ├── PlanePrimitive.cpp ├── PolyhedralPrimitive.cpp ├── Polyhedron.cpp ├── Primitive.cpp ├── PrismaticJoint.cpp ├── QLCPD.cpp ├── RCArticulatedBody.cpp ├── RCArticulatedBodyFwdDynAlgo.cpp ├── RestingContactHandler.cpp.bak ├── RevoluteJoint.cpp ├── RigidBody.cpp ├── Rosenbrock4Integrator.cpp ├── RungeKuttaFehlbergIntegrator.cpp ├── RungeKuttaImplicitIntegrator.cpp ├── RungeKuttaIntegrator.cpp ├── SDFReader.cpp ├── SSL.cpp ├── SSR.cpp ├── ScrewJoint.cpp ├── SignedDistDot.cpp ├── Simulator.cpp ├── SingleBody.cpp ├── SparseJacobian.cpp ├── SpherePrimitive.cpp ├── SphericalJoint.cpp ├── StokesDragForce.cpp ├── SustainedUnilateralConstraintHandler.cpp ├── TessellatedPolyhedron.cpp ├── Tetrahedron.cpp ├── ThickTriangle.cpp ├── TimeSteppingSimulator.cpp ├── TorusPrimitive.cpp ├── Triangle.cpp ├── TriangleMeshPrimitive.cpp ├── URDFReader.cpp ├── UnilateralConstraint.cpp ├── UniversalJoint.cpp ├── VariableEulerIntegrator.cpp ├── VariableStepIntegrator.cpp ├── Visualizable.cpp ├── XMLReader.cpp ├── XMLTree.cpp └── XMLWriter.cpp └── test ├── PolyhedronTest.cpp ├── TestDie.cpp ├── TestOffsetSphere.cpp ├── TestSparseJacobian.cpp ├── TestTorus.cpp ├── VClipTest.cpp ├── box.xml ├── qp.cpp ├── sphere.xml └── torus.xml /.gitignore: -------------------------------------------------------------------------------- 1 | release 2 | debug 3 | build 4 | -------------------------------------------------------------------------------- /CMakeModules/FindCBLAS.cmake: -------------------------------------------------------------------------------- 1 | # - Find CBLAS 2 | # Find the native CBLAS headers and libraries. 3 | # 4 | # CBLAS_LIBRARIES - List of libraries when using cblas. 5 | # CBLAS_FOUND - True if cblas found. 6 | # 7 | # Copyright 2009-2011 The VOTCA Development Team (http://www.votca.org) 8 | # 9 | # Licensed under the Apache License, Version 2.0 (the "License"); 10 | # you may not use this file except in compliance with the License. 11 | # You may obtain a copy of the License at 12 | # 13 | # http://www.apache.org/licenses/LICENSE-2.0 14 | # 15 | # Unless required by applicable law or agreed to in writing, software 16 | # distributed under the License is distributed on an "AS IS" BASIS, 17 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | # See the License for the specific language governing permissions and 19 | # limitations under the License. 20 | # 21 | 22 | find_library(CBLAS_LIBRARY NAMES blas cblas HINTS $ENV{CBLASDIR}/lib $ENV{CBLASDIR}/lib64 ) 23 | 24 | set(CBLAS_LIBRARIES ${CBLAS_LIBRARY} ) 25 | 26 | include(FindPackageHandleStandardArgs) 27 | # handle the QUIETLY and REQUIRED arguments and set FFTW3_FOUND to TRUE 28 | # if all listed variables are TRUE 29 | 30 | find_package_handle_standard_args(CBLAS DEFAULT_MSG CBLAS_LIBRARY ) 31 | 32 | if (CBLAS_FOUND) 33 | include(CheckLibraryExists) 34 | check_library_exists("${CBLAS_LIBRARY}" cblas_dsyrk "" FOUND_DSYRK) 35 | if(NOT FOUND_DSYRK) 36 | message(FATAL_ERROR "Could not find cblas_dsyrk in ${CBLAS_LIBRARY}, take a look at the error message in ${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/CMakeError.log to find out what was going wrong. If you don't have pkg-config installed you will most likely have to set CBLAS_LIBRARY by hand (i.e. -DCBLAS_LIBRARY='/path/to/libcblas.so') !") 37 | endif(NOT FOUND_DSYRK) 38 | endif (CBLAS_FOUND) 39 | 40 | mark_as_advanced( CBLAS_LIBRARY ) 41 | 42 | -------------------------------------------------------------------------------- /CMakeModules/FindLAPACKLite.cmake: -------------------------------------------------------------------------------- 1 | # - Find LAPACK library 2 | # This module finds an installed fortran library that implements the LAPACK 3 | # linear-algebra interface (see http://www.netlib.org/lapack/). 4 | # 5 | # The approach follows that taken for the autoconf macro file, acx_lapack.m4 6 | # (distributed at http://ac-archive.sourceforge.net/ac-archive/acx_lapack.html). 7 | # 8 | # This module sets the following variables: 9 | # LAPACK_FOUND - set to true if a library implementing the LAPACK interface 10 | # is found 11 | # LAPACK_LINKER_FLAGS - uncached list of required linker flags (excluding -l 12 | # and -L). 13 | # LAPACK_LIBRARIES - uncached list of libraries (using full path name) to 14 | # link against to use LAPACK 15 | # BLA_STATIC if set on this determines what kind of linkage we do (static) 16 | # BLA_VENDOR if set checks only the specified vendor, if not set checks 17 | # all the possibilities 18 | ### List of vendors (BLA_VENDOR) valid in this module 19 | ## Intel(mkl), ACML,Apple, NAS, Generic 20 | 21 | #============================================================================= 22 | # Copyright 2007-2009 Kitware, Inc. 23 | # 24 | # Distributed under the OSI-approved BSD License (the "License"); 25 | # see accompanying file Copyright.txt for details. 26 | # 27 | # This software is distributed WITHOUT ANY WARRANTY; without even the 28 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 29 | # See the License for more information. 30 | #============================================================================= 31 | # (To distribute this file outside of CMake, substitute the full 32 | # License text for the above reference.) 33 | 34 | find_library(LAPACK_LIBRARY NAMES lapack-3.1 lapack-3 lapack $ENV{LAPACKDIR}/lib $ENV{LAPACKDIR}/lib64) 35 | 36 | set(LAPACK_LIBRARIES ${LAPACK_LIBRARY}) 37 | 38 | include(FindPackageHandleStandardArgs) 39 | find_package_handle_standard_args(LAPACK DEFAULT_MSG LAPACK_LIBRARY) 40 | 41 | mark_as_advanced(LAPACK_LIBRARY) 42 | 43 | -------------------------------------------------------------------------------- /CMakeModules/FindQHULL.cmake: -------------------------------------------------------------------------------- 1 | # Find qhull header and library. 2 | # 3 | 4 | # This module defines the following uncached variables: 5 | # QHULL_FOUND, if false, do not try to use qhull. 6 | # QHULL_INCLUDE_DIRS, where to find qhull/qhull_a.h. 7 | # QHULL_LIBRARIES, the libraries to link against to use the qhull library 8 | # QHULL_LIBRARY_DIRS, the directory where the qhull library is found. 9 | 10 | find_path( 11 | QHULL_INCLUDE_DIR 12 | qhull/qhull_a.h libqhull/qhull_a.h 13 | PATHS /usr/local/include /usr/include 14 | ) 15 | 16 | if( QHULL_INCLUDE_DIR ) 17 | find_library( 18 | QHULL_LIBRARY 19 | NAMES libqhullstatic libqhull qhullstatic qhull 20 | PATHS /usr/local/lib /usr/lib 21 | ) 22 | if( QHULL_LIBRARY ) 23 | set(QHULL_LIBRARY_DIR "") 24 | get_filename_component(QHULL_LIBRARY_DIRS ${QHULL_LIBRARY} PATH) 25 | # Set uncached variables as per standard. 26 | set(QHULL_FOUND ON) 27 | set(QHULL_INCLUDE_DIRS ${QHULL_INCLUDE_DIR}) 28 | set(QHULL_LIBRARIES ${QHULL_LIBRARY}) 29 | endif(QHULL_LIBRARY) 30 | else(QHULL_INCLUDE_DIR) 31 | message(FATAL_ERROR "FindQHull: Could not find qhull_a.h") 32 | endif(QHULL_INCLUDE_DIR) 33 | 34 | if(QHULL_FOUND) 35 | if(NOT QHULL_FIND_QUIETLY) 36 | message(STATUS "FindQHull: Found both qhull_a.h and libqhull.a") 37 | endif(NOT QHULL_FIND_QUIETLY) 38 | else(QHULL_FOUND) 39 | if(QHULL_FIND_REQUIRED) 40 | message(FATAL_ERROR "FindQHull: Could not find qhull_a.h and/or libqhull.a") 41 | endif(QHULL_FIND_REQUIRED) 42 | endif(QHULL_FOUND) 43 | -------------------------------------------------------------------------------- /CMakeModules/FindQPOASES.cmake: -------------------------------------------------------------------------------- 1 | # Find qpOASES header and library. 2 | # 3 | 4 | # This module defines the following uncached variables: 5 | # QPOASES_FOUND, if false, do not try to use qpOASES. 6 | # QPOASES_INCLUDE_DIRS, where to find qpOASES/include 7 | # QPOASES_LIBRARIES, the libraries to link against to use the qpoases library 8 | # QPOASES_LIBRARY_DIRS, the directory where the qpoases library is found. 9 | 10 | # look for QPOASES 11 | find_library(QPOASES_LIBRARY qpOASES NAMES libqpOASES.a libqpOASES.dylib /usr/local/lib /usr/lib) 12 | 13 | find_path(QPOASES_INCLUDE_DIR qpOASES PATHS /usr/local/include /usr/include) 14 | 15 | if (USE_QPOASES) 16 | if (QPOASES_LIBRARY AND QPOASES_INCLUDE_DIR) 17 | 18 | set(QPOASES_LIBRARY_DIR "") 19 | get_filename_component(QPOASES_LIBRARY_DIRS ${QPOASES_LIBRARY} PATH) 20 | # Set uncached variables as per standard. 21 | set(QPOASES_FOUND ON) 22 | set(QPOASES_INCLUDE_DIRS ${QPOASES_INCLUDE_DIR}) 23 | set(QPOASES_LIBRARIES ${QPOASES_LIBRARY}) 24 | endif (QPOASES_FOUND AND QPOASES_INCLUDE_DIR) 25 | endif (USE_QPOASES) 26 | 27 | if(QPOASES_LIBRARY) 28 | if(NOT QPOASES_FIND_QUIETLY) 29 | message(STATUS "FindQPOASES: Found both include/qpoases and libqpOASES.a") 30 | endif(NOT QPOASES_FIND_QUIETLY) 31 | else(QPOASES_LIBRARY) 32 | if(QPOASES_INCLUDE_DIR) 33 | message(FATAL_ERROR "FindQPOASES: Could not find libqpoases.a") 34 | endif(QPOASES_INCLUDE_DIR) 35 | endif(QPOASES_LIBRARY) 36 | 37 | -------------------------------------------------------------------------------- /CMakeModules/FindRavelin.cmake: -------------------------------------------------------------------------------- 1 | # Find Ravelin header and library. 2 | # 3 | 4 | # This module defines the following uncached variables: 5 | # RAVELIN_FOUND, if false, do not try to use Ravelin. 6 | # RAVELIN_INCLUDE_DIRS, where to find Ravelin/Ravelin_a.h. 7 | # RAVELIN_LIBRARIES, the libraries to link against to use the Ravelin library 8 | # RAVELIN_LIBRARY_DIRS, the directory where the Ravelin library is found. 9 | 10 | find_path( 11 | RAVELIN_INCLUDE_DIR 12 | Ravelin/VectorNd.h 13 | PATHS /usr/local/include /usr/include 14 | ) 15 | 16 | if( RAVELIN_INCLUDE_DIR ) 17 | find_library( 18 | RAVELIN_LIBRARY 19 | NAMES libRavelin Ravelin 20 | PATHS /usr/local/lib /usr/lib 21 | ) 22 | if( RAVELIN_LIBRARY ) 23 | set(RAVELIN_LIBRARY_DIR "") 24 | get_filename_component(RAVELIN_LIBRARY_DIRS ${RAVELIN_LIBRARY} PATH) 25 | # Set uncached variables as per standard. 26 | set(RAVELIN_FOUND ON) 27 | set(RAVELIN_INCLUDE_DIRS ${RAVELIN_INCLUDE_DIR}) 28 | set(RAVELIN_LIBRARIES ${RAVELIN_LIBRARY}) 29 | endif(RAVELIN_LIBRARY) 30 | else(RAVELIN_INCLUDE_DIR) 31 | message(FATAL_ERROR "FindRavelin: Could not find VectorNd.h") 32 | endif(RAVELIN_INCLUDE_DIR) 33 | 34 | if(RAVELIN_FOUND) 35 | if(NOT RAVELIN_FIND_QUIETLY) 36 | message(STATUS "FindRavelin: Found both VectorNd.h and libRavelin.a") 37 | endif(NOT RAVELIN_FIND_QUIETLY) 38 | else(RAVELIN_FOUND) 39 | if(RAVELIN_FIND_REQUIRED) 40 | message(FATAL_ERROR "FindRavelin: Could not find VectorNd.h and/or libRavelin.a") 41 | endif(RAVELIN_FIND_REQUIRED) 42 | endif(RAVELIN_FOUND) 43 | -------------------------------------------------------------------------------- /CMakeModules/FindSuperLU.cmake: -------------------------------------------------------------------------------- 1 | 2 | # Umfpack lib usually requires linking to a blas library. 3 | # It is up to the user of this module to find a BLAS and link to it. 4 | 5 | if (SUPERLU_INCLUDES AND SUPERLU_LIBRARIES) 6 | set(SUPERLU_FIND_QUIETLY TRUE) 7 | endif (SUPERLU_INCLUDES AND SUPERLU_LIBRARIES) 8 | 9 | find_path(SUPERLU_INCLUDES 10 | NAMES 11 | supermatrix.h 12 | PATHS 13 | ${SUPERLU_PATH}/include 14 | $ENV{SUPERLUDIR}/include 15 | ${INCLUDE_INSTALL_DIR} 16 | PATH_SUFFIXES 17 | superlu 18 | ) 19 | 20 | find_library(SUPERLU_LIBRARIES 21 | NAMES 22 | superlu 23 | superlu_4.3 24 | PATHS 25 | ${SUPERLU_PATH}/lib 26 | $ENV{SUPERLUDIR}/lib 27 | ${LIB_INSTALL_DIR}) 28 | 29 | include(FindPackageHandleStandardArgs) 30 | find_package_handle_standard_args(SUPERLU DEFAULT_MSG 31 | SUPERLU_INCLUDES SUPERLU_LIBRARIES) 32 | 33 | mark_as_advanced(SUPERLU_INCLUDES SUPERLU_LIBRARIES) 34 | 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Moby 2 | ==== 3 | 4 | The _Moby_ multi-rigid body dynamics simulator. See the [wiki](https://github.com/PositronicsLab/Moby/wiki) for proper documentation. 5 | -------------------------------------------------------------------------------- /cmake_uninstall.cmake.in: -------------------------------------------------------------------------------- 1 | if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 3 | endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 4 | 5 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | foreach(file ${files}) 8 | message(STATUS "Uninstalling $ENV{DESTDIR}${file}") 9 | if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 10 | exec_program( 11 | "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" 12 | OUTPUT_VARIABLE rm_out 13 | RETURN_VALUE rm_retval 14 | ) 15 | if(NOT "${rm_retval}" STREQUAL 0) 16 | message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") 17 | endif(NOT "${rm_retval}" STREQUAL 0) 18 | else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 19 | message(STATUS "File $ENV{DESTDIR}${file} does not exist.") 20 | endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 21 | endforeach(file) 22 | 23 | -------------------------------------------------------------------------------- /example/bouncing-ball/bouncing-ball.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /example/contact-constrained-pendulum/contact-constrained-pendulum-init.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * "Controller" for constrained pendulum example 3 | ****************************************************************************/ 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using boost::shared_ptr; 14 | using namespace Ravelin; 15 | using namespace Moby; 16 | 17 | Moby::RigidBodyPtr l1; 18 | boost::shared_ptr sim; 19 | boost::shared_ptr grav; 20 | 21 | // setup simulator callback 22 | void post_step_callback(Simulator* sim) 23 | { 24 | const unsigned Y = 1, Z = 2; 25 | 26 | // output the energy of the link 27 | std::ofstream out("energy.dat", std::ostream::app); 28 | Transform3d gTw = Pose3d::calc_relative_pose(l1->get_pose(), GLOBAL); 29 | double KE = l1->calc_kinetic_energy(); 30 | double PE = l1->get_inertia().m*(gTw.x[Y]+1.0)*-grav->gravity[Y]; 31 | out << KE << " " << PE << " " << (KE+PE) << std::endl; 32 | out.close(); 33 | SVelocityd v = l1->get_velocity(); 34 | } 35 | 36 | /// plugin must be "extern C" 37 | extern "C" { 38 | 39 | void init(void* separator, const std::map& read_map, double time) 40 | { 41 | const unsigned Z = 2; 42 | 43 | // kill the existing files 44 | std::ofstream out("energy.dat"); 45 | out.close(); 46 | out.open("cvio.dat"); 47 | out.close(); 48 | 49 | // get a reference to the ConstraintSimulator instance 50 | for (std::map::const_iterator i = read_map.begin(); 51 | i !=read_map.end(); i++) 52 | { 53 | // Find the simulator reference 54 | if (!sim) 55 | sim = boost::dynamic_pointer_cast(i->second); 56 | if (i->first == "l1") 57 | l1 = boost::dynamic_pointer_cast(i->second); 58 | if (!grav) 59 | grav = boost::dynamic_pointer_cast(i->second); 60 | } 61 | 62 | sim->post_step_callback_fn = &post_step_callback; 63 | } 64 | } // end extern C 65 | -------------------------------------------------------------------------------- /example/contact-constrained-pendulum/contact-constrained-pendulum.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /example/events/rotating-box.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /example/gears/pendulum-gears.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /example/joint-limits/limit-double-pendulum.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /example/joint-limits/limit-pendulum.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /example/mrobot/mrobot.pacer: -------------------------------------------------------------------------------- 1 | 2 | ERROR 3 | 4 | 5 | 6 | true 7 | 0 0 0.0 0 0 0 8 | 9 | 10 | 11 | 0left_wheel_hinge 0right_wheel_hinge 12 | 0 0 13 | true true 14 | 1000000 1000000 15 | 16 | 17 | 18 | left_wheel right_wheel chassis 19 | 0 0 0 0 0 0 0 0 0 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /example/mrobot/mrobot.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 33 | 34 | 35 | 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /example/parts-feeder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(test) 4 | 5 | include (CheckIncludeFiles) 6 | include (CheckLibraryExists) 7 | 8 | set (CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/CMakeModules) 9 | set (PROJECT_SRC_DIR .) 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 11 | find_package(Boost REQUIRED) 12 | 13 | # you may have to add additional includes in here... 14 | add_definitions(-DBUILD_DOUBLE) 15 | add_definitions(-DUSE_OSG) 16 | add_definitions(-DSAFESTATIC=static) 17 | add_definitions(-DUSE_GLPK) 18 | 19 | include_directories( 20 | . 21 | ../../include 22 | /usr/include 23 | /usr/include/libxml2 24 | /usr/local/include 25 | ${Boost_INCLUDE_DIR} 26 | /opt/local/include 27 | /opt/X11/include 28 | ) 29 | 30 | link_directories( 31 | /usr/local/lib 32 | /opt/X11/lib 33 | ) 34 | 35 | add_library(${PROJECT_NAME}Plugin MODULE feeder.cpp) 36 | target_link_libraries(${PROJECT_NAME}Plugin Moby Ravelin) 37 | -------------------------------------------------------------------------------- /example/parts-feeder/feeder.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * Controller for LINKS robot 3 | ****************************************************************************/ 4 | #include 5 | #include 6 | 7 | boost::shared_ptr sim; 8 | Moby::RCArticulatedBodyPtr slide; 9 | 10 | void controller_callback(Moby::ControlledBodyPtr dbp, double t, void*) 11 | { 12 | double p = 0.001*cos(t*(2.0*M_PI)*500.0); 13 | double v = -0.001*sin(t*1000.0) *(2.0*M_PI)*500.0; 14 | double Kp = 1e3 15 | ,Kv = 1e1 16 | ; 17 | 18 | for(int i=0;iget_joints().size();i++){ 19 | if(slide->get_joints()[i]->num_dof() == 0) continue; 20 | static Ravelin::VectorNd U(1); 21 | U[0] = Kp*(p - slide->get_joints()[i]->q[0]) + Kv*(v - slide->get_joints()[i]->qd[0]); 22 | std::cout << "x = [ " << slide->get_joints()[i]->q[0] << " , " << slide->get_joints()[i]->qd[0] << " ];" << std::endl; 23 | std::cout << "x* = [ " << p << " , " << v << " ];" << std::endl; 24 | std::cout << "U = [ " << U[0] << " ];" << std::endl; 25 | slide->get_joints()[i]->add_force(U); 26 | } 27 | } 28 | 29 | // ============================================================================ 30 | // ================================ CALLBACKS ================================= 31 | 32 | /// plugin must be "extern C" 33 | extern "C" { 34 | 35 | void init(void* separator, const std::map& read_map, double time) 36 | { 37 | // If use robot is active also init dynamixel controllers 38 | // get a reference to the TimeSteppingSimulator instance 39 | for (std::map::const_iterator i = read_map.begin(); 40 | i !=read_map.end(); i++) 41 | { 42 | // Find the simulator reference 43 | if (!sim) 44 | sim = boost::dynamic_pointer_cast(i->second); 45 | 46 | // find the robot reference 47 | if (!slide) 48 | { 49 | slide = boost::dynamic_pointer_cast(i->second); 50 | } 51 | } 52 | 53 | slide->controller = &controller_callback; 54 | 55 | } 56 | } // end extern C 57 | -------------------------------------------------------------------------------- /example/passive-walker/walker.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /example/planar-joint/constrained.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /example/plugin-template.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * Controller plugin template 3 | ****************************************************************************/ 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using boost::shared_ptr; 15 | using namespace Ravelin; 16 | using namespace Moby; 17 | 18 | // pointers to objects found within the XML file 19 | boost::shared_ptr sim; 20 | 21 | // post-simulation step callback 22 | void post_step_callback(Simulator* s) 23 | { 24 | } 25 | 26 | // control function 27 | void controller(ControlledBodyPtr body, double time, void* controller_data) 28 | { 29 | 30 | } 31 | 32 | /// plugin must be "extern C" 33 | extern "C" { 34 | 35 | // initialization function is called by Moby; setup callbacks here 36 | void init(void* separator, const std::map& read_map, double time) 37 | { 38 | // get a reference to the TimeSteppingSimulator instance 39 | for (std::map::const_iterator i = read_map.begin(); 40 | i !=read_map.end(); i++) 41 | { 42 | // Find the simulator reference 43 | if (!sim) 44 | sim = boost::dynamic_pointer_cast(i->second); 45 | 46 | // example of looking for a body and setting up a callback function for it 47 | // if (i->first == "ground") 48 | // { 49 | // ControlledBodyPtr ground = boost::dynamic_pointer_cast(i->second); 50 | // assert(ground); 51 | // ground->controller = &controller; 52 | // } 53 | } 54 | 55 | sim->post_step_callback_fn = &post_step_callback; 56 | } 57 | } // end extern C 58 | -------------------------------------------------------------------------------- /example/polyhedra/spinning-box-frictional.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /example/polyhedra/spinning-box-frictionless.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /example/reduced-coords/double-pendulum.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /example/reduced-coords/pendulum-gears-impact.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /example/reduced-coords/pendulum.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /example/relative-visualization/box-plane.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /example/rimless-wheel/params.h: -------------------------------------------------------------------------------- 1 | #ifndef _RIMLESS_WHEEL_PARAMS_H_ 2 | #define _RIMLESS_WHEEL_PARAMS_H_ 3 | 4 | const double R = 1.0; // radius from center of mass to end of blade 5 | const double W = 0; // width of a blade 6 | const unsigned N_SPOKES = 6; // number of spokes in the wheel 7 | const bool FIND_MAP = false; 8 | 9 | #endif 10 | 11 | -------------------------------------------------------------------------------- /example/rolling-torus/torus.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /example/simple-contact/box-plugin.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * "Controller" for outputting data from the simple box examples 3 | ****************************************************************************/ 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using boost::shared_ptr; 14 | using namespace Ravelin; 15 | using namespace Moby; 16 | 17 | Moby::RigidBodyPtr box; 18 | boost::shared_ptr sim; 19 | boost::shared_ptr grav; 20 | 21 | // setup simulator callback 22 | void post_step_callback(Simulator* sim) 23 | { 24 | // output the sliding velocity at the contact 25 | std::ofstream out("ke.dat", std::ostream::app); 26 | out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl; 27 | out.close(); 28 | } 29 | 30 | /// plugin must be "extern C" 31 | extern "C" { 32 | 33 | void init(void* separator, const std::map& read_map, double time) 34 | { 35 | const unsigned Z = 2; 36 | 37 | // wipe out existing file 38 | std::ofstream out("ke.dat"); 39 | out.close(); 40 | 41 | // get a reference to the TimeSteppingSimulator instance 42 | for (std::map::const_iterator i = read_map.begin(); 43 | i !=read_map.end(); i++) 44 | { 45 | // Find the simulator reference 46 | if (!sim) 47 | sim = boost::dynamic_pointer_cast(i->second); 48 | if (i->first == "box") 49 | box = boost::dynamic_pointer_cast(i->second); 50 | if (!grav) 51 | grav = boost::dynamic_pointer_cast(i->second); 52 | } 53 | 54 | sim->post_step_callback_fn = &post_step_callback; 55 | } 56 | } // end extern C 57 | -------------------------------------------------------------------------------- /example/simple-contact/cylinder.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /example/simple-contact/ramp.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /example/simple-contact/simplest.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /example/simple-contact/spinning-box-frictional.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /example/simple-contact/spinning-box-frictionless.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /example/sims-in-code/block.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "viewer.h" 9 | 10 | int main( void ) { 11 | 12 | boost::shared_ptr sim( new Moby::Simulator() ); 13 | 14 | boost::shared_ptr g( new Moby::GravityForce() ); 15 | g->gravity = Ravelin::Vector3d( 0, 0, -9.8 ); 16 | 17 | Moby::PrimitivePtr box( new Moby::BoxPrimitive(1,1,1) ); 18 | box->set_mass( 1000 ); 19 | 20 | Moby::RigidBodyPtr rb( new Moby::RigidBody() ); 21 | rb->id = "block"; 22 | rb->set_visualization_data( box->create_visualization() ); 23 | rb->set_inertia( box->get_inertia() ); 24 | rb->set_enabled( true ); 25 | rb->get_recurrent_forces().push_back( g ); 26 | 27 | rb->set_pose( Ravelin::Pose3d( Ravelin::Quatd(0,0,0,1), Ravelin::Origin3d(0,0,0) ) ); 28 | 29 | sim->add_dynamic_body( rb ); 30 | 31 | Viewer viewer( sim, Ravelin::Origin3d(0,0,4), Ravelin::Origin3d(0,0,0), Ravelin::Origin3d(1,0,0) ); 32 | 33 | while(true) { 34 | if( !viewer.update() ) break; 35 | 36 | sim->step( 0.001 ); 37 | boost::shared_ptr pose = rb->get_pose(); 38 | std::cout << "t: " << sim->current_time << " x: " << pose->x << std::endl; 39 | } 40 | 41 | return 0; 42 | } 43 | 44 | -------------------------------------------------------------------------------- /example/sims-in-code/viewer.h: -------------------------------------------------------------------------------- 1 | #ifndef _VIEWER_H_ 2 | #define _VIEWER_H_ 3 | 4 | #include 5 | #include 6 | 7 | #ifdef USE_OSG 8 | #include 9 | #include 10 | #include 11 | #include 12 | #endif 13 | 14 | class Viewer { 15 | public: 16 | 17 | Viewer( boost::shared_ptr sim, Ravelin::Origin3d camera_position, Ravelin::Origin3d camera_viewpoint, Ravelin::Origin3d camera_up ) : _sim(sim) { 18 | 19 | #ifdef USE_OSG 20 | // convert parameters to compatible osg types 21 | osg::Vec3d osg_eye(camera_position[0],camera_position[1],camera_position[2]); 22 | osg::Vec3d osg_center(camera_viewpoint[0], camera_viewpoint[1], camera_viewpoint[2]); 23 | osg::Vec3d osg_up(camera_up[0], camera_up[1], camera_up[2]); 24 | 25 | // initialize viewer 26 | _viewer.setThreadingModel(osgViewer::Viewer::SingleThreaded); 27 | 28 | // initialize handlers 29 | _viewer.setCameraManipulator(new osgGA::TrackballManipulator()); 30 | _viewer.addEventHandler(new osgGA::StateSetManipulator(_viewer.getCamera()->getOrCreateStateSet())); 31 | _viewer.addEventHandler(new osgViewer::WindowSizeHandler); 32 | _viewer.addEventHandler(new osgViewer::StatsHandler); 33 | 34 | // initialize window 35 | _viewer.setUpViewInWindow(0, 0, 1024, 1024); 36 | 37 | // initialize camera 38 | osg::Camera* camera = _viewer.getCamera(); 39 | camera->setViewMatrixAsLookAt(osg_eye, osg_center, osg_up); 40 | _viewer.getCameraManipulator()->setHomePosition(osg_eye, osg_center, osg_up); 41 | 42 | // establish the scene data 43 | _root = new osg::Group; 44 | _root->addChild(sim->get_persistent_vdata()); 45 | _root->addChild(sim->get_transient_vdata()); 46 | _viewer.setSceneData(_root); 47 | _viewer.realize(); 48 | #endif // USE_OSG 49 | } 50 | 51 | virtual ~Viewer( void ) { 52 | 53 | } 54 | 55 | bool update( void ) { 56 | _sim->update_visualization(); 57 | 58 | #ifdef USE_OSG 59 | if( _viewer.done() ) return false; 60 | _viewer.frame(); 61 | #endif // USE_OSG 62 | 63 | return true; 64 | } 65 | 66 | private: 67 | #ifdef USE_OSG 68 | osg::Group* _root; // the osg scene graph root node 69 | osgViewer::Viewer _viewer; // the osg viewer 70 | #endif // USE_OSG 71 | 72 | boost::shared_ptr _sim; // the simulator 73 | }; 74 | 75 | 76 | #endif // _VIEWER_H_ 77 | -------------------------------------------------------------------------------- /example/stacks/stack2.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /example/tare/pendulum.xml: -------------------------------------------------------------------------------- 1 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /example/ur10/block.obj: -------------------------------------------------------------------------------- 1 | # cube.obj 2 | # 3 | 4 | g cube 5 | v -0.006 -0.0097 -0.012 6 | v -0.006 -0.0097 0.012 7 | v -0.006 0.0097 -0.012 8 | v -0.006 0.0097 0.012 9 | v 0.006 -0.0097 -.012 10 | v 0.006 -0.0097 0.012 11 | v 0.006 0.0097 -.012 12 | v 0.006 0.0097 0.012 13 | 14 | vn 0.0 0.0 1.0 15 | vn 0.0 0.0 -1.0 16 | vn 0.0 1.0 0.0 17 | vn 0.0 -1.0 0.0 18 | vn 1.0 0.0 0.0 19 | vn -1.0 0.0 0.0 20 | 21 | f 1//2 7//2 5//2 22 | f 1//2 3//2 7//2 23 | f 1//6 4//6 3//6 24 | f 1//6 2//6 4//6 25 | f 3//3 8//3 7//3 26 | f 3//3 4//3 8//3 27 | f 5//5 7//5 8//5 28 | f 5//5 8//5 6//5 29 | f 1//4 5//4 6//4 30 | f 1//4 6//4 2//4 31 | f 2//1 6//1 8//1 32 | f 2//1 8//1 4//1 33 | -------------------------------------------------------------------------------- /example/ur10/meshes/Base.obj.mtl: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | newmtl DefaultMaterial 5 | Kd 0.6 0.6 0.6 6 | illum 1 7 | 8 | -------------------------------------------------------------------------------- /example/ur10/meshes/UpperArm.obj.mtl: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | newmtl DefaultMaterial 5 | Kd 0.6 0.6 0.6 6 | illum 1 7 | 8 | -------------------------------------------------------------------------------- /example/ur10/meshes/schunk_base.obj: -------------------------------------------------------------------------------- 1 | # cube.obj 2 | # 3 | 4 | g cube 5 | v -0.04 -0.035 -0.021 6 | v -0.04 -0.035 0.021 7 | v -0.04 0.035 -0.021 8 | v -0.04 0.035 0.021 9 | v 0.04 -0.035 -0.021 10 | v 0.04 -0.035 0.021 11 | v 0.04 0.035 -0.021 12 | v 0.04 0.035 0.021 13 | 14 | vn 0.0 0.0 1.0 15 | vn 0.0 0.0 -1.0 16 | vn 0.0 1.0 0.0 17 | vn 0.0 -1.0 0.0 18 | vn 1.0 0.0 0.0 19 | vn -1.0 0.0 0.0 20 | 21 | f 1//2 7//2 5//2 22 | f 1//2 3//2 7//2 23 | f 1//6 4//6 3//6 24 | f 1//6 2//6 4//6 25 | f 3//3 8//3 7//3 26 | f 3//3 4//3 8//3 27 | f 5//5 7//5 8//5 28 | f 5//5 8//5 6//5 29 | f 1//4 5//4 6//4 30 | f 1//4 6//4 2//4 31 | f 2//1 6//1 8//1 32 | f 2//1 8//1 4//1 33 | -------------------------------------------------------------------------------- /example/ur10/meshes/schunk_left_finger.obj: -------------------------------------------------------------------------------- 1 | # cube.obj 2 | # 3 | 4 | g cube 5 | v -0.006 -0.0097 -0.012 6 | v -0.006 -0.0097 0.012 7 | v -0.006 0.0097 -0.012 8 | v -0.006 0.0097 0.012 9 | v 0.006 -0.0097 -.012 10 | v 0.006 -0.0097 0.012 11 | v 0.006 0.0097 -.012 12 | v 0.006 0.0097 0.012 13 | 14 | vn 0.0 0.0 1.0 15 | vn 0.0 0.0 -1.0 16 | vn 0.0 1.0 0.0 17 | vn 0.0 -1.0 0.0 18 | vn 1.0 0.0 0.0 19 | vn -1.0 0.0 0.0 20 | 21 | f 1//2 7//2 5//2 22 | f 1//2 3//2 7//2 23 | f 1//6 4//6 3//6 24 | f 1//6 2//6 4//6 25 | f 3//3 8//3 7//3 26 | f 3//3 4//3 8//3 27 | f 5//5 7//5 8//5 28 | f 5//5 8//5 6//5 29 | f 1//4 5//4 6//4 30 | f 1//4 6//4 2//4 31 | f 2//1 6//1 8//1 32 | f 2//1 8//1 4//1 33 | -------------------------------------------------------------------------------- /example/ur10/meshes/schunk_right_finger.obj: -------------------------------------------------------------------------------- 1 | # cube.obj 2 | # 3 | 4 | g cube 5 | v -0.006 -0.0097 -0.012 6 | v -0.006 -0.0097 0.012 7 | v -0.006 0.0097 -0.012 8 | v -0.006 0.0097 0.012 9 | v 0.006 -0.0097 -.012 10 | v 0.006 -0.0097 0.012 11 | v 0.006 0.0097 -.012 12 | v 0.006 0.0097 0.012 13 | 14 | vn 0.0 0.0 1.0 15 | vn 0.0 0.0 -1.0 16 | vn 0.0 1.0 0.0 17 | vn 0.0 -1.0 0.0 18 | vn 1.0 0.0 0.0 19 | vn -1.0 0.0 0.0 20 | 21 | f 1//2 7//2 5//2 22 | f 1//2 3//2 7//2 23 | f 1//6 4//6 3//6 24 | f 1//6 2//6 4//6 25 | f 3//3 8//3 7//3 26 | f 3//3 4//3 8//3 27 | f 5//5 7//5 8//5 28 | f 5//5 8//5 6//5 29 | f 1//4 5//4 6//4 30 | f 1//4 6//4 2//4 31 | f 2//1 6//1 8//1 32 | f 2//1 8//1 4//1 33 | -------------------------------------------------------------------------------- /example/ur10/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ur10_schunk_hybrid 5 | 1.0 6 | model.sdf 7 | 8 | 9 | ur10_schunk_hybrid 10 | ur10_schunk_hybrid 11 | 12 | 13 | 14 | The ur10 schunk hybrid model. 15 | 16 | 17 | -------------------------------------------------------------------------------- /example/ur10/schunk_mpg_80/driver.out-00000000-0.001000.mtl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PositronicsLab/Moby/a025fae7e8123a8bf0f21d78953f83ef3dd9737c/example/ur10/schunk_mpg_80/driver.out-00000000-0.001000.mtl -------------------------------------------------------------------------------- /example/ur10/schunk_mpg_80/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | SCHUNK MPG 80 5 | 1.0 6 | model.sdf 7 | 8 | 9 | James Taylor 10 | jrt@gwu.edu 11 | 12 | 13 | 14 | The SCHUNK MPG 80 model 15 | 16 | 17 | -------------------------------------------------------------------------------- /example/ur10/schunk_mpg_80/schunk.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /example/ur10/ur10.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | -------------------------------------------------------------------------------- /example/ur10/ur10/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ur10 5 | 1.0 6 | ur10.sdf 7 | 8 | 9 | ur10 10 | ur10 11 | 12 | 13 | 14 | The ur10 model. 15 | 16 | 17 | -------------------------------------------------------------------------------- /example/urdf/pendulum-urdf.xml: -------------------------------------------------------------------------------- 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 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /example/urdf/pendulum.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /include/Moby/AABB.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_AABB_H_ 8 | #define _MOBY_AABB_H_ 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// An axis-aligned bounding box 16 | class AABB : public BV 17 | { 18 | public: 19 | AABB() {} 20 | 21 | template 22 | AABB(InputIterator begin, InputIterator end); 23 | 24 | virtual void transform(const Ravelin::Transform3d& T, BV* result) const; 25 | virtual bool outside(const Point3d& point, double tol = NEAR_ZERO) const { return AABB::outside(*this, point, tol); } 26 | virtual bool intersects(const LineSeg3& seg, double& tmin, double tmax, Point3d& q) const { return AABB::intersects(*this, seg, tmin, tmax, q); } 27 | virtual std::ostream& to_vrml(std::ostream& out, const Ravelin::Pose3d& T) const; 28 | virtual BVPtr calc_swept_BV(CollisionGeometryPtr g, const Ravelin::SVelocityd& v) const; 29 | virtual double calc_volume() const; 30 | virtual boost::shared_ptr get_relative_pose() const { return minp.pose; } 31 | virtual Point3d get_lower_bounds() const; 32 | virtual Point3d get_upper_bounds() const; 33 | OBB get_OBB() const; 34 | static bool intersects(const AABB& a, const AABB& b); 35 | static bool intersects(const AABB& a, const AABB& b, const Ravelin::Transform3d& aTb); 36 | static bool outside(const AABB& a, const Point3d& point, double tol = NEAR_ZERO); 37 | static bool intersects(const AABB& a, const LineSeg3& seg, double& tmin, double tmax, Point3d& q); 38 | static void get_closest_point(const AABB& a, const Point3d& p, Point3d& closest); 39 | static double get_farthest_point(const AABB& a, const Point3d& p, Point3d& farthest); 40 | AABB& operator=(const AABB& a); 41 | 42 | /// The lower corner of the AABB 43 | Point3d minp; 44 | 45 | /// The upper corner of the AABB; 46 | Point3d maxp; 47 | }; // end class 48 | 49 | #include "AABB.inl" 50 | 51 | } // end namespace 52 | 53 | #endif 54 | 55 | -------------------------------------------------------------------------------- /include/Moby/AABB.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | // For outputting description of AABB (primarily for debugging purposes) 8 | inline std::ostream& operator<<(std::ostream& out, const AABB& a) 9 | { 10 | out << " (address): " << &a << std::endl; 11 | out << " lower corner: " << a.minp << std::endl; 12 | out << " upper corner: " << a.maxp << std::endl; 13 | out << " volume: " << a.calc_volume() << std::endl; 14 | out << " children:"; 15 | BOOST_FOREACH(BVPtr child, a.children) 16 | out << " " << child; 17 | out << std::endl; 18 | 19 | return out; 20 | } 21 | 22 | /// Constructs an axis-aligned bounding box using a set of points 23 | /** 24 | * \param begin an iterator to the beginning of a container of type Ravelin::Vector3 25 | * \param end an iterator to the end of a container of type Ravelin::Vector3 26 | */ 27 | template 28 | AABB::AABB(InputIterator begin, InputIterator end) 29 | { 30 | const unsigned THREE_D = 3; 31 | 32 | // init the corners of the box 33 | minp = *begin; 34 | begin++; 35 | maxp = minp; 36 | 37 | while (begin != end) 38 | { 39 | for (unsigned i=0; i< THREE_D; i++) 40 | { 41 | if ((*begin)[i] < minp[i]) 42 | minp[i] = (*begin)[i]; 43 | if ((*begin)[i] > maxp[i]) 44 | maxp[i] = (*begin)[i]; 45 | } 46 | begin++; 47 | } 48 | } 49 | 50 | -------------------------------------------------------------------------------- /include/Moby/ArticulatedBody.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2011 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | /// Gets joint limit constraints 8 | template 9 | OutputIterator ArticulatedBody::find_limit_constraints(OutputIterator output_begin) const 10 | { 11 | for (unsigned i=0; i< _joints.size(); i++) 12 | { 13 | boost::shared_ptr joint = boost::dynamic_pointer_cast(_joints[i]); 14 | for (unsigned j=0; j< joint->num_dof(); j++) 15 | { 16 | 17 | // get the current joint position and velocity 18 | double q = joint->q[j] + joint->_q_tare[j]; 19 | 20 | // setup an constraint for this joint/dof in case we need it 21 | UnilateralConstraint e; 22 | e.constraint_type = UnilateralConstraint::eLimit; 23 | e.limit_joint = joint; 24 | e.limit_dof = j; 25 | e.limit_epsilon = joint->limit_restitution; 26 | 27 | // check whether we are already at a limit 28 | if (q >= joint->hilimit[j]) 29 | { 30 | // add constraint for upper limit 31 | e.limit_upper = true; 32 | *output_begin++ = e; 33 | } 34 | if (q <= joint->lolimit[j]) 35 | { 36 | e.limit_upper = false; 37 | *output_begin++ = e; 38 | } 39 | } 40 | } 41 | 42 | return output_begin; 43 | } 44 | 45 | -------------------------------------------------------------------------------- /include/Moby/Base.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _BASE_H 8 | #define _BASE_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Moby { 18 | 19 | /// Class from which all Moby classes are derived 20 | class Base : public Ravelin::virtual_enable_shared_from_this 21 | { 22 | public: 23 | Base(); 24 | Base(const Base* b); 25 | virtual ~Base() {} 26 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 27 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 28 | 29 | /// Static method for cloning a shared pointer 30 | template 31 | static boost::shared_ptr clone(boost::shared_ptr x) { return (!x) ? x : boost::shared_ptr(new T(*x)); } 32 | 33 | /// Any relevant userdata 34 | boost::shared_ptr userdata; 35 | 36 | /// The unique ID for this object 37 | std::string id; 38 | }; 39 | 40 | } // end namespace Moby 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /include/Moby/BoundingSphere.inl: -------------------------------------------------------------------------------- 1 | template 2 | BoundingSphere::BoundingSphere(ForwardIterator begin, ForwardIterator end) 3 | { 4 | const double INF = std::numeric_limits::max(); 5 | const unsigned THREE_D = 3; 6 | 7 | // determine an AABB bounding the points 8 | Point3d lo(INF, INF, INF), hi(-INF, -INF, -INF); 9 | for (ForwardIterator i = begin; i != end; i++) 10 | for (unsigned j=0; j< THREE_D; j++) 11 | { 12 | if ((*i)[j] < lo[j]) 13 | lo[j] = (*i)[j]; 14 | if ((*i)[j] > hi[j]) 15 | hi[j] = (*i)[j]; 16 | } 17 | 18 | // center of the circle will be at the center of the AABB 19 | center = (lo + hi) * (double) 0.5; 20 | 21 | // set the radius to zero initially 22 | radius = (double) 0.0; 23 | 24 | // now, determine the radius 25 | for (ForwardIterator i = begin; i != end; i++) 26 | { 27 | double dist = (*i - center).norm(); 28 | if (dist > radius) 29 | radius = dist; 30 | } 31 | } 32 | 33 | -------------------------------------------------------------------------------- /include/Moby/CP.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _CP_H 8 | #define _CP_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Moby { 18 | 19 | class PolyhedralPrimitive; 20 | 21 | /// Determines closest points/common points between two polytopes 22 | class CP 23 | { 24 | public: 25 | static double find_cpoint(boost::shared_ptr A, boost::shared_ptr B, boost::shared_ptr pA, boost::shared_ptr pB, Point3d& cpA, Point3d& cpB); 26 | 27 | private: 28 | static bool lp_seidel(const Ravelin::MatrixNd& A, const Ravelin::VectorNd& b, const Ravelin::VectorNd& c, const Ravelin::VectorNd& l, const Ravelin::VectorNd& u, Ravelin::VectorNd& x); 29 | static Ravelin::VectorNd& insert_component(const Ravelin::VectorNd& x, unsigned k, Ravelin::VectorNd& xn); 30 | static Ravelin::VectorNd& remove_component(const Ravelin::VectorNd& x, unsigned k, Ravelin::VectorNd& xn); 31 | static double finitize(double x); 32 | }; // end class 33 | 34 | } // end namespace 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /include/Moby/CollisionDetection.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | /// Gets the set of single bodies in the collision detector 8 | template 9 | OutputIterator CollisionDetection::get_single_bodies(OutputIterator output_begin) const 10 | { 11 | std::list> sbs; 12 | BOOST_FOREACH(CollisionGeometryPtr cg, _geoms) 13 | sbs.push_back(cg->get_single_body()); 14 | sbs.sort(); 15 | return std::copy(sbs.begin(), std::unique(sbs.begin(), sbs.end()), output_begin); 16 | } 17 | 18 | /// Gets the set of rigid bodies in the collision detector 19 | template 20 | OutputIterator CollisionDetection::get_rigid_bodies(OutputIterator output_begin) const 21 | { 22 | std::list rbs; 23 | BOOST_FOREACH(CollisionGeometryPtr cg, _geoms) 24 | { 25 | RigidBodyPtr rb = boost::dynamic_pointer_cast(cg->get_single_body()); 26 | if (rb) 27 | rbs.push_back(rb); 28 | } 29 | rbs.sort(); 30 | return std::copy(rbs.begin(), std::unique(rbs.begin(), rbs.end()), output_begin); 31 | } 32 | 33 | /// Gets the set of dynamic bodies in the collision detector 34 | template 35 | OutputIterator CollisionDetection::get_dynamic_bodies(OutputIterator output_begin) const 36 | { 37 | std::list dbs; 38 | BOOST_FOREACH(CollisionGeometryPtr cg, _geoms) 39 | { 40 | boost::shared_ptr sb = cg->get_single_body(); 41 | ArticulatedBodyPtr ab = sb->get_articulated_body(); 42 | if (ab) 43 | dbs.push_back(boost::dynamic_pointer_cast(ab)); 44 | else 45 | dbs.push_back(sb); 46 | } 47 | dbs.sort(); 48 | return std::copy(dbs.begin(), std::unique(dbs.begin(), dbs.end()), output_begin); 49 | } 50 | 51 | 52 | -------------------------------------------------------------------------------- /include/Moby/Constants.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_CONSTANTS_H 8 | #define _MOBY_CONSTANTS_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Moby { 18 | 19 | // constants 20 | const boost::shared_ptr GLOBAL; 21 | const double NEAR_ZERO = std::sqrt(std::numeric_limits::epsilon()); 22 | const Ravelin::Matrix3d ZEROS_3x3 = Ravelin::Matrix3d::zero(); 23 | const Ravelin::Matrix3d IDENTITY_3x3 = Ravelin::Matrix3d::identity(); 24 | const Ravelin::VectorNd EMPTY_VEC(0); 25 | 26 | // debugging bits 27 | const unsigned LOG_SIMULATOR = 1; 28 | const unsigned LOG_CONSTRAINT = 2; 29 | const unsigned LOG_DYNAMICS = 4; 30 | const unsigned LOG_BV = 8; 31 | const unsigned LOG_ADF = 16; 32 | const unsigned LOG_COLDET = 32; 33 | const unsigned LOG_COMPGEOM = 64; 34 | const unsigned LOG_LINALG = 128; 35 | const unsigned LOG_OPT = 256; 36 | const unsigned LOG_DEFORM = 512; 37 | 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /include/Moby/Contact.inl: -------------------------------------------------------------------------------- 1 | template 2 | OutputIterator Contact::get_super_bodies(OutputIterator begin) const 3 | { 4 | ControlledBodyPtr db1, db2; 5 | unsigned nb = get_super_bodies(db1, db2); 6 | switch (nb) 7 | { 8 | case 0: break; 9 | case 1: if (db1) *begin++ = db1; break; 10 | case 2: if (db1) *begin++ = db1; if (db2) *begin++ = db2; break; 11 | default: 12 | assert(false); 13 | } 14 | return begin; 15 | } 16 | 17 | 18 | -------------------------------------------------------------------------------- /include/Moby/ContactParameters.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2011 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_CONTACT_PARAMETERS_H_ 8 | #define _MOBY_CONTACT_PARAMETERS_H_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace Moby { 15 | 16 | class ContactParameters : public Base 17 | { 18 | public: 19 | ContactParameters(); 20 | ContactParameters(BasePtr o1, BasePtr o2); 21 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 22 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 23 | 24 | /// The objects in contact 25 | Ravelin::sorted_pair objects; 26 | 27 | /// Coefficient of restitution for contact (default is 0.0) 28 | double epsilon; 29 | 30 | /// Coefficient of Coulomb friction for contact (default is 0.0) 31 | double mu_coulomb; 32 | 33 | /// Coefficient of viscous friction for contact (default is 0.0) 34 | double mu_viscous; 35 | 36 | /// contact compliance (inverse of stiffness- zero compliance equals true rigidity) 37 | double compliance; 38 | 39 | /// Penalty Method Depth Penalty 40 | double penalty_Kp; 41 | 42 | /// Penalty Method Interpenetration Speed 43 | double penalty_Kv; 44 | 45 | /// Number of edges in the polygon friction cone, minimum of 4 (default 4) 46 | unsigned NK; 47 | }; // end class 48 | 49 | } // end namespace 50 | 51 | #endif // #ifdef MOBY_CONTACT_PARAMETERS_H_ 52 | -------------------------------------------------------------------------------- /include/Moby/DampingForce.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _DAMPING_FORCE_H 8 | #define _DAMPING_FORCE_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines a force that damps rigid bodies on any given step 16 | class DampingForce : public RecurrentForce 17 | { 18 | public: 19 | DampingForce() {} 20 | DampingForce(const DampingForce& source); 21 | virtual ~DampingForce() {} 22 | virtual void add_force(boost::shared_ptr body); 23 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 24 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 25 | 26 | /// The mapping from bodies to linear damping constants 27 | std::map, double> kl; 28 | 29 | /// The mapping from bodies to angular damping constants 30 | std::map, double> ka; 31 | 32 | /// The mapping from bodies to linear squared damping constants 33 | std::map, double> klsq; 34 | 35 | /// The mapping from bodies to angular squared damping constants 36 | std::map, double> kasq; 37 | 38 | private: 39 | static void add_damping(boost::shared_ptr rb, double ld, double ad, double ldsq, double adsq); 40 | }; // end class 41 | } // end namespace 42 | 43 | #endif 44 | 45 | -------------------------------------------------------------------------------- /include/Moby/DegenerateTriangleException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _DEGENERATE_TRIANGLE_EXCEPTION_H_ 8 | #define _DEGENERATE_TRIANGLE_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when trying to perform an operation that requires a normal on a degenerate triangle 15 | class DegenerateTriangleException : public std::runtime_error 16 | { 17 | public: 18 | DegenerateTriangleException() : std::runtime_error("Tried to perform an operation that requires a normal on a degenerate triangle") {} 19 | }; // end class 20 | 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/Dissipation.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _DISSIPATION_H 8 | #define _DISSIPATION_H 9 | 10 | #include 11 | 12 | namespace Moby { 13 | class Dissipation : public Base 14 | { 15 | public: 16 | Dissipation(); 17 | virtual ~Dissipation() {} 18 | void apply(const std::vector >& bodies); 19 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 20 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 21 | 22 | /// The mapping from bodies to decay coefficients 23 | std::map, double> _coeffs; 24 | }; // end class 25 | } // end namespace 26 | 27 | #endif 28 | 29 | -------------------------------------------------------------------------------- /include/Moby/FastThreadable.h: -------------------------------------------------------------------------------- 1 | #ifndef _MOBY_FAST_THREADABLE_H_ 2 | #define _MOBY_FAST_THREADABLE_H_ 3 | 4 | #ifdef _OPENMP 5 | #include 6 | #include 7 | #endif 8 | 9 | namespace Moby { 10 | 11 | /// This class exists solely to make static declaration of dynamically allocated variables safe 12 | template 13 | class FastThreadable 14 | { 15 | private: 16 | #ifdef _OPENMP 17 | std::vector _x; 18 | #else 19 | T _x; 20 | #endif 21 | 22 | public: 23 | FastThreadable() 24 | { 25 | #ifdef _OPENMP 26 | _x.resize(omp_get_max_threads()); 27 | #endif 28 | } 29 | 30 | T& operator()() 31 | { 32 | #ifdef _OPENMP 33 | return _x[omp_get_thread_num()]; 34 | #else 35 | return _x; 36 | #endif 37 | } 38 | }; 39 | 40 | } // end namespace 41 | 42 | #endif 43 | 44 | -------------------------------------------------------------------------------- /include/Moby/FixedJoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2009 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _FIXED_JOINT_H 8 | #define _FIXED_JOINT_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines a joint for fixing two bodies together or fixing one body to the ground 16 | class FixedJoint : public Joint, public Ravelin::FixedJointd 17 | { 18 | public: 19 | FixedJoint(); 20 | virtual ~FixedJoint() {} 21 | FixedJoint(boost::weak_ptr inboard, boost::weak_ptr outboard); 22 | virtual unsigned num_dof() const { return FixedJointd::num_dof(); } 23 | virtual bool is_singular_config() const { return FixedJointd::is_singular_config(); } 24 | virtual void evaluate_constraints(double C[]) { FixedJointd::evaluate_constraints(C); } 25 | virtual const std::vector& get_spatial_axes_dot() { return FixedJointd::get_spatial_axes_dot(); } 26 | virtual void determine_q(Ravelin::VectorNd& q) { FixedJointd::determine_q(q); } 27 | virtual boost::shared_ptr get_induced_pose() { return FixedJointd::get_induced_pose(); } 28 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 29 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 30 | }; // end class 31 | } // end namespace 32 | 33 | #endif 34 | 35 | -------------------------------------------------------------------------------- /include/Moby/GJK.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _GJK_H 8 | #define _GJK_H 9 | 10 | #include 11 | namespace Moby { 12 | 13 | /// An implementation of the GJK algorithm 14 | class GJK 15 | { 16 | public: 17 | static double do_gjk(boost::shared_ptr A, boost::shared_ptr B, boost::shared_ptr pA, boost::shared_ptr pB, Point3d& cpA, Point3d& cpB, unsigned max_iter = 1000); 18 | 19 | private: 20 | struct SVertex 21 | { 22 | Point3d v; // vA - vB: the vertex in the simplex 23 | Point3d vA, vB; // vertices from geometry A and B 24 | std::ostream& output(std::ostream& out) const; 25 | 26 | SVertex() {} 27 | SVertex(const Point3d& a, const Point3d& b) 28 | { 29 | vA = a; 30 | vB = b; 31 | v = Ravelin::Pose3d::transform_point(GLOBAL, a) - 32 | Ravelin::Pose3d::transform_point(GLOBAL, b); 33 | } 34 | }; 35 | 36 | class Simplex 37 | { 38 | public: 39 | enum SimplexType { ePoint, eSegment, eTriangle, eTetra }; 40 | SimplexType get_simplex_type() const { return _type; } 41 | Simplex(const SVertex& p) { _type = ePoint; _v1 = p; } 42 | void add(const SVertex& p); 43 | Point3d find_closest_and_simplify(); 44 | Point3d find_closest(); 45 | const SVertex& get_vertex(unsigned i) const; 46 | unsigned num_vertices() const; 47 | std::ostream& output(std::ostream& out) const; 48 | 49 | private: 50 | SimplexType _type; 51 | SVertex _v1, _v2, _v3, _v4; 52 | }; 53 | 54 | }; // end class 55 | 56 | } // end namespace 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /include/Moby/Gears.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _GEARS_H 8 | #define _GEARS_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace Moby { 23 | 24 | class VisualizationData; 25 | 26 | /// Defines a gear ("joint") constraint 27 | /** 28 | * \todo implement a rest position for q? 29 | */ 30 | class Gears : public Joint 31 | { 32 | public: 33 | 34 | Gears(); 35 | virtual ~Gears() {} 36 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 37 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 38 | virtual void set_inboard_pose(boost::shared_ptr link, bool update_joint_pose); 39 | virtual void set_outboard_pose(boost::shared_ptr link, bool update_joint_pose); 40 | virtual void evaluate_constraints(double C[]); 41 | virtual void evaluate_constraints_dot(double C[]); 42 | virtual void calc_constraint_jacobian(bool inboard, Ravelin::MatrixNd& Cq); 43 | virtual void calc_constraint_jacobian_dot(bool inboard, Ravelin::MatrixNd& Cq); 44 | virtual unsigned num_dof() const { return 0; } 45 | virtual unsigned num_constraint_eqns() const { return 1; } 46 | virtual void determine_q(Ravelin::VectorNd& q) {} 47 | virtual boost::shared_ptr get_induced_pose() { return boost::shared_ptr(); } 48 | virtual bool is_singular_config() const { return false; } 49 | virtual const std::vector& get_spatial_axes_dot() { return _sdot; } 50 | 51 | private: 52 | double _ratio; // the input:output ratio for the gears 53 | 54 | std::vector _sdot; // this will be unused 55 | }; // end class 56 | } // end namespace 57 | 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /include/Moby/GravityForce.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _GRAVITY_FORCE_H 8 | #define _GRAVITY_FORCE_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | class GravityForce : public RecurrentForce 15 | { 16 | public: 17 | GravityForce(); 18 | GravityForce(const GravityForce& source); 19 | virtual ~GravityForce() {} 20 | virtual void add_force(boost::shared_ptr body); 21 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 22 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 23 | 24 | /// The gravity vector 25 | Ravelin::Vector3d gravity; 26 | }; // end class 27 | } // end namespace 28 | 29 | #endif 30 | 31 | -------------------------------------------------------------------------------- /include/Moby/HashClasses.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _HASH_CLASSES_H 8 | #define _HASH_CLASSES_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace Moby { 15 | 16 | /// Hashing function class for string objects 17 | class StringHash 18 | { 19 | public: 20 | size_t operator()(const std::string s) const { __gnu_cxx::hash H; return H(s.c_str()); } 21 | }; 22 | 23 | /// Equivalence function class for string objects (performs case-insensitive equivalence test) 24 | class StringEqual 25 | { 26 | public: 27 | bool operator()(const std::string s1, const std::string s2) const { return strcasecmp(s1.c_str(), s2.c_str()) == 0; } 28 | }; 29 | 30 | } // end namespace Moby 31 | #endif 32 | -------------------------------------------------------------------------------- /include/Moby/ImpactToleranceException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2012 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _IMPACT_TOLERANCE_EXCEPTION_H_ 8 | #define _IMPACT_TOLERANCE_EXCEPTION_H_ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace Moby { 15 | 16 | /// Exception thrown when constraint violation is greater than a desired tolerance 17 | class ImpactToleranceException : public std::runtime_error 18 | { 19 | public: 20 | ImpactToleranceException(const std::list& impacting_constraints, double max_vio) : std::runtime_error("Constraint velocity is below tolerance after treatment!") { constraints = impacting_constraints; violation = max_vio; } 21 | 22 | virtual ~ImpactToleranceException() throw() { } 23 | 24 | std::list constraints; 25 | double violation; // the amount of constraint violation 26 | }; // end class 27 | 28 | 29 | } // end namespace 30 | 31 | #endif 32 | 33 | -------------------------------------------------------------------------------- /include/Moby/IndexedTetra.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _INDEXED_TETRA_ 8 | #define _INDEXED_TETRA_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// An indexed tetrahedron 15 | struct IndexedTetra 16 | { 17 | IndexedTetra() { a = b = c = d = std::numeric_limits::max(); } 18 | IndexedTetra(unsigned v1, unsigned v2, unsigned v3, unsigned v4) { a = v1; b = v2; c = v3; d = v4; } 19 | static void determine_barycentric_coords(const Point3d& v1, const Point3d& v2, const Point3d& v3, const Point3d& v4, const Point3d& p, double& u, double& v, double& w); 20 | static Point3d calc_point(const Point3d& v1, const Point3d& v2, const Point3d& v3, const Point3d& v4, double u, double v, double w); 21 | 22 | /// Index of the first vertex of the tetrahedron 23 | unsigned a; 24 | 25 | /// Index of the second vertex of the tetrahedron 26 | unsigned b; 27 | 28 | /// Index of the third vertex of the tetrahedron 29 | unsigned c; 30 | 31 | /// Index of the fourth vertex of the tetrahedron 32 | unsigned d; 33 | 34 | }; // end struct 35 | 36 | } // end namespace 37 | 38 | #endif 39 | 40 | -------------------------------------------------------------------------------- /include/Moby/IndexedTetraArray.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | inline std::ostream& operator<<(std::ostream& out, const IndexedTetraArray& mesh) 8 | { 9 | const std::vector& verts = mesh.get_vertices(); 10 | const std::vector& tetra = mesh.get_tetra(); 11 | BOOST_FOREACH(const IndexedTetra& tet, tetra) 12 | { 13 | out << " : " << tet.a << " " << tet.b << " " << tet.c << " " << tet.d << std::endl; 14 | out << " : " << verts[tet.a] << " " << verts[tet.b] << " " << verts[tet.c] << " " << verts[tet.d] << std::endl; 15 | } 16 | 17 | return out; 18 | } 19 | 20 | /// Creates an indexed tetrahedral mesh from containers of vertices and tetra 21 | /** 22 | * \param vertices an iterator to the beginning of a container of Ravelin::Origin3d 23 | * objects 24 | * \param verts_end an iterator to the end of a container of Ravelin::Origin3d objects 25 | * \param tetra_begin an iterator to the beginning of a container of 26 | * IndexedTetra objects 27 | * \param tetra_end an iterator to the end of a container of IndexedTetra objects 28 | */ 29 | template 30 | IndexedTetraArray::IndexedTetraArray(ForwardIterator1 verts_begin, ForwardIterator1 verts_end, ForwardIterator2 tetra_begin, ForwardIterator2 tetra_end) 31 | { 32 | _vertices = boost::shared_ptr >(new std::vector(verts_begin, verts_end)); 33 | _tetra = boost::shared_ptr >(new std::vector(tetra_begin, tetra_end)); 34 | 35 | // validate that indices are within range and tetrahedra oriented correctly 36 | validate(); 37 | } 38 | 39 | 40 | -------------------------------------------------------------------------------- /include/Moby/IndexedTri.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _INDEXED_TRI_ 8 | #define _INDEXED_TRI_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// An indexed triangle 15 | struct IndexedTri 16 | { 17 | IndexedTri() { a = b = c = std::numeric_limits::max(); } 18 | IndexedTri(unsigned v1, unsigned v2, unsigned v3) { a = v1; b = v2; c = v3; } 19 | 20 | /// Reverses the orientation of the triangle 21 | IndexedTri& reverse() { std::swap(b, c); return *this; } 22 | 23 | /// Index of the first vertex of the triangle 24 | unsigned a; 25 | 26 | /// Index of the second vertex of the triangle 27 | unsigned b; 28 | 29 | /// Index of the third vertex of the triangle 30 | unsigned c; 31 | 32 | bool operator<(const IndexedTri& t) const 33 | { 34 | if (a < t.a) 35 | return true; 36 | else if (a == t.a) 37 | { 38 | if (b < t.b) 39 | return true; 40 | else if (b == t.b && c < t.c) 41 | return true; 42 | } 43 | return false; 44 | } 45 | }; 46 | 47 | } // end namespace 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /include/Moby/InvalidIndexException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_INVALID_INDEX_EXCEPTION_H_ 8 | #define _MOBY_INVALID_INDEX_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when trying to access the index beyond the range of the data 15 | class InvalidIndexException : public std::runtime_error 16 | { 17 | public: 18 | InvalidIndexException() : std::runtime_error("Invalid index") {} 19 | }; // end class 20 | 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/InvalidStateException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_INVALID_STATE_EXCEPTION_H_ 8 | #define _MOBY_INVALID_STATE_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when an integrator tries to evaluate a derivative at an invalid state 15 | class InvalidStateException : public std::runtime_error 16 | { 17 | public: 18 | InvalidStateException() : std::runtime_error("Integrator tries to evaluate derivative at invalid state") {} 19 | InvalidStateException(const char* error) : std::runtime_error(error) {} 20 | }; // end class 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/InvalidVelocityException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_INVALID_VELOCITY_EXCEPTION_H_ 8 | #define _MOBY_INVALID_VELOCITY_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when an integrator tries to evaluate a derivative at an invalid velocity 15 | class InvalidVelocityException : public std::runtime_error 16 | { 17 | public: 18 | InvalidVelocityException(double t) : std::runtime_error("Integrator tries to evaluate derivative at invalid velocity") { evaluation_time = t; } 19 | double evaluation_time; 20 | }; // end class 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/LCPSolverException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_LCP_SOLVER_EXCEPTION_H_ 8 | #define _MOBY_LCP_SOLVER_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when LCP solver can't solve 15 | class LCPSolverException : public std::runtime_error 16 | { 17 | public: 18 | LCPSolverException() : std::runtime_error("Unable to solve event LCP/QP!") {} 19 | }; // end class 20 | 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/LP.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _LP_H 8 | #define _LP_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace Moby { 15 | 16 | /// Determines closest points/common points between two polytopes 17 | class LP 18 | { 19 | public: 20 | static bool lp_seidel(const Ravelin::MatrixNd& A, const Ravelin::VectorNd& b, const Ravelin::VectorNd& c, const Ravelin::VectorNd& l, const Ravelin::VectorNd& u, Ravelin::VectorNd& x); 21 | 22 | private: 23 | static Ravelin::VectorNd& insert_component(const Ravelin::VectorNd& x, unsigned k, Ravelin::VectorNd& xn); 24 | static Ravelin::VectorNd& remove_component(const Ravelin::VectorNd& x, unsigned k, Ravelin::VectorNd& xn); 25 | static double finitize(double x); 26 | }; // end class 27 | 28 | } // end namespace 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /include/Moby/Log.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2009 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_LOG_H_ 8 | #define _MOBY_LOG_H_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Moby { 18 | 19 | #ifdef NDEBUG 20 | #define FILE_LOG(level) if (false) Log().get(level) 21 | #define LOGGING(level) false 22 | #else 23 | #define FILE_LOG(level) if ((level & Log::reporting_level) > 0) Log().get(level) 24 | #define LOGGING(level) ((level & Log::reporting_level) > 0) 25 | #endif 26 | 27 | struct OutputToFile 28 | { 29 | static std::ofstream stream; 30 | 31 | static void output(const std::string& msg); 32 | }; 33 | 34 | template 35 | class Log 36 | { 37 | public: 38 | Log() 39 | { 40 | #ifdef THREADSAFE 41 | std::cerr << "Log() warning: this class is not thread-safe!" << std::endl; 42 | #endif 43 | } 44 | 45 | std::ostringstream& get(unsigned level = 0) 46 | { 47 | time_t rawtime; 48 | std::time(&rawtime); 49 | tm* ptm = std::gmtime(&rawtime); 50 | os << "- " << ptm->tm_hour << ":" << ptm->tm_min << ":" << ptm->tm_sec; 51 | os << " " << level << ": "; 52 | message_level = level; 53 | return os; 54 | } 55 | 56 | ~Log() 57 | { 58 | if ((message_level & reporting_level) > 0) 59 | OutputPolicy::output(os.str()); 60 | } 61 | 62 | static unsigned reporting_level; 63 | 64 | private: 65 | std::ostringstream os; 66 | unsigned message_level; 67 | }; // end class 68 | 69 | // make the default warning level zero (no reporting) 70 | template 71 | unsigned Log::reporting_level = 0; 72 | 73 | } // end namespace 74 | 75 | #endif 76 | 77 | -------------------------------------------------------------------------------- /include/Moby/MissizeException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_MISSIZE_EXCEPTION_H_ 8 | #define _MOBY_MISSIZE_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when trying to initialize a fixed size vector/matrix with the wrong size 15 | class MissizeException : public std::runtime_error 16 | { 17 | public: 18 | MissizeException() : std::runtime_error("Matrix / vector size mismatch") {} 19 | }; // end class 20 | 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/Node.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _DEFORMABLE_BODY_NODE_H 8 | #define _DEFORMABLE_BODY_NODE_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines a node in a deformable body 16 | struct Node 17 | { 18 | Node() 19 | { 20 | x.set_zero(); 21 | xd.set_zero(); 22 | xdd.set_zero(); 23 | f.set_zero(); 24 | mass = (double) 0.0; 25 | } 26 | 27 | /// The global position of the node 28 | Point3d x; 29 | 30 | /// The linear velocity of the node 31 | Ravelin::Vector3d xd; 32 | 33 | /// The linear acceleration of the node 34 | Ravelin::Vector3d xdd; 35 | 36 | /// The mass of the node (if any) 37 | double mass; 38 | 39 | /// The force accumulator for the node 40 | Ravelin::Vector3d f; 41 | }; // end class 42 | 43 | } // end namespace 44 | 45 | #endif 46 | 47 | -------------------------------------------------------------------------------- /include/Moby/NonsquareMatrixException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2012 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_NONSQUARE_MATRIX_EXCEPTION_H_ 8 | #define _MOBY_NONSQUARE_MATRIX_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when trying to initialize a fixed size vector/matrix with the wrong size 15 | class NonsquareMatrixException : public std::runtime_error 16 | { 17 | public: 18 | NonsquareMatrixException() : std::runtime_error("Matrix should be square!") {} 19 | }; // end class 20 | 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/NumericalException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_NUMERICAL_EXCEPTION_H_ 8 | #define _MOBY_NUMERICAL_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when general numerical error occurs 15 | class NumericalException : public std::runtime_error 16 | { 17 | public: 18 | NumericalException() : std::runtime_error("NumericalException error") {} 19 | NumericalException(const char* error) : std::runtime_error(error) {} 20 | }; // end class 21 | 22 | 23 | } // end namespace 24 | 25 | #endif 26 | 27 | -------------------------------------------------------------------------------- /include/Moby/OSGGroupWrapper.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2007 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _OSG_GROUP_WRAPPER_H_ 8 | #define _OSG_GROUP_WRAPPER_H_ 9 | 10 | #include 11 | 12 | namespace osg 13 | { 14 | class Node; 15 | class Group; 16 | } 17 | 18 | namespace Moby { 19 | 20 | /// A wrapper for OpenInventor OSGGroup class, supporting serialization 21 | class OSGGroupWrapper : public virtual Base 22 | { 23 | public: 24 | OSGGroupWrapper(); 25 | OSGGroupWrapper(osg::Node* n); 26 | OSGGroupWrapper(const std::string& filename); 27 | ~OSGGroupWrapper(); 28 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 29 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 30 | osg::Group* get_group() { return _group; } 31 | 32 | private: 33 | osg::Group* _group; 34 | }; // end class 35 | 36 | } // end namespace 37 | 38 | #endif 39 | 40 | -------------------------------------------------------------------------------- /include/Moby/PairwiseDistInfo.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_PAIRWISE_DIST_INFO_H_ 8 | #define _MOBY_PAIRWISE_DIST_INFO_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | struct PairwiseDistInfo 15 | { 16 | CollisionGeometryPtr a; // the first geometry 17 | CollisionGeometryPtr b; // the second geometry 18 | double dist; // the signed distance 19 | Point3d pa; // the closest point on geometry A 20 | Point3d pb; // the closest point on geometry B 21 | }; // end struct 22 | 23 | } // end namespace 24 | 25 | #endif 26 | 27 | -------------------------------------------------------------------------------- /include/Moby/PenaltyConstraintHandler.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Samuel Zapolsky 3 | * This library is distributed under the terms of the Apache V2.0 license 4 | ****************************************************************************/ 5 | 6 | #ifndef _PENALTY_EVENT_HANDLER_H 7 | #define _PENALTY_EVENT_HANDLER_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace Moby { 20 | 21 | /// Defines the mechanism for handling Penalty constraints 22 | class PenaltyConstraintHandler 23 | { 24 | public: 25 | PenaltyConstraintHandler(); 26 | void process_constraints(const std::vector& constraints) const; 27 | private: 28 | void apply_model(const std::vector& constraints) const; 29 | static double sqr(double x) { return x*x; } 30 | }; // end class 31 | } // end namespace 32 | 33 | #endif 34 | 35 | -------------------------------------------------------------------------------- /include/Moby/PlanarJoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _PLANAR_JOINT_H 8 | #define _PLANAR_JOINT_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines an actuated spherical joint 16 | class PlanarJoint : public Joint, public Ravelin::PlanarJointd 17 | { 18 | public: 19 | PlanarJoint(); 20 | virtual ~PlanarJoint() {} 21 | PlanarJoint(boost::weak_ptr inboard, boost::weak_ptr outboard); 22 | virtual unsigned num_dof() const { return PlanarJointd::num_dof(); } 23 | virtual bool is_singular_config() const { return PlanarJointd::is_singular_config(); } 24 | virtual void evaluate_constraints(double C[]) { PlanarJointd::evaluate_constraints(C); } 25 | virtual const std::vector& get_spatial_axes_dot() { return PlanarJointd::get_spatial_axes_dot(); } 26 | virtual void determine_q(Ravelin::VectorNd& q) { PlanarJointd::determine_q(q); } 27 | virtual boost::shared_ptr get_induced_pose() { return PlanarJointd::get_induced_pose(); } 28 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 29 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 30 | }; // end class 31 | } // end namespace 32 | 33 | #endif 34 | 35 | -------------------------------------------------------------------------------- /include/Moby/PolyhedralPrimitive.inl: -------------------------------------------------------------------------------- 1 | template 2 | OutputIterator PolyhedralPrimitive::get_halfspaces(const Polyhedron& poly, boost::shared_ptr pose, const Ravelin::Transform3d& wTpose, OutputIterator output_begin) 3 | { 4 | // get the normals and faces from polyhedron A, transforming to global frame 5 | for (unsigned i=0; i< poly.get_faces().size(); i++) 6 | { 7 | // get the face 8 | boost::shared_ptr f = poly.get_faces()[i]; 9 | 10 | // get the plane corresponding to the face; the plane will be in the 11 | // global frame, when it should really be defined in poseA 12 | Plane pu = f->get_plane(); 13 | Ravelin::Vector3d pu_normal = pu.get_normal(); 14 | assert(pu_normal.pose == GLOBAL); 15 | pu_normal.pose = pose; 16 | pu.set_normal(pu_normal); 17 | 18 | // get the plane corresponding to the face, transformed to global frame 19 | Plane p = pu.transform(wTpose); 20 | 21 | // save the halfspace 22 | *output_begin++ = std::make_pair(p.get_normal(), p.offset); 23 | } 24 | 25 | return output_begin; 26 | } 27 | 28 | -------------------------------------------------------------------------------- /include/Moby/Primitive.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | /// Gets the vertex indices from selected facets of an IndexedTriArray 8 | /** 9 | * \param tris the triangle mesh 10 | * \param fselect_begin iterator to a container of type unsigned (facet indices) 11 | * \param fselect_end iterator to a container of type unsigned (facet indices) 12 | * \param output beginning iterator to a container of type Point3d; unique 13 | * vertices are copied here on return 14 | * \return ending iterator to a container of type Point3d; unique vertices 15 | * are copied here on return 16 | */ 17 | template 18 | OutputIterator Primitive::get_vertices(const IndexedTriArray& tris, InputIterator fselect_begin, InputIterator fselect_end, OutputIterator output) 19 | { 20 | // init a list of vertices 21 | std::list verts; 22 | 23 | // get facets from the triangle array 24 | const std::vector& facets = tris.get_facets(); 25 | while (fselect_begin != fselect_end) 26 | { 27 | const IndexedTri& f = facets[*fselect_begin++]; 28 | verts.push_back(f.a); 29 | verts.push_back(f.b); 30 | verts.push_back(f.c); 31 | } 32 | 33 | // sort the list so that we don't process repeated vertices 34 | verts.sort(); 35 | std::list::const_iterator new_end = std::unique(verts.begin(), verts.end()); 36 | 37 | // copy vertices to output 38 | const std::vector& vertices = tris.get_vertices(); 39 | for (std::list::const_iterator i = verts.begin(); i != new_end; i++) 40 | *output++ = vertices[*i]; 41 | 42 | return output; 43 | } 44 | 45 | -------------------------------------------------------------------------------- /include/Moby/PrismaticJoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _PRISMATIC_JOINT_H 8 | #define _PRISMATIC_JOINT_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines an actuated prismatic joint 16 | /** 17 | * \todo implement a rest position for q? 18 | */ 19 | class PrismaticJoint : public Joint, public Ravelin::PrismaticJointd 20 | { 21 | public: 22 | PrismaticJoint(); 23 | virtual ~PrismaticJoint() {} 24 | PrismaticJoint(boost::weak_ptr inboard, boost::weak_ptr outboard); 25 | virtual unsigned num_dof() const { return PrismaticJointd::num_dof(); } 26 | virtual bool is_singular_config() const { return PrismaticJointd::is_singular_config(); } 27 | virtual void evaluate_constraints(double C[]) { PrismaticJointd::evaluate_constraints(C); } 28 | virtual const std::vector& get_spatial_axes_dot() { return PrismaticJointd::get_spatial_axes_dot(); } 29 | virtual void determine_q(Ravelin::VectorNd& q) { PrismaticJointd::determine_q(q); } 30 | virtual boost::shared_ptr get_induced_pose() { return PrismaticJointd::get_induced_pose(); } 31 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 32 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 33 | }; // end class 34 | } // end namespace 35 | 36 | #endif 37 | 38 | -------------------------------------------------------------------------------- /include/Moby/QLCPD.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the GNU Lesser General Public 4 | * License (found in COPYING). 5 | ****************************************************************************/ 6 | 7 | #ifndef _OPT_QPACTIVESET_H 8 | #define _OPT_QPACTIVESET_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace Moby { 16 | 17 | class QLCPD 18 | { 19 | public: 20 | QLCPD(); 21 | 22 | template 23 | bool qp_activeset(const Mat1& H, const Vec1& c, const Vec2& lb, const Vec3& ub, const Mat2& M, const Vec4& q, const Mat3& A, const Vec5& b, Vec6& z); 24 | 25 | template 26 | bool find_closest_feasible(const Vec1& lb, const Vec2& ub, const Mat1& M, const Vec3& q, const Mat2& A, const Vec4& b, Vec5& z); 27 | 28 | template 29 | bool lp_activeset(const Vec1& c, const Vec2& lb, const Vec3& ub, const Mat1& M, const Vec4& q, const Mat2& A, const Vec5& b, Vec6& z); 30 | 31 | private: 32 | static bool rel_equal(double x, double y, double tol = NEAR_ZERO) { return std::fabs(x-y) <= tol*std::max(std::fabs(x), std::max(std::fabs(y), (double) 1.0)); } 33 | 34 | void qlcpd(int* n, int* m, int* k, int* kmax, int* maxg, 35 | double* a, int* la, double* x, double* bl, double* bu, 36 | double *f, double* fmin, double *g, 37 | double* r, double* w, double* e, int* ls, double* alp, int* lp, 38 | int* mlp, int* peq, double* ws, int* lws, double* v, 39 | int* nv, int* lin, double* rgtol, int* mode, int* ifail, int* mxgr, 40 | int* iprint, int* nout); 41 | 42 | // sets values for QLCPD 43 | void set_values(int n, int m); 44 | 45 | // workspace sizes for active set solver 46 | int _mxws, _mxlws; 47 | 48 | // temporaries for active set solver 49 | Ravelin::VectorNd _lb, _ub, _r, _e, _w, _v, _alp, _g; 50 | Ravelin::MatrixNd _X; 51 | std::vector _ws; 52 | std::vector _ls, _lp, _lws; 53 | }; // end class 54 | 55 | #include "QLCPD.inl" 56 | 57 | } // end namespace 58 | 59 | #endif 60 | 61 | -------------------------------------------------------------------------------- /include/Moby/QP.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_QP_H 8 | #define _MOBY_QP_H 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | // class for solving positive definite QPs with box constraints 15 | class QP 16 | { 17 | private: 18 | Ravelin::LinAlgd _LA; 19 | 20 | // temporary variables for qp_gradproj and find_cauchy_point() 21 | Ravelin::VectorNd _p, _Gp, _Gx_c, _Gx, _H, _Wzz; 22 | Ravelin::MatrixNd _ZtGZ; 23 | Ravelin::VectorNd _xz, _cz, _lz, _uz, _dxz; 24 | Ravelin::VectorNd _r1, _r2, _w, _w1, _w2, _v, _xstar, _workv, _y; 25 | std::vector _inactive_set; 26 | std::vector _alphas; 27 | 28 | public: 29 | #include "QP.inl" 30 | }; // end class 31 | 32 | } // end namespace 33 | 34 | #endif 35 | 36 | -------------------------------------------------------------------------------- /include/Moby/RCArticulatedBodyInvDynAlgo.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _RC_ARTICULATED_BODY_INV_DYN_ALGO_H 8 | #define _RC_ARTICULATED_BODY_INV_DYN_ALGO_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | class RCArticulatedBody; 16 | 17 | /// Wrapper class for passing data to and from inverse dynamics algorithms 18 | class RCArticulatedBodyInvDynData 19 | { 20 | public: 21 | 22 | /// External force applied to this link 23 | Ravelin::SForced wext; 24 | 25 | /// Inner joint acceleration (desired) 26 | Ravelin::VectorNd qdd; 27 | }; 28 | 29 | /// Abstract class for performing inverse dynamics computation on a reduced-coordinate articulatedc body 30 | class RCArticulatedBodyInvDynAlgo : public virtual Base 31 | { 32 | public: 33 | RCArticulatedBodyInvDynAlgo() { } 34 | virtual ~RCArticulatedBodyInvDynAlgo() {} 35 | 36 | /// Classes must implement the method below 37 | /** 38 | * Given the current base position and velocity, the joint positions and 39 | * joint velocities, the external forces on all links (including the 40 | * base), and the desired joint and base accelerations, the inverse 41 | * dynamics method must compute the forces applied to the joint actuators 42 | * to achieve these accelerations. 43 | * \return a map from joints to computed actuator forces 44 | */ 45 | virtual std::map, Ravelin::VectorNd> calc_inv_dyn(boost::shared_ptr body, const std::map, RCArticulatedBodyInvDynData>& inv_dyn_data) = 0; 46 | }; // end class 47 | } // end namespace 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /include/Moby/RecurrentForce.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _RECURRENT_FORCE 8 | #define _RECURRENT_FORCE 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace Moby { 17 | 18 | /// Used for applying forces to the simulation on every time step 19 | /** 20 | * Recurrent forces may be constant (like gravity) or vary by the velocity 21 | * of the body (like wind resistance). The recurrent force can be either an 22 | * actual force, or a torque, or both. 23 | */ 24 | class RecurrentForce : public virtual Base 25 | { 26 | public: 27 | virtual ~RecurrentForce() { } 28 | 29 | /// Abstract method for applying this force/torque to a body 30 | virtual void add_force(boost::shared_ptr body) = 0; 31 | }; // end class 32 | 33 | } // end namespace Moby 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/Moby/RevoluteJoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _REVOLUTE_JOINT_H 8 | #define _REVOLUTE_JOINT_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines an actuated revolute joint 16 | class RevoluteJoint : public Joint, public Ravelin::RevoluteJointd 17 | { 18 | public: 19 | RevoluteJoint(); 20 | virtual ~RevoluteJoint() {} 21 | RevoluteJoint(boost::weak_ptr inboard, boost::weak_ptr outboard); 22 | virtual unsigned num_dof() const { return RevoluteJointd::num_dof(); } 23 | virtual bool is_singular_config() const { return RevoluteJointd::is_singular_config(); } 24 | virtual void evaluate_constraints(double C[]) { RevoluteJointd::evaluate_constraints(C); } 25 | virtual const std::vector& get_spatial_axes_dot() { return RevoluteJointd::get_spatial_axes_dot(); } 26 | virtual void determine_q(Ravelin::VectorNd& q) { RevoluteJointd::determine_q(q); } 27 | virtual boost::shared_ptr get_induced_pose() { return RevoluteJointd::get_induced_pose(); } 28 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 29 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 30 | }; // end class 31 | } // end namespace 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /include/Moby/RigidBody.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | template 8 | OutputIterator RigidBody::get_parent_links(OutputIterator begin) const 9 | { 10 | BOOST_FOREACH(boost::shared_ptr j, _inner_joints) 11 | *begin++ = get_parent_link(j); 12 | 13 | return begin; 14 | } 15 | 16 | template 17 | OutputIterator RigidBody::get_child_links(OutputIterator begin) const 18 | { 19 | BOOST_FOREACH(boost::shared_ptr j, _outer_joints) 20 | *begin++ = get_child_link(j); 21 | 22 | return begin; 23 | } 24 | 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/SSL.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2011 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | /// Outputs the SSL to the specified stream 8 | inline std::ostream& operator<<(std::ostream& o, const SSL& s) 9 | { 10 | o << "p1: " << std::endl << s.p1; 11 | o << "p2: " << std::endl << s.p2; 12 | o << "radius: " << s.radius << std::endl; 13 | return o; 14 | } 15 | 16 | -------------------------------------------------------------------------------- /include/Moby/SignedDistDot.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _SIGNED_DIST_DOT_H 8 | #define _SIGNED_DIST_DOT_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace Moby { 20 | 21 | class ConstraintSimulator; 22 | 23 | /// Defines the mechanism for handling impact constraints 24 | class SignedDistDot 25 | { 26 | friend class ConstraintSimulator; 27 | friend class ConstraintStabilization; 28 | 29 | public: 30 | static void compute_signed_dist_dot_Jacobians(UnilateralConstraintProblemData& q, Ravelin::MatrixNd& CnT, Ravelin::MatrixNd& CsT, Ravelin::MatrixNd& CtT, Ravelin::MatrixNd& LT, Ravelin::VectorNd& Cdot_v); 31 | 32 | private: 33 | static double calc_signed_dist(boost::shared_ptr sb1, boost::shared_ptr sb2); 34 | static void restore_coords_and_velocities(const std::vector >& isect, std::map, Ravelin::VectorNd>& gc_map, std::map, Ravelin::VectorNd>& gv_map); 35 | static void integrate_positions(const std::vector >& isect, double dt); 36 | static void apply_impulse(const UnilateralConstraint& contact_constraint, const Ravelin::Vector3d& dir); 37 | static void apply_impulse(const UnilateralConstraint& contact_constraint); 38 | 39 | // a pointer to the simulator 40 | boost::shared_ptr _simulator; 41 | }; // end class 42 | } // end namespace 43 | 44 | #endif 45 | 46 | -------------------------------------------------------------------------------- /include/Moby/Simulator.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | /// Integrates both position and velocity of rigid _bodies 8 | /** 9 | * \return the size of step taken 10 | */ 11 | template 12 | double Simulator::integrate(double dt, ForwardIterator begin, ForwardIterator end) 13 | { 14 | Ravelin::VectorNd gc, gv, ga, gve; 15 | std::vector island_ijoints; 16 | 17 | // begin timing dynamics 18 | tms cstart; 19 | clock_t start = times(&cstart); 20 | 21 | // pre-calculate dynamics 22 | precalc_fwd_dyn(); 23 | 24 | // calculate forward dynamics 25 | calc_fwd_dyn(dt); 26 | 27 | // update generalized velocities and use new generalized 28 | // velocities to calculate new generalized coordinates 29 | for (unsigned j=0; j< _bodies.size(); j++) 30 | { 31 | // NOTE: coordinates must be set first to ensure that frame information 32 | // is correct for velocities: mixed pose will be incorrect if 33 | // generalized_velocity is set first 34 | boost::shared_ptr db = boost::dynamic_pointer_cast(_bodies[j]); 35 | db->get_generalized_acceleration(ga); 36 | db->get_generalized_velocity(Ravelin::DynamicBodyd::eSpatial, gv); 37 | db->get_generalized_velocity(Ravelin::DynamicBodyd::eEuler, gve); 38 | ga *= dt; 39 | gv += ga; 40 | 41 | // integrate the generalized position forward 42 | gve *= dt; 43 | db->get_generalized_coordinates_euler(gc); 44 | gc += gve; 45 | db->set_generalized_coordinates_euler(gc); 46 | db->set_generalized_velocity(Ravelin::DynamicBodyd::eSpatial, gv); 47 | } 48 | 49 | // tabulate dynamics computation 50 | tms cstop; 51 | clock_t stop = times(&cstop); 52 | dynamics_time += (double) (stop-start)/CLOCKS_PER_SEC; 53 | 54 | return dt; 55 | } 56 | 57 | 58 | -------------------------------------------------------------------------------- /include/Moby/SingularException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_SINGULAR_EXCEPTION_H_ 8 | #define _MOBY_SINGULAR_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when trying to invert/solve with a non-invertible matrix 15 | class SingularException : public std::runtime_error 16 | { 17 | public: 18 | SingularException() : std::runtime_error("Matrix is singular") {} 19 | }; // end class 20 | 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/SparseJacobian.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _SPARSE_JACOBIAN_H 8 | #define _SPARSE_JACOBIAN_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace Moby { 15 | 16 | /// Matrix block 17 | /** 18 | * The block itself is dense and starts at row st_row_idx and column 19 | * st_col_idx of the dense matrix. 20 | */ 21 | struct MatrixBlock 22 | { 23 | unsigned st_col_idx, st_row_idx; 24 | unsigned rows() const { return block.rows(); } 25 | unsigned columns() const { return block.columns(); } 26 | Ravelin::MatrixNd block; 27 | }; 28 | 29 | /// A Sparse Jacobian representation along with multiplication routines 30 | class SparseJacobian 31 | { 32 | public: 33 | SparseJacobian() { rows = cols = 0; } 34 | Ravelin::VectorNd& mult(const Ravelin::VectorNd& x, Ravelin::VectorNd& result) const; 35 | Ravelin::MatrixNd& mult(const Ravelin::MatrixNd& x, Ravelin::MatrixNd& result) const; 36 | Ravelin::MatrixNd& transpose_mult(const Ravelin::MatrixNd& x, Ravelin::MatrixNd& result) const; 37 | Ravelin::MatrixNd& mult(const std::vector& M, unsigned result_cols, Ravelin::MatrixNd& result) const; 38 | Ravelin::MatrixNd& mult(const std::vector& M, Ravelin::MatrixNd& result) const; 39 | Ravelin::MatrixNd& mult_transpose(const SparseJacobian& M, Ravelin::MatrixNd& result) const; 40 | Ravelin::MatrixNd& to_dense(Ravelin::MatrixNd& M) const; 41 | 42 | // vector of Jacobian blocks 43 | std::vector blocks; 44 | 45 | // the number of rows and columns in the dense Jacobian 46 | unsigned rows, cols; 47 | }; 48 | 49 | } // end namespace 50 | #endif 51 | 52 | -------------------------------------------------------------------------------- /include/Moby/SphericalJoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2007 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _SPHERICAL_JOINT_H 8 | #define _SPHERICAL_JOINT_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines an actuated spherical joint 16 | class SphericalJoint : public Joint, public Ravelin::SphericalJointd 17 | { 18 | public: 19 | SphericalJoint(); 20 | virtual ~SphericalJoint() {} 21 | SphericalJoint(boost::weak_ptr inboard, boost::weak_ptr outboard); 22 | virtual unsigned num_dof() const { return SphericalJointd::num_dof(); } 23 | virtual bool is_singular_config() const { return SphericalJointd::is_singular_config(); } 24 | virtual void evaluate_constraints(double C[]) { SphericalJointd::evaluate_constraints(C); } 25 | virtual const std::vector& get_spatial_axes_dot() { return SphericalJointd::get_spatial_axes_dot(); } 26 | virtual void determine_q(Ravelin::VectorNd& q) { SphericalJointd::determine_q(q); } 27 | virtual boost::shared_ptr get_induced_pose() { return SphericalJointd::get_induced_pose(); } 28 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 29 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 30 | }; // end class 31 | } // end namespace 32 | 33 | #endif 34 | 35 | -------------------------------------------------------------------------------- /include/Moby/StokesDragForce.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2009 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _STOKES_DRAG_FORCE_H 8 | #define _STOKES_DRAG_FORCE_H 9 | 10 | #include 11 | 12 | namespace Moby { 13 | class StokesDragForce : public RecurrentForce 14 | { 15 | public: 16 | StokesDragForce(); 17 | StokesDragForce(const StokesDragForce& source); 18 | virtual ~StokesDragForce() {} 19 | virtual void add_force(boost::shared_ptr body); 20 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 21 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 22 | 23 | /// The drag coefficient 24 | double b, b_ang; 25 | }; // end class 26 | } // end namespace 27 | 28 | #endif 29 | 30 | -------------------------------------------------------------------------------- /include/Moby/SustainedUnilateralConstraintSolveFailException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2014 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_RESTING_EVENT_FAIL_EXCEPTION_H_ 8 | #define _MOBY_RESTING_EVENT_FAIL_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when general numerical error occurs 15 | class SustainedUnilateralConstraintSolveFailException : public std::runtime_error 16 | { 17 | public: 18 | SustainedUnilateralConstraintSolveFailException() : std::runtime_error("SustainedUnilateralConstraintSolveFailException error") {} 19 | SustainedUnilateralConstraintSolveFailException(const char* error) : std::runtime_error(error) {} 20 | }; // end class 21 | 22 | 23 | } // end namespace 24 | 25 | #endif 26 | 27 | -------------------------------------------------------------------------------- /include/Moby/TessellatedPolyhedron.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | /// Constructs a TessellatedPolyhedron from iterators to a container holding Point3d objects and iterators to a container holding IndexedTri types 8 | template 9 | TessellatedPolyhedron::TessellatedPolyhedron(InputIterator1 verts_begin, InputIterator1 verts_end, InputIterator2 facets_begin, InputIterator2 facets_end) 10 | { 11 | // indicate that convexity has not been computed 12 | _convexity_computed = false; 13 | 14 | // create the mesh 15 | _mesh = IndexedTriArray(verts_begin, verts_end, facets_begin, facets_end); 16 | 17 | // calculate the bounding box 18 | calc_bounding_box(); 19 | } 20 | 21 | -------------------------------------------------------------------------------- /include/Moby/Tetrahedron.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _TETRAHEDRON_ 8 | #define _TETRAHEDRON_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace Moby { 16 | 17 | /// An indexed tetrahedron; should be oriented ccw 18 | struct Tetrahedron 19 | { 20 | Tetrahedron() { } 21 | Tetrahedron(const Point3d& v1, const Point3d& v2, const Point3d& v3, const Point3d& v4) { a = v1; b = v2; c = v3; d = v4; } 22 | double calc_signed_dist(const Point3d& p) const; 23 | double calc_signed_dist(const Point3d& p, Point3d& cp) const; 24 | void determine_barycentric_coords(const Point3d& p, double& u, double& v, double& w) const; 25 | Point3d calc_point(double u, double v, double w) const; 26 | Point3d calc_centroid() const; 27 | double calc_volume() const; 28 | bool outside(const Point3d& p, double tol = NEAR_ZERO) const; 29 | 30 | /// First vertex of the tetrahedron 31 | Point3d a; 32 | 33 | /// Second vertex of the tetrahedron 34 | Point3d b; 35 | 36 | /// Third vertex of the tetrahedron 37 | Point3d c; 38 | 39 | /// Fourth vertex of the tetrahedron 40 | Point3d d; 41 | 42 | private: 43 | static Ravelin::LinAlgd _LA; 44 | }; // end struct 45 | 46 | } // end namespace 47 | 48 | #endif 49 | 50 | -------------------------------------------------------------------------------- /include/Moby/ThickTriangle.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _THICK_TRIANGLE_H_ 8 | #define _THICK_TRIANGLE_H_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace Moby { 18 | 19 | /// Thick triangle for intersection / query testing 20 | class ThickTriangle 21 | { 22 | public: 23 | ThickTriangle(const Triangle& t, double epsilon) { construct_from_triangle(t, epsilon); } 24 | void operator=(const ThickTriangle& t) { _planes = t._planes; _normal = t._normal; tri = t.tri; } 25 | bool intersect_seg(const LineSeg3& seg, double& tnear, Point3d& isect) const; 26 | bool intersect_seg(const LineSeg3& seg, double& tnear, Point3d& isect, Ravelin::Vector3d& normal) const; 27 | bool point_inside(const Point3d& point) const; 28 | Ravelin::Vector3d determine_normal(const Point3d& p) const; 29 | const std::list& planes() const { return _planes; } 30 | void construct_from_triangle(const Triangle& t, double epsilon); 31 | boost::shared_ptr get_pose() const; 32 | 33 | /// The original triangle 34 | Triangle tri; 35 | 36 | private: 37 | std::list _planes; 38 | Ravelin::Vector3d _normal; 39 | }; // end class 40 | 41 | inline std::ostream& operator<<(std::ostream& out, const ThickTriangle& t) 42 | { 43 | BOOST_FOREACH(const Plane& p, t.planes()) 44 | std::cout << "plane: " << p << std::endl; 45 | 46 | return out; 47 | } 48 | 49 | } // end namespace 50 | 51 | #endif 52 | 53 | -------------------------------------------------------------------------------- /include/Moby/TimeSteppingSimulator.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _TS_SIMULATOR_H 8 | #define _TS_SIMULATOR_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace Moby { 21 | 22 | class ContactParameters; 23 | class CollisionDetection; 24 | class CollisionGeometry; 25 | 26 | /// An event-driven simulator 27 | class TimeSteppingSimulator : public ConstraintSimulator 28 | { 29 | friend class CollisionDetection; 30 | 31 | public: 32 | TimeSteppingSimulator(); 33 | virtual ~TimeSteppingSimulator() {} 34 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 35 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 36 | virtual double step(double dt); 37 | boost::shared_ptr get_contact_parameters(CollisionGeometryPtr geom1, CollisionGeometryPtr geom2) const; 38 | 39 | // the minimum step that the simulator should take (default = 1e-8) 40 | double min_step_size; 41 | 42 | /// Determines whether two geometries are not checked 43 | std::set > unchecked_pairs; 44 | 45 | /// Vectors set and passed to collision detection 46 | std::vector > _x0, _x1; 47 | 48 | /// Gets the shared pointer for this 49 | boost::shared_ptr get_this() { return boost::dynamic_pointer_cast(shared_from_this()); } 50 | 51 | protected: 52 | bool constraints_met(const std::vector& current_pairwise_distances); 53 | std::set > get_current_contact_geoms() const; 54 | double do_mini_step(double dt); 55 | void step_si_Euler(double dt); 56 | double calc_next_CA_Euler_step(double contact_dist_thresh) const; 57 | }; // end class 58 | 59 | } // end namespace 60 | 61 | #endif 62 | 63 | 64 | -------------------------------------------------------------------------------- /include/Moby/TriangleMeshPrimitive.inl: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2009 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | /// Gets the vertex indices from selected facets of an IndexedTriArray 8 | /** 9 | * \param tris the triangle mesh 10 | * \param fselect_begin iterator to a container of type unsigned (facet indices) 11 | * \param fselect_end iterator to a container of type unsigned (facet indices) 12 | * \param output beginning iterator to a container of type Point3d; unique 13 | * vertices are copied here on return 14 | * \return ending iterator to a container of type Point3d; unique vertices 15 | * are copied here on return 16 | */ 17 | template 18 | OutputIterator TriangleMeshPrimitive::get_vertices(const IndexedTriArray& tris, InputIterator fselect_begin, InputIterator fselect_end, OutputIterator output, boost::shared_ptr P) 19 | { 20 | // init a list of vertices 21 | std::list verts; 22 | 23 | // get facets from the triangle array 24 | const std::vector& facets = tris.get_facets(); 25 | while (fselect_begin != fselect_end) 26 | { 27 | const IndexedTri& f = facets[*fselect_begin++]; 28 | verts.push_back(f.a); 29 | verts.push_back(f.b); 30 | verts.push_back(f.c); 31 | } 32 | 33 | // sort the list so that we don't process repeated vertices 34 | verts.sort(); 35 | std::list::const_iterator new_end = std::unique(verts.begin(), verts.end()); 36 | 37 | // copy vertices to output 38 | const std::vector& vertices = tris.get_vertices(); 39 | for (std::list::const_iterator i = verts.begin(); i != new_end; i++) 40 | *output++ = Point3d(vertices[*i], P); 41 | 42 | return output; 43 | } 44 | 45 | -------------------------------------------------------------------------------- /include/Moby/UndefinedAxisException.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2013 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _UNDEFINED_AXIS_EXCEPTION_H_ 8 | #define _UNDEFINED_AXIS_EXCEPTION_H_ 9 | 10 | #include 11 | 12 | namespace Moby { 13 | 14 | /// Exception thrown when axis has not been defined but user calls operation that requires its definition 15 | class UndefinedAxisException : public std::runtime_error 16 | { 17 | public: 18 | UndefinedAxisException() : std::runtime_error("Axis was not defined prior to operation that requires a defined access") {} 19 | }; // end class 20 | 21 | 22 | } // end namespace 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /include/Moby/UnilateralConstraint.inl: -------------------------------------------------------------------------------- 1 | template 2 | void UnilateralConstraint::insertion_sort(BidirectionalIterator first, BidirectionalIterator last) 3 | { 4 | // exit if nothing to do 5 | if (first == last) 6 | return; 7 | 8 | BidirectionalIterator min = first; 9 | 10 | // loop 11 | BidirectionalIterator i = first; 12 | i++; 13 | for (; i != last; i++) 14 | if (*i < *min) 15 | min = i; 16 | 17 | // swap the iterators 18 | std::iter_swap(first, min); 19 | while (++first != last) 20 | for (BidirectionalIterator j = first; *j < *(j-1); --j) 21 | std::iter_swap((j-1), j); 22 | } 23 | 24 | template 25 | OutputIterator UnilateralConstraint::get_super_bodies(OutputIterator begin) const 26 | { 27 | boost::shared_ptr db1, db2; 28 | unsigned nb = get_super_bodies(db1, db2); 29 | switch (nb) 30 | { 31 | case 0: break; 32 | case 1: if (db1) *begin++ = db1; break; 33 | case 2: if (db1) *begin++ = db1; if (db2) *begin++ = db2; break; 34 | default: 35 | assert(false); 36 | } 37 | return begin; 38 | } 39 | 40 | 41 | -------------------------------------------------------------------------------- /include/Moby/UniversalJoint.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2007 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _UNIVERSAL_JOINT_H 8 | #define _UNIVERSAL_JOINT_H 9 | 10 | #include 11 | #include 12 | 13 | namespace Moby { 14 | 15 | /// Defines an actuated universal joint 16 | class UniversalJoint : public Joint, public Ravelin::UniversalJointd 17 | { 18 | public: 19 | UniversalJoint(); 20 | virtual ~UniversalJoint() {} 21 | UniversalJoint(boost::weak_ptr inboard, boost::weak_ptr outboard); 22 | virtual unsigned num_dof() const { return UniversalJointd::num_dof(); } 23 | virtual bool is_singular_config() const { return UniversalJointd::is_singular_config(); } 24 | virtual void evaluate_constraints(double C[]) { UniversalJointd::evaluate_constraints(C); } 25 | virtual const std::vector& get_spatial_axes_dot() { return UniversalJointd::get_spatial_axes_dot(); } 26 | virtual void determine_q(Ravelin::VectorNd& q) { UniversalJointd::determine_q(q); } 27 | virtual boost::shared_ptr get_induced_pose() { return UniversalJointd::get_induced_pose(); } 28 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 29 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 30 | }; // end class 31 | } // end namespace 32 | 33 | #endif 34 | 35 | -------------------------------------------------------------------------------- /include/Moby/Visualizable.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _VISUALIZABLE_H 8 | #define _VISUALIZABLE_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace osg { 17 | class Node; 18 | class MatrixTransform; 19 | class Group; 20 | } 21 | 22 | namespace Moby { 23 | 24 | /// Class that allows for visualizing simulation data 25 | /** 26 | * This class uses the OSGGroupWrapper class to permit sharing and 27 | * serialization of visualization data. 28 | */ 29 | class Visualizable : public virtual Base 30 | { 31 | public: 32 | Visualizable(); 33 | Visualizable(const Visualizable* v) : Base(v) { } 34 | virtual ~Visualizable(); 35 | virtual void update_visualization(); 36 | void set_visualization_relative_pose(const Ravelin::Pose3d& P); 37 | virtual void set_visualization_data(osg::Node* vdata); 38 | virtual void set_visualization_data(OSGGroupWrapperPtr vdata); 39 | static osg::Group* construct_from_node(boost::shared_ptr node, const std::map& id_map); 40 | osg::Group* get_visualization_data() const; 41 | virtual void save_to_xml(XMLTreePtr node, std::list >& shared_objects) const; 42 | virtual void load_from_xml(boost::shared_ptr node, std::map& id_map); 43 | 44 | /// Gets the pose for this visualizable object 45 | boost::shared_ptr get_visualization_pose() { return _vF; } 46 | 47 | protected: 48 | 49 | /// The relative pose 50 | boost::shared_ptr _vF; 51 | 52 | /// The underlying visualization data 53 | OSGGroupWrapperPtr _vizdata; 54 | }; // end class 55 | } // end namespace 56 | 57 | #endif // (ends #ifndef _VISUALIZABLE_H) 58 | 59 | -------------------------------------------------------------------------------- /include/Moby/XMLWriter.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2007 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_XML_WRITER_H 8 | #define _MOBY_XML_WRITER_H 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace Moby { 15 | 16 | /// Class for writing Base objects (and their derivatives) to XML 17 | class XMLWriter 18 | { 19 | public: 20 | static void serialize_to_xml(const std::string& filename, const std::list >& objects); 21 | static void serialize_to_xml(const std::string& filename, boost::shared_ptr object); 22 | }; 23 | 24 | } // end namespace 25 | 26 | #endif 27 | 28 | -------------------------------------------------------------------------------- /include/Moby/insertion_sort: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2013 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #ifndef _MOBY_INSERTION_SORT_ 8 | #define _MOBY_INSERTION_SORT_ 9 | 10 | namespace Moby { 11 | 12 | /// Does insertion sort -- custom comparison function not supported (uses operator<) 13 | template 14 | static void insertion_sort(BidirectionalIterator first, BidirectionalIterator last) 15 | { 16 | // safety check; exit if nothing to do 17 | if (first == last) 18 | return; 19 | 20 | BidirectionalIterator min = first; 21 | 22 | // loop 23 | BidirectionalIterator i = first; 24 | i++; 25 | for (; i != last; i++) 26 | if (*i < *min) 27 | min = i; 28 | 29 | // swap the iterators 30 | std::iter_swap(first, min); 31 | while (++first != last) 32 | for (BidirectionalIterator j = first; *j < *(j-1); --j) 33 | std::iter_swap((j-1), j); 34 | } // end function 35 | 36 | } // end namespace 37 | 38 | #endif 39 | 40 | -------------------------------------------------------------------------------- /include/Moby/permute.h: -------------------------------------------------------------------------------- 1 | #ifndef _MOBY_PERMUTE_ 2 | #define _MOBY_PERMUTE_ 3 | 4 | #include 5 | 6 | /** 7 | * This function, which acts like an STL algorithm, selects elements with 8 | * given integer indices from a container. 9 | * 10 | * Example: 11 | * double x[10] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }; 12 | * int i[4] = {3, 1, 5, 7}; 13 | * double results[4]; 14 | * 15 | * select(x, i, i+4, results); 16 | * // results now contains { 4, 2, 6, 8 } 17 | */ 18 | 19 | namespace Moby { 20 | 21 | /** 22 | * \param mapping_begin beginning iterator to mapping giving source indices 23 | * \param mapping_end ending iterator to mapping giving source indices 24 | * \param input_begin beginning iterator to source 25 | * \param output_begin beginning iterator to target 26 | */ 27 | template 28 | void permute(ForwardIterator mapping_begin, ForwardIterator mapping_end, RandomAccessIterator1 input_begin, RandomAccessIterator2 output_begin) 29 | { 30 | // setup the target index 31 | unsigned target_index = 0; 32 | 33 | for (ForwardIterator i = mapping_begin; i != mapping_end; i++) 34 | output_begin[target_index++] = input_begin[*i]; 35 | } 36 | 37 | } // end namespace 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /include/Moby/qpOASES.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2016 Evan Drumwright 3 | * This library is distributed under the terms of the GNU Lesser General Public 4 | * License (found in COPYING). 5 | ****************************************************************************/ 6 | 7 | #ifndef _OPT_QPOASES_H 8 | #define _OPT_QPOASES_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace Moby { 16 | 17 | class QPOASES 18 | { 19 | public: 20 | QPOASES() { 21 | } 22 | 23 | template 24 | bool qp_activeset(const Mat1& H, const Vec1& c, const Vec2& lb, const Vec3& ub, const Mat2& M, const Vec4& q, const Mat3& A, const Vec5& b, Vec6& z); 25 | 26 | private: 27 | static bool rel_equal(double x, double y, double tol = NEAR_ZERO) { return std::fabs(x-y) <= tol*std::max(std::fabs(x), std::max(std::fabs(y), (double) 1.0)); } 28 | }; // end class 29 | 30 | #include "qpOASES.inl" 31 | 32 | } // end namespace 33 | 34 | #endif 35 | 36 | -------------------------------------------------------------------------------- /include/Moby/select: -------------------------------------------------------------------------------- 1 | #ifndef _MOBY_SELECT_ 2 | #define _MOBY_SELECT_ 3 | 4 | #include 5 | 6 | /** 7 | * This function, which acts like an STL algorithm, selects elements with 8 | * given integer indices from a container. 9 | * 10 | * Example: 11 | * double x[10] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }; 12 | * int i[4] = {3, 1, 5, 7}; 13 | * double results[4]; 14 | * 15 | * select(x, i, i+4, results); 16 | * // results now contains { 4, 2, 6, 8 } 17 | */ 18 | 19 | namespace Moby { 20 | 21 | /** 22 | * \param container_begin iterator to the beginning of the container 23 | * \param indices_begin iterator to the beginning of the indices 24 | * \param indices_end iterator to the end of the indices 25 | * \param output_begin iterator to the beginning of the output 26 | * \return iterator to the end of the output 27 | */ 28 | template 29 | OutputIterator select(ForwardIterator1 container_begin, ForwardIterator2 indices_begin, ForwardIterator2 indices_end, OutputIterator output_begin) 30 | { 31 | int last = 0; 32 | while (indices_begin != indices_end) 33 | { 34 | std::advance(container_begin, *indices_begin - last); 35 | last = *indices_begin++; 36 | *output_begin++ = *container_begin; 37 | } 38 | 39 | return output_begin; 40 | } 41 | 42 | } // end namespace 43 | 44 | #endif 45 | 46 | -------------------------------------------------------------------------------- /programs/adjust-center.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace Ravelin; 5 | using namespace Moby; 6 | 7 | int main(int argc, char* argv[]) 8 | { 9 | const unsigned X = 0, Y = 1, Z = 2; 10 | 11 | if (argc < 4) 12 | { 13 | std::cerr << "syntax: adjust-center +x +y +z" << std::endl; 14 | return -1; 15 | } 16 | 17 | IndexedTriArray itas; 18 | std::list tris; 19 | 20 | // load file 21 | itas = IndexedTriArray::read_from_obj(std::string(argv[1])); 22 | itas.get_tris(std::back_inserter(tris), GLOBAL); 23 | 24 | // get the adjustments 25 | Origin3d adjust; 26 | adjust[X] = std::atof(argv[2]); 27 | adjust[Y] = std::atof(argv[3]); 28 | adjust[Z] = std::atof(argv[4]); 29 | Transform3d T; 30 | T.x = adjust; 31 | 32 | // write the result 33 | IndexedTriArray xlat = itas.transform(T); 34 | 35 | // write the file to the new filename 36 | std::string fname = "adjusted." + std::string(argv[1]); 37 | std::cout << "writing centered file " << fname << std::endl; 38 | IndexedTriArray::write_to_obj(xlat, fname); 39 | } 40 | -------------------------------------------------------------------------------- /programs/center.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace Ravelin; 5 | using namespace Moby; 6 | 7 | int main(int argc, char* argv[]) 8 | { 9 | if (argc < 1) 10 | { 11 | std::cerr << "syntax: center ... " << std::endl; 12 | return -1; 13 | } 14 | 15 | std::vector itas; 16 | std::list tris; 17 | 18 | // load all files 19 | for (int i=1; i< argc; i++) 20 | { 21 | itas.push_back(IndexedTriArray::read_from_obj(std::string(argv[i]))); 22 | itas.back().get_tris(std::back_inserter(tris), GLOBAL); 23 | } 24 | 25 | // compute the center-of-mass 26 | Origin3d com(CompGeom::calc_centroid_3D(tris.begin(), tris.end())); 27 | Transform3d T; 28 | T.x = -com; 29 | std::cout << "center of mass: " << com << std::endl; 30 | 31 | // write the result 32 | for (int i=1; i< argc; i++) 33 | { 34 | // translate the mesh 35 | IndexedTriArray xlat = itas[i-1].transform(T); 36 | 37 | // write the file to the new filename 38 | std::string fname = "centered." + std::string(argv[i]); 39 | std::cout << "writing centered file " << fname << std::endl; 40 | IndexedTriArray::write_to_obj(xlat, fname); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /programs/convexify.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace Ravelin; 7 | using namespace Moby; 8 | 9 | int main(int argc, char* argv[]) 10 | { 11 | if (argc != 3) 12 | { 13 | std::cerr << "syntax: convexify " << std::endl; 14 | std::cerr << std::endl; 15 | std::cerr << "convexify takes the description of a 3D geometry from the input file (a Wavefront OBJ" << std::endl; 16 | std::cerr << "file) and constructs a new Wavefront 3D file of the convex hull of that file." << std::endl; 17 | return -1; 18 | } 19 | 20 | // read in the file 21 | IndexedTriArray mesh = IndexedTriArray::read_from_obj(std::string(argv[1])); 22 | 23 | // get the vertices 24 | const std::vector& vertices = mesh.get_vertices(); 25 | 26 | // compute the convex hull 27 | TessellatedPolyhedronPtr p = CompGeom::calc_convex_hull(vertices.begin(), vertices.end()); 28 | 29 | // write the resulting mesh to the output file 30 | if (!p) 31 | { 32 | std::cerr << "convexify() - fatal error computing convex hull! (sorry I can't tell you more!" << std::endl; 33 | return -1; 34 | } 35 | 36 | // write the file 37 | p->get_mesh().write_to_obj(std::string(argv[2])); 38 | } 39 | 40 | -------------------------------------------------------------------------------- /programs/main.cpp: -------------------------------------------------------------------------------- 1 | namespace Moby{ 2 | extern int init(int argc, char** argv); 3 | } 4 | 5 | int main(int argc, char** argv){ 6 | return Moby::init(argc, argv); 7 | } 8 | -------------------------------------------------------------------------------- /programs/render-movie-simple.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # simple script for rendering a movie that takes in static camera positions 4 | # and targets 5 | 6 | usage() 7 | { 8 | echo "render_movie_simple.sh " 9 | echo " -- sample camera position: 0 5 10" 10 | echo " -- sample camera target: 0 0 0" 11 | echo " -- sample up vector: 0 1 0" 12 | exit 13 | } 14 | 15 | main () 16 | { 17 | # ffmpeg requires particular numbering 18 | a=1 19 | for i in $6/driver.out-*.osg; do 20 | new=$(printf "img%04d.png" ${a}); 21 | echo $2/moby-render $i -p $3 $4 $5 -t $7 $8 $9 -u ${11} ${12} ${13} $6/${new}; 22 | [ -f $6/$new ] && echo "Already created $new" || screen -d -m $2/moby-render $i -p $3 $4 $5 -t $7 $8 $9 -u ${11} ${12} ${13} $6/${new}; 23 | let a=a+1; 24 | done 25 | 26 | # number all images img0001.png, etc... 27 | echo ffmpeg -r $1 -i $6/img%04d.png -f mp4 -q:v 0 -vcodec mpeg4 ${10} 28 | ffmpeg -r $1 -i $6/img%04d.png -f mp4 -q:v 0 -vcodec mpeg4 ${10} 29 | } 30 | 31 | 32 | # check for proper number of arguments 33 | [ "$#" -ne 13 ] && ( usage && exit) || main $1 $2 $3 $4 $5 $6 $7 $8 $9 ${10} ${11} ${12} ${13} 34 | 35 | 36 | -------------------------------------------------------------------------------- /programs/render-movie.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # more sophisticated script for rendering a movie that takes in dynamic 4 | # camera positions and targets 5 | 6 | usage() 7 | { 8 | echo "render_movie.sh " 9 | exit 10 | } 11 | 12 | main() 13 | { 14 | 15 | 16 | # commands like this can be used to modify the osg files 17 | #rpl -q "ColorMode DIFFUSE" "ColorMode AMBIENT_AND_DIFFUSE" $1/*.osg 18 | 19 | # renders the osg files to images in order expected by ffmpeg 20 | 21 | a=1 22 | for i in $DATA_PATH/driver.out-*.osg; do 23 | new=$(printf "img%04d.png" ${a}) 24 | pnt=$(awk 'NR == n' n=${a} $CAMERA_FILE) 25 | tgt=$(awk 'NR == n' n=${a} $TARGET_FILE) 26 | [ -f $DATA_PATH/$new ] && echo "Already created $new" || screen -d -m $RENDER_PATH/moby-render $i -p $pnt -t $tgt $DATA_PATH/${new} 27 | let a=a+1 28 | done 29 | 30 | read -rsp $'Press any key to continue to making video... \n' -n1 key 31 | 32 | # render at 25fps 33 | ffmpeg -r 100 -i $DATA_PATH/img%04d.png -f mp4 -q:v 0 -vcodec mpeg4 $MOVIE_FILE 34 | } 35 | 36 | # check for proper number of arguments 37 | [ "$#" -ne 5 ] && ( usage && exit) 38 | 39 | echo "Input count: " $# " , Values: " $@ 40 | echo "" $1 41 | echo "" $2 42 | echo "" $3 43 | echo "" $4 44 | echo "" $5 45 | 46 | RENDER_PATH=$1 47 | DATA_PATH=$2 48 | CAMERA_FILE=$3 49 | TARGET_FILE=$4 50 | MOVIE_FILE=$5 51 | 52 | main 53 | -------------------------------------------------------------------------------- /regress/absolute-coords.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/bouncing-ball.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/contact-constrained-pendulum.setup: -------------------------------------------------------------------------------- 1 | -s=0.0001 2 | -mt=10 3 | -p=libcontact-constrained-pendulum-init.so 4 | 5 | -------------------------------------------------------------------------------- /regress/cylinder.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/fixed-articulated-table.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/gears.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/joint-limits.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/pendulum-urdf.setup: -------------------------------------------------------------------------------- 1 | -s=0.001 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/planar-joint.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/ramp.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/reduced-coords.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/regenerate-regression-data: -------------------------------------------------------------------------------- 1 | # !/bin/bash 2 | # script for regenerating regression data 3 | 4 | # setup the plugin path 5 | source $1setup.sh 6 | 7 | # test the articulated body examples 8 | echo "Regenerating data for chain2" 9 | $1moby-regress -mt=10 ../example/chain-contact/chain2.xml chain2.dat 10 | echo "Regenerating data for chain5" 11 | $1moby-regress -mt=10 ../example/chain-contact/chain5.xml chain5.dat 12 | 13 | # test the driving robot example 14 | #echo "Regenerating data for mobile robot example" 15 | #$1moby-regress -mt=10 -p=../example/mrobot/libcontroller.so ../example/mrobot/pioneer2.xml pioneer.dat 16 | 17 | # test the box example(s) 18 | echo "Regenerating data for sitting box" 19 | $1moby-regress -mt=10 ../example/contact_simple/contact.xml sitting-box.dat 20 | echo "Regenerating data for spinning box #1" 21 | $1moby-regress -mt=10 ../example/contact_simple/spinning-box1.xml spinning-box1.dat 22 | echo "Regenerating data for spinning box #2" 23 | $1moby-regress -mt=10 ../example/contact_simple/spinning-box2.xml spinning-box2.dat 24 | 25 | # test the sphere and cylinder examples 26 | echo "Regenerating data for the sliding to rolling sphere" 27 | $1moby-regress -mt=10 ../example/contact_simple/sphere.xml sphere.dat 28 | echo "Regenerating data for the bouncing spheres" 29 | $1moby-regress -mt=10 ../example/contact_simple/Zeno.xml Zeno.dat 30 | 31 | # test the simple floating base examples 32 | echo "Regenerating data for the fixed, articulated table" 33 | $1moby-regress -mt=10 ../example/contact_simple/fixed-articulated-table.xml fixed-articulated-table.dat 34 | echo "Regenerating data for the articulated table" 35 | $1moby-regress -s=.01 -mt=10 ../example/contact_simple/articulated-table.xml articulated-table.dat 36 | 37 | # test the stack problems 38 | echo "Regenerating data for sphere stack" 39 | $1moby-regress -mt=1 ../example/contact_simple/sphere-stack.xml sphere-stack.dat 40 | 41 | # test the rimless wheel example 42 | export RIMLESS_WHEEL_THETAD=0.24 43 | echo "Regenerating data for rimless wheel example" 44 | $1moby-regress -mt=8 -p=librimless-wheel-init.so ../example/rimless-wheel/wheel.xml rimless-wheel.dat 45 | 46 | # test the contact constrained pendulum 47 | echo "Regenerating data for contact constrained pendulum" 48 | $1moby-regress -mt=6.5 -p=$1/libcontact-constrained-pendulum-init.so ../example/contact_simple/contact-constrained-pendulum.xml contact-constrained-pendulum.dat 49 | 50 | 51 | -------------------------------------------------------------------------------- /regress/rimless-wheel.setup: -------------------------------------------------------------------------------- 1 | -s=0.001 2 | -mt=10 3 | -p=librimless-wheel-init.so 4 | 5 | -------------------------------------------------------------------------------- /regress/rolling-torus.setup: -------------------------------------------------------------------------------- 1 | -s=0.0001 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/spinning-boxes.setup: -------------------------------------------------------------------------------- 1 | -s=0.01 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /regress/stacks.setup: -------------------------------------------------------------------------------- 1 | -s=0.001 2 | -mt=10 3 | 4 | -------------------------------------------------------------------------------- /src/Base.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using boost::shared_ptr; 13 | using namespace Moby; 14 | 15 | namespace Moby { 16 | class BaseData 17 | { 18 | public: 19 | BaseData(const Base* b); 20 | void populate(Base* b); 21 | 22 | private: 23 | boost::shared_ptr _userdata; 24 | }; // end class 25 | } // end namespace 26 | 27 | BaseData::BaseData(const Base* b) 28 | { 29 | _userdata = b->userdata; 30 | } 31 | 32 | void BaseData::populate(Base* b) 33 | { 34 | b->userdata = _userdata; 35 | } 36 | 37 | Base::Base() 38 | { 39 | std::ostringstream oss; 40 | oss << this; 41 | id = oss.str(); 42 | } 43 | 44 | Base::Base(const Base* b) 45 | { 46 | userdata = b->userdata; 47 | std::ostringstream oss; 48 | oss << this; 49 | id = oss.str(); 50 | } 51 | 52 | /// Method for loading the data for this object from XML 53 | /** 54 | * \param node the subtree under which all data necessary to load this 55 | * object is stored 56 | * \param id_map a map from node IDs to read objects 57 | */ 58 | void Base::load_from_xml(shared_ptr node, std::map& id_map) 59 | { 60 | // read the ID, if specified 61 | XMLAttrib 62 | * id_attrib = node->get_attrib("id"); 63 | if (id_attrib) 64 | id = id_attrib->get_string_value(); 65 | 66 | // verify that the ID does not already exist in the map 67 | if (id_map.find(id) != id_map.end()) 68 | { 69 | std::cerr << "Base::load_from_xml() - \"unique\" ID '" << id << "' "; 70 | std::cerr << " already exists in the id_map! Adding anyway..."; 71 | std::cerr << std::endl; 72 | } 73 | 74 | // add the ID to the ID map 75 | id_map[id] = Base::shared_from_this(); 76 | } 77 | 78 | /// Method for saving this object to XML 79 | /** 80 | * \param node the XML node to which this object should be serialized 81 | * \param on output, a list of shared objects which should also be serialized 82 | */ 83 | void Base::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 84 | { 85 | // set the ID for the node 86 | node->attribs.insert(XMLAttrib("id", id)); 87 | } 88 | 89 | -------------------------------------------------------------------------------- /src/EulerIntegrator.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | 10 | using boost::shared_ptr; 11 | using Ravelin::VectorNd; 12 | using namespace Moby; 13 | 14 | /// Method for 1st-order Euler integration 15 | void EulerIntegrator::integrate(VectorNd& x, VectorNd& (*f)(const VectorNd&, double, double, void*, VectorNd&), double time, double step_size, void* data) 16 | { 17 | // save the old time 18 | const double old_time = time; 19 | 20 | // update the time 21 | time += step_size; 22 | 23 | // compute new state 24 | x += (f(x, old_time, step_size, data, _dx) *= step_size); 25 | } 26 | 27 | /// Implements Base::save_to_xml() 28 | void EulerIntegrator::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 29 | { 30 | Integrator::save_to_xml(node, shared_objects); 31 | node->name = "EulerIntegrator"; 32 | } 33 | 34 | -------------------------------------------------------------------------------- /src/FixedJoint.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2009 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace Ravelin; 15 | using namespace Moby; 16 | using std::vector; 17 | using boost::shared_ptr; 18 | 19 | /// Initializes the joint 20 | /** 21 | * The inboard and outboard links are set to NULL. 22 | */ 23 | FixedJoint::FixedJoint() : Joint(), FixedJointd() 24 | { 25 | init_data(); // FixedJointd()'s constructor should call our virtual... 26 | // ... method but this is not happening. 27 | } 28 | 29 | /// Initializes the joint with the specified inboard and outboard links 30 | FixedJoint::FixedJoint(boost::weak_ptr inboard, boost::weak_ptr outboard) : Joint(inboard, outboard), FixedJointd() 31 | { 32 | } 33 | 34 | /// Implements Base::load_from_xml() 35 | void FixedJoint::load_from_xml(shared_ptr node, std::map& id_map) 36 | { 37 | // read the information from the articulated body joint 38 | Joint::load_from_xml(node, id_map); 39 | 40 | // verify that the node name is correct 41 | assert(strcasecmp(node->name.c_str(), "FixedJoint") == 0); 42 | } 43 | 44 | /// Implements Base::save_to_xml() 45 | void FixedJoint::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 46 | { 47 | // get info from Joint::save_to_xml() 48 | Joint::save_to_xml(node, shared_objects); 49 | 50 | // rename the node 51 | node->name = "FixedJoint"; 52 | } 53 | 54 | -------------------------------------------------------------------------------- /src/Integrator.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2008 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace Moby; 12 | 13 | /// Implements Base::save_to_xml() 14 | void Integrator::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 15 | { 16 | // call parent method 17 | Base::save_to_xml(node, shared_objects); 18 | 19 | // set the node's name 20 | node->name = "Integrator"; 21 | } 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/Log.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace Moby; 4 | 5 | std::ofstream OutputToFile::stream; 6 | 7 | void OutputToFile::output(const std::string& msg) 8 | { 9 | if (!stream.is_open()) 10 | { 11 | std::ofstream stderr_stream("/dev/stderr", std::ofstream::app); 12 | stderr_stream << msg << std::flush; 13 | } 14 | else 15 | stream << msg << std::flush; 16 | } 17 | 18 | -------------------------------------------------------------------------------- /src/PlanarJoint.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2015 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using std::vector; 16 | using boost::shared_ptr; 17 | using namespace Ravelin; 18 | using boost::dynamic_pointer_cast; 19 | using namespace Moby; 20 | 21 | /// Initializes the joint 22 | /** 23 | * The axes of rotation are each set to [0 0 0]. The inboard 24 | * and outboard links are set to NULL. 25 | */ 26 | PlanarJoint::PlanarJoint() : Joint(), PlanarJointd() 27 | { 28 | init_data(); // PlanarJointd()'s constructor should call our virtual... 29 | // ... method but this is not happening. 30 | } 31 | 32 | /// Initializes the joint with the specified inboard and outboard links 33 | /** 34 | * The axis of rotation is set to [0 0 0]. 35 | */ 36 | PlanarJoint::PlanarJoint(boost::weak_ptr inboard, boost::weak_ptr outboard) : Joint(inboard, outboard), PlanarJointd() 37 | { 38 | } 39 | 40 | /// Implements Base::load_from_xml() 41 | void PlanarJoint::load_from_xml(shared_ptr node, std::map& id_map) 42 | { 43 | // verify that the node name is correct 44 | assert(strcasecmp(node->name.c_str(), "PlanarJoint") == 0); 45 | 46 | // read the plane normal, if given 47 | XMLAttrib* normal_attrib = node->get_attrib("normal"); 48 | if (normal_attrib) 49 | { 50 | Vector3d normal; 51 | normal_attrib->get_vector_value(normal); 52 | set_normal(normal); 53 | } 54 | 55 | // read the information from the articulated body joint 56 | Joint::load_from_xml(node, id_map); 57 | } 58 | 59 | /// Implements Base::save_to_xml() 60 | void PlanarJoint::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 61 | { 62 | // get info from Joint::save_to_xml() 63 | Joint::save_to_xml(node, shared_objects); 64 | 65 | // rename the node 66 | node->name = "PlanarJoint"; 67 | 68 | // save normal 69 | node->attribs.insert(XMLAttrib("normal", _normal)); 70 | } 71 | 72 | -------------------------------------------------------------------------------- /src/PrismaticJoint.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using std::vector; 16 | using boost::shared_ptr; 17 | using namespace Ravelin; 18 | using namespace Moby; 19 | 20 | /// Initializes the joint 21 | /** 22 | * The axis of rotation is set to [0 0 0]. The inboard 23 | * and outboard links are set to NULL. 24 | */ 25 | PrismaticJoint::PrismaticJoint() : Joint(), PrismaticJointd() 26 | { 27 | init_data(); // PrismaticJointd()'s constructor should call our virtual... 28 | // ... method but this is not happening. 29 | } 30 | 31 | /// Initializes the joint with the specified inboard and outboard links 32 | /** 33 | * The axis of rotation is set to [0 0 0]. 34 | */ 35 | PrismaticJoint::PrismaticJoint(boost::weak_ptr inboard, boost::weak_ptr outboard) : Joint(inboard, outboard) 36 | { 37 | } 38 | 39 | /// Implements Base::load_from_xml() 40 | void PrismaticJoint::load_from_xml(shared_ptr node, std::map& id_map) 41 | { 42 | // read the information from the articulated body joint 43 | Joint::load_from_xml(node, id_map); 44 | 45 | // verify that the node name is correct 46 | assert(strcasecmp(node->name.c_str(), "PrismaticJoint") == 0); 47 | 48 | // read the joint axis 49 | XMLAttrib* axis_attrib = node->get_attrib("axis"); 50 | if (axis_attrib) 51 | { 52 | Vector3d axis; 53 | axis_attrib->get_vector_value(axis); 54 | axis.normalize(); 55 | set_axis(axis); 56 | } 57 | } 58 | 59 | /// Implements Base::save_to_xml() 60 | void PrismaticJoint::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 61 | { 62 | // get the majority of the info from Joint::save_to_xml() 63 | Joint::save_to_xml(node, shared_objects); 64 | 65 | // rename the node 66 | node->name = "PrismaticJoint"; 67 | 68 | // save the joint axis (global coords) 69 | Vector3d u0 = Pose3d::transform_vector(shared_ptr(), _u); 70 | node->attribs.insert(XMLAttrib("axis", u0)); 71 | } 72 | 73 | -------------------------------------------------------------------------------- /src/RCArticulatedBodyFwdDynAlgo.cpp: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /src/RevoluteJoint.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2006 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using boost::shared_ptr; 16 | using namespace Ravelin; 17 | using namespace Moby; 18 | 19 | /// Initializes the joint 20 | /** 21 | * The axis of rotation is set to [0 0 0]. The inboard 22 | * and outboard links are set to NULL. 23 | */ 24 | RevoluteJoint::RevoluteJoint() : Joint(), RevoluteJointd() 25 | { 26 | init_data(); // RevoluteJointd()'s constructor should call our virtual... 27 | // ... method but this is not happening. 28 | } 29 | 30 | /// Initializes the joint with the specified inboard and outboard links 31 | /** 32 | * The axis of rotation is set to [0 0 0]. 33 | */ 34 | RevoluteJoint::RevoluteJoint(boost::weak_ptr inboard, boost::weak_ptr outboard) : Joint(inboard, outboard), RevoluteJointd() 35 | { 36 | } 37 | 38 | /// Implements Base::load_from_xml() 39 | void RevoluteJoint::load_from_xml(shared_ptr node, std::map& id_map) 40 | { 41 | // read the information from the articulated body joint 42 | Joint::load_from_xml(node, id_map); 43 | 44 | // verify that the node name is correct 45 | assert(strcasecmp(node->name.c_str(), "RevoluteJoint") == 0); 46 | 47 | // read the global joint axis, if given 48 | XMLAttrib* axis_attrib = node->get_attrib("axis"); 49 | if (axis_attrib) 50 | { 51 | Vector3d axis; 52 | axis_attrib->get_vector_value(axis); 53 | set_axis(axis); 54 | } 55 | } 56 | 57 | /// Implements Base::save_to_xml() 58 | void RevoluteJoint::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 59 | { 60 | // get info from Joint::save_to_xml() 61 | Joint::save_to_xml(node, shared_objects); 62 | 63 | // rename the node 64 | node->name = "RevoluteJoint"; 65 | 66 | // save the joint axis (global coordinates) 67 | Vector3d u0 = Pose3d::transform_vector(shared_ptr(), _u); 68 | node->attribs.insert(XMLAttrib("axis", u0)); 69 | } 70 | 71 | -------------------------------------------------------------------------------- /src/RungeKuttaIntegrator.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2005 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | 10 | using boost::shared_ptr; 11 | using Ravelin::VectorNd; 12 | using namespace Moby; 13 | 14 | /// Method for 4th-order Runge-Kutta integration 15 | void RungeKuttaIntegrator::integrate(VectorNd& x, VectorNd& (*f)(const VectorNd&, double, double, void*, VectorNd&), double time, double step_size, void* data) 16 | { 17 | const double ONE_SIXTH = 1.0/6.0; 18 | const double ONE_THIRD = 1.0/3.0; 19 | SAFESTATIC VectorNd k1, k2, k3, k4, tmp; 20 | 21 | // compute k1 22 | f(x, time, step_size, data, k1) *= step_size; 23 | 24 | // compute k2 25 | const double HALF_STEP = step_size * (double) 0.5; 26 | ((tmp = k1) *= 0.5) += x; 27 | f(tmp, time + HALF_STEP, HALF_STEP, data, k2) *= step_size; 28 | 29 | // compute k3 30 | ((tmp = k2) *= 0.5) += x; 31 | f(tmp, time + HALF_STEP, HALF_STEP, data, k3) *= step_size; 32 | 33 | // compute k4 34 | ((tmp = k3) *= 0.5) += x; 35 | f(tmp, time + step_size, step_size, data, k4) *= step_size; 36 | 37 | // update the time 38 | time += step_size; 39 | 40 | // compute new state 41 | k1 *= ONE_SIXTH; 42 | k2 *= ONE_THIRD; 43 | k3 *= ONE_THIRD; 44 | k4 *= ONE_SIXTH; 45 | x += k1; 46 | x += k2; 47 | x += k3; 48 | x += k4; 49 | 50 | return; 51 | } 52 | 53 | void RungeKuttaIntegrator::load_from_xml(shared_ptr node, std::map& id_map) 54 | { 55 | assert(strcasecmp(node->name.c_str(), "RungeKuttaIntegrator") == 0); 56 | Integrator::load_from_xml(node, id_map); 57 | } 58 | 59 | void RungeKuttaIntegrator::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 60 | { 61 | Integrator::save_to_xml(node, shared_objects); 62 | node->name = "RungeKuttaIntegrator"; 63 | } 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/SingleBody.cpp: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /src/VariableStepIntegrator.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright 2010 Evan Drumwright 3 | * This library is distributed under the terms of the Apache V2.0 4 | * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0). 5 | ****************************************************************************/ 6 | 7 | #include 8 | #include 9 | 10 | using boost::shared_ptr; 11 | using namespace Moby; 12 | 13 | /// Implements Base::load_from_xml() 14 | void VariableStepIntegrator::load_from_xml(shared_ptr node, std::map& id_map) 15 | { 16 | Integrator::load_from_xml(node, id_map); 17 | 18 | // get the absolute error tolerance 19 | XMLAttrib* aerr_attrib = node->get_attrib("abs-err-tol"); 20 | if (aerr_attrib) 21 | aerr_tolerance = aerr_attrib->get_real_value(); 22 | 23 | // get the relative error tolerance 24 | XMLAttrib* rerr_attrib = node->get_attrib("rel-err-tol"); 25 | if (rerr_attrib) 26 | rerr_tolerance = rerr_attrib->get_real_value(); 27 | 28 | // get the minimum step size 29 | XMLAttrib* mss_attrib = node->get_attrib("min-step-size"); 30 | if (mss_attrib) 31 | min_step_size = mss_attrib->get_real_value(); 32 | } 33 | 34 | /// Implements Base::save_to_xml() 35 | void VariableStepIntegrator::save_to_xml(XMLTreePtr node, std::list >& shared_objects) const 36 | { 37 | Integrator::save_to_xml(node, shared_objects); 38 | node->name = "VariableStepIntegrator"; 39 | 40 | // save the method data 41 | node->attribs.insert(XMLAttrib("rel-err-tol", rerr_tolerance)); 42 | node->attribs.insert(XMLAttrib("abs-err-tol", aerr_tolerance)); 43 | node->attribs.insert(XMLAttrib("min-step-size", min_step_size)); 44 | } 45 | 46 | -------------------------------------------------------------------------------- /test/PolyhedronTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | using namespace Ravelin; 12 | using namespace Moby; 13 | 14 | int main() 15 | { 16 | std::vector v(8); 17 | v[0] = Origin3d(-1.0, -1.0, -1.0); 18 | v[1] = Origin3d(-1.0, -1.0, +1.0); 19 | v[2] = Origin3d(-1.0, +1.0, -1.0); 20 | v[3] = Origin3d(-1.0, +1.0, +1.0); 21 | v[4] = Origin3d(+1.0, -1.0, -1.0); 22 | v[5] = Origin3d(+1.0, -1.0, +1.0); 23 | v[6] = Origin3d(+1.0, +1.0, -1.0); 24 | v[7] = Origin3d(+1.0, +1.0, +1.0); 25 | 26 | 27 | TessellatedPolyhedronPtr p_tess = CompGeom::calc_convex_hull(v.begin(), v.end()); 28 | Polyhedron p_test = p_tess->to_polyhedron(); 29 | std::ofstream out("box.wrl"); 30 | Polyhedron::to_vrml(out, p_test, Origin3d(1,1,1), true); 31 | out.close(); 32 | } 33 | -------------------------------------------------------------------------------- /test/TestOffsetSphere.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "gtest/gtest.h" 6 | 7 | using boost::shared_ptr; 8 | using boost::dynamic_pointer_cast; 9 | using std::map; 10 | using std::vector; 11 | using namespace Ravelin; 12 | using namespace Moby; 13 | 14 | void find(const map& read_map, shared_ptr& rb, const std::string& id) 15 | { 16 | for (map::const_iterator i = read_map.begin(); i != read_map.end(); i++) 17 | { 18 | rb = dynamic_pointer_cast(i->second); 19 | if (rb && rb->id == id) 20 | return; 21 | } 22 | } 23 | 24 | void find(const map& read_map, shared_ptr& sim) 25 | { 26 | for (map::const_iterator i = read_map.begin(); i != read_map.end(); i++) 27 | { 28 | sim = dynamic_pointer_cast(i->second); 29 | if (sim) 30 | return; 31 | } 32 | } 33 | 34 | TEST(Torus, KineticEnergy) 35 | { 36 | const double TOL = 1e-6; 37 | const double DT = 1e-3; 38 | const unsigned N_STEPS = (unsigned) (3.0/DT); 39 | const std::string FNAME("sphere.xml"); 40 | const std::string SPHERE_ID("sphere"); 41 | shared_ptr sim; 42 | shared_ptr sphere; 43 | 44 | // log contact 45 | Moby::Log::reporting_level = (LOG_SIMULATOR | LOG_CONSTRAINT); 46 | Moby::OutputToFile::stream.open("logging.out"); 47 | 48 | // load in the sphere file 49 | map READ_MAP = XMLReader::read(FNAME); 50 | 51 | // look for the simulator 52 | find(READ_MAP, sim); 53 | 54 | // look for the sphere 55 | find(READ_MAP, sphere, SPHERE_ID); 56 | 57 | // modify the initial conditions 58 | 59 | // compute the kinetic energy in the global frame 60 | double KE1 = sphere->calc_kinetic_energy(sphere->get_mixed_pose()); 61 | 62 | // integrate for a few seconds 63 | for (unsigned i=0; i< N_STEPS; i++) 64 | sim->step(DT); 65 | 66 | // compute the kinetic energy in the global frame again 67 | double KE2 = sphere->calc_kinetic_energy(sphere->get_mixed_pose()); 68 | 69 | Moby::OutputToFile::stream.close(); 70 | 71 | // compare the two kinetic energies 72 | EXPECT_NEAR(KE1, KE2, TOL); 73 | } 74 | 75 | -------------------------------------------------------------------------------- /test/box.xml: -------------------------------------------------------------------------------- 1 | 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /test/qp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main( void ) { 7 | Moby::QPOASES _qp; 8 | 9 | const double INF = 1e+29; 10 | 11 | // Setup H matrix 12 | Ravelin::MatrixNd H; 13 | H.resize(2, 2); 14 | H(0, 0) = 4; 15 | H(0, 1) = 1; 16 | H(1, 0) = 1; 17 | H(1, 1) = 2; 18 | 19 | // linear coefs 20 | Ravelin::VectorNd p; 21 | p.resize(2); 22 | p[0] = 1; 23 | p[1] = 1; 24 | 25 | // Inequality constraints 26 | Ravelin::MatrixNd M; 27 | M.resize(2, 2); 28 | M(0, 0) = 1; 29 | M(0, 1) = 0; 30 | M(1, 1) = 1; 31 | M(1, 0) = 0; 32 | 33 | Ravelin::VectorNd q; 34 | q.resize(2); 35 | q[0] = 0; 36 | q[1] = 0; 37 | 38 | // Setup equality constraint 39 | Ravelin::MatrixNd A; 40 | A.resize(1, 2); 41 | A(0, 0) = 1; 42 | A(0, 1) = 1; 43 | 44 | Ravelin::VectorNd b; 45 | b.resize(1); 46 | b[0] = 1; 47 | 48 | // Setup the x bounds 49 | Ravelin::VectorNd lb, ub; 50 | lb.resize(2); 51 | ub.resize(2); 52 | lb[0] = 0; 53 | lb[1] = 0; 54 | ub[0] = INF; 55 | ub[1] = INF; 56 | 57 | // Output 58 | Ravelin::VectorNd z; 59 | z.resize(2); 60 | z[0] = 0; 61 | z[1] = 0; 62 | 63 | if (!_qp.qp_activeset(H, p, lb, ub, M, q, A, b, z)){ 64 | std::cout << "QP failed to find feasible point" << std::endl; 65 | } else { 66 | std::cout << "QP solved successfully: " << z << std::endl; 67 | } 68 | return 0; 69 | } 70 | 71 | -------------------------------------------------------------------------------- /test/sphere.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /test/torus.xml: -------------------------------------------------------------------------------- 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 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | --------------------------------------------------------------------------------