diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e06a2020..1eeeaa02 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -14,11 +14,27 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-20.04, windows-2019] - sofa_branch: [master, v21.12] + os: [ubuntu-20.04, macos-11, windows-2019] + sofa_branch: [master] + python_version: ['3.8'] # Steps represent a sequence of tasks that will be executed as part of the job steps: + - name: (Mac) Workaround for homebrew + shell: bash + if: runner.os == 'macOS' + run: | + rm -f /usr/local/bin/2to3 + rm -f /usr/local/bin/idle3 + rm -f /usr/local/bin/pydoc3 + rm -f /usr/local/bin/python3 + rm -f /usr/local/bin/python3-config + rm -f /usr/local/bin/2to3-3.11 + rm -f /usr/local/bin/idle3.11 + rm -f /usr/local/bin/pydoc3.11 + rm -f /usr/local/bin/python3.11 + rm -f /usr/local/bin/python3.11-config + - name: Setup SOFA and environment id: sofa uses: sofa-framework/sofa-setup-action@v4 @@ -26,6 +42,22 @@ jobs: sofa_root: ${{ github.workspace }}/sofa sofa_version: ${{ matrix.sofa_branch }} sofa_scope: 'standard' + sofa_with_sofapython3: 'false' + python_version: '${{ matrix.python_version }}' + + - name: Install SofaPython3 + shell: bash + run: | + SofaPython3_ROOT="$GITHUB_WORKSPACE/SofaPython3" + mkdir -p "${{ runner.temp }}/sp3_tmp/zip" "${{ runner.temp }}/sp3_tmp/binaries" "$SofaPython3_ROOT" + url="https://github.com/sofa-framework/SofaPython3/releases/download" + url="${url}/release-master-nightly/SofaPython3_master-nightly_python-${{ matrix.python_version }}_for-SOFA-${{ matrix.sofa_branch }}_${{ runner.os }}.zip" + echo "Getting SofaPython3 from $url" + curl --output "${{ runner.temp }}/sp3_tmp/SofaPython3.zip" -L $url + unzip -qq "${{ runner.temp }}/sp3_tmp/SofaPython3.zip" -d "${{ runner.temp }}/sp3_tmp/binaries" + mv "${{ runner.temp }}"/sp3_tmp/binaries/SofaPython3_*/* "$SofaPython3_ROOT" + echo "SofaPython3_ROOT=$SofaPython3_ROOT" | tee -a $GITHUB_ENV + echo "SofaPython3_DIR=$SofaPython3_ROOT/lib/cmake/SofaPython3" | tee -a $GITHUB_ENV - name: Checkout source code uses: actions/checkout@v2 @@ -35,27 +67,26 @@ jobs: - name: Build and install shell: bash run: | + cmake_options="-GNinja \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX="$WORKSPACE_INSTALL_PATH" \ + -DCMAKE_PREFIX_PATH="$SOFA_ROOT/lib/cmake" \ + -DPYTHON_ROOT=$PYTHON_ROOT -DPython_ROOT=$PYTHON_ROOT \ + -DPYTHON_EXECUTABLE=$PYTHON_EXE -DPython_EXECUTABLE=$PYTHON_EXE" + if [ -e "$(command -v ccache)" ]; then + cmake_options="$cmake_options -DCMAKE_C_COMPILER_LAUNCHER=ccache -DCMAKE_CXX_COMPILER_LAUNCHER=ccache" + fi + cmake_options="$(echo $cmake_options)" # prettify + if [[ "$RUNNER_OS" == "Windows" ]]; then cmd //c "${{ steps.sofa.outputs.vs_vsdevcmd }} \ && cd /d $WORKSPACE_BUILD_PATH \ - && cmake \ - -GNinja \ - -DCMAKE_PREFIX_PATH="$SOFA_ROOT/lib/cmake" \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX="$WORKSPACE_INSTALL_PATH" \ - ../src \ + && cmake $cmake_options ../src \ && ninja install" else cd "$WORKSPACE_BUILD_PATH" ccache -z - cmake \ - -GNinja \ - -DCMAKE_C_COMPILER_LAUNCHER=ccache \ - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ - -DCMAKE_PREFIX_PATH=$SOFA_ROOT/lib/cmake \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX="$WORKSPACE_INSTALL_PATH" \ - ../src + cmake $cmake_options ../src ninja install echo ${CCACHE_BASEDIR} ccache -s @@ -137,4 +168,4 @@ jobs: fail_on_unmatched_files: true files: | artifacts/CosseratPlugin_*_Linux.zip - artifacts/CosseratPlugin_*_Windows.zip \ No newline at end of file + artifacts/CosseratPlugin_*_Windows.zip diff --git a/.gitignore b/.gitignore index 23a350f5..331e485c 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,7 @@ Untitled*.ipynb TAGS python/ scenes/ +.DS_Store/ +.DS_Store +.vscode/ +.vscode diff --git a/CMakeLists.txt b/CMakeLists.txt index a391be5f..d85a7a26 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,33 +1,14 @@ cmake_minimum_required(VERSION 3.12) project(CosseratPlugin VERSION 21.12.0) -# set (COSSERAT_VERSION ${PROJECT_VERSION}) include(cmake/environment.cmake) -find_package(SofaFramework REQUIRED) -find_package(SofaUserInteraction REQUIRED) -find_package(SofaBaseMechanics REQUIRED) -find_package(SofaConstraint REQUIRED) -find_package(SofaRigid REQUIRED) -find_package(SofaMiscMapping REQUIRED) -find_package(SofaEngine REQUIRED) -find_package(SofaSparseSolver REQUIRED) - -sofa_find_package(Sofa.GL QUIET) -sofa_find_package(SofaBaseTopology REQUIRED) - -# option(COSSERAT_IGNORE_ERRORS "Enable this option to ignore the recommendations and generate anyway." ON) - -# if(NOT COSSERAT_IGNORE_ERRORS) -# if(NOT PLUGIN_SOFAPYTHON AND NOT PLUGIN_SOFAPYTHON3) -# message(SEND_ERROR " -# You should not use the plugin Cosserat without enabling PLUGIN_SOFAPYTHON or PLUGIN_SOFAPYTHON3. -# Since all the scenes to test this plugin are using one of this two plugins. -# To fix this error you can either follow the recommendation or enable COSSERAT_IGNORE_ERRORS.") -# endif() -# endif() - -# set(COSSERAT_PYTHON ${SofaPython_FOUND}) # config.h.in +find_package(Sofa.Config REQUIRED) +sofa_find_package(Sofa.Component.Constraint.Lagrangian.Model REQUIRED) +sofa_find_package(Sofa.Component.StateContainer REQUIRED) +sofa_find_package(Sofa.Component.Mapping.NonLinear REQUIRED) +sofa_find_package(Sofa.GL REQUIRED) +sofa_find_package(Sofa.Component.Topology.Container.Dynamic REQUIRED) sofa_find_package(STLIB QUIET) if(STLIB_FOUND) @@ -36,87 +17,88 @@ else() message("-- The highly recommended 'STLIB' plugin is missing. You can compile Cosserat but some of the provided python examples will not work. ") endif() -set(HEADER_FILES - src/initCosserat.h - src/mapping/BaseCosserat.h - src/mapping/BaseCosserat.inl - src/mapping/DiscreteCosseratMapping.h - src/mapping/DiscreteCosseratMapping.inl - src/mapping/DiscreteDynamicCosseratMapping.h - src/mapping/DiscreteDynamicCosseratMapping.inl - src/mapping/ProjectionEngine.h - src/mapping/ProjectionEngine.inl - src/mapping/DifferenceMultiMapping.h - src/mapping/DifferenceMultiMapping.inl - src/mapping/RigidDistanceMapping.h - src/mapping/RigidDistanceMapping.inl - src/forcefield/BeamHookeLawForceField.h - src/forcefield/BeamHookeLawForceField.inl - src/forcefield/BeamPlasticLawForceField.h - src/forcefield/BeamPlasticLawForceField.inl - src/forcefield/CosseratInternalActuation.h - src/forcefield/CosseratInternalActuation.inl - src/constraint/CosseratSlidingConstraint.h - src/constraint/CosseratSlidingConstraint.inl - src/mapping/LegendrePolynomialsMapping.h - src/mapping/LegendrePolynomialsMapping.inl +set(SRC_ROOT_DIR src/${PROJECT_NAME}) +set(HEADER_FILES + ${SRC_ROOT_DIR}/config.h.in + ${SRC_ROOT_DIR}/mapping/BaseCosserat.h + ${SRC_ROOT_DIR}/mapping/BaseCosserat.inl + ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.h + ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.inl + ${SRC_ROOT_DIR}/mapping/DiscreteDynamicCosseratMapping.h + ${SRC_ROOT_DIR}/mapping/DiscreteDynamicCosseratMapping.inl + ${SRC_ROOT_DIR}/mapping/ProjectionEngine.h + ${SRC_ROOT_DIR}/mapping/ProjectionEngine.inl + ${SRC_ROOT_DIR}/mapping/DifferenceMultiMapping.h + ${SRC_ROOT_DIR}/mapping/DifferenceMultiMapping.inl + ${SRC_ROOT_DIR}/mapping/RigidDistanceMapping.h + ${SRC_ROOT_DIR}/mapping/RigidDistanceMapping.inl + ${SRC_ROOT_DIR}/engine/PointsManager.h + ${SRC_ROOT_DIR}/engine/PointsManager.inl + ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.h + ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.inl + ${SRC_ROOT_DIR}/forcefield/BeamPlasticLawForceField.h + ${SRC_ROOT_DIR}/forcefield/BeamPlasticLawForceField.inl + ${SRC_ROOT_DIR}/forcefield/CosseratInternalActuation.h + ${SRC_ROOT_DIR}/forcefield/CosseratInternalActuation.inl + ${SRC_ROOT_DIR}/constraint/CosseratSlidingConstraint.h + ${SRC_ROOT_DIR}/constraint/CosseratSlidingConstraint.inl + ${SRC_ROOT_DIR}/mapping/LegendrePolynomialsMapping.h + ${SRC_ROOT_DIR}/mapping/LegendrePolynomialsMapping.inl + ${SRC_ROOT_DIR}/constraint/CosseratNeedleSlidingConstraint.h + ${SRC_ROOT_DIR}/constraint/CosseratNeedleSlidingConstraint.inl ) - set(SOURCE_FILES - src/initCosserat.cpp - src/mapping/BaseCosserat.cpp - src/mapping/DiscreteCosseratMapping.cpp - src/mapping/DiscreteDynamicCosseratMapping.cpp - src/mapping/ProjectionEngine.cpp - src/mapping/DifferenceMultiMapping.cpp - src/mapping/RigidDistanceMapping.cpp - src/forcefield/BeamHookeLawForceField.cpp - src/forcefield/BeamPlasticLawForceField.cpp - src/forcefield/CosseratInternalActuation.cpp - src/constraint/CosseratSlidingConstraint.cpp - src/mapping/LegendrePolynomialsMapping.cpp + ${SRC_ROOT_DIR}/initCosseratPlugin.cpp + ${SRC_ROOT_DIR}/mapping/BaseCosserat.cpp + ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.cpp + ${SRC_ROOT_DIR}/mapping/DiscreteDynamicCosseratMapping.cpp + ${SRC_ROOT_DIR}/mapping/ProjectionEngine.cpp + ${SRC_ROOT_DIR}/mapping/DifferenceMultiMapping.cpp + ${SRC_ROOT_DIR}/mapping/RigidDistanceMapping.cpp + ${SRC_ROOT_DIR}/engine/PointsManager.cpp + ${SRC_ROOT_DIR}/forcefield/BeamHookeLawForceField.cpp + ${SRC_ROOT_DIR}/forcefield/BeamPlasticLawForceField.cpp + ${SRC_ROOT_DIR}/forcefield/CosseratInternalActuation.cpp + ${SRC_ROOT_DIR}/constraint/CosseratSlidingConstraint.cpp + ${SRC_ROOT_DIR}/mapping/LegendrePolynomialsMapping.cpp + ${SRC_ROOT_DIR}/constraint/CosseratNeedleSlidingConstraint.cpp ) - -set(README_FILE "Cosserat.txt" ) - -find_package(SoftRobots QUIET) +sofa_find_package(SoftRobots QUIET) if(SoftRobots_FOUND) list(APPEND HEADER_FILES - src/constraint/CosseratActuatorConstraint.h - src/constraint/CosseratActuatorConstraint.inl -# src/constraint/CosseratUnilateralInteractionConstraint.h -# src/constraint/CosseratUnilateralInteractionConstraint.inl - - src/constraint/CosseratNeedleSlidingConstraint.h - src/constraint/CosseratNeedleSlidingConstraint.inl - src/constraint/QPSlidingConstraint.h - src/constraint/QPSlidingConstraint.inl + ${SRC_ROOT_DIR}/constraint/CosseratActuatorConstraint.h + ${SRC_ROOT_DIR}/constraint/CosseratActuatorConstraint.inl +# ${SRC_ROOT_DIR}/constraint/CosseratUnilateralInteractionConstraint.h +# ${SRC_ROOT_DIR}/constraint/CosseratUnilateralInteractionConstraint.inl + + + ${SRC_ROOT_DIR}/constraint/QPSlidingConstraint.h + ${SRC_ROOT_DIR}/constraint/QPSlidingConstraint.inl ) list(APPEND SOURCE_FILES - src/constraint/CosseratActuatorConstraint.cpp - src/constraint/CosseratNeedleSlidingConstraint.cpp -# src/constraint/CosseratUnilateralInteractionConstraint.cpp - src/constraint/QPSlidingConstraint.cpp + ${SRC_ROOT_DIR}/constraint/CosseratActuatorConstraint.cpp + + #${SRC_ROOT_DIR}/constraint/CosseratUnilateralInteractionConstraint.cpp + ${SRC_ROOT_DIR}/constraint/QPSlidingConstraint.cpp ) endif() file(GLOB_RECURSE RESOURCE_FILES "*.md" "*.psl" "*.py" "*.pyscn" "*.py3scn" "*.scn" "*.ah") -IF(WIN32) - add_definitions(-D_WINSOCKAPI_) -ENDIF(WIN32) +if(WIN32) + add_definitions(-D_WINSOCKAPI_) +endif() add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ${DOC_FILES} ${RESOURCE_FILES} ) - -target_include_directories(${PROJECT_NAME} PUBLIC "$") -target_include_directories(${PROJECT_NAME} PUBLIC "$") -target_include_directories(${PROJECT_NAME} PUBLIC "$") - -set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DSOFA_BUILD_COSSERAT") -set_target_properties(${PROJECT_NAME} PROPERTIES DEBUG_POSTFIX "_d") -target_link_libraries(${PROJECT_NAME} Sofa.Helper SofaCore SofaConstraint SofaBaseMechanics SofaUserInteraction SofaRigid SofaMiscMapping SofaSparseSolver SofaEngine) +target_link_libraries(${PROJECT_NAME} + Sofa.Component.Constraint.Lagrangian.Model + Sofa.Component.StateContainer + Sofa.Component.Mapping.NonLinear + Sofa.GL + Sofa.Component.Topology.Container.Dynamic +) if(Sofa.GL_FOUND) target_link_libraries(${PROJECT_NAME} Sofa.GL) @@ -124,29 +106,31 @@ endif() if(SoftRobots_FOUND) target_link_libraries(${PROJECT_NAME} SoftRobots) - message("-- Found dependency : 'SoftRobots' plugin .") + message("-- Found dependency: 'SoftRobots' plugin.") endif() sofa_install_pythonscripts(PLUGIN_NAME ${PROJECT_NAME} PYTHONSCRIPTS_SOURCE_DIR "python") -find_file(SofaPython3Tools NAMES "SofaPython3/lib/cmake/SofaPython3/SofaPython3Tools.cmake") +find_file(SofaPython3Tools NAMES "SofaPython3/lib/cmake/SofaPython3/SofaPython3Tools.cmake") if(SofaPython3Tools) - message("-- Found SofaPython3Tools. Python3 packages will be installed.") + message("-- Found SofaPython3Tools.") include(${SofaPython3Tools}) - set(SP3_PYTHON_PACKAGES_DIRECTORY "python3/site-packages") else() # try again with the find_package mechanism find_package(SofaPython3 QUIET) endif() - if(SofaPython3Tools OR SofaPython3_FOUND) - SP3_add_python_package( - SOURCE_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/python3/cosserat - TARGET_DIRECTORY cosserat - ) - message("-- Found SofaPython3_FOUND. Python3 packages will be installed.") + message("-- Python3 packages will be installed.") + set(SP3_PYTHON_PACKAGES_DIRECTORY "python3/site-packages") + # SP3_add_python_package( + # SOURCE_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/python3/cosserat + # TARGET_DIRECTORY cosserat + # ) + add_subdirectory(${SRC_ROOT_DIR}/Binding) endif() +#add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/src/CosseratPlugin/Binding) + ## Install rules for the library and headers; CMake package configurations files sofa_create_package_with_targets( PACKAGE_NAME ${PROJECT_NAME} @@ -157,15 +141,11 @@ sofa_create_package_with_targets( RELOCATABLE "plugins" ) -# find_package(GTest CONFIG QUIET) -# if (NOT GTest_FOUND) -# # This find_package needs to be executed at MOST once, else it will fails on the second call -# find_package(GMock QUIET) -# endif () - -option(COSSERAT_BUILD_TESTS "Build unit tests" OFF ) -if(COSSERAT_BUILD_TESTS) - add_subdirectory(Tests) +# Tests +# If SOFA_BUILD_TESTS exists and is OFF, then these tests will be auto-disabled +cmake_dependent_option(COSSERATPLUGIN_BUILD_TESTS "Compile the tests" OFF "SOFA_BUILD_TESTS OR NOT DEFINED SOFA_BUILD_TESTS" OFF) +if(COSSERATPLUGIN_BUILD_TESTS) + add_subdirectory(tests) endif() include(cmake/packaging.cmake) diff --git a/CosseratPluginConfig.cmake.in b/CosseratPluginConfig.cmake.in index 22ab742e..312e77a4 100644 --- a/CosseratPluginConfig.cmake.in +++ b/CosseratPluginConfig.cmake.in @@ -1,7 +1,23 @@ -# CMake package configuration file for the CosseratPlugin plugin +# CMake package configuration file for the plugin @PROJECT_NAME@ +@PACKAGE_GUARD@ @PACKAGE_INIT@ -find_package(SofaFramework REQUIRED) +set(COSSERATPLUGIN_HAVE_SOFA_GL @COSSERATPLUGIN_HAVE_SOFA_GL@) +set(COSSERATPLUGIN_HAVE_SOFTROBOTS @COSSERATPLUGIN_HAVE_SOFTROBOTS@) -check_required_components(CosseratPlugin) +find_package(Sofa.Component.Constraint.Lagrangian.Model QUIET REQUIRED) +find_package(Sofa.Component.StateContainer QUIET REQUIRED) +find_package(Sofa.Component.Mapping.NonLinear QUIET REQUIRED) +find_package(Sofa.GL QUIET REQUIRED) +find_package(Sofa.Component.Topology.Container.Dynamic QUIET REQUIRED) + +if(COSSERATPLUGIN_HAVE_SOFTROBOTS) + find_package(SoftRobots QUIET REQUIRED) +endif() + +if(NOT TARGET @PROJECT_NAME@) + include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") +endif() + +check_required_components(@PROJECT_NAME@) diff --git a/cmake/environment.cmake b/cmake/environment.cmake index c24e22c9..ea3089d5 100644 --- a/cmake/environment.cmake +++ b/cmake/environment.cmake @@ -1,6 +1,6 @@ # CMake modules path, for our FindXXX.cmake modules list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/Modules) +list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules) list(APPEND CMAKE_PREFIX_PATH ${PROJECT_BINARY_DIR}) list(APPEND CMAKE_PREFIX_PATH ${PROJECT_BINARY_DIR}/extlibs) @@ -9,9 +9,9 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) set(CMAKE_BUILD_TYPE "Release") endif() -## Force default install prefix -if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT OR NOT "${PROJECT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}") - set(CMAKE_INSTALL_PREFIX "${PROJECT_BINARY_DIR}/install/${PROJECT_NAME}" CACHE PATH "Install path prefix, prepended onto install directories." FORCE) +## Force default install prefix if building out-of-tree +if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT AND NOT "${PROJECT_NAME}" STREQUAL "Sofa") + set(CMAKE_INSTALL_PREFIX "${CMAKE_BINARY_DIR}/install/${PROJECT_NAME}" CACHE PATH "Install path prefix, prepended onto install directories." FORCE) endif() message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") @@ -23,9 +23,9 @@ if(WIN32) else() set(LIBRARY_OUTPUT_DIRECTORY ${ARCHIVE_OUTPUT_DIRECTORY}) endif() -set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${ARCHIVE_OUTPUT_DIRECTORY}) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${RUNTIME_OUTPUT_DIRECTORY}) -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/${LIBRARY_OUTPUT_DIRECTORY}) +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${ARCHIVE_OUTPUT_DIRECTORY}) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${RUNTIME_OUTPUT_DIRECTORY}) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/${LIBRARY_OUTPUT_DIRECTORY}) ## RPATH if(UNIX) diff --git a/doc/Presentation_plasticity_theory.pdf b/doc/Presentation_plasticity_theory.pdf deleted file mode 100644 index 7b52e607..00000000 Binary files a/doc/Presentation_plasticity_theory.pdf and /dev/null differ diff --git a/python3/cosserat/__init__.py b/python3/cosserat/__init__.py index 8de08b16..2f0a6787 100644 --- a/python3/cosserat/__init__.py +++ b/python3/cosserat/__init__.py @@ -9,7 +9,7 @@ The library can be used with scenes written in python3 Contents of the library -********************** +********************** .. autosummary:: :toctree: _autosummary @@ -18,8 +18,9 @@ cosserat.actuators cosserat.inverse cosserat.testScene + cosserat.needle """ -__all__ = ["cosseratObject", "nonLinearCosserat", "actuators", "scenes", "needle", "inverse", "usefulFunctions"] +__all__ = ["utils","cosseratObject", "nonLinearCosserat", "actuators", "scenes", "needle", "inverse", "usefulFunctions", "createFemRegularGrid"] diff --git a/python3/cosserat/actuators/directFingerActuator.py b/python3/cosserat/actuators/directFingerActuator.py index 4fa03c24..e9a89947 100644 --- a/python3/cosserat/actuators/directFingerActuator.py +++ b/python3/cosserat/actuators/directFingerActuator.py @@ -1,9 +1,14 @@ # -*- coding: utf-8 -*- -"""Basic scene using Cosserat in SofaPython3. -Based on the work done with SofaPython. See POEMapping.pyscn. +"""_summary_ Basic scene using Cosserat in SofaPython3. + The Finger is modeled usind FEM modèle while de cable is modeled using cosserat theory. + The link between these two meachanical models is constraint based using Lagrangian Multiplier + +Returns: + _type_: _description_ """ + __authors__ = "younesssss" __contact__ = "adagolodjo@protonmail.com, yinoussa.adagolodjo@inria.fr" __version__ = "1.0.0" @@ -14,9 +19,11 @@ import Sofa import os from splib3.numerics import Quat +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry +from cosserat.cosseratObject import Cosserat # from stlib3.scene import Node -path = os.path.dirname(os.path.abspath(__file__))+'/../mesh/' +path = f'{os.path.dirname(os.path.abspath(__file__))}/../mesh/' class Animation(Sofa.Core.Controller): @@ -34,7 +41,6 @@ def __init__(self, *args, **kwargs): return def _extracted_from_onKeypressedEvent_10(self, qOld, posA, angularRate): - qNew = Quat.createFromEuler([0., angularRate, 0.], 'ryxz') qNew.normalize() qNew.rotateFromQuat(qOld) @@ -51,11 +57,6 @@ def onKeypressedEvent(self, event): self._extracted_from_onKeypressedEvent_10( self, qOld, posA, self.angularRate) - # qNew = Quat.createFromEuler([0., self.angularRate, 0.], 'ryxz') - # qNew.normalize() - # qNew.rotateFromQuat(qOld) - # for i in range(0, 4): - # posA[0][i+3] = qNew[i] if ord(key) == 21: # down with self.rigidBaseMO.rest_position.writeable() as posA: @@ -66,13 +67,6 @@ def onKeypressedEvent(self, event): self._extracted_from_onKeypressedEvent_10( qOld, posA, -self.angularRate) - # qNew = Quat.createFromEuler( - # [0., -self.angularRate, 0.], 'ryxz') - # qNew.normalize() - # qNew.rotateFromQuat(qOld) - # for i in range(0, 4): - # posA[0][i+3] = qNew[i] - if ord(key) == 18: # left with self.rigidBaseMO.rest_position.writeable() as posA: posA[0][0] -= self.rate @@ -84,19 +78,15 @@ def onKeypressedEvent(self, event): def createScene(rootNode): from stlib3.scene import MainHeader - # from stlib3.physics.collision import CollisionMesh - MainHeader(rootNode, plugins=["SoftRobots", "SofaSparseSolver", 'SofaDeformable', 'SofaEngine', - 'SofaImplicitOdeSolver', 'SofaLoader', 'SofaSimpleFem', "SofaPreconditioner", - "SofaOpenglVisual", "CosseratPlugin", "SofaConstraint"], + MainHeader(rootNode, plugins=pluginList, repositoryPaths=[os.getcwd()]) rootNode.addObject( - 'VisualStyle', displayFlags='showVisualModels showInteractionForceFields showWireframe') + 'VisualStyle', displayFlags='showVisualModels showInteractionForceFields showWireframe showMechanicalMappings') rootNode.addObject('FreeMotionAnimationLoop') rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", maxIterations="500", printLog="0") - # ContactHeader(rootNode, alarmDistance=2.5, contactDistance=2, frictionCoef=0.08) gravity = [0, 0, 0] rootNode.gravity.value = gravity @@ -112,8 +102,8 @@ def createScene(rootNode): finger.addObject('SparseLDLSolver', name='preconditioner') # Add a componant to load a VTK tetrahedral mesh and expose the resulting topology in the scene . - finger.addObject('MeshVTKLoader', name='loader', filename=path + 'finger.vtk', translation="-17.5 -12.5 7.5", - rotation="0 180 0") + finger.addObject('MeshVTKLoader', name='loader', filename=path + + 'finger.vtk', translation="-17.5 -12.5 7.5", rotation="0 180 0") finger.addObject('TetrahedronSetTopologyContainer', src='@loader', name='container') finger.addObject('TetrahedronSetTopologyModifier') @@ -144,26 +134,14 @@ def createScene(rootNode): showIndices="1") femPoints.addObject('BarycentricMapping') - ########################################## - # Finger auto-Collision # - ########################################## - # CollisionMesh(finger, - # surfaceMeshFileName="mesh/fingerCollision_part1.stl", - # name="CollisionMeshAuto1", translation="-17.5 -12.5 7.5", rotation="0 180 0", collisionGroup=[1]) - # - # CollisionMesh(finger, - # surfaceMeshFileName="mesh/fingerCollision_part2.stl", - # name="CollisionMeshAuto2", translation="-17.5 -12.5 7.5", rotation="0 180 0", collisionGroup=[2]) - finger.addObject('LinearSolverConstraintCorrection') ########################################## # Visualization # ########################################## fingerVisu = finger.addChild('visu') - fingerVisu.addObject( - 'MeshSTLLoader', filename=path+"finger.stl", name="loader", translation="-17.5 -12.5 7.5", - rotation="0 180 0") + fingerVisu.addObject('MeshSTLLoader', filename=f'{path}finger.stl', + name="loader", translation="-17.5 -12.5 7.5", rotation="0 180 0") fingerVisu.addObject('OglModel', src="@loader", template='ExtVec3f', color="0.0 0.7 0.7") fingerVisu.addObject('BarycentricMapping') @@ -177,72 +155,15 @@ def createScene(rootNode): cableNode.addObject('SparseLUSolver', name='solver') cableNode.addObject('GenericConstraintCorrection') - # ############### - # RigidBase - ############### - rigidBaseNode = cableNode.addChild('rigidBase') - RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', name="RigidBaseMO", - position="0 0 0 0 0 0 1", showObject='1', showObjectScale='5.') - rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness="50000", - angularStiffness="50000", external_points="0", mstate="@RigidBaseMO", points="0", - template="Rigid3d") + beamGeometrie = {'init_pos': [0., 0., 0.], 'tot_length': 81, 'nbSectionS': 12, + 'nbFramesF': 30, 'buildCollisionModel': 0, 'beamMass': 0.} + cosserat = cableNode.addChild(Cosserat(parent=cableNode, cosseratGeometry=beamGeometrie, radius=0.5, + useCollisionModel=True, name="cosserat", youngModulus=5e6, poissonRatio=0.4)) + + cableNode.addObject(Animation(cosserat.rigidBaseNode.RigidBaseMO, + cosserat.cosseratCoordinateNode.cosseratCoordinateMO)) + mappedFrameNode = cosserat.cosseratFrame - ############### - # Rate of angular Deformation (2 sections) - ############### - position = [[0., 0., 0.], [0., 0., 0.], [0., 0., 0.], - [0., 0., 0.], [0., 0., 0.], [0., 0., 0.]] - longeur = [15, 15, 15, 15, 6, 15] # beams size - rateAngularDeformNode = cableNode.addChild('rateAngularDeform') - rateAngularDeformMO = rateAngularDeformNode.addObject('MechanicalObject', template='Vec3d', - name='rateAngularDeformMO', position=position, - showIndices="1") - BeamHookeLawForce = rateAngularDeformNode.addObject('BeamHookeLawForceField', crossSectionShape='circular', - length=longeur, radius='0.50', youngModulus='5e6') - - ################################ - # Animation (to move the dofs) # - ################################ - animate = Animation(RigidBaseMO, rateAngularDeformMO) - rootNode.addObject(animate) - - ############## - # Frames # - ############## - frames = [ - "0.0 0 0 0 0 0 1 5 0 0 0 0 0 1 10.0 0 0 0 0 0 1 15.0 0 0 0 0 0 1 20.0 0 0 0 0 0 1 " - "30.0 0 0 0 0 0 1 35.0 0 0 0 0 0 1 40.0 0 0 0 0 0 1 45.0 0 0 0 0 0 1 55.0 0 0 0 0 0 1 " - "60.0 0 0 0 0 0 1 66.0 0 0 0 0 0 1 71.0 0 0 0 0 0 1 76.0 0 0 0 0 0 1 81.0 0 0 0 0 0 1"] - # the node of the frame needs to inherit from rigidBaseMO and rateAngularDeform - mappedFrameNode = rigidBaseNode.addChild('MappedFrames') - rateAngularDeformNode.addChild(mappedFrameNode) - framesMO = mappedFrameNode.addObject( - 'MechanicalObject', template='Rigid3d', name="FramesMO", position=frames, showObject='1', showObjectScale='1') - - # The mapping has two inputs: RigidBaseMO and rateAngularDeformMO - # one output: FramesMO - inputMO = rateAngularDeformMO.getLinkPath() - inputMO_rigid = RigidBaseMO.getLinkPath() - outputMO = framesMO.getLinkPath() - - curv_abs_input = '0 15 30 45 60 66 81' - curv_abs_output = '0.0 5 10 15 20 30 35 40 45 55 60 66 71 76 81' - # mappedFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=curv_abs_input, - # curv_abs_output=curv_abs_output, input1=inputMO, input2=inputMO_rigid, - # output=outputMO, debug='0', max=2.e-3, deformationAxis=1) - mappedFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=curv_abs_input, - curv_abs_output=curv_abs_output, input1=inputMO, input2=inputMO_rigid, - output=outputMO, debug='0', max=6.e-2, deformationAxis=1, nonColored="0", radius=5) - - # actuators = mappedFrameNode.addObject('actuators') - # actuator0 = actuators.addObject('SlidingActuator', name="SlidingActuator0", template='Rigid3d', - # direction='0 0 0 1 0 0', indices=1, maxForce='100000', minForce='-30000') - cable_position = [[0.0, 0.0, 0.0], [5.0, 0.0, 0.0], [10.0, 0.0, 0.0], [15.0, 0.0, 0.0], [20.0, 0.0, 0.0], - [30.0, 0.0, 0.0], [35.0, 0.0, 0.0], [ - 40.0, 0.0, 0.0], [45.0, 0.0, 0.0], - [55.0, 0.0, 0.0], [60.0, 0.0, 0.0], [66.0, 0.0, 0.0], [ - 71.0, 0.0, 0.0], [76.0, 0.0, 0.0], - [81.0, 0.0, 0.0]] # This create a new node in the scene. This node is appended to the finger's node. slidingPoint = mappedFrameNode.addChild('slidingPoint') @@ -250,7 +171,7 @@ def createScene(rootNode): # mechanical modelling. In the case of a cable it is a set of positions specifying # the points where the cable is passing by. slidingPointMO = slidingPoint.addObject('MechanicalObject', name="cablePos", - position=cable_position, showObject="1", showIndices="0") + position=cosserat.frames3D, showObject="1", showIndices="0") slidingPoint.addObject('IdentityMapping') mappedPointsNode = slidingPoint.addChild('MappedPoints') diff --git a/python3/cosserat/cosserat-tuto.html b/python3/cosserat/cosserat-tuto.html new file mode 100644 index 00000000..41446624 --- /dev/null +++ b/python3/cosserat/cosserat-tuto.html @@ -0,0 +1,115 @@ + + + + + + + tripod-tuto + + + + + +

+ + +
    +

  • you have installed SOFA with the STLIB and SoftRobots plugins.

    • +

  • you have basic knowledge of the Python programming language. If this is not the case you can go to PythonTutorials.

    • +

  • you have basic knowledge of scene modelling with SOFA. If not, please complete the FirstStep tutorial first.

    • +
+ + + + \ No newline at end of file diff --git a/python3/cosserat/cosserat-tuto.py b/python3/cosserat/cosserat-tuto.py new file mode 100644 index 00000000..b8788556 --- /dev/null +++ b/python3/cosserat/cosserat-tuto.py @@ -0,0 +1,2 @@ +def createScene(rootNode): + return rootNode diff --git a/python3/cosserat/cosseratObject.py b/python3/cosserat/cosseratObject.py index 2e9956ae..92578cfe 100644 --- a/python3/cosserat/cosseratObject.py +++ b/python3/cosserat/cosseratObject.py @@ -11,24 +11,15 @@ # from dataclasses import dataclass import Sofa +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry from splib3.numerics import Quat +from cosserat.utils import addEdgeCollision, addPointsCollision -from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry cosserat_config = {'init_pos': [0., 0., 0.], 'tot_length': 6, 'nbSectionS': 6, 'nbFramesF': 12, 'buildCollisionModel': 1, 'beamMass': 0.22} # @dataclass -def addEdgeCollision(parentNode, position3D, edges): - collisInstrumentCombined = parentNode.addChild('collisInstrumentCombined') - collisInstrumentCombined.addObject('EdgeSetTopologyContainer', name="collisEdgeSet", position=position3D, - edges=edges) - collisInstrumentCombined.addObject('EdgeSetTopologyModifier', name="collisEdgeModifier") - collisInstrumentCombined.addObject('MechanicalObject', name="CollisionDOFs") - collisInstrumentCombined.addObject('LineCollisionModel', bothSide="1", group='2') - collisInstrumentCombined.addObject('PointCollisionModel', bothSide="1", group='2') - collisInstrumentCombined.addObject('IdentityMapping', name="mapping") - return collisInstrumentCombined class Cosserat(Sofa.Prefab): @@ -49,40 +40,59 @@ class Cosserat(Sofa.Prefab): Cosserat Mapping // it allow the transfer from the local to the global frame } """ - properties = [ - {'name': 'name', 'type': 'string', 'help': 'Node name', 'default': 'Cosserat'}, + prefabParameters = [ + {'name': 'name', 'type': 'string', + 'help': 'Node name', 'default': 'Cosserat'}, {'name': 'position', 'type': 'Rigid3d::VecCoord', 'help': 'Cosserat base position', 'default': [[0., 0., 0., 0, 0, 0, 1.]]}, - {'name': 'translation', 'type': 'Vec3d', 'help': 'Cosserat base Rotation', 'default': [0., 0., 0.]}, - {'name': 'rotation', 'type': 'Vec3d', 'help': 'Cosserat base Rotation', 'default': [0., 0., 0.]}, - {'name': 'youngModulus', 'type': 'double', 'help': 'Beam Young modulus', 'default': 1.e6}, - {'name': 'poissonRatio', 'type': 'double', 'help': 'Beam poisson ratio', 'default': 0.4}, - {'name': 'shape', 'type': 'string', 'help': 'beam section', 'default': "circular"}, - {'name': 'radius', 'type': 'double', 'help': 'the radius in case of circular section', 'default': 1.0}, - {'name': 'length_Y', 'type': 'double', 'help': 'the radius in case of circular section', 'default': 1.0}, - {'name': 'length_Z', 'type': 'double', 'help': 'the radius in case of circular section', 'default': 1.0}, + {'name': 'translation', 'type': 'Vec3d', + 'help': 'Cosserat base Rotation', 'default': [0., 0., 0.]}, + {'name': 'rotation', 'type': 'Vec3d', + 'help': 'Cosserat base Rotation', 'default': [0., 0., 0.]}, + {'name': 'youngModulus', 'type': 'double', + 'help': 'Beam Young modulus', 'default': 1.e6}, + {'name': 'poissonRatio', 'type': 'double', + 'help': 'Beam poisson ratio', 'default': 0.4}, + {'name': 'shape', 'type': 'string', + 'help': 'beam section', 'default': "circular"}, + {'name': 'radius', 'type': 'double', + 'help': 'the radius in case of circular section', 'default': 0.02}, + {'name': 'length_Y', 'type': 'double', + 'help': 'the radius in case of circular section', 'default': 1.0}, + {'name': 'length_Z', 'type': 'double', + 'help': 'the radius in case of circular section', 'default': 1.0}, {'name': 'rayleighStiffness', 'type': 'double', 'help': 'Rayleigh damping - stiffness matrix coefficient', 'default': 0.0}, {'name': 'attachingToLink', 'type': 'string', 'help': 'a rest shape force field will constraint the object ' - 'to follow arm position', 'default': '0'}, + 'to follow arm position', 'default': '1'}, {'name': 'showObject', 'type': 'string', 'help': ' Draw object arrow ', 'default': '0'}] def __init__(self, *args, **kwargs): Sofa.Prefab.__init__(self, *args, **kwargs) self.cosseratGeometry = kwargs['cosseratGeometry'] self.beamMass = self.cosseratGeometry['beamMass'] - self.parent = kwargs.get('parent', None) + self.parent = kwargs.get('parent') + self.useInertiaParams = False + self.radius = kwargs.get('radius', ) if self.parent.hasObject("EulerImplicitSolver") is False: + print('The code does not have parent EulerImplicite') self.solverNode = self.addSolverNode() else: self.solverNode = self.parent + if 'inertialParams' in kwargs: + self.useInertiaParams = True + self.inertialParams = kwargs['inertialParams'] + self.rigidBaseNode = self.addRigidBaseNode() [positionS, curv_abs_inputS, longeurS, framesF, curv_abs_outputF, self.frames3D] = \ BuildCosseratGeometry(self.cosseratGeometry) - self.cosseratCoordinateNode = self.addCosseratCoordinate(positionS, longeurS) - self.cosseratFrame = self.addCosseratFrame(framesF, curv_abs_inputS, curv_abs_outputF) + + self.cosseratCoordinateNode = self.addCosseratCoordinate( + positionS, longeurS) + self.cosseratFrame = self.addCosseratFrame( + framesF, curv_abs_inputS, curv_abs_outputF) # print(f'=== > {curv_abs_inputS}') def init(self): @@ -92,70 +102,103 @@ def addCollisionModel(self): tab_edges = buildEdges(self.frames3D) return addEdgeCollision(self.cosseratFrame, self.frames3D, tab_edges) + def addPointCollisionModel(self, nodeName='CollisionPoints'): + tab_edges = buildEdges(self.frames3D) + return addPointsCollision(self.cosseratFrame, self.frames3D, tab_edges, nodeName) + + def addSlidingPoints(self): + slidingPoint = self.cosseratFrame.addChild('slidingPoint') + slidingPoint.addObject('MechanicalObject', name="slidingPointMO", position=self.frames3D, + showObject="0", showIndices="0") + slidingPoint.addObject('IdentityMapping') + return slidingPoint + + def addSlidingPointsWithContainer(self): + slidingPoint = self.cosseratFrame.addChild('slidingPoint') + container = slidingPoint.addObject("PointSetTopologyContainer") + modifier = slidingPoint.addObject("PointSetTopologyModifier") + slidingPoint.addObject('MechanicalObject', name="slidingPointMO", position=self.frames3D, + showObject="1", showIndices="0") + slidingPoint.addObject('IdentityMapping') + return slidingPoint + def addSolverNode(self): solverNode = self.addChild('solverNode') - solverNode.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.1') - solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness="0.2", rayleighMass='0.1') + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") solverNode.addObject('GenericConstraintCorrection') return solverNode def addRigidBaseNode(self): rigidBaseNode = self.addChild('rigidBase') - trans = [t for t in self.translation.value] - rot = [r for r in self.rotation.value] + # trans = [t for t in self.translation.value] + trans = list(self.translation.value) + rot = list(self.rotation.value) # @todo converter - positions = [] - for pos in self.position.value: - _pos = [p for p in pos] - positions.append(_pos) + positions = [list(pos) for pos in self.position.value] - rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', name="RigidBaseMO", - showObjectScale=0.2, translation=trans, position=positions, - rotation=rot, showObject=int(self.showObject.value)) + rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', name="RigidBaseMO", showObjectScale=0.2, + translation=trans, position=positions, rotation=rot, + showObject=int(self.showObject.value)) # one can choose to set this to false and directly attach the beam base # to a control object in order to be able to drive it. if int(self.attachingToLink.value): print("Adding the rest shape to the base") - rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', - stiffness=5000, angularStiffness=5000, external_points=0, - mstate="@RigidBaseMO", points=0, template="Rigid3d") + rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness=1e8, angularStiffness=1.e8, + external_points=0, mstate="@RigidBaseMO", points=0, template="Rigid3d") return rigidBaseNode - def addCosseratCoordinate(self, positionS, longeurS): + def addCosseratCoordinate(self, bendingStates, listOfSectionsLength): cosseratCoordinateNode = self.addChild('cosseratCoordinate') cosseratCoordinateNode.addObject('MechanicalObject', template='Vec3d', name='cosseratCoordinateMO', - position=positionS, + position=bendingStates, showIndices=0) - cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, - length=longeurS, youngModulus=self.youngModulus.value, - poissonRatio=self.poissonRatio.value, - radius=self.radius.value, - rayleighStiffness=self.rayleighStiffness.value, - lengthY=self.length_Y.value, lengthZ=self.length_Z.value) + + if self.useInertiaParams is False: + cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, + length=listOfSectionsLength, radius=self.radius.value, + youngModulus=self.youngModulus.value, poissonRatio=self.poissonRatio.value, + rayleighStiffness=self.rayleighStiffness.value, + lengthY=self.length_Y.value, lengthZ=self.length_Z.value) + else: + GA = self.inertialParams['GA'] + GI = self.inertialParams['GI'] + EA = self.inertialParams['EA'] + EI = self.inertialParams['EI'] + print(f'{GA}') + cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, + length=listOfSectionsLength, radius=self.radius.value, useInertiaParams=True, + GI=GI, GA=GA, EI=EI, EA=EA, rayleighStiffness=self.rayleighStiffness.value, + lengthY=self.length_Y.value, lengthZ=self.length_Z.value) return cosseratCoordinateNode def addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): - cosseratInSofaFrameNode = self.rigidBaseNode.addChild('cosseratInSofaFrameNode') + cosseratInSofaFrameNode = self.rigidBaseNode.addChild( + 'cosseratInSofaFrameNode') self.cosseratCoordinateNode.addChild(cosseratInSofaFrameNode) - framesMO = cosseratInSofaFrameNode.addObject('MechanicalObject', template='Rigid3d', - name="FramesMO", position=framesF, - showObject=int(self.showObject.value), showObjectScale=0.1) + framesMO = cosseratInSofaFrameNode.addObject( + 'MechanicalObject', template='Rigid3d', name="FramesMO", position=framesF, showObject=0, showObjectScale=0.1) if self.beamMass != 0.: - cosseratInSofaFrameNode.addObject('UniformMass', totalMass=self.beamMass, showAxisSizeFactor='0') + cosseratInSofaFrameNode.addObject( + 'UniformMass', totalMass=self.beamMass, showAxisSizeFactor='0') + cosseratInSofaFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=curv_abs_inputS, curv_abs_output=curv_abs_outputF, name='cosseratMapping', input1=self.cosseratCoordinateNode.cosseratCoordinateMO.getLinkPath(), input2=self.rigidBaseNode.RigidBaseMO.getLinkPath(), - output=framesMO.getLinkPath(), debug=0, radius=0) + output=framesMO.getLinkPath(), debug=0, radius=self.radius.value) if self.beamMass != 0.: self.solverNode.addObject('MechanicalMatrixMapper', template='Vec3,Rigid3', object1=self.cosseratCoordinateNode.cosseratCoordinateMO.getLinkPath(), object2=self.rigidBaseNode.RigidBaseMO.getLinkPath(), nodeToParse=cosseratInSofaFrameNode.getLinkPath()) + return cosseratInSofaFrameNode @@ -165,26 +208,29 @@ def createScene(rootNode): 'SofaExporter']]) rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' 'hideBoundingCollisionModels hireForceFields ' - 'hideInteractionForceFields hideWireframe') + 'hideInteractionForceFields hideWireframe showMechanicalMappings') rootNode.findData('dt').value = 0.01 rootNode.findData('gravity').value = [0., -9.81, 0.] rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + rootNode.addObject('GenericConstraintSolver', + tolerance=1e-5, maxIterations=5e2) rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") solverNode = rootNode.addChild('solverNode') - solverNode.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.1') - solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness="0.2", rayleighMass='0.1') + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") solverNode.addObject('GenericConstraintCorrection') cosserat = solverNode.addChild( - Cosserat(parent=solverNode, cosseratGeometry=cosserat_config, name="cosserat", radius=0.2)) + Cosserat(parent=solverNode, cosseratGeometry=cosserat_config, name="cosserat", radius=0.15)) # use this to add the collision if the beam will interact with another object collisionModel = cosserat.addCollisionModel() - # attach force at the beam tip, + # Attach a force at the beam tip, # we can attach this force to non mechanical node thanks to the MechanicalMatrixMapper component beamFrame = cosserat.cosseratFrame beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-2, indices=12, diff --git a/python3/cosserat/createFemRegularGrid.py b/python3/cosserat/createFemRegularGrid.py index aabedcdf..579850fa 100644 --- a/python3/cosserat/createFemRegularGrid.py +++ b/python3/cosserat/createFemRegularGrid.py @@ -11,6 +11,7 @@ __copyright__ = "(c) 2021,Inria" __date__ = "March 16 2021" + def createFemCube(parentNode): FemNode = parentNode.addChild("FemNode") FemNode.addObject('VisualStyle', displayFlags='showBehaviorModels hideCollisionModels hideBoundingCollisionModels ' @@ -45,8 +46,63 @@ def createFemCube(parentNode): surfaceNode.addObject('TriangleSetTopologyContainer', name="surfContainer", src="@../../GelSurface/Container") surfaceNode.addObject('MechanicalObject', name='msSurface') surfaceNode.addObject('TriangleCollisionModel', name='surface') + surfaceNode.addObject('LineCollisionModel', name='line') + surfaceNode.addObject('BarycentricMapping') + + gelNode.addObject('LinearSolverConstraintCorrection') + + return FemNode + + +def createFemCubeWithParams(parentNode, geometry): + FemNode = parentNode.addChild("FemNode") + # FemNode.addObject('VisualStyle', displayFlags='showBehaviorModels hideCollisionModels + # hideBoundingCollisionModels ' 'showForceFields hideInteractionForceFields showWireframe') + gelVolume = FemNode.addChild("gelVolume") + gelVolume.addObject("RegularGridTopology", name="HexaTop", n=geometry.mesh, min=geometry.minVol, + max=geometry.maxVol) + cont = gelVolume.addObject("TetrahedronSetTopologyContainer", name="TetraContainer", position="@HexaTop.position") + gelVolume.addObject("TetrahedronSetTopologyModifier", name="Modifier") + gelVolume.addObject("Hexa2TetraTopologicalMapping", input="@HexaTop", output="@TetraContainer", swapping="false") + + GelSurface = FemNode.addChild("GelSurface") + GelSurface.addObject("TriangleSetTopologyContainer", name="triangleContainer", + position="@../gelVolume/HexaTop.position") + GelSurface.addObject("TriangleSetTopologyModifier", name="Modifier") + GelSurface.addObject("Tetra2TriangleTopologicalMapping", input="@../gelVolume/TetraContainer", + output="@triangleContainer", flipNormals="false") + + gelNode = FemNode.addChild("gelNode") + # gelNode.addObject('VisualStyle', displayFlags='showVisualModels hideBehaviorModels showCollisionModels ' + # 'hideMappings hideForceFields showWireframe ' + # 'showInteractionForceFields hideForceFields') + gelNode.addObject("EulerImplicitSolver", rayleighMass=geometry.rayleigh, rayleighStiffness=geometry.rayleigh) + gelNode.addObject('SparseLDLSolver', name='precond') + # gelNode.addObject('ShewchukPCGLinearSolver', name='linearSolver', iterations='500', tolerance='1.0e-14', + # preconditioners="precond") + # gelNode.addObject('SparseLDLSolver', name='precond', template='CompressedRowSparseMatrix3d') + gelNode.addObject('TetrahedronSetTopologyContainer', src="@../gelVolume/TetraContainer", name='container') + # gelNode.addObject('TetrahedronSetTopologyModifier') + gelNode.addObject('MechanicalObject', name='tetras', template='Vec3d') + gelNode.addObject('TetrahedronFEMForceField', template='Vec3d', name='FEM', method='large', + poissonRatio=geometry.poissonRatio, youngModulus=geometry.youngModulus) + # gelNode.addObject('UniformMass', totalMass='5') + gelNode.addObject('BoxROI', name='ROI1', box=geometry.box, drawBoxes='true') + gelNode.addObject('RestShapeSpringsForceField', points='@ROI1.indices', stiffness='1e12') + + surfaceNode = gelNode.addChild("surfaceNode") + surfaceNode.addObject('TriangleSetTopologyContainer', name="surfContainer", + src="@../../GelSurface/triangleContainer") + surfaceNode.addObject('MechanicalObject', name='msSurface') + surfaceNode.addObject('TriangleCollisionModel', name='surface') + surfaceNode.addObject('LineCollisionModel', name='line') surfaceNode.addObject('BarycentricMapping') + visu = surfaceNode.addChild("visu") + + visu.addObject("OglModel", name="Visual", src="@../surfContainer", color="0.0 0.1 0.9 0.40" ) + visu.addObject("BarycentricMapping", input="@..", output="@Visual") + # gelNode.addObject('GenericConstraintCorrection', linearSolver='@precond') gelNode.addObject('LinearSolverConstraintCorrection') return FemNode diff --git a/python3/cosserat/examples/PCS_Example1.py b/python3/cosserat/examples/PCS_Example1.py new file mode 100644 index 00000000..44a7636a --- /dev/null +++ b/python3/cosserat/examples/PCS_Example1.py @@ -0,0 +1,110 @@ +# -*- coding: utf-8 -*- +""" + Cosserat class in SofaPython3. +""" + +__authors__ = "younesssss" +__contact__ = "adagolodjo@protonmail.com" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "Jan, 17 2022" + +import Sofa +from cosserat.cosseratObject import Cosserat +from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry +from math import sqrt + +# @todo ================ Unit: N, m, Kg, Pa ================ +LegendrePolyOrder = 5 # P_{k_2}=P_{k_3} +initialStrain = [[0., 0., 0], [0., 0., 0], + [0., 0., 0], [0., 0., 0], [0., 0., 0]] + +YM = 1.0e8 +PR = 0. +rayleighStiffness = 1.e-3 # Nope +firstOrder = 1 + +EI = 1.e2 +coeff = 0 + +F1 = [0., 0., 0., 0., (coeff*1.)/sqrt(2), (coeff*1.)/sqrt(2)] # N + +# Inertia parameter +Rb = 0.02/2. # beam radius in m +length = 1. # in m +nbSection = 5 # +deltaT = 0.02 # s + +inertialParams = {'GI': 1.5708, 'GA': 3.1416e4, + 'EI': 0.7854, 'EA': 3.1416e4, 'L': length, 'Rb': Rb} +nonLinearConfig = {'init_pos': [0., 0., 0.], 'tot_length': length, 'nbSectionS': nbSection, + 'nbFramesF': 30, 'buildCollisionModel': 0, 'beamMass': 0.} + + +class ForceController(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.frames = kwargs['cosseratFrames'] + self.forceNode = kwargs['forceNode'] + self.size = nonLinearConfig['nbFramesF'] + self.applyForce = True + self.forceCoeff = coeff + # self.cosseratGeometry = kwargs['cosseratGeometry'] + + def onAnimateEndEvent(self, event): + if self.applyForce: + with self.forceNode.force.writeable() as force: + vec = [0., 0., 0., 0., (self.forceCoeff * 1.) / + sqrt(2), (self.forceCoeff * 1.) / sqrt(2)] + for i, v in enumerate(vec): + force[i] = v + # print(f' The new force: {force}') + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "+": + self.forceCoeff += 1 + print(f' The new force coeff is : {self.forceCoeff}') + elif key == "-": + self.forceCoeff -= 1 + print(f' The new force coeff is : {self.forceCoeff}') + + +def createScene(rootNode): + rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList, + ['SofaEngine', 'SofaLoader', 'SofaSimpleFem', + 'SofaExporter']]) + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hireForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.findData('dt').value = deltaT + # rootNode.findData('gravity').value = [0., -9.81, 0.] + rootNode.findData('gravity').value = [0., 0., 0.] + # rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') + # rootNode.addObject('FreeMotionAnimationLoop') + # rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") + + solverNode = rootNode.addChild('solverNode') + solverNode.addObject('EulerImplicitSolver', rayleighStiffness=rayleighStiffness, rayleighMass='0.', + firstOrder=firstOrder) + # solverNode.addObject('SparseLDLSolver', name='solver', + # template="CompressedRowSparseMatrixd") + solverNode.addObject('CGLinearSolver', tolerance=1.e-12, iterations=1000, threshold=1.e-18) + + # use this if the collision model if the beam will interact with another object + needCollisionModel = 0 + PCS_Cosserat = solverNode.addChild( + Cosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, inertialParams=inertialParams, radius=Rb, + useCollisionModel=needCollisionModel, name="cosserat", youngModulus=YM, poissonRatio=PR, + rayleighStiffness=rayleighStiffness)) + + beamFrame = PCS_Cosserat.cosseratFrame + constForce = beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-8, + indices=nonLinearConfig['nbFramesF'], force=F1) + + solverNode.addObject(ForceController( + parent=solverNode, cosseratFrames=beamFrame.FramesMO, forceNode=constForce)) + + return rootNode diff --git a/python3/cosserat/examples/PCS_Example2.py b/python3/cosserat/examples/PCS_Example2.py new file mode 100644 index 00000000..25ac0e4c --- /dev/null +++ b/python3/cosserat/examples/PCS_Example2.py @@ -0,0 +1,111 @@ +# -*- coding: utf-8 -*- +""" + Cosserat class in SofaPython3. +""" + +__authors__ = "younesssss" +__contact__ = "adagolodjo@protonmail.com" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "Jan, 17 2022" + +import Sofa +from cosserat.cosseratObject import Cosserat +from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry +from math import sqrt + +# @todo ================ Unit: N, m, Kg, Pa ================ +from splib3.numerics import Quat + +LegendrePolyOrder = 5 +# initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0]] +initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0]] + +YM = 4.015e8 +rayleighStiffness = 0.2 # Nope +forceCoeff = 100 +F1 = [0., forceCoeff, 0., 0., 0., 0.] # Nope +Rb = 0.57/2. # @todo ==> beam radius in m +length = 100. # @todo ==> beam length in m +nbSection = 30 # P_{k_2}=P_{k_3} +nbFrames = 60 +firstOrder = 1 + +inertialParams = {'GI': 3.5e7, 'GA': 1.61e8, 'EI': 3.5e7, 'EA': 1.61e8, 'L': length, 'Rb': Rb} +nonLinearConfig = {'init_pos': [0., 0., 0.], 'tot_length': length, 'nbSectionS': nbSection, + 'nbFramesF': nbFrames, 'buildCollisionModel': 0, 'beamMass': 0.} + + +class ForceController(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.frames = kwargs['cosseratFrames'] + self.forceNode = kwargs['forceNode'] + self.size = nonLinearConfig['nbFramesF'] + self.applyForce = True + self.forceCoeff = forceCoeff + # self.cosseratGeometry = kwargs['cosseratGeometry'] + + + if self.applyForce: + position = self.frames.position[self.size] # get the last rigid of the cosserat frame + orientation = Quat(position[3], position[4], position[5], position[6]) # get the orientation + # Get the force direction in order to remain orthogonal to the last section of beam + with self.forceNode.force.writeable() as force: + vec = orientation.rotate([0., self.forceCoeff, 0.]) + # print(f' The new vec is : {vec}') + for count in range(3): + force[count] = vec[count] + if self.forceCoeff < 13.1e4: + self.forceCoeff += 100 + else: + print(f' The new force coeff is : {self.forceCoeff}') + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "+": + self.forceCoeff += 10 + print(f' The new force coeff is : {self.forceCoeff}') + elif key == "-": + self.forceCoeff -= 0.2 + print(f' The new force coeff is : {self.forceCoeff}') + + +def createScene(rootNode): + rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList, + ['SofaEngine', 'SofaLoader', 'SofaSimpleFem', + 'SofaExporter']]) + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hireForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.findData('dt').value = 0.02 + # rootNode.findData('gravity').value = [0., -9.81, 0.] + rootNode.findData('gravity').value = [0., 0., 0.] + # rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') + # rootNode.addObject('FreeMotionAnimationLoop') + # rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") + + solverNode = rootNode.addChild('solverNode') + # solverNode.addObject('EulerImplicitSolver', rayleighStiffness="0.", rayleighMass='0.') + solverNode.addObject('EulerImplicitSolver', rayleighStiffness=0, rayleighMass='0.', + firstOrder=firstOrder) + solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + # solverNode.addObject('EigenSimplicialLDLT', name='solver', template="CompressedRowSparseMatrixMat3x3d" ) + + # solverNode.addObject('CGLinearSolver', tolerance=1.e-12, iterations=1000, threshold=1.e-18) + + needCollisionModel = 0 # use this if the collision model if the beam will interact with another object + PCS_Cosserat = solverNode.addChild( + Cosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, + inertialParams=inertialParams, name="cosserat", radius=Rb, youngModulus=YM)) + + beamFrame = PCS_Cosserat.cosseratFrame + + constForce = beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=0.0, + indices=nonLinearConfig['nbFramesF'], force=F1) + + solverNode.addObject(ForceController(parent=solverNode, cosseratFrames=beamFrame.FramesMO, forceNode=constForce)) + + return rootNode diff --git a/python3/cosserat/examples/example2.py b/python3/cosserat/examples/PCS_Example3.py similarity index 78% rename from python3/cosserat/examples/example2.py rename to python3/cosserat/examples/PCS_Example3.py index 95c3a02b..9bb0a235 100644 --- a/python3/cosserat/examples/example2.py +++ b/python3/cosserat/examples/PCS_Example3.py @@ -11,7 +11,7 @@ import Sofa from cosserat.cosseratObject import Cosserat -from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat +# from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry from math import sqrt @@ -20,7 +20,8 @@ LegendrePolyOrder = 5 # initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0]] -initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0]] +initialStrain = [[0., 0., 0], [0., 0., 0], + [0., 0., 0], [0., 0., 0], [0., 0., 0]] YM = 4.015e8 rayleighStiffness = 0.2 # Nope forceCoeff = 0.05 @@ -45,8 +46,11 @@ def __init__(self, *args, **kwargs): def onAnimateEndEvent(self, event): if self.applyForce: - position = self.frames.position[self.size] # get the last rigid of the cosserat frame - orientation = Quat(position[3], position[4], position[5], position[6]) # get the orientation + # get the last rigid of the cosserat frame + position = self.frames.position[self.size] + # get the orientation + orientation = Quat( + position[3], position[4], position[5], position[6]) # Get the force direction in order to remain orthogonal to the last section of beam with self.forceNode.force.writeable() as force: vec = orientation.rotate([0., self.forceCoeff, 0.]) @@ -80,17 +84,23 @@ def createScene(rootNode): rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") solverNode = rootNode.addChild('solverNode') - solverNode.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.') + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness="0.2", rayleighMass='0.') # solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") - solverNode.addObject('EigenSimplicialLDLT', name='solver', template="CompressedRowSparseMatrixMat3x3d" ) + solverNode.addObject('EigenSimplicialLDLT', name='solver', + template="CompressedRowSparseMatrixMat3x3d") # solverNode.addObject('CGLinearSolver', tolerance=1.e-12, iterations=1000, threshold=1.e-18) - needCollisionModel = 0 # use this if the collision model if the beam will interact with another object - nonLinearCosserat = solverNode.addChild( - nonCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, - name="cosserat", radius=Rb, youngModulus=YM, legendreControlPoints=initialStrain, - order=LegendrePolyOrder)) + # use this if the collision model if the beam will interact with another object + needCollisionModel = 0 + # PCS_Cosserat = solverNode.addChild( + # Cosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, + # inertialParams=inertialParams, name="cosserat", radius=Rb, youngModulus=YM)) + PCS_Cosserat = solverNode.addChild( + Cosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, + inertialParams=inertialParams, name="cosserat", radius=Rb, youngModulus=YM)) + cosseratNode = nonLinearCosserat.legendreControlPointsNode cosseratNode.addObject('MechanicalMatrixMapper', template='Vec3,Vec3', object1=cosseratNode.getLinkPath(), @@ -101,13 +111,11 @@ def createScene(rootNode): beamFrame = nonLinearCosserat.cosseratFrame constForce = beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=0.02, - indices=nonLinearConfig['nbFramesF'], force=F1) + indices=nonLinearConfig['nbFramesF'], force=F1) nonLinearCosserat = solverNode.addObject( ForceController(parent=solverNode, cosseratFrames=beamFrame.FramesMO, forceNode=constForce)) - - # # solverNode2 = rootNode.addChild('solverNode2') # # solverNode2.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.1') # # solverNode2.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") @@ -119,4 +127,4 @@ def createScene(rootNode): # beamFrame2.addObject('ConstantForceField', name='constForce', showArrowSize=0, indices=12, # force=[0., 0., 0., 0., 450., 0.]) - return rootNode \ No newline at end of file + return rootNode diff --git a/python3/cosserat/examples/example1.py b/python3/cosserat/examples/PNLS_Example1.py similarity index 80% rename from python3/cosserat/examples/example1.py rename to python3/cosserat/examples/PNLS_Example1.py index 59301577..2fc384f0 100644 --- a/python3/cosserat/examples/example1.py +++ b/python3/cosserat/examples/PNLS_Example1.py @@ -22,18 +22,20 @@ YM = 1.0e8 PR = 0. rayleighStiffness = 1.e-3 # Nope -firstOrder = 0 +firstOrder = 1 EI = 1.e2 -coeff = 1 +coeff = 0 F1 = [0., 0., 0., 0., (coeff*1.)/sqrt(2), (coeff*1.)/sqrt(2)] # N +### Inertia parameter Rb = 0.02/2. # beam radius in m -length = 1 # in m +length = 1. # in m nbSection = 5 # deltaT = 0.02 # s +inertialParams = {'GI': 1.5708, 'GA': 3.1416e4, 'EI': 0.7854, 'EA': 3.1416e4, 'L': length, 'Rb': Rb} nonLinearConfig = {'init_pos': [0., 0., 0.], 'tot_length': length, 'nbSectionS': nbSection, 'nbFramesF': 30, 'buildCollisionModel': 0, 'beamMass': 0.} @@ -72,11 +74,11 @@ def createScene(rootNode): 'SofaExporter']]) rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' 'hideBoundingCollisionModels hireForceFields ' - 'hideInteractionForceFields hideWireframe') + 'hideInteractionForceFields hideWireframe showMechanicalMappings') rootNode.findData('dt').value = deltaT # rootNode.findData('gravity').value = [0., -9.81, 0.] rootNode.findData('gravity').value = [0., 0., 0.] - rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') + # rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') # rootNode.addObject('FreeMotionAnimationLoop') # rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") @@ -90,9 +92,11 @@ def createScene(rootNode): needCollisionModel = 0 # use this if the collision model if the beam will interact with another object nonLinearCosserat = solverNode.addChild( - nonCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, - name="cosserat", radius=Rb, youngModulus=YM, legendreControlPoints=initialStrain, poissonRatio=PR, - order=LegendrePolyOrder, rayleighStiffness=rayleighStiffness)) + nonCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, inertialParams=inertialParams, + useCollisionModel=needCollisionModel, name="cosserat", radius=Rb, youngModulus=YM, + legendreControlPoints=initialStrain, poissonRatio=PR,order=LegendrePolyOrder, + rayleighStiffness=rayleighStiffness, + activatedMMM=False)) cosseratNode = nonLinearCosserat.legendreControlPointsNode cosseratNode.addObject('MechanicalMatrixMapper', template='Vec3,Vec3', object1=cosseratNode.getLinkPath(), @@ -108,16 +112,4 @@ def createScene(rootNode): nonLinearCosserat = solverNode.addObject( ForceController(parent=solverNode, cosseratFrames=beamFrame.FramesMO, forceNode=constForce)) - - # # solverNode2 = rootNode.addChild('solverNode2') - # # solverNode2.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.1') - # # solverNode2.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") - # # solverNode2.addObject('GenericConstraintCorrection') - # # cosserat2 = solverNode2.addChild(Cosserat(parent=solverNode2, cosseratGeometry=linearConfig, - # # useCollisionModel=needCollisionModel, name="cosserat2", radius=0.1)) - # - # beamFrame2 = cosserat2.cosseratFrame - # beamFrame2.addObject('ConstantForceField', name='constForce', showArrowSize=0, indices=12, - # force=[0., 0., 0., 0., 450., 0.]) - return rootNode \ No newline at end of file diff --git a/python3/cosserat/examples/PNLS_Example2.py b/python3/cosserat/examples/PNLS_Example2.py new file mode 100644 index 00000000..442183b3 --- /dev/null +++ b/python3/cosserat/examples/PNLS_Example2.py @@ -0,0 +1,120 @@ +# -*- coding: utf-8 -*- +""" + Cosserat class in SofaPython3. +""" + +__authors__ = "younesssss" +__contact__ = "adagolodjo@protonmail.com" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "Jan, 17 2022" + +import Sofa +from cosserat.cosseratObject import Cosserat +from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry +from math import sqrt + +# @todo ================ Unit: N, m, Kg, Pa ================ +from splib3.numerics import Quat + +LegendrePolyOrder = 5 +# initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0]] +initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0]] + +YM = 4.015e8 +rayleighStiffness = 0.2 # Nope +forceCoeff = 100 +F1 = [0., forceCoeff, 0., 0., 0., 0.] # Nope +Rb = 0.57/2. # @todo ==> 0.57/2. # beam radius in m +length = 100. # @todo ==> 100 # in m +nbSection = 30 # P_{k_2}=P_{k_3} +nbFrames = 60 +firstOrder = 1 + +inertialParams = {'GI': 3.5e7, 'GA': 1.61e8, 'EI': 3.5e7, 'EA': 1.61e8, 'L': length, 'Rb': Rb} +nonLinearConfig = {'init_pos': [0., 0., 0.], 'tot_length': length, 'nbSectionS': nbSection, + 'nbFramesF': nbFrames, 'buildCollisionModel': 0, 'beamMass': 0.} + + +class ForceController(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.frames = kwargs['cosseratFrames'] + self.forceNode = kwargs['forceNode'] + self.size = nonLinearConfig['nbFramesF'] + self.applyForce = True + self.forceCoeff = forceCoeff + # self.cosseratGeometry = kwargs['cosseratGeometry'] + + def onAnimateEndEvent(self, event): + if self.applyForce: + position = self.frames.position[self.size] # get the last rigid of the cosserat frame + orientation = Quat(position[3], position[4], position[5], position[6]) # get the orientation + # Get the force direction in order to remain orthogonal to the last section of beam + with self.forceNode.force.writeable() as force: + vec = orientation.rotate([0., self.forceCoeff, 0.]) + # print(f' The new vec is : {vec}') + for count in range(3): + force[count] = vec[count] + if self.forceCoeff < 13.1e4: + self.forceCoeff += 100 + else: + print(f' The new force coeff is : {self.forceCoeff}') + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "+": + self.forceCoeff += 10 + print(f' The new force coeff is : {self.forceCoeff}') + elif key == "-": + self.forceCoeff -= 0.2 + print(f' The new force coeff is : {self.forceCoeff}') + + +def createScene(rootNode): + rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList, + ['SofaEngine', 'SofaLoader', 'SofaSimpleFem', + 'SofaExporter']]) + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hireForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.findData('dt').value = 0.02 + # rootNode.findData('gravity').value = [0., -9.81, 0.] + rootNode.findData('gravity').value = [0., 0., 0.] + # rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') + # rootNode.addObject('FreeMotionAnimationLoop') + # rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") + + solverNode = rootNode.addChild('solverNode') + # solverNode.addObject('EulerImplicitSolver', rayleighStiffness="0.", rayleighMass='0.') + solverNode.addObject('EulerImplicitSolver', rayleighStiffness=0, rayleighMass='0.', + firstOrder=firstOrder) + solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + # solverNode.addObject('EigenSimplicialLDLT', name='solver', template="CompressedRowSparseMatrixMat3x3d" ) + + # solverNode.addObject('CGLinearSolver', tolerance=1.e-12, iterations=1000, threshold=1.e-18) + + needCollisionModel = 0 # use this if the collision model if the beam will interact with another object + nonLinearCosserat = solverNode.addChild( + nonCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, + name="cosserat", radius=Rb, youngModulus=YM, legendreControlPoints=initialStrain, + order=LegendrePolyOrder, inertialParams=inertialParams, + activatedMMM=True)) + cosseratNode = nonLinearCosserat.legendreControlPointsNode + cosseratNode.addObject('MechanicalMatrixMapper', template='Vec3,Vec3', + object1=cosseratNode.getLinkPath(), + object2=cosseratNode.getLinkPath(), + name='cosseratCoordinateNodeMapper', + nodeToParse=nonLinearCosserat.cosseratCoordinateNode.getLinkPath()) + + beamFrame = nonLinearCosserat.cosseratFrame + + constForce = beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-5, + indices=nonLinearConfig['nbFramesF'], force=F1) + + nonLinearCosserat = solverNode.addObject( + ForceController(parent=solverNode, cosseratFrames=beamFrame.FramesMO, forceNode=constForce)) + + return rootNode \ No newline at end of file diff --git a/python3/cosserat/examples/PNLS_Example3.py b/python3/cosserat/examples/PNLS_Example3.py new file mode 100644 index 00000000..a3169f07 --- /dev/null +++ b/python3/cosserat/examples/PNLS_Example3.py @@ -0,0 +1,147 @@ +# -*- coding: utf-8 -*- +""" + Cosserat class in SofaPython3. +""" + +__authors__ = "younesssss" +__contact__ = "adagolodjo@protonmail.com" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "Jan, 17 2022" + +import Sofa +from cosserat.cosseratObject import Cosserat +from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry +from math import sqrt +from splib3.numerics import Quat +from math import pi + +# @todo ================ Unit: N, m, Kg, Pa ================ +from splib3.numerics import Quat + +LegendrePolyOrder = 5 +# initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0]] +initialStrain = [[0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0], [0., 0., 0]] + +YM = 4.015e8 +rayleighStiffness = 0.2 # Nope +forceCoeff = 0. +F1 = [0., 0, 0., 0., 0., 0.] # Nope +Rb = 0.0176/2. # @todo ==> beam radius in m +length = 20. # @todo ==> beam length in m +nbSection = 20 # P_{k_2}=P_{k_3} +nbFrames = 60 +firstOrder = 1 + +inertialParams = {'GI': 1800, 'GA': 1e4, 'EI': 1000, 'EA': 5.137e7, 'L': length, 'Rb': Rb} +nonLinearConfig = {'init_pos': [0., 0., 0.], 'tot_length': length, 'nbSectionS': nbSection, + 'nbFramesF': nbFrames, 'buildCollisionModel': 0, 'beamMass': 0.} + + +class ForceController(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.frames = kwargs['cosseratFrames'] + self.controllerNode = kwargs['controller'] + self.size = nonLinearConfig['nbFramesF'] + self.applyForce = True + self.forceCoeff = forceCoeff + self.phase1 = True + self.phase2 = False + # self.cosseratGeometry = kwargs['cosseratGeometry'] + + def onAnimateEndEvent(self, event): + if self.applyForce: + if self.phase1: + if self.forceCoeff < 2*pi: + with self.controllerNode.position.writeable() as position: + # vec = Quat() + frames = self.frames.position[self.size] + vec = Quat(frames[3], frames[4], frames[5], frames[6]) # get the orientation + # print(f' The forceCoeff is : {self.forceCoeff}') + print(f' The vec before : {vec}') + # get the orientation + vec.rotateFromEuler([0.01, 0., 0.]) + vec.normalize() + print(f' The vec after : {vec}') + for i, v in enumerate(vec): + position[0][i+3] = v + self.forceCoeff += 0.01 + else: + self.phase2 = True + + if self.phase2: + with self.controllerNode.position.writeable() as position: + if position[0][0] > 10.: + position[0][0] -= 0.01 + print(f' The new end effector is : {position[0]}') + + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "+": + self.phase1 = False + self.phase2 = True + print(' The phase 2 is activated !') + elif key == "-": + self.phase1 = True + self.phase2 = False + print(' The phase 1 is activated !') + + +def createScene(rootNode): + rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList, + ['SofaEngine', 'SofaLoader', 'SofaSimpleFem', + 'SofaExporter']]) + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hireForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.findData('dt').value = 0.02 + # rootNode.findData('gravity').value = [0., -9.81, 0.] + rootNode.findData('gravity').value = [0., 0., 0.] + rootNode.addObject('BackgroundSetting', color='1 1 1') + # rootNode.addObject('FreeMotionAnimationLoop') + # rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + # rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") + + solverNode = rootNode.addChild('solverNode') + # solverNode.addObject('EulerImplicitSolver', rayleighStiffness="0.", rayleighMass='0.') + solverNode.addObject('EulerImplicitSolver', rayleighStiffness=0, rayleighMass='0.', + firstOrder=firstOrder) + solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + # solverNode.addObject('EigenSimplicialLDLT', name='solver', template="CompressedRowSparseMatrixMat3x3d" ) + + # solverNode.addObject('CGLinearSolver', tolerance=1.e-12, iterations=1000, threshold=1.e-18) + + needCollisionModel = 0 # use this if the collision model if the beam will interact with another object + nonLinearCosserat = solverNode.addChild( + nonCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, useCollisionModel=needCollisionModel, + name="cosserat", radius=Rb, youngModulus=YM, legendreControlPoints=initialStrain, + order=LegendrePolyOrder, inertialParams=inertialParams, showObject='1', + activatedMMM=True)) + beamFrame = nonLinearCosserat.cosseratFrame + + # @todo attach the end effector of the beam with a control point + EndEffectorColler = rootNode.addChild('EndEffectorController') + controlMo = EndEffectorColler.addObject('MechanicalObject', template='Rigid3d', name="controlEndEffector", + showObjectScale=0.3, position=[length, 0, 0, 0, 0, 0, 1], showObject=True) + + beamFrame.addObject('RestShapeSpringsForceField', name='spring', + stiffness=1e8, angularStiffness=1e8, external_points=0, + external_rest_shape=controlMo.getLinkPath(), points=nbFrames, template="Rigid3d") + + cosseratNode = nonLinearCosserat.legendreControlPointsNode + cosseratNode.addObject('MechanicalMatrixMapper', template='Vec3,Vec3', + object1=cosseratNode.getLinkPath(), + object2=cosseratNode.getLinkPath(), + name='cosseratCoordinateNodeMapper', + nodeToParse=nonLinearCosserat.cosseratCoordinateNode.getLinkPath()) + + constForce = beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-9, + indices=nonLinearConfig['nbFramesF'], force=F1) + + nonLinearCosserat = solverNode.addObject( + ForceController(parent=solverNode, cosseratFrames=beamFrame.FramesMO, controller=controlMo)) + + return rootNode \ No newline at end of file diff --git a/python3/cosserat/examples/example3.py b/python3/cosserat/examples/example3.py new file mode 100644 index 00000000..d41bd002 --- /dev/null +++ b/python3/cosserat/examples/example3.py @@ -0,0 +1,122 @@ +# -*- coding: utf-8 -*- +""" + Cosserat class in SofaPython3. +""" + +__authors__ = "younesssss" +__contact__ = "adagolodjo@protonmail.com" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "Jan, 17 2022" + +import Sofa +from cosserat.cosseratObject import Cosserat +from cosserat.nonLinearCosserat import NonLinearCosserat as nonCosserat +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry +from math import sqrt + +# @todo ================ Unit: N, m, Kg, Pa ================ +LegendrePolyOrder = 5 # P_{k_2}=P_{k_3} +initialStrain = [[0., 0., 0], [0., 0., 0], + [0., 0., 0], [0., 0., 0], [0., 0., 0]] + +YM = 1.0e8 +PR = 0. +rayleighStiffness = 1.e-3 # Nope +firstOrder = 1 + +EI = 1.e2 +coeff = 1 + +F1 = [0., 0., 0., 0., (coeff*1.)/sqrt(2), (coeff*1.)/sqrt(2)] # N + +# Inertia parameter +Rb = 0.02/2. # beam radius in m +length = 1. # in m +nbSection = 5 # +deltaT = 0.02 # s + +inertialParams = {'GI': 1.5708, 'GA': 3.1416e4, + 'EI': 0.7854, 'EA': 3.1416e4, 'L': length, 'Rb': Rb} +nonLinearConfig = {'init_pos': [0., 0., 0.], 'tot_length': length, 'nbSectionS': nbSection, + 'nbFramesF': 30, 'buildCollisionModel': 0, 'beamMass': 0.} + + +class ForceController(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.frames = kwargs['cosseratFrames'] + self.forceNode = kwargs['forceNode'] + self.size = nonLinearConfig['nbFramesF'] + self.applyForce = True + self.forceCoeff = coeff + # self.cosseratGeometry = kwargs['cosseratGeometry'] + + def onAnimateEndEvent(self, event): + if self.applyForce: + with self.forceNode.force.writeable() as force: + vec = [0., 0., 0., 0., (self.forceCoeff * 1.) / + sqrt(2), (self.forceCoeff * 1.) / sqrt(2)] + for i, v in enumerate(vec): + force[i] = v + # print(f' The new force: {force}') + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "+": + self.forceCoeff += 1 + print(f' The new force coeff is : {self.forceCoeff}') + elif key == "-": + self.forceCoeff -= 1 + print(f' The new force coeff is : {self.forceCoeff}') + + +def createScene(rootNode): + rootNode.addObject('RequiredPlugin', name='plugins', pluginName=[pluginList, + ['SofaEngine', 'SofaLoader', 'SofaSimpleFem', + 'SofaExporter']]) + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hireForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.findData('dt').value = deltaT + # rootNode.findData('gravity').value = [0., -9.81, 0.] + rootNode.findData('gravity').value = [0., 0., 0.] + rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') + # rootNode.addObject('FreeMotionAnimationLoop') + # rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") + + solverNode = rootNode.addChild('solverNode') + solverNode.addObject('EulerImplicitSolver', rayleighStiffness=rayleighStiffness, rayleighMass='0.', + firstOrder=firstOrder) + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") + # solverNode.addObject('SparseLUSolver', name='solver', template="CompressedRowSparseMatrixd") + # solverNode.addObject('CGLinearSolver', tolerance=1.e-12, iterations=1000, threshold=1.e-18) + + # use this if the collision model if the beam will interact with another object + needCollisionModel = 0 + nonLinearCosserat = solverNode.addChild( + nonCosserat(parent=solverNode, cosseratGeometry=nonLinearConfig, inertialParams=inertialParams, + useCollisionModel=needCollisionModel, name="cosserat", radius=Rb, youngModulus=YM, + legendreControlPoints=initialStrain, poissonRatio=PR, order=LegendrePolyOrder, + rayleighStiffness=rayleighStiffness)) + cosseratNode = nonLinearCosserat.legendreControlPointsNode + cosseratNode.addObject('MechanicalMatrixMapper', template='Vec3,Vec3', + object1=cosseratNode.getLinkPath(), + object2=cosseratNode.getLinkPath(), + name='cosseratCoordinateNodeMapper', + nodeToParse=nonLinearCosserat.cosseratCoordinateNode.getLinkPath()) + + beamFrame = nonLinearCosserat.cosseratFrame + + constForce = beamFrame.addObject('ConstantForceField', name='constForce', showArrowSize=1.e-8, + indices=nonLinearConfig['nbFramesF'], force=F1) + + nonLinearCosserat = solverNode.addObject( + ForceController(parent=solverNode, cosseratFrames=beamFrame.FramesMO, forceNode=constForce)) + + return rootNode + + +# if( and ( or (prop("Status") == "Not started", or (prop("Status") == "Upcoming", contains(prop("Status"), "Overdue"))), and (now() > prop("Deadline"), prop("Done") == false)), "🚨 Overdue 🚨", if( or(prop("Status") == "Archived", contains(prop("Status"), "Done")), "🏅VALIDÉ", "DOING")) diff --git a/python3/cosserat/examples/twoSlidingCosseratBeams.py b/python3/cosserat/examples/twoSlidingCosseratBeams.py index ff69f504..3c85eb01 100644 --- a/python3/cosserat/examples/twoSlidingCosseratBeams.py +++ b/python3/cosserat/examples/twoSlidingCosseratBeams.py @@ -18,7 +18,7 @@ from cosserat.usefulFunctions import pluginList # from stlib3.scene import Node -path = os.path.dirname(os.path.abspath(__file__)) + '/../mesh/' +path = f'{os.path.dirname(os.path.abspath(__file__))}/../mesh/' class Animation(Sofa.Core.Controller): @@ -80,12 +80,15 @@ def createScene(rootNode): rootNode.findData('gravity').value = [0., 0, 0.] rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') rootNode.addObject('FreeMotionAnimationLoop') - rootNode.addObject('GenericConstraintSolver', tolerance=1e-5, maxIterations=5e2) + rootNode.addObject('GenericConstraintSolver', + tolerance=1e-5, maxIterations=5e2) rootNode.addObject('Camera', position="-35 0 280", lookAt="0 0 0") solverNode = rootNode.addChild('solverNode') - solverNode.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.1') - solverNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness="0.2", rayleighMass='0.1') + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") # solverNode.addObject('SparseLUSolver', name='solver') solverNode.addObject('GenericConstraintCorrection') @@ -108,7 +111,7 @@ def createScene(rootNode): cosserat1 = solverNode.addChild( Cosserat(parent=solverNode, cosseratGeometry=cosserat_config1, name="cosseratBeam1", radius=0.2, showObject='1', - attachingToLink='1',position=[[0., -2, 0., 0, 0, 0, 1]])) + attachingToLink='1', position=[[0., -2, 0., 0, 0, 0, 1]])) cable_position = [[i*2., 2., 0.] for i in range(11)] diff --git a/python3/cosserat/inverse/InverseCosseratNeedleWithConstraints.py b/python3/cosserat/inverse/InverseCosseratNeedleWithConstraints.py index 1bc71906..b3c1b380 100644 --- a/python3/cosserat/inverse/InverseCosseratNeedleWithConstraints.py +++ b/python3/cosserat/inverse/InverseCosseratNeedleWithConstraints.py @@ -4,6 +4,9 @@ Based on the work done with SofaPython. See POEMapping.py. """ +# from usefulFunctions import MoveTargetProcess +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry, MoveTargetProcess +from createFemRegularGrid import createFemCube __authors__ = "younesssss" __contact__ = "adagolodjo@protonmail.com, yinoussa.adagolodjo@inria.fr" __version__ = "1.0.0" @@ -14,16 +17,16 @@ import sys sys.path.append('../') -from createFemRegularGrid import createFemCube -from usefulFunctions import BuildCosseratGeometry, MoveTargetProcess def effectorTarget(parentNode, position=[80., 0., 0.35857]): target = parentNode.createChild("Target") target.addObject("EulerImplicitSolver", firstOrder=True) - target.addObject('VisualStyle', displayFlags='showVisualModels showInteractionForceFields showCollisionModels') + target.addObject( + 'VisualStyle', displayFlags='showVisualModels showInteractionForceFields showCollisionModels') target.addObject("CGLinearSolver") - targetMO = target.addObject("MechanicalObject", name="dofs", position=position, showObject=0, showObjectScale=0, drawMode=0, showColor=[1., 1., 1., 1.]) + targetMO = target.addObject("MechanicalObject", name="dofs", position=position, + showObject=0, showObjectScale=0, drawMode=0, showColor=[1., 1., 1., 1.]) target.addObject('SphereCollisionModel', radius='2') target.addObject("UncoupledConstraintCorrection") return [target, targetMO] @@ -31,10 +34,9 @@ def effectorTarget(parentNode, position=[80., 0., 0.35857]): def createScene(rootNode): from stlib3.scene import MainHeader - MainHeader(rootNode, plugins=["SoftRobots", "SoftRobots.Inverse", "SofaSparseSolver", 'SofaDeformable', - "SofaPreconditioner", "SofaOpenglVisual", "CosseratPlugin", "BeamAdapter", - "SofaGeneralRigid", "SofaImplicitOdeSolver", 'SofaEngine', 'SofaMeshCollision', - 'SofaSimpleFem', 'SofaTopologyMapping', 'SofaConstraint'], + MainHeader(rootNode, plugins=pluginList + + ["SoftRobots.Inverse", "Sofa.Component.LinearSolver.Iterative", "Sofa.Component.Collision.Response.Contact", + "Sofa.Component.Collision.Geometry", "Sofa.Component.Collision.Detection.Intersection", "Sofa.Component.Collision.Detection.Algorithm"], repositoryPaths=[os.getcwd()]) rootNode.addObject('VisualStyle', displayFlags='showVisualModels hideBehaviorModels showCollisionModels ' @@ -42,10 +44,12 @@ def createScene(rootNode): 'showWireframe') rootNode.addObject('FreeMotionAnimationLoop') - rootNode.createObject('QPInverseProblemSolver', printLog='0', epsilon=0.0002) + rootNode.createObject('QPInverseProblemSolver', + printLog='0', epsilon=0.0002) rootNode.addObject('CollisionPipeline', depth="6", verbose="0", draw="1") rootNode.addObject('BruteForceDetection', name="N2") - rootNode.addObject('DefaultContactManager', response="FrictionContact", responseParams="mu=0.65") + rootNode.addObject('DefaultContactManager', + response="FrictionContact", responseParams="mu=0.65") rootNode.addObject('LocalMinDistance', name="Proximity", alarmDistance="0.6", contactDistance="0.44", angleCone="0.01") @@ -59,7 +63,8 @@ def createScene(rootNode): # New adds to use the sliding Actuator # ######################################### cableNode = rootNode.addChild('cableNode') - cableNode.addObject('EulerImplicitSolver', firstOrder="0", rayleighStiffness="1.0", rayleighMass='0.1') + cableNode.addObject('EulerImplicitSolver', firstOrder="0", + rayleighStiffness="1.0", rayleighMass='0.1') cableNode.addObject('SparseLUSolver', name='solver') cableNode.addObject('GenericConstraintCorrection') @@ -75,16 +80,19 @@ def createScene(rootNode): ################################################################# ## Sliding actuator to guide the base of the needle, only the ## - ## translation are tacking into account here. + # translation are tacking into account here. ################################################################# - for j in range(0, 6): + for j in range(6): direction = [0, 0, 0, 0, 0, 0] direction[j] = 1 - rigidBaseNode.addObject('SlidingActuator', name="SlidingActuator" + str(j), template='Rigid3d', + rigidBaseNode.addObject('SlidingActuator', name=f"SlidingActuator{str(j)}", template='Rigid3d', direction=direction, indices=0, maxForce='1e6', minForce='-30000') + cosserat_config = {'init_pos': [0., 0., 0.], 'tot_length': 80, 'nbSectionS': 8, + 'nbFramesF': 16, 'beamMass': 0.22} + [positionS, curv_abs_inputS, longeurS, framesF, curv_abs_outputF, cable_positionF] = \ - BuildCosseratGeometry(nbSection=8, nbFrames=16, totalLength=80) + BuildCosseratGeometry(cosserat_config) ############################################# # Rate of angular Deformation (2 sections) @@ -147,7 +155,8 @@ def createScene(rootNode): femPos = [" 41.0 0 0 45 0 0 50 0 0 55 0 0 "] gelNode = cubeNode.getChild('gelNode') femPoints = gelNode.addChild('femPoints') - inputFEMCable = femPoints.addObject('MechanicalObject', name="pointsInFEM", position=femPos, showIndices="1") + inputFEMCable = femPoints.addObject( + 'MechanicalObject', name="pointsInFEM", position=femPos, showIndices="1") femPoints.addObject('BarycentricMapping') #################################################################################### @@ -158,16 +167,18 @@ def createScene(rootNode): effector = mappedFrameNode.addChild('fingertip') effMO = effector.addObject('MechanicalObject', position=[80., 0., 0.35857]) posEffector = effector.addObject('PositionEffector', template='Vec3d', indices="0", - effectorGoal=targetMO.getLinkPath()+'.position') # "@../../../../FemNode/gelNode/target/targetMO.position" + effectorGoal=targetMO.getLinkPath()+'.position') # "@../../../../FemNode/gelNode/target/targetMO.position" targetNode.addObject(MoveTargetProcess(posEffector)) - effector.addObject('SkinningMapping', nbRef='1', mapForces='false', mapMasses='false') + effector.addObject('SkinningMapping', nbRef='1', + mapForces='false', mapMasses='false') mappedPointsNode = framePoints.addChild('MappedPoints') mappedPoints = mappedPointsNode.addObject('MechanicalObject', template='Vec3d', position=femPos, name="FramesMO", showObject='1', showIndices='1', showObjectScale='1') - mappedPointsNode.addObject('CosseratEquality', name="QPConstraint", eqDisp='0.0', lastPointIsFixed="false") + mappedPointsNode.addObject( + 'CosseratEquality', name="QPConstraint", eqDisp='0.0') - ## Get the tree mstate links for the mapping + # Get the tree mstate links for the mapping inputCableMO = framePointsMO.getLinkPath() inputFEMCableMO = inputFEMCable.getLinkPath() outputPointMO = mappedPoints.getLinkPath() @@ -180,4 +191,3 @@ def createScene(rootNode): # rootNode.addObject(CostController(name="CostController", goal_pos=goalPos, effMO=effMO, solver=qp_solver)) # return rootNode - diff --git a/python3/cosserat/inverse/inverseCosseratNeedle.py b/python3/cosserat/inverse/inverseCosseratNeedle.py index 7bb591ea..2800c95c 100644 --- a/python3/cosserat/inverse/inverseCosseratNeedle.py +++ b/python3/cosserat/inverse/inverseCosseratNeedle.py @@ -12,15 +12,16 @@ import os import sys +from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry sys.path.append('../') -path = os.path.dirname(os.path.abspath(__file__)) + '/../mesh/' +path = f'{os.path.dirname(os.path.abspath(__file__))}/../mesh/' def createScene(rootNode): from stlib3.scene import MainHeader - MainHeader(rootNode, plugins=["SoftRobots", "SoftRobots.Inverse", "SofaSparseSolver", 'SofaDeformable', - "SofaPreconditioner", "SofaOpenglVisual", "CosseratPlugin", - "SofaGeneralRigid", "SofaImplicitOdeSolver"], + MainHeader(rootNode, plugins=pluginList + + ["SoftRobots.Inverse", "Sofa.Component.LinearSolver.Iterative", "Sofa.Component.Collision.Response.Contact", + "Sofa.Component.Collision.Geometry", "Sofa.Component.Collision.Detection.Intersection", "Sofa.Component.Collision.Detection.Algorithm"], repositoryPaths=[os.getcwd()]) rootNode.addObject('VisualStyle', displayFlags='showVisualModels hideBehaviorModels showCollisionModels ' @@ -80,7 +81,7 @@ def createScene(rootNode): for j in range(3): direction = [0, 0, 0, 0, 0, 0] direction[j] = 1 - rigidBaseNode.addObject('SlidingActuator', name="SlidingActuator" + str(j), template='Rigid3d', + rigidBaseNode.addObject('SlidingActuator', name=f'SlidingActuator{str(j)}', template='Rigid3d', direction=direction, indices=0, maxForce='100000', minForce='-30000') ############################################# @@ -112,10 +113,10 @@ def createScene(rootNode): 'BeamHookeLawForceField', crossSectionShape='circular', length=longeurS, radius='0.5', youngModulus='5e6') for i in range(1, 6): - rateAngularDeformNode.addObject('SlidingActuator', name="SlidingActuatory" + str(i), template='Vec3d', + rateAngularDeformNode.addObject('SlidingActuator', name=f"SlidingActuatory{str(i)}", template='Vec3d', direction='0 1 0 ', indices=i, maxForce='100000', minForce='-30000') for i in range(1, 6): - rateAngularDeformNode.addObject('SlidingActuator', name="SlidingActuatorz" + str(i), template='Vec3d', + rateAngularDeformNode.addObject('SlidingActuator', name=f"SlidingActuatorz{str(i)}", template='Vec3d', direction='0 0 1', indices=i, maxForce='100000', minForce='-30000') ############## diff --git a/python3/cosserat/needle/CosserateLikeNeedle.py b/python3/cosserat/needle/CosserateLikeNeedle.py index 4b4b9a2c..6b306c28 100644 --- a/python3/cosserat/needle/CosserateLikeNeedle.py +++ b/python3/cosserat/needle/CosserateLikeNeedle.py @@ -7,7 +7,7 @@ from splib3.numerics import Quat import sys sys.path.append('../') -from createFemRegularGrid import createFemCube +from cosserat.createFemRegularGrid import createFemCube __authors__ = "younesssss" __contact__ = "adagolodjo@protonmail.com, yinoussa.adagolodjo@inria.fr" __version__ = "1.0.0" diff --git a/python3/cosserat/needle/__init__.py b/python3/cosserat/needle/__init__.py index 32bfbf15..3490a999 100644 --- a/python3/cosserat/needle/__init__.py +++ b/python3/cosserat/needle/__init__.py @@ -4,9 +4,13 @@ Content: ******** -.. scenes related to the use of cosserat model needle -.. in a direct simulation + .. scenes related to the use of cosserat model needle in a direct simulation + .. autosummary:: + :toctree: _autosummary + + cosserat.needle.params + - """ -#from needle import cosseratNeedle + +__all__ = ["params", "needleController"] diff --git a/python3/cosserat/needle/needleController.py b/python3/cosserat/needle/needleController.py new file mode 100644 index 00000000..2c28b6ab --- /dev/null +++ b/python3/cosserat/needle/needleController.py @@ -0,0 +1,126 @@ +__authors__ = "younesssss" +__contact__ = "adagolodjo@protonmail.com, yinoussa.adagolodjo@inria.fr" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "March 8 2021" + +import Sofa + +import numpy as np +import Sofa.CosseratPlugin +from params import ConstraintsParams +from cosserat.utils import computeDistanceBetweenPoints +import params + + +class Animation(Sofa.Core.Controller): + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.rigidBaseMO = args[0].rigidBaseNode.RigidBaseMO + self.rateAngularDeformMO = args[0].cosseratCoordinateNode.cosseratCoordinateMO + + self.needleSlidingState = args[0].cosseratFrame.slidingPoint.slidingPointMO + self.needleCollisionModel = args[0].cosseratFrame.needleCollision + + self.contactListener = args[1] + self.generic = args[2] + self.entryPoint = [] + self.threshold = 3. + + self.constraintPointsNode = args[3] + self.pointManager = self.constraintPointsNode.pointsManager + + self.constraintPts = self.constraintPointsNode.constraintPointsMo + self.constraintPtsContainer = self.constraintPointsNode.constraintPtsContainer + self.constraintPtsModifier = self.constraintPointsNode.constraintPtsModifier + self.inside = False + self.addNewPoint = False + self.rootNode = args[4] + + self.rate = 0.2 + self.angularRate = 0.02 + self.tipForce = [0., 0., 0] + return + + def onAnimateEndEvent(self, event): + if self.contactListener.getContactPoints() and not self.inside: + # @Info: check if the contact force is large enough to go through the tissue + if self.generic.constraintForces and self.generic.constraintForces[0] > self.threshold: + # 1. @Info: Save the entryPoint and contact force + vec = self.contactListener.getContactPoints()[0][1] + self.entryPoint = [vec[0], vec[1], vec[2]] + self.tipForce = [self.generic.constraintForces[0], self.generic.constraintForces[1], + self.generic.constraintForces[2]] + + # 2. @Info: deactivate the contact constraint + self.needleCollisionModel.findData('activated').value = 0 + + # @Info 3. Add entryPoint point as the first constraint point in FEM + self.pointManager.addNewPointToState() + self.inside = True + + elif self.tipForce[0] > self.threshold: + print( + "Please activate computeConstraintForces data field inside the GenericConstraint component") + # 5. todo: If the user is pulling out the needle and the needle tip is behind the entryPoint, + # 5. todo: call the remove constraint function to remove those constraint points + # todo: this can be handle with : + # todo: 5.1 self.inside=False + # todo: 5.2 self.needleCollisionModel.findData('activated').value = 1 + else: + if self.inside: + # @info 1. check if the needle reached the distance to create/remove a constraint point + slidingPos = self.needleSlidingState.position.array() + constraintPos = self.constraintPts.position.array() + + if computeDistanceBetweenPoints(slidingPos, constraintPos) > \ + params.ConstraintsParams.constraintDistance: + self.pointManager.addNewPointToState() + else: + pass + # print("=====> The distance between the tip and the last constraint is < constraint Distance <=====") + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "M": + self.pointManager.addNewPointToState() + if key == "D": + self.pointManager.removeLastPointfromState() + if key == "B": + self.rootNode.findData('animate').value = 1 + + if ord(key) == 18: # left + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][0] -= self.rate + elif ord(key) == 20: # right + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][0] += self.rate + + elif ord(key) == 21: # down + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][1] -= self.rate + + elif ord(key) == 19: # up + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][1] += self.rate + + if key == "I": # + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][2] -= self.rate + elif key == "K": # + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][2] += self.rate + # def onAnimateBeginEvent(self, event): + # if self.inside & self.addNewPoint: + # print("Inside the volume") + # self.addConstraintPointToFem(self.entryPoint) + # + # # 4. todo: add new constraint points inside the volume if needed. + # # todo: This depends on the choice of the algorithm + # # expl1: one can compare tip position related to the last constraint position inside the volume and + # # when this > than the constraintDistance we add new constraint point + # # self.addPointToFem() + # # expl2: one can compare the tip position related to the entry point and when + # # this > than the constraintDistance + # + # diff --git a/python3/cosserat/needle/needleInteractionTest.py b/python3/cosserat/needle/needleInteractionTest.py new file mode 100644 index 00000000..9d402bbc --- /dev/null +++ b/python3/cosserat/needle/needleInteractionTest.py @@ -0,0 +1,214 @@ +# -*- coding: utf-8 -*- +"""Basic scene using Cosserat in SofaPython3. + +Based on the work done with SofaPython. See POEMapping.py +""" + +from params import NeedleParameters +from cosserat.usefulFunctions import pluginList +from cosserat.createFemRegularGrid import createFemCubeWithParams +from cosserat.cosseratObject import Cosserat +import Sofa +from splib3.numerics import Quat +import sys + +sys.path.append('../') + +__authors__ = "younesssss" +__contact__ = "adagolodjo@protonmail.com, yinoussa.adagolodjo@inria.fr" +__version__ = "1.0.0" +__copyright__ = "(c) 2021,Inria" +__date__ = "March 8 2021" + + +params = NeedleParameters() + +needleGeometryConfig = {'init_pos': [0., 0., 0.], 'tot_length': params.Geometry.totalLength, + 'nbSectionS': params.Geometry.nbSections, 'nbFramesF': params.Geometry.nbFrames, + 'buildCollisionModel': 1, 'beamMass': params.Physics.mass} + + +class Animation(Sofa.Core.Controller): + + def __init__(self, *args, **kwargs): + Sofa.Core.Controller.__init__(self, *args, **kwargs) + self.rigidBaseMO = args[0] + self.rateAngularDeformMO = args[1] + self.contactListener = args[2] + self.generic = args[3] + self.entryPoint = [] + self.threshold = 1. + self.needleCollisionModel = args[4] + self.constraintPoints = args[5] + self.inside = False + + self.rate = 0.2 + self.angularRate = 0.02 + self.tipForce = [0., 0., 0] + return + + def onAnimateEndEvent(self, event): + if self.contactListener.getContactPoints() and not self.inside: + vec = self.contactListener.getContactPoints()[0][1] + tip = [vec[0], vec[1], vec[2]] + + if self.generic.constraintForces and self.generic.constraintForces[0] > self.threshold: + # @info 1. Save the entryPoint + self.entryPoint = tip + + Force = self.generic.constraintForces + self.tipForce = [Force[0], Force[1], Force[2]] + + # @info 2. deactivate the contact constraint + self.needleCollisionModel.findData('activated').value = 0 + + # @info 3. Add entryPoint point as the first constraint point in FEM + with self.constraintPoints.position.writeable() as pos: + pos.resize(1) + pos[0] = self.entryPoint + print(f' ====> The tip is : {tip}') + print(f' ====> The entryPoint is : {self.entryPoint}') + print(f' ====> The constraintPoints is : {pos[0]}') + + self.inside = True + + elif self.tipForce[0] > self.threshold: + print( + "Please activate computeConstraintForces data field inside the GenericConstraint component") + elif self.inside: + # 4. todo: add new constraint points inside the volume if needed. + # todo: This depends on the choice of the algorithm + # expl1: one can compare tip position related to the last constraint position inside the volume and + # when this > than the constraintDistance we add new constraint point + # addNewConstraintPoint() + + # 5. todo: If the user is pulling out the needle and the needle tip is behind is before the entryPoint, + # todo: activated contact constraint. + # 5.1 self.inside=False + # 5.2 self.needleCollisionModel.findData('activated').value = 1 + pass + + def onKeypressedEvent(self, event): + key = event['key'] + if key == "k": # - + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[5][1] -= self.angularRate + + # ######## Reste rigid position ######### + elif key == "+": # up + with self.rigidBaseMO.rest_position.writeable() as posA: + qOld = Quat() + for i in range(4): + qOld[i] = posA[0][i + 3] + qNew = Quat.createFromEuler([0., self.angularRate, 0.], 'ryxz') + qNew.normalize() + qNew.rotateFromQuat(qOld) + for i in range(4): + posA[0][i + 3] = qNew[i] + + elif key == "-": # down + with self.rigidBaseMO.rest_position.writeable() as posA: + qOld = Quat() + for i in range(4): + qOld[i] = posA[0][i + 3] + + qNew = Quat.createFromEuler( + [0., -self.angularRate, 0.], 'ryxz') + qNew.normalize() + qNew.rotateFromQuat(qOld) + for i in range(4): + posA[0][i + 3] = qNew[i] + + if ord(key) == 18: # left + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][0] -= self.rate + elif ord(key) == 20: # right + print( + f' ====> contactListener : {self.contactListener.getContactPoints()}') + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][0] += self.rate + + elif ord(key) == 21: # down + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][1] -= self.rate + + elif ord(key) == 19: # up + with self.rigidBaseMO.rest_position.writeable() as posA: + posA[0][1] += self.rate + + +def createScene(rootNode): + rootNode.addObject( + 'RequiredPlugin', pluginName=pluginList, printLog='0') + + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hireForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.addObject('DefaultPipeline') + rootNode.addObject("DefaultVisualManagerLoop") + rootNode.addObject('RuleBasedContactManager', + responseParams='mu=0.1', response='FrictionContactConstraint') + rootNode.addObject('BruteForceBroadPhase') + rootNode.addObject('BVHNarrowPhase') + # rootNode.addObject('LocalMinDistance', alarmDistance=1.0, contactDistance=0.01) + rootNode.addObject('LocalMinDistance', name="Proximity", alarmDistance=0.5, + contactDistance=params.contact.contactDistance, + coneFactor=params.contact.coneFactor, angleCone=0.1) + + rootNode.addObject('FreeMotionAnimationLoop') + generic = rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", + maxIterations="500", computeConstraintForces=1, printLog="0") + + gravity = [0, 0, 0] + rootNode.gravity.value = gravity + rootNode.addObject('BackgroundSetting', color='1 1 1') + rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight") + # ############### + # New adds to use the sliding Actuator + ############### + solverNode = rootNode.addChild('solverNode') + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness=params.Physics.rayleighStiffness) + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") + solverNode.addObject('GenericConstraintCorrection') + + needle = solverNode.addChild( + Cosserat(parent=solverNode, cosseratGeometry=needleGeometryConfig, radius=params.Geometry.radius, + name="needle", youngModulus=params.Physics.youngModulus, poissonRatio=params.Physics.poissonRatio, + rayleighStiffness=params.Physics.rayleighStiffness)) + needleCollisionModel = needle.addPointCollisionModel() + slidingPoint = needle.addSlidingPoints() + + # Create FEM Node + # TODO: Where we handle Sliding constraints, + # we need to be able added or remove dynamically + # TODO: @Paul is in charge of creating this in python + # + femPos = [] + cubeNode = createFemCubeWithParams(rootNode, params.FemParams) + gelNode = cubeNode.getChild('gelNode') + femPoints = gelNode.addChild('femPoints') + inputFEMCable = femPoints.addObject( + 'MechanicalObject', name="pointsInFEM", position=femPos, showIndices="1") + femPoints.addObject('BarycentricMapping') + + mappedPointsNode = slidingPoint.addChild('MappedPoints') + femPoints.addChild(mappedPointsNode) + mappedPoints = mappedPointsNode.addObject( + 'MechanicalObject', template='Vec3d', position=femPos, name="FramesMO") + + inputCableMO = slidingPoint.slidingPointMO.getLinkPath() + inputFEMCableMO = inputFEMCable.getLinkPath() + outputPointMO = mappedPoints.getLinkPath() + + conttactL = rootNode.addObject('ContactListener', name="contactListener", + collisionModel1=cubeNode.gelNode.surfaceNode.surface.getLinkPath(), + collisionModel2=needleCollisionModel.pointColli.getLinkPath()) + rootNode.addObject(Animation(needle.rigidBaseNode.RigidBaseMO, needle.cosseratCoordinateNode.cosseratCoordinateMO, + conttactL, generic, needleCollisionModel, inputFEMCable)) + mappedPointsNode.addObject( + 'CosseratNeedleSlidingConstraint', name="QPConstraint") + mappedPointsNode.addObject('DifferenceMultiMapping', name="pointsMulti", input1=inputFEMCableMO, lastPointIsFixed=0, + input2=inputCableMO, output=outputPointMO, direction="@../../FramesMO.position") + return rootNode diff --git a/python3/cosserat/needle/needleTissueInteraction.py b/python3/cosserat/needle/needleTissueInteraction.py new file mode 100644 index 00000000..bfb0dcda --- /dev/null +++ b/python3/cosserat/needle/needleTissueInteraction.py @@ -0,0 +1,104 @@ +# -*- coding: utf-8 -*- +"""Basic scene using Cosserat in SofaPython3. + +Based on the work done with SofaPython. See POEMapping.py +""" + +from cosserat.needle.needleController import Animation +from params import NeedleParameters, GeometryParams, PhysicsParams, FemParams, ContactParams +from cosserat.usefulFunctions import pluginList +from cosserat.createFemRegularGrid import createFemCubeWithParams +from cosserat.cosseratObject import Cosserat +from cosserat.utils import addConstraintPoint +import sys + +# params = NeedleParameters() + +sys.path.append('../') + +nbFrames = GeometryParams.nbFrames +needleGeometryConfig = {'init_pos': [0., 0., 0.], 'tot_length': GeometryParams.totalLength, + 'nbSectionS': GeometryParams.nbSections, 'nbFramesF': nbFrames, + 'buildCollisionModel': 1, 'beamMass': PhysicsParams.mass} + + +def createScene(rootNode): + rootNode.addObject( + 'RequiredPlugin', pluginName=pluginList, printLog='0') + + rootNode.addObject('VisualStyle', displayFlags='showVisualModels showBehaviorModels hideCollisionModels ' + 'hideBoundingCollisionModels hireForceFields ' + 'hideInteractionForceFields hideWireframe showMechanicalMappings') + rootNode.addObject('DefaultPipeline') + rootNode.addObject("DefaultVisualManagerLoop") + rootNode.addObject('RuleBasedContactManager', + responseParams='mu=0.1', response='FrictionContactConstraint') + rootNode.addObject('BruteForceBroadPhase') + rootNode.addObject('BVHNarrowPhase') + # rootNode.addObject('LocalMinDistance', alarmDistance=1.0, contactDistance=0.01) + rootNode.addObject('LocalMinDistance', name="Proximity", alarmDistance=0.5, + contactDistance=ContactParams.contactDistance, + coneFactor=ContactParams.coneFactor, angleCone=0.1) + + rootNode.addObject('FreeMotionAnimationLoop') + generic = rootNode.addObject('GenericConstraintSolver', tolerance="1e-20", + maxIterations="500", computeConstraintForces=1, printLog="0") + + gravity = [0, 0, 0] + rootNode.gravity.value = gravity + rootNode.addObject('BackgroundSetting', color='0 0.168627 0.211765') + # rootNode.addObject('OglSceneFrame', style="Arrows", alignment="TopRight") + # ############### + # New adds to use the sliding Actuator + ############### + solverNode = rootNode.addChild('solverNode') + solverNode.addObject('EulerImplicitSolver', + rayleighStiffness=PhysicsParams.rayleighStiffness) + solverNode.addObject('SparseLDLSolver', name='solver', + template="CompressedRowSparseMatrixd") + solverNode.addObject('GenericConstraintCorrection') + + needle = solverNode.addChild( + Cosserat(parent=solverNode, cosseratGeometry=needleGeometryConfig, radius=GeometryParams.radius, + name="needle", youngModulus=PhysicsParams.youngModulus, poissonRatio=PhysicsParams.poissonRatio, + rayleighStiffness=PhysicsParams.rayleighStiffness)) + needleCollisionModel = needle.addPointCollisionModel("needleCollision") + + # These state is mapped on the needle and used to compute the distance between the needle and the + # FEM constraint points + slidingPoint = needle.addSlidingPoints() + + # ----------------- + # Start the volume definition + # ----------------- + cubeNode = createFemCubeWithParams(rootNode, FemParams) + gelNode = cubeNode.getChild('gelNode') + # FEM constraint points + constraintPointNode = addConstraintPoint( + gelNode, slidingPoint.getLinkPath()) + + # @info : This is the constraint point that will be used to compute the distance between the needle and the volume + conttactL = rootNode.addObject('ContactListener', name="contactListener", + collisionModel1=cubeNode.gelNode.surfaceNode.surface.getLinkPath(), + collisionModel2=needleCollisionModel.collisionStats.getLinkPath()) + + # These stats will represents the distance between the contraint point in the volume and + # their projection on the needle + # It 's also important to say that the x direction is not taken into account + distanceStatsNode = slidingPoint.addChild('distanceStatsNode') + constraintPointNode.addChild(distanceStatsNode) + constraintPoinMo = distanceStatsNode.addObject('MechanicalObject', name="distanceStats", template="Vec3d", + position=[], listening="1", showObject="1", showObjectScale="0.1") + inputVolumeMo = constraintPointNode.constraintPointsMo.getLinkPath() + inputNeedleMo = slidingPoint.slidingPointMO.getLinkPath() + outputDistanceMo = distanceStatsNode.distanceStats.getLinkPath() + + # --------------------------------------------------- + # @info: Start controller node + rootNode.addObject(Animation(needle, conttactL, generic, + constraintPointNode, rootNode, constraintPoinMo)) + + distanceStatsNode.addObject( + 'CosseratNeedleSlidingConstraint', name="computeDistanceComponent") + distanceStatsNode.addObject('DifferenceMultiMapping', name="pointsMulti", input1=inputVolumeMo, lastPointIsFixed=0, + input2=inputNeedleMo, output=outputDistanceMo, direction="@../../FramesMO.position") diff --git a/python3/cosserat/needle/params.py b/python3/cosserat/needle/params.py new file mode 100644 index 00000000..3cc30d53 --- /dev/null +++ b/python3/cosserat/needle/params.py @@ -0,0 +1,64 @@ + +from dataclasses import dataclass +import string + +# Units are cm, kg, s + + +@dataclass +class GeometryParams: + radius: float = 0.1 + nbSections: int = 16 + nbFrames: int = 15 + totalLength: float = 15. + +@dataclass +class PhysicsParams: + youngModulus: float = 1.20e9 + poissonRatio: float = 0.4 + mass: float = 0.3 + rayleighStiffness: float = 0.1 + + +@dataclass +class FemParams: + youngModulus: float = 100 + poissonRatio: float = 0.48 + mass: float = 0.3 + rayleigh: float = 0.1 + minVol: string = "16. -8. -5." + maxVol: string = "40. 8. 5." + mesh: string = "6 6 6" + box: string = "15 -10 -10 41 -6 10" + + +@dataclass +class ContactParams: + contactDistance: float = 0.01 + alarmDistance: float = 0.1 + dataMu: string = "mu=0.1" + angleCone: float = "0.01" + coneFactor: float = "0" + + +@dataclass +class NeedleParameters: + # Geometry: GeometryParams = GeometryParams() + # Physics: PhysicsParams = PhysicsParams() + # Fem: FemParams = FemParams() + # contact: ContactParams = ContactParams() + def display(): + print(f''' + # Geometry + {string.ascii_uppercase[:4]} + {string.ascii_uppercase[4:6]} + {string.ascii_uppercase[6:8]} + {string.ascii_uppercase[8:10]} + ''' + ) + +@dataclass +class ConstraintsParams: + constraintDistance: float = 1.3 # distance between two constraint points + entryForce: float = 0.3 # The required force to penetrate the volume + diff --git a/python3/cosserat/nonLinearCosserat.py b/python3/cosserat/nonLinearCosserat.py index 405be684..b1dd5b2c 100644 --- a/python3/cosserat/nonLinearCosserat.py +++ b/python3/cosserat/nonLinearCosserat.py @@ -13,7 +13,6 @@ import Sofa from cosserat.usefulFunctions import buildEdges, pluginList, BuildCosseratGeometry - linearConfig = {'init_pos': [0., 0., 0.], 'tot_length': 1, 'nbSectionS': 15, 'nbFramesF': 30, 'buildCollisionModel': 1, 'beamMass': 0.22} @@ -64,7 +63,8 @@ class NonLinearCosserat(Sofa.Prefab): {'name': 'radius', 'type': 'double', 'help': 'the radius in case of circular section', 'default': 1.0}, {'name': 'length_Y', 'type': 'double', 'help': 'the radius in case of circular section', 'default': 1.0}, {'name': 'length_Z', 'type': 'double', 'help': 'the radius in case of circular section', 'default': 1.0}, - {'name': 'rayleighStiffness', 'type': 'double', 'help': 'Rayleigh damping - stiffness matrix coefficient', 'default': 0.0}, + {'name': 'rayleighStiffness', 'type': 'double', 'help': 'Rayleigh damping - stiffness matrix coefficient', + 'default': 0.0}, {'name': 'attachingToLink', 'type': 'string', 'help': 'a rest shape force field will constraint the object ' 'to follow arm position', 'default': '1'}, {'name': 'showObject', 'type': 'string', 'help': ' Draw object arrow ', 'default': '0'}] @@ -77,11 +77,18 @@ def __init__(self, *args, **kwargs): self.parent = kwargs['parent'] self.legendreControlPos = kwargs['legendreControlPoints'] self.polynomOrder = kwargs['order'] + self.useInertiaParams = False + self.totalLength = self.cosseratGeometry['tot_length'] + self.activatedMMM = kwargs['activatedMMM'] if self.parent.hasObject("EulerImplicitSolver") is False: self.solverNode = self.addSolverNode() else: self.solverNode = self.parent # self.solverNode = self.parent + if 'inertialParams' in kwargs: + self.useInertiaParams = True + self.inertialParams = kwargs['inertialParams'] + self.rigidBaseNode = self.addRigidBaseNode() [positionS, curv_abs_inputS, sectionLength, framesF, curv_abs_outputF, frames3D] = \ BuildCosseratGeometry(self.cosseratGeometry) @@ -91,6 +98,8 @@ def __init__(self, *args, **kwargs): if self.needCollisionModel: tab_edges = buildEdges(frames3D) self.cosseratFrameCollision = addEdgeCollision(self.cosseratFrame, frames3D, tab_edges) + # Inertia parameters + def init(self): pass @@ -118,7 +127,7 @@ def addRigidBaseNode(self): # to a control object in order to be able to drive it. if int(self.attachingToLink.value): rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', - stiffness=1e8, angularStiffness=1.e8, external_points=0, + stiffness=1e14, angularStiffness=1.e14, external_points=0, mstate="@RigidBaseMO", points=0, template="Rigid3d") return rigidBaseNode @@ -138,16 +147,26 @@ def addCosseratCoordinate(self, positionS, longeurS, curv_abs_inputS): cosseratCoordinateNode.addObject('MechanicalObject', template='Vec3d', name='cosseratCoordinateMO', position=positionXi, showIndices=0) - cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, - length=longeurS, - youngModulus=self.youngModulus.value, - poissonRatio=self.poissonRatio.value, - rayleighStiffness=self.rayleighStiffness.value, - radius=self.radius.value, - lengthY=self.length_Y.value, lengthZ=self.length_Z.value) - - localCurv = curv_abs_inputS - controlPointsAbs = [k*(1./self.polynomOrder) for k in range(1, self.polynomOrder)] + if self.useInertiaParams is False: + cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, + length=longeurS, radius=self.radius.value, + youngModulus=self.youngModulus.value, poissonRatio=self.poissonRatio.value, + rayleighStiffness=self.rayleighStiffness.value, + lengthY=self.length_Y.value, lengthZ=self.length_Z.value) + else: + GA = self.inertialParams['GA'] + GI = self.inertialParams['GI'] + EA = self.inertialParams['EA'] + EI = self.inertialParams['EI'] + print(f'{GA}') + cosseratCoordinateNode.addObject('BeamHookeLawForceField', crossSectionShape=self.shape.value, + length=longeurS, radius=self.radius.value, useInertiaParams=True, + GI=GI, GA=GA, EI=EI, EA=EA, rayleighStiffness=self.rayleighStiffness.value, + lengthY=self.length_Y.value, lengthZ=self.length_Z.value) + print(f'==========> curv_abs_inputS: {curv_abs_inputS}') + localCurv = [x/self.totalLength for x in curv_abs_inputS] + print(f'==========> localCurv: {localCurv}') + controlPointsAbs = [k * (1. / self.polynomOrder) for k in range(1, self.polynomOrder)] controlPointsAbs.append(1.0) cosseratCoordinateNode.addObject('LegendrePolynomialsMapping', curvAbscissa=localCurv, order=self.polynomOrder, controlPointsAbs=controlPointsAbs, applyRestPosition=True) @@ -167,7 +186,7 @@ def addCosseratFrame(self, framesF, curv_abs_inputS, curv_abs_outputF): input1=self.cosseratCoordinateNode.cosseratCoordinateMO.getLinkPath(), input2=self.rigidBaseNode.RigidBaseMO.getLinkPath(), output=framesMO.getLinkPath(), debug=0, radius=self.radius) - if self.beamMass != 0.: + if self.beamMass != 0. or self.activatedMMM == True: self.solverNode.addObject('MechanicalMatrixMapper', template='Vec3,Rigid3', object1=self.cosseratCoordinateNode.cosseratCoordinateMO.getLinkPath(), object2=self.rigidBaseNode.RigidBaseMO.getLinkPath(), diff --git a/python3/cosserat/plasticity/CosseratBeamCreator.py b/python3/cosserat/plasticity/CosseratBeamCreator.py new file mode 100644 index 00000000..1244cbdb --- /dev/null +++ b/python3/cosserat/plasticity/CosseratBeamCreator.py @@ -0,0 +1,128 @@ +# -*- coding: utf-8 -*- +""" +Created on Thu Oct 14 2021 +@author: ckrewcun +""" + +# -*- coding: utf-8 -*- + +""" + Auxiliary methods for Cosserat beam topology generation +""" + +def generateRegularSectionsAndFrames(totLength, nbBeams, nbFrames): + + # This method generates the parameters to pass to a Cosserat mode. It basically distributes uniformly a required + # number of sections (Cosserat beams) and a required number of frames (Rigid3d), over a given length. + # All coordinated are computed by default along the X axis. From the distribution of sections and frames, + # different input parameters for the Cosserat components (forcefield, mapping) are returned: + # - The length of each section (BeamHookeLawForceField.length) + # - The curvilinear coordinate of each section extremities (DiscreteCosseratMapping.curv_abs_input) + # - The section DoFs (3 DoFs of strain for torsion and bending) + # - The Rigid3d DoFs representing the frames + # - The curvilinear coordinate of each frame (DiscreteCosseratMapping.curv_abs_output) + # - 3D position of the frames + edge topology connecting them (for collision modelling) + + beamLengths = [] + curvAbsInput = [] + beamStrainDoFs = [] + + frameRigidDoFs = [] + frame3DDoFs = [] + frameEdges = [] + curvAbsOutput = [] + + # Computing section lengths and sections curvilinear coordinates + + individualBeamLength = totLength / nbBeams + lengthIncr = 0.0 + curvAbsInput.append(0.0) + for i in range(0, nbBeams): + beamLengths.append(individualBeamLength) + lengthIncr += individualBeamLength + curvAbsInput.append(lengthIncr) + beamStrainDoFs.append([0., 0., 0.]) + # NB: there are nbBeams+1 values in curvAbsInput, accounting for all section extremities + + # Computing frames Rigid3d coordinates and curvilinear coordinates + + frameIntervalDim = totLength / (nbFrames-1) # nbFrames-1 because the last frame is at the end of the last frame interval + lengthIncr = 0.0 + # Adding first frame + frameRigidDoFs.append([0., 0., 0., 0., 0., 0., 1.]) + frame3DDoFs.append([0., 0., 0.]) + curvAbsOutput.append(0.0) + # Completing + for i in range(0, nbFrames-1): + lengthIncr += frameIntervalDim + frameRigidDoFs.append([lengthIncr, 0., 0., 0., 0., 0., 1.]) + frame3DDoFs.append([lengthIncr, 0., 0.]) + frameEdges.append(i) + frameEdges.append(i+1) + curvAbsOutput.append(lengthIncr) + + return {'sectionLengths': beamLengths, 'curvAbsInput': curvAbsInput, 'sectionDoFs': beamStrainDoFs, + 'frameRigidDoFs': frameRigidDoFs, 'frame3DDoFs': frame3DDoFs, 'frameEdges': frameEdges, + 'curvAbsOutput': curvAbsOutput} + + +# TO DO : to be verified +def generateRegularBeamsWithSameNbFrames(totLength, nbBeams, nbFramesPerBeam): + + # This method generates the parameters to pass to a Cosserat mode. It basically distributes uniformly a required + # number of sections (Cosserat beams) and a required number of frames (Rigid3d), over a given length. + # All coordinated are computed by default along the X axis. From the distribution of sections and frames, + # different input parameters for the Cosserat components (forcefield, mapping) are returned: + # - The length of each section (BeamHookeLawForceField.length) + # - The curvilinear coordinate of each section extremities (DiscreteCosseratMapping.curv_abs_input) + # - The section DoFs (3 DoFs of strain for torsion and bending) + # - The Rigid3d DoFs representing the frames + # - The curvilinear coordinate of each frame (DiscreteCosseratMapping.curv_abs_output) + # - 3D position of the frames + edge topology connecting them (for collision modelling) + + beamLengths = [] + curvAbsInput = [] + beamStrainDoFs = [] + + frame6DDoFs = [] + frame3DDoFs = [] + frameEdgeList = [] + curvAbsOutput = [] + + # Computing section lengths and sections curvilinear coordinates + + individualBeamLength = totLength / nbBeams + lengthIncr = 0.0 + curvAbsInput.append(0.0) + for i in range(0, nbBeams): + beamLengths.append(individualBeamLength) + lengthIncr += individualBeamLength + curvAbsInput.append(lengthIncr) + beamStrainDoFs.append([0., 0., 0.]) + # NB: there are nbBeams+1 values in curvAbsInput, accounting for all section extremities + + # Computing frames Rigid3d coordinates and curvilinear coordinates + frameIntervalDim = individualBeamLength / nbFramesPerBeam + + for beamId in range(nbBeams): + lengthIncr = 0.0 + beamCurvAbs = curvAbsInput[beamId] + for frameId in range(nbFramesPerBeam): + currentFrameCurvAbs = beamCurvAbs+lengthIncr + frames6DDofs.append([currentFrameCurvAbs, 0., 0., 0., 0., 0., 1.]) + curvOutput.append(currentFrameCurvAbs) + frames3DDofs.append([currentFrameCurvAbs, 0., 0.]) + lengthIncr+=frameIntervalDim + + # Adding last frame + curvOutput.append(totLength) + frames3DDofs.append([totLength, 0., 0.]) + frames6DDofs.append([totLength, 0., 0., 0., 0., 0., 1.]) + + for i in range(0, len(frames3DDofs) - 1): + frameEdgeList.append(i) + frameEdgeList.append(i + 1) + + return {'beamLengths': beamLengths, 'curvAbsInput': curvAbsInput, 'beamStrainDoFs': beamStrainDoFs, + 'frame6DDoFs': frame6DDoFs, 'frame3DDoFs': frame3DDoFs, 'frameEdgeList': frameEdgeList, + 'curvAbsOutput': curvAbsOutput} diff --git a/python3/cosserat/plasticity/plasticBeamBendingControlled.py b/python3/cosserat/plasticity/plasticBeamBendingControlled.py new file mode 100644 index 00000000..1e3ec746 --- /dev/null +++ b/python3/cosserat/plasticity/plasticBeamBendingControlled.py @@ -0,0 +1,202 @@ +# -*- coding: utf-8 -*- +""" +Created on Thu Oct 5 2021 + +@author: ckrewcun +Most of the code is duplicated from python3/cosserat/scenes/testScenes/testCosseratScene +""" + +import Sofa +import os + +# constants +GRAVITY = 9810 +TOT_MASS = 0.1 +DENSITY = 0.02 +DT = 1e-2 + +# -------------------------------------# +# ----- Python controller ----- # +# -------------------------------------# + +class BendingController(Sofa.Core.Controller): + + def __init__(self, *args, **kwargs): + # These are needed (and the normal way to override from a python class) + Sofa.Core.Controller.__init__(self, *args, **kwargs) + + self.totalTime = 0.0 + self.nbIterations = 0 + self.bendingMoment = 2e8 + self.activated = True + + def onAnimateBeginEvent(self, event): # called at each begin of animation step + if self.nbIterations == 2030: + self.triggerBendingMoment() + + self.totalTime = self.totalTime + DT + self.nbIterations = self.nbIterations + 1 + + def onAnimateEndEvent(self, event): # called at each end of animation step + pass + + def onKeypressedEvent(self, event): + # Press L key triggers the creation of new objects in the scene + if event['key'] == 'F': + self.triggerBendingMoment() + + def triggerBendingMoment(self): + root = self.getContext() + with root.beamNode.rateAngularDeform.Moment.forces.writeable() as moment: + if (self.activated): + # Bending moment is currently activated, it should be deactivated + print("Bending moment deactivated") + moment[0] = [0.0, 0.0, 0.0] + self.activated = False + else: + # self.activated = False => bending moment should be reactivated + print("Bending moment activated") + moment[0] = [0.0, self.bendingMoment, self.bendingMoment] + self.activated = True + +# -------------------------------------# +# ----- SOFA scene ----- # +# -------------------------------------# + +pluginNameList = 'Sofa.Component.LinearSolver.Direct' \ + ' Sofa.Component.MechanicalLoad' \ + ' Sofa.Component.ODESolver.Backward' \ + ' Sofa.Component.SolidMechanics.Spring' \ + ' Sofa.Component.StateContainer' \ + ' Sofa.Component.Visual' \ + ' SofaPython3 CosseratPlugin' + +visualFlagList = 'showVisualModels showBehaviorModels showCollisionModels' \ + ' hideBoundingCollisionModels hideForceFields' \ + ' hideInteractionForceFields hideWireframe' \ + ' showMechanicalMappings' + +def createScene(rootNode): + + rootNode.addObject('RequiredPlugin', pluginName=pluginNameList, printLog='0') + rootNode.addObject('VisualStyle', displayFlags=visualFlagList) + + rootNode.addObject('DefaultVisualManagerLoop') + rootNode.findData('dt').value = DT + rootNode.findData('gravity').value = [0., 0., -GRAVITY] + + rootNode.addObject('DefaultAnimationLoop') + + # ----- Python controller ----- # + + rootNode.addObject(BendingController(name="BendingController")) + + # ----- Plastic Cosserat beam parameters ----- # + + # Define: the total length of the beam + totalLength = 100.0 + + # Define: the number of section, the total length and the length of each beam. + nbBeams = 1 + oneBeamLength = totalLength / nbBeams + + # Define: the number of frame and the length between each frame. + nbFrames = 20 + distBetweenFrames = totalLength / nbFrames + + # ----- Rigid base ----- # + + beamNode = rootNode.addChild('beamNode') + beamNode.addObject('EulerImplicitSolver', rayleighStiffness="1.2", rayleighMass='1.1') + beamNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + + rigidBaseNode= beamNode.addChild('rigidBase') + RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', + name="RigidBaseMO", position=[0., 0., 0., 0, 0, 0, 1], showObject=1, + showObjectScale=2.) + rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness="5.e8", angularStiffness="5.e8", + external_points="0", mstate="@RigidBaseMO", points="0", template="Rigid3d") + + # ----- Rate of angular deformation ----- # + # Define the length of each beam in a list, the positions of each beam + + beamStrainDoFs = [] + beamLengths = [] + sum = 0. + beamCurvAbscissa = [] + beamCurvAbscissa.append(0.0) + for i in range(nbBeams): + beamStrainDoFs.append([0, 0, 0]) + beamLengths.append(oneBeamLength) + sum += beamLengths[i] + beamCurvAbscissa.append(sum) + beamCurvAbscissa[nbBeams] = totalLength + + # Define angular rate which is the torsion(x) and bending (y, z) of each section + rateAngularDeformNode = beamNode.addChild('rateAngularDeform') + rateAngularDeformMO = rateAngularDeformNode.addObject('MechanicalObject', + template='Vec3d', + name='rateAngularDeformMO', + position=beamStrainDoFs, + showIndices=0, + rest_position=[0.0, 0.0, 0.0]) + + beamCrossSectionShape='circular' + sectionRadius = 2.0 + poissonRatio = 0.45 + beamPoissonRatioList = [poissonRatio]*nbBeams + youngModulus = 5.0e6 + beamYoungModulusList = [youngModulus]*nbBeams + yieldStress = 5.0e4 + yieldStressList = [yieldStress]*nbBeams + plasticModulus = 2.0e5 + plasticModulusList = [plasticModulus]*nbBeams + hardeningCoeff = 0.5 + hardeningCoefficientList = [hardeningCoeff]*nbBeams + rateAngularDeformNode.addObject('BeamPlasticLawForceField', name="beamForceField", + crossSectionShape=beamCrossSectionShape, + radius=sectionRadius, variantSections="true", + length=beamLengths, poissonRatioList=beamPoissonRatioList, + youngModulusList=beamYoungModulusList, + initialYieldStresses=yieldStressList, + plasticModuli=plasticModulusList, + mixedHardeningCoefficients= hardeningCoefficientList) + + rateAngularDeformNode.addObject('ConstantForceField', name='Moment', indices="0", + forces="0 2e8 2e8") + + # ----- Frames ----- # + + # Define local frames related to each section and parameters framesCurvAbscissa + frames6DDoFs = [] + frames3DDoFs = [] + framesCurvAbscissa = [] + for i in range(nbFrames): + sol = i * distBetweenFrames + frames3DDoFs.append([sol, 0, 0]) + frames6DDoFs.append([sol, 0, 0, 0, 0, 0, 1]) + framesCurvAbscissa.append(sol) + + frames6DDoFs.append([totalLength, 0, 0, 0, 0, 0, 1]) + framesCurvAbscissa.append(totalLength) + + # The node of the frame needs to inherit from rigidBaseMO and rateAngularDeform + mappedFrameNode = rigidBaseNode.addChild('MappedFrames') + rateAngularDeformNode.addChild(mappedFrameNode) + + framesMO = mappedFrameNode.addObject('MechanicalObject', template='Rigid3d', + name="FramesMO", position=frames6DDoFs, + showObject=1, showObjectScale=1) + + # The mapping has two inputs: RigidBaseMO and rateAngularDeformMO + # one output: FramesMO + inputMO = rateAngularDeformMO.getLinkPath() + inputMO_rigid = RigidBaseMO.getLinkPath() + outputMO = framesMO.getLinkPath() + + mappedFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=beamCurvAbscissa, + curv_abs_output=framesCurvAbscissa, input1=inputMO, input2=inputMO_rigid, + output=outputMO, forcefield='@../../rateAngularDeform/beamForceField', + nonColored=False, debug=0) + + return rootNode diff --git a/python3/cosserat/plasticity/plasticBeamBendingControlledComparison.py b/python3/cosserat/plasticity/plasticBeamBendingControlledComparison.py new file mode 100644 index 00000000..a65b1b4f --- /dev/null +++ b/python3/cosserat/plasticity/plasticBeamBendingControlledComparison.py @@ -0,0 +1,293 @@ +# -*- coding: utf-8 -*- +""" +Created on Thu Oct 5 2021 + +@author: ckrewcun +Most of the code is duplicated from python3/cosserat/scenes/testScenes/testCosseratScene +""" + +import Sofa +import os + +# constants +GRAVITY = 9810 +TOT_MASS = 0.1 +DENSITY = 0.02 +DT = 1e-2 + +# -------------------------------------# +# ----- Python controller ----- # +# -------------------------------------# + +class BendingController(Sofa.Core.Controller): + + def __init__(self, *args, **kwargs): + # These are needed (and the normal way to override from a python class) + Sofa.Core.Controller.__init__(self, *args, **kwargs) + + self.totalTime = 0.0 + self.nbIterations = 0 + self.bendingMoment = 2e8 + self.activated = True + + def onAnimateBeginEvent(self, event): # called at each begin of animation step + if self.nbIterations == 2030: + self.triggerBendingMoment() + + self.totalTime = self.totalTime + DT + self.nbIterations = self.nbIterations + 1 + + def onAnimateEndEvent(self, event): # called at each end of animation step + pass + + def onKeypressedEvent(self, event): + # Press L key triggers the creation of new objects in the scene + if event['key'] == 'F': + self.triggerBendingMoment() + + def triggerBendingMoment(self): + root = self.getContext() + with root.beamNode.rateAngularDeform.Moment.forces.writeable() as moment: + with root.elaBeamNode.rateAngularDeform.Moment.forces.writeable() as elasticMoment: + if (self.activated): + # Bending moment is currently activated, it should be deactivated + print("Bending moment deactivated") + moment[0] = [0.0, 0.0, 0.0] + elasticMoment[0] = [0.0, 0.0, 0.0] + self.activated = False + else: + # self.activated = False => bending moment should be reactivated + print("Bending moment activated") + moment[0] = [0.0, self.bendingMoment, self.bendingMoment] + elasticMoment[0] = [0.0, self.bendingMoment, self.bendingMoment] + self.activated = True + +# -------------------------------------# +# ----- SOFA scene ----- # +# -------------------------------------# + +pluginNameList = 'Sofa.Component.LinearSolver.Direct' \ + ' Sofa.Component.MechanicalLoad' \ + ' Sofa.Component.ODESolver.Backward' \ + ' Sofa.Component.SolidMechanics.Spring' \ + ' Sofa.Component.StateContainer' \ + ' Sofa.Component.Visual' \ + ' SofaPython3 CosseratPlugin' + +visualFlagList = 'showVisualModels showBehaviorModels showCollisionModels' \ + ' hideBoundingCollisionModels hideForceFields' \ + ' hideInteractionForceFields hideWireframe' \ + ' showMechanicalMappings' + +def createScene(rootNode): + + rootNode.addObject('RequiredPlugin', pluginName=pluginNameList, printLog='0') + rootNode.addObject('VisualStyle', displayFlags=visualFlagList) + + rootNode.addObject('DefaultVisualManagerLoop') + rootNode.findData('dt').value = DT + rootNode.findData('gravity').value = [0., 0., -GRAVITY] + + rootNode.addObject('DefaultAnimationLoop') + + # ----- Python controller ----- # + + rootNode.addObject(BendingController(name="BendingController")) + + # -------------------------------------# + # ----- Plastic Cosserat beam ----- # + # -------------------------------------# + + # ----- Cosserat beam parameters ----- # + + # Define: the total length of the beam + totalLength = 100.0 + + # Define: the number of section, the total length and the length of each beam. + nbBeams = 1 + oneBeamLength = totalLength / nbBeams + + # Define: the number of frame and the length between each frame. + nbFrames = 20 + distBetweenFrames = totalLength / nbFrames + + # ----- Rigid base ----- # + + beamNode = rootNode.addChild('beamNode') + beamNode.addObject('EulerImplicitSolver', rayleighStiffness="1.2", rayleighMass='1.1') + beamNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + + rigidBaseNode= beamNode.addChild('rigidBase') + RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', + name="RigidBaseMO", position=[0., 0., 0., 0, 0, 0, 1], + showObject=1, + showObjectScale=2.) + rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness="5.e8", angularStiffness="5.e8", + external_points="0", mstate="@RigidBaseMO", points="0", template="Rigid3d") + + # ----- Rate of angular deformation ----- # + # Define the length of each beam in a list, the positions of each beam + + beamStrainDoFs = [] + beamLengths = [] + sum = 0. + beamCurvAbscissa = [] + beamCurvAbscissa.append(0.0) + for i in range(nbBeams): + beamStrainDoFs.append([0, 0, 0]) + beamLengths.append(oneBeamLength) + sum += beamLengths[i] + beamCurvAbscissa.append(sum) + beamCurvAbscissa[nbBeams] = totalLength + + # Define angular rate which is the torsion(x) and bending (y, z) of each section + rateAngularDeformNode = beamNode.addChild('rateAngularDeform') + rateAngularDeformMO = rateAngularDeformNode.addObject('MechanicalObject', + template='Vec3d', + name='rateAngularDeformMO', + position=beamStrainDoFs, + showIndices=0, + rest_position=[0.0, 0.0, 0.0]) + + beamCrossSectionShape='circular' + sectionRadius = 2.0 + poissonRatio = 0.45 + beamPoissonRatioList = [poissonRatio]*nbBeams + youngModulus = 5.0e6 + beamYoungModulusList = [youngModulus]*nbBeams + yieldStress = 5.0e4 + yieldStressList = [yieldStress]*nbBeams + plasticModulus = 2.0e5 + plasticModulusList = [plasticModulus]*nbBeams + hardeningCoeff = 0.5 + hardeningCoefficientList = [hardeningCoeff]*nbBeams + rateAngularDeformNode.addObject('BeamPlasticLawForceField', name="beamForceField", + crossSectionShape=beamCrossSectionShape, + radius=sectionRadius, variantSections="true", + length=beamLengths, poissonRatioList=beamPoissonRatioList, + youngModulusList=beamYoungModulusList, + initialYieldStresses=yieldStressList, + plasticModuli=plasticModulusList, + mixedHardeningCoefficients= hardeningCoefficientList) + + rateAngularDeformNode.addObject('ConstantForceField', name='Moment', indices="0", + forces="0 2e8 2e8") + + # ----- Frames ----- # + + # Define local frames related to each section and parameters framesCurvAbscissa + frames6DDoFs = [] + frames3DDoFs = [] + framesCurvAbscissa = [] + for i in range(nbFrames): + sol = i * distBetweenFrames + frames3DDoFs.append([sol, 0, 0]) + frames6DDoFs.append([sol, 0, 0, 0, 0, 0, 1]) + framesCurvAbscissa.append(sol) + + frames6DDoFs.append([totalLength, 0, 0, 0, 0, 0, 1]) + framesCurvAbscissa.append(totalLength) + + # The node of the frame needs to inherit from rigidBaseMO and rateAngularDeform + mappedFrameNode = rigidBaseNode.addChild('MappedFrames') + rateAngularDeformNode.addChild(mappedFrameNode) + + framesMO = mappedFrameNode.addObject('MechanicalObject', template='Rigid3d', + name="FramesMO", position=frames6DDoFs, + showObject=1, showObjectScale=1) + + # The mapping has two inputs: RigidBaseMO and rateAngularDeformMO + # one output: FramesMO + inputMO = rateAngularDeformMO.getLinkPath() + inputMO_rigid = RigidBaseMO.getLinkPath() + outputMO = framesMO.getLinkPath() + + mappedFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=beamCurvAbscissa, + curv_abs_output=framesCurvAbscissa, input1=inputMO, input2=inputMO_rigid, + output=outputMO, forcefield='@../../rateAngularDeform/beamForceField', + nonColored=False, debug=0) + + # -------------------------------------# + # ----- Elastic Cosserat beam ----- # + # -------------------------------------# + + # ----- Cosserat beam parameters ----- # + + # Same parameters as for the plastic beam : + # totalLength = 100.0 + # nbBeams = 1 + # oneBeamLength = totalLength / nbBeams + # nbFrames = 20 + # distBetweenFrames = totalLength / nbFrames + + # ----- Rigid base ----- # + + elaBeamNode = rootNode.addChild('elaBeamNode') + elaBeamNode.addObject('EulerImplicitSolver', rayleighStiffness="1.2", rayleighMass='1.1') + elaBeamNode.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") + + rigidBaseNode= elaBeamNode.addChild('rigidBase') + RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', + name="RigidBaseMO", position=[0., 0., 10., 0, 0, 0, 1], + showObject=1, + showObjectScale=2.) + rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness="5.e8", angularStiffness="5.e8", + external_points="0", mstate="@RigidBaseMO", points="0", template="Rigid3d") + + # ----- Rate of angular deformation ----- # + # Define: the length of each beam in a list, the positions of each beam + + # beamStrainDoFs, beamLengths, beamCurvAbscissa are the same as for the plastic beam + + rateAngularDeformNode = elaBeamNode.addChild('rateAngularDeform') + rateAngularDeformMO = rateAngularDeformNode.addObject('MechanicalObject', + template='Vec3d', + name='rateAngularDeformMO', + position=beamStrainDoFs, + showIndices=0, + rest_position=[0.0, 0.0, 0.0]) + + # Elastic behaviour law + rateAngularDeformNode.addObject('BeamHookeLawForceField', + name='hookeBeamForceField', + crossSectionShape='circular', length=beamLengths, + radius=2., youngModulus=5e6) + rateAngularDeformNode.addObject('ConstantForceField', name='Moment', indices="0", + forces="0 2e8 2e8") + + # ----- Frames ----- # + + # Define local frames related to each section and parameters curv_abs_outputF + frames6DDoFs = [] + frames3DDoFs = [] + framesCurvAbscissa = [] + for i in range(nbFrames): + sol = i * distBetweenFrames + frames3DDoFs.append([sol, 0, 10]) + frames6DDoFs.append([sol, 0, 10, 0, 0, 0, 1]) + framesCurvAbscissa.append(sol) + + frames6DDoFs.append([totalLength, 0, 10, 0, 0, 0, 1]) + framesCurvAbscissa.append(totalLength) + + # The node of the frame needs to inherit from rigidBaseMO and rateAngularDeform + mappedFrameNode = rigidBaseNode.addChild('MappedFrames') + rateAngularDeformNode.addChild(mappedFrameNode) + + framesMO = mappedFrameNode.addObject('MechanicalObject', template='Rigid3d', + name="FramesMO", position=frames6DDoFs, + showObject=1, showObjectScale=1) + + # The mapping has two inputs: RigidBaseMO and rateAngularDeformMO + # one output: FramesMO + inputMO = rateAngularDeformMO.getLinkPath() + inputMO_rigid = RigidBaseMO.getLinkPath() + outputMO = framesMO.getLinkPath() + + mappedFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=beamCurvAbscissa, + curv_abs_output=framesCurvAbscissa, input1=inputMO, input2=inputMO_rigid, + output=outputMO, forcefield='@../../rateAngularDeform/hookeBeamForceField', + nonColored=False, debug=0) + + + return rootNode diff --git a/python3/cosserat/usefulFunctions.py b/python3/cosserat/usefulFunctions.py index ac3183b1..42227009 100644 --- a/python3/cosserat/usefulFunctions.py +++ b/python3/cosserat/usefulFunctions.py @@ -29,12 +29,16 @@ draw_cylinder = 1 add_collision_point = 1 -pluginNameList = 'SofaConstraint SofaDeformable SofaImplicitOdeSolver SofaMeshCollision SofaPreconditioner' \ - ' SofaGeneralTopology SofaOpenglVisual SofaGeneralRigid SoftRobots SofaSparseSolver' \ - ' CosseratPlugin SofaBoundaryCondition SofaGeneralAnimationLoop SofaRigid' # BeamAdapter -pluginList = ['SofaConstraint', 'SofaDeformable', 'SofaImplicitOdeSolver', 'SofaMeshCollision', 'SofaPreconditioner', - 'SofaGeneralTopology', 'SofaOpenglVisual', 'SofaGeneralRigid', 'SoftRobots', 'SofaSparseSolver', - 'CosseratPlugin', 'SofaBoundaryCondition', 'SofaGeneralAnimationLoop', 'SofaRigid', 'EigenLinearSolvers'] +pluginList = ['SofaPreconditioner', 'Sofa.Component.IO.Mesh', 'Sofa.Component.Mass', 'Sofa.Component.Topology.Container.Dynamic', 'Sofa.GL.Component.Rendering3D', + 'SoftRobots', + 'Sofa.Component.Collision.Geometry', 'Sofa.Component.Collision.Detection.Intersection', 'Sofa.Component.Collision.Response.Mapper', 'Sofa.Component.Collision.Response.Contact','Sofa.Component.Constraint.Lagrangian.Correction', 'Sofa.Component.SceneUtility', 'Sofa.Component.SolidMechanics.FEM.Elastic', + 'CosseratPlugin', 'Sofa.Component.AnimationLoop', 'Sofa.Component.Constraint.Lagrangian.Solver', 'Sofa.Component.Engine.Select', 'Sofa.Component.Setting', + "Sofa.Component.LinearSolver.Direct", "Sofa.Component.MechanicalLoad", "Sofa.Component.ODESolver.Backward", "Sofa.Component.SolidMechanics.Spring", "Sofa.Component.Visual", 'Sofa.Component.Collision.Detection.Algorithm', 'Sofa.Component.Mapping', + "Sofa.Component.Collision.Detection.Intersection", "Sofa.Component.Collision.Geometry", "Sofa.Component.Collision.Response.Contact", "Sofa.Component.Mapping.MappedMatrix", "Sofa.Component.Topology.Container.Grid","Sofa.Component.Mapping.NonLinear", + "Sofa.Component.Constraint.Lagrangian.Model", + "Sofa.Component.Collision.Response.Mapper", + "Sofa.Component.Topology.Mapping", "Sofa.Component.LinearSolver.Iterative"] + PRig = 0.38 # poison ratio for the rigid part of the beam PSoft = 0.43 # poison ratio for the soft part of the beam @@ -90,7 +94,8 @@ def __init__(self, *args, **kwargs): self.rigidBaseNode = args[0] self.rateAngularDeformNode = args[1] self.frameNode = args[0].getChild('MappedFrames') - self.cable3DPosNode = self.frameNode.getChild('CollisInstrumentCombined') + self.cable3DPosNode = self.frameNode.getChild( + 'CollisInstrumentCombined') self.rigidBaseMO = self.rigidBaseNode.getChild('RigidBaseMO') self.dX = 0.0025 self.angularRate = 15. @@ -156,15 +161,18 @@ def computeCosseratParameters(self): def createCylinderNode(self, length=0.5, abscissa=0.0, index=0): translation = [abscissa, 0., 0.] color = 'grey' if length == 0.5 else 'blue' - CylinderFEMNode = self.frameNode.addChild('CylinderFEMNode' + str(index)) + CylinderFEMNode = self.frameNode.addChild( + 'CylinderFEMNode' + str(index)) CylinderFEMNode.addObject('CylinderGridTopology', name="grid", nx="5", ny="5", nz="8", length=length, radius=0.25, axis="1 0 0") CylinderFEMNode.addObject('MeshTopology', src="@grid") - CylinderFEMNode.addObject('MechanicalObject', name="cylinder", template="Vec3d", translation=translation) + CylinderFEMNode.addObject( + 'MechanicalObject', name="cylinder", template="Vec3d", translation=translation) CylinderFEMNode.addObject('SkinningMapping', nbRef='1') Visu = CylinderFEMNode.addChild('Visu') Visu.addObject('OglModel', name="Visual", color=color) - Visu.addObject('IdentityMapping', input="@../cylinder", output="@Visual") + Visu.addObject('IdentityMapping', + input="@../cylinder", output="@Visual") def initGraph(self): # @info compute cosserat parameters @@ -173,9 +181,12 @@ def initGraph(self): self.frameNode.mapping.curv_abs_output.value = self.curvOut self.frameNode.mapping.curv_abs_input.value = self.curvInput self.rateAngularDeformNode.rateAngularDeformMO.position.value = self.rateAngular - self.rateAngularDeformNode.beamHookeLaw.findData('length').value = rateLength - self.rateAngularDeformNode.beamHookeLaw.findData('youngModululsList').value = youngModulusList - self.rateAngularDeformNode.beamHookeLaw.findData('poissonRatioList').value = poisonRatioList + self.rateAngularDeformNode.beamHookeLaw.findData( + 'length').value = rateLength + self.rateAngularDeformNode.beamHookeLaw.findData( + 'youngModululsList').value = youngModulusList + self.rateAngularDeformNode.beamHookeLaw.findData( + 'poissonRatioList').value = poisonRatioList if draw_cylinder == 1: # Draw the cylinder over the beam @@ -280,7 +291,8 @@ def buildPlasticity(self): # print("=+++++> idx: {} and rate is : {}".format(idx,pos[self.insertionIndex])) if abs(pos[self.insertionIndex]) > self.elsaticityRate: # print("=+++++> idx: {} and rate is : {}".format(idx,pos[self.insertionIndex])) - restPos[idx][self.insertionIndex] = pos[self.insertionIndex] - self.elsaticityRate + restPos[idx][self.insertionIndex] = pos[self.insertionIndex] - \ + self.elsaticityRate # print("The positions: {} restPos: {}".format(pos[self.insertionIndex], restPos[idx][self.insertionIndex])) """ ============================================================ @@ -293,7 +305,8 @@ def usePlasticityWithTable(self): with self.mechanicalObjectMO.rest_position.writeable() as restPos: for idx, pos in enumerate(positions): if abs(pos[self.insertionIndex]) > plasticityTab[idx]: - restPos[idx][self.insertionIndex] = pos[self.insertionIndex] - plasticityTab[idx] + restPos[idx][self.insertionIndex] = pos[self.insertionIndex] - \ + plasticityTab[idx] # ####################### @@ -346,7 +359,8 @@ def __init__(self, *args, **kwargs): Sofa.Core.Controller.__init__(self, *args, **kwargs) self.targetMO = args[0] self.targetIndex = 0 - self.targetList = [[85.0, 0, 0.35857], [85.0, 0.5, 0.35857], [86.0, 1.5, 0.35857], [87.0, 2.5, 0.35857]] + self.targetList = [[85.0, 0, 0.35857], [85.0, 0.5, 0.35857], [ + 86.0, 1.5, 0.35857], [87.0, 2.5, 0.35857]] pos = self.targetMO.findData('effectorGoal').value print("effectorGoal :", pos[0]) @@ -398,7 +412,8 @@ def onKeypressedEvent(self, event): with self.inputConstraintPointsMO.position.writeable() as posA: print("0. List The position :", posA) - self.inputConstraintPointsMO.position.value = np.array([[40, 0, 0], [45, 0, 0]]) + self.inputConstraintPointsMO.position.value = np.array( + [[40, 0, 0], [45, 0, 0]]) with self.inputConstraintPointsMO.position.writeable() as posA: print("1. List The position :", posA) @@ -409,7 +424,8 @@ def onKeypressedEvent(self, event): # posA.append(self.needleCollisionMO.position[self.tipIndex]) indice = self.diffMapping.findData('indices') - print("====> indices :", self.diffMapping.addPointProcess([40.0, 0., 0.])) + print("====> indices :", + self.diffMapping.addPointProcess([40.0, 0., 0.])) if key == "-": # - with self.inputConstraintPointsMO.rest_position.writeable() as posA: @@ -444,7 +460,8 @@ def Cube( # cubeNode.addObject('UncoupledConstraintCorrection') objectCollis = cubeNode.addChild('collision') - objectCollis.addObject('MeshObjLoader', name="loader", filename=surfaceMeshFileName, triangulate="true") + objectCollis.addObject('MeshObjLoader', name="loader", + filename=surfaceMeshFileName, triangulate="true") objectCollis.addObject('MeshTopology', src="@loader") objectCollis.addObject('MechanicalObject') objectCollis.addObject('TriangleCollisionModel') diff --git a/python3/cosserat/utils.py b/python3/cosserat/utils.py index 72ad8aae..c832564f 100644 --- a/python3/cosserat/utils.py +++ b/python3/cosserat/utils.py @@ -1,293 +1,63 @@ -from robocop.usefulFunction import BuildCosseratGeometry, buildEdges - - -def addSofaPlugins(node, newPlugins): - plugins = node.SofaPlugins.pluginName.value - for plugin in newPlugins.split(): - plugins.append(plugin) - node.SofaPlugins.pluginName = plugins - - -def addRigidObject(node, filename, collisionFilename=None, position=None, scale=None, - textureFilename='', color=None, density=0.002, name='Object', withSolver=True): - if color is None: - color = [1, 1, 1] - if scale is None: - scale = [1, 1, 1] - if position is None: - position = [0, 0, 0, 0, 0, 0, 1] - if collisionFilename is None: - collisionFilename = filename - - _object = node.addChild(name) - _object.addObject('RequiredPlugin', name='SofaPlugins', pluginName='SofaRigid SofaLoader') - _object.addObject('MechanicalObject', template='Rigid3', position=position, showObject=False, showObjectScale=5) - - if withSolver: - _object.addObject('EulerImplicitSolver') - _object.addObject('CGLinearSolver', tolerance=1e-5, iterations=25) - _object.addObject('UncoupledConstraintCorrection') - - visu = _object.addChild('Visu') - visu.addObject('MeshObjLoader', name='loader', filename=filename, scale3d=scale) - visu.addObject('OglModel', src='@loader', texturename=textureFilename, color=color if textureFilename == '' else '') - visu.addObject('RigidMapping') - - _object.addObject('GenerateRigidMass', name='mass', density=density, src=visu.loader.getLinkPath()) - _object.mass.init() - translation = list(_object.mass.centerToOrigin.value) - _object.addObject('UniformMass', vertexMass="@mass.rigidMass") - - visu.loader.translation = translation - - collision = _object.addChild('Collision') - collision.addObject('MeshObjLoader', name='loader', filename=collisionFilename, scale3d=scale) - collision.addObject('MeshTopology', src='@loader') - collision.addObject('MechanicalObject', translation=translation) - collision.addObject('TriangleCollisionModel') - collision.addObject('LineCollisionModel') - collision.addObject('PointCollisionModel') - collision.addObject('RigidMapping') - - return _object +import numpy as np def addEdgeCollision(parentNode, position3D, edges): - CollisInstrumentCombined = parentNode.addChild('CollisInstrumentCombined') - CollisInstrumentCombined.addObject('EdgeSetTopologyContainer', name="collisEdgeSet", position=position3D, + collisInstrumentCombined = parentNode.addChild('collisInstrumentCombined') + collisInstrumentCombined.addObject('EdgeSetTopologyContainer', name="collisEdgeSet", position=position3D, edges=edges) - CollisInstrumentCombined.addObject('EdgeSetTopologyModifier', name="collisEdgeModifier") - CollisInstrumentCombined.addObject('MechanicalObject', name="CollisionDOFs") - CollisInstrumentCombined.addObject('LineCollisionModel', bothSide="1", group='2') - CollisInstrumentCombined.addObject('PointCollisionModel', bothSide="1", group='2') - CollisInstrumentCombined.addObject('IdentityMapping', name="mapping") - - -def createCosserat(parent, config, config_material, config_simu, name="Cosserat", orientation=None, radius=0., - last_frame=None, rigidBase=None, _showObjectScale=0.02, solver=None, fixedBase=True): - if last_frame is None: - last_frame = [] - if orientation is None: - orientation = [0, 0, 0, 1] - [x, y, z] = config['init_pos'] - buildCollision = config['collision'] - - Young_modulus = config_material['youngModulus'] - poissonRatio = config_material['poissonRatio'] - length_Y = config_material['length_Y'] - length_Z = config_material['length_Z'] - shape = config_material['shape'] - - # some parameters - stiffness = config_simu['stiffness'] - angularStiffness = config_simu['angularStiffness'] - - # ################################ - # # RigidBase ## - # ################################ - - if parent.hasObject("EulerImplicitSolver") is False: - base = parent.addChild(name) - # base.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.1') - base.addObject('EulerImplicitSolver', rayleighStiffness="1.2", rayleighMass='1.1') - base.addObject('SparseLDLSolver', name='solver', template="CompressedRowSparseMatrixd") - base.addObject('GenericConstraintCorrection') - else: - # This means you already defined the solver node and give it as parent of this function - base = parent.addChild(name) - - # base.addObject('EulerImplicitSolver', rayleighStiffness="0.2", rayleighMass='0.1') - - if rigidBase is None: - rigidBaseNode = base.addChild('rigidBase') - - RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', - name="RigidBaseMO", position=[x, y, z] + orientation, showObject=1, - showObjectScale=0.2) - if fixedBase: - rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring' + str(x), stiffness=stiffness, - angularStiffness=angularStiffness, external_points=0, mstate="@RigidBaseMO", - points=0, - template="Rigid3d") - else: - rigidBaseNode = base.addChild(rigidBase) - RigidBaseMO = rigidBase.RigidBaseMO - - [positionS, curv_abs_inputS, longeurS, framesF, curv_abs_outputF, frames3D] = \ - BuildCosseratGeometry(config) - # ################################ - # # Rate of angular Deformation ## - # ################################ - rateAngularDeformNode = base.addChild('rateAngularDeform') - rateAngularDeformMO = rateAngularDeformNode.addObject('MechanicalObject', - template='Vec3d', name='rateAngularDeformMO', - position=positionS, showIndices=0) - rateAngularDeformNode.addObject('BeamHookeLawForceField', crossSectionShape=shape, - length=longeurS, youngModulus=Young_modulus, poissonRatio=poissonRatio, - lengthY=length_Y, lengthZ=length_Z) - # ################################ - # # Define Cosserat Frames ## - # ################################ - # The node of the frame needs to inherit from rigidBaseMO and rateAngularDeform - mappedFrameNode = rigidBaseNode.addChild('MappedFrames') - rateAngularDeformNode.addChild(mappedFrameNode) - framesMO = mappedFrameNode.addObject('MechanicalObject', template='Rigid3d', - name="FramesMO", position=framesF, - showObject=1, showObjectScale=_showObjectScale) - mappedFrameNode.addObject('UniformMass', totalMass="0.00022", showAxisSizeFactor='0') - # The mapping has two inputs: RigidBaseMO and rateAngularDeformMO - # one output: FramesMO - inputMO = rateAngularDeformMO.getLinkPath() - inputMO_rigid = RigidBaseMO.getLinkPath() - outputMO = framesMO.getLinkPath() - - mappedFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=curv_abs_inputS, - curv_abs_output=curv_abs_outputF, input1=inputMO, input2=inputMO_rigid, - output=outputMO, debug=0, radius=radius) + collisInstrumentCombined.addObject('EdgeSetTopologyModifier', name="collisEdgeModifier") + collisInstrumentCombined.addObject('MechanicalObject', name="CollisionDOFs") + collisInstrumentCombined.addObject('LineCollisionModel', bothSide="1", group='2') + collisInstrumentCombined.addObject('PointCollisionModel', group='2') + collisInstrumentCombined.addObject('IdentityMapping', name="mapping") + return collisInstrumentCombined + + +# @info: This function is used to build the beam collision node +def addPointsCollision(parentNode, position3D, edges, nodeName): + collisInstrumentCombined = parentNode.addChild(nodeName) + collisInstrumentCombined.addObject('EdgeSetTopologyContainer', name="beamContainer", position=position3D, + edges=edges) + collisInstrumentCombined.addObject('EdgeSetTopologyModifier', name="beamModifier") + collisInstrumentCombined.addObject('MechanicalObject', name="collisionStats", showObject=False, showIndices=False) + collisInstrumentCombined.addObject('PointCollisionModel', name="beamColMod", group='2') + # collisInstrumentCombined.addObject('IdentityMapping', name="beamMapping") + collisInstrumentCombined.addObject('RigidMapping', name="beamMapping") + return collisInstrumentCombined - """====================================================""" - """ =+++> actuator collision =+++++<==""" - """====================================================""" - if buildCollision: - tab_edges = buildEdges(frames3D) - addEdgeCollision(mappedFrameNode, frames3D, tab_edges) - base.addObject('MechanicalMatrixMapper', template='Vec3,Rigid3', object1=inputMO, - object2=inputMO_rigid, nodeToParse=mappedFrameNode.getLinkPath()) - return base +# @info: This function is used to build the constraint node +def addConstraintPoint(parentNode, beamPath): + constraintPointsNode = parentNode.addChild('constraintPoints') + constraintPointsNode.addObject("PointSetTopologyContainer", name="constraintPtsContainer", listening="1") + constraintPointsNode.addObject("PointSetTopologyModifier", name="constraintPtsModifier", listening="1") + constraintPointsNode.addObject("MechanicalObject", template="Vec3d", showObject=True, showIndices=True, + name="constraintPointsMo", position=[], showObjectScale=0, listening="1") + # print(f' ====> The beamTip tip is : {dir(beamPath)}') + constraintPointsNode.addObject('PointsManager', name="pointsManager", listening="1", + beamPath="/solverNode/needle/rigidBase/cosseratInSofaFrameNode/slidingPoint" + "/slidingPointMO") -def createCosserat_2(parent, config, config_material, name="Cosserat", orientation=[0, 0, 0, 1], radius=0.01, - last_frame=[], rigidBase=None, controlePointMo=None): - [x, y, z, q0, q1, q2, q3] = config['init_pos'] - tot_length = config['tot_length'] - # buildCollision = config['collision'] + constraintPointsNode.addObject('BarycentricMapping', useRestPosition="false", listening="1") + return constraintPointsNode - Young_modulus = config_material['youngModulus'] - poissonRatio = config_material['poissonRatio'] - length_Y = config_material['length_Y'] - length_Z = config_material['length_Z'] - shape = config_material['shape'] - nbSectionS = config['nbSectionS'] - lengthS = tot_length / nbSectionS +def addSlidingPoints(parenNode, frames3D): + slidingPoint = parenNode.addChild('slidingPoint') + slidingPoint.addObject('MechanicalObject', name="slidingPointMO", position=frames3D, + showObject=False, showIndices=False) + slidingPoint.addObject('IdentityMapping') + return slidingPoint - nbFramesF = config['nbFramesF'] - lengthF = tot_length / nbFramesF - ################################# - ## RigidBase ## - ################################# - base = parent.addChild(name) - # base.addObject('EulerImplicitSolver', firstOrder=0, rayleighStiffness=0.2, rayleighMass=0.1) - # base.addObject('SparseLDLSolver', name='solver') - # base.addObject('GenericConstraintCorrection') +def getLastConstraintPoint(constraintPointsNode): + return constraintPointsNode.getObject('constraintPointsMo').position[-1] - if rigidBase is None: - rigidBaseNode = base.addChild('rigidBase') - RigidBaseMO = rigidBaseNode.addObject('MechanicalObject', template='Rigid3d', - name="RigidBaseMO", position=[x, y, z, q0, q1, q2, q3], showObject=0, - showObjectScale=2.) - # rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring' + str(x), stiffness=5000, - # angularStiffness=5000, external_points=0, mstate="@RigidBaseMO", points=0, - # template="Rigid3d") - if controlePointMo is None: - rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness="1e1", - angularStiffness="1e1", - external_points="0", mstate="@RigidBaseMO", points="0", template="Rigid3d") - else: - rigidBaseNode.addObject('RestShapeSpringsForceField', name='spring', stiffness="1.e6", - angularStiffness="1.e6", - external_rest_shape=controlePointMo, external_points="0", - mstate="@RigidBaseMO", points="0", template="Rigid3d") +def computeDistanceBetweenPoints(constraintPointPos, slidingPointPos): + if len(constraintPointPos) != 0: + return np.linalg.norm(constraintPointPos[-1] - slidingPointPos[-1]) else: - rigidBaseNode = base.addChild(rigidBase) - RigidBaseMO = rigidBase.RigidBaseMO - - ################################# - ## Rate of angular Deformation ## - ################################# - # Define: the length of each beam in a list, the positions of each beam - # (flexion, torsion), the abs of each section - positionS = [] - longeurS = [] - sum = x - curv_abs_inputS = [x] - # curv_abs_inputS.append(x) - - for i in range(nbSectionS): - positionS.append([0, 0, 0]) - longeurS.append((((i + 1) * lengthS) - i * lengthS)) - sum += longeurS[i] - curv_abs_inputS.append(sum) - - curv_abs_inputS[nbSectionS] = tot_length + x - - # Define: sofa elements - rateAngularDeformNode = base.addChild('rateAngularDeform') - rateAngularDeformMO = rateAngularDeformNode.addObject('MechanicalObject', - template='Vec3d', name='rateAngularDeformMO', - position=positionS, showIndices=0) - rateAngularDeformNode.addObject('BeamHookeLawForceField', crossSectionShape=shape, length=longeurS, - radius=radius, youngModulus=Young_modulus, poissonRatio=poissonRatio) - # ################################ - # # Frame ## - # ################################ - # Define: the abs of each frame and the position of each frame. - framesF = [] - frames3D = [] - curv_abs_outputF = [] - for i in range(nbFramesF): - sol = i * lengthF - framesF.append([sol + x, y, z, 0, 0, 0, 1]) - frames3D.append([sol + x, y, z]) - curv_abs_outputF.append(sol + x) - - framesF.append([tot_length + x, y, z, 0, 0, 0, 1]) - frames3D.append([tot_length + x, y, z]) - curv_abs_outputF.append(tot_length + x) - - # framesF = [[x, y, z, 0, 0, 0, 1]] + framesF - # curv_abs_outputF = [x] + curv_abs_outputF - - # The node of the frame needs to inherit from rigidBaseMO and rateAngularDeform - mappedFrameNode = rigidBaseNode.addChild('MappedFrames') - rateAngularDeformNode.addChild(mappedFrameNode) - framesMO = mappedFrameNode.addObject('MechanicalObject', template='Rigid3d', - name="FramesMO", position=framesF, - showObject=1, showObjectScale=3) - mappedFrameNode.addObject('UniformMass', totalMass="0.00022", showAxisSizeFactor='0') - # The mapping has two inputs: RigidBaseMO and rateAngularDeformMO - # one output: FramesMO - inputMO = rateAngularDeformMO.getLinkPath() - inputMO_rigid = RigidBaseMO.getLinkPath() - outputMO = framesMO.getLinkPath() - - mappedFrameNode.addObject('DiscreteCosseratMapping', curv_abs_input=curv_abs_inputS, - curv_abs_output=curv_abs_outputF, input1=inputMO, input2=inputMO_rigid, - output=outputMO, debug=0, radius=0) - - # for i, orient in enumerate(last_frame): - # lastFrame = base.addChild('lastFrame_' + str(i)) - # lastFrameMo = lastFrame.addObject('MechanicalObject', template='Rigid3d', - # name="RigidBaseMO", position=[x + tot_length, y, z] + orient, showObject=0, - # showObjectScale=2.) - # lastFrame.addObject("RigidRigidMapping", name="mapLastFrame", - # input=framesMO.getLinkPath(), - # output=lastFrameMo.getLinkPath(), index=nbFramesF + 1, - # globalToLocalCoords=True) - base.addObject('MechanicalMatrixMapper', template='Vec3,Rigid3', object1=inputMO, - object2=inputMO_rigid, nodeToParse=mappedFrameNode.getLinkPath()) - - # if buildCollision: - # tab_edges = buildEdges(frames3D) - # CollisInstrumentCombined = mappedFrameNode.addChild('CollisInstrumentCombined') - # CollisInstrumentCombined.addObject('EdgeSetTopologyContainer', name="collisEdgeSet", position=frames3D, - # edges=tab_edges) - # CollisInstrumentCombined.addObject('EdgeSetTopologyModifier', name="colliseEdgeModifier") - # CollisInstrumentCombined.addObject('MechanicalObject', name="CollisionDOFs") - # CollisInstrumentCombined.addObject('LineCollisionModel', bothSide="1", group='2', proximity="0.01") - # CollisInstrumentCombined.addObject('PointCollisionModel', bothSide="1", group='2') - # CollisInstrumentCombined.addObject('IdentityMapping', name="mapping") - return base + # print("No constraint points yet") + return 0 diff --git a/src/CosseratPlugin/Binding/Binding_PointsManager.cpp b/src/CosseratPlugin/Binding/Binding_PointsManager.cpp new file mode 100644 index 00000000..1301d0cc --- /dev/null +++ b/src/CosseratPlugin/Binding/Binding_PointsManager.cpp @@ -0,0 +1,50 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2021 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#include +#include +#include +#include "Binding_PointsManager.h" +#include +#include + +typedef sofa::core::behavior::PointsManager PointsManager; + +namespace py { +using namespace pybind11; +} + +using namespace sofa::core::objectmodel; +using namespace sofa::core::topology; + +namespace sofapython3 { + +void moduleAddPointsManager(py::module& m) { + py::class_> c(m, "PointsManager"); + + /// register the PointSetTopologyModifier binding in the downcasting subsystem + PythonFactory::registerType([](sofa::core::objectmodel::Base* object) { + return py::cast(dynamic_cast(object)); + }); + + c.def("addNewPointToState", &PointsManager::addNewPointToState); + c.def("removeLastPointfromState", &PointsManager::removeLastPointfromState); +} + +} // namespace sofapython3 diff --git a/src/CosseratPlugin/Binding/Binding_PointsManager.h b/src/CosseratPlugin/Binding/Binding_PointsManager.h new file mode 100644 index 00000000..55413c0a --- /dev/null +++ b/src/CosseratPlugin/Binding/Binding_PointsManager.h @@ -0,0 +1,29 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2021 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#pragma once + +#include + +namespace sofapython3 { + +void moduleAddPointsManager(pybind11::module &m); + +} // namespace sofapython3 diff --git a/src/CosseratPlugin/Binding/CMakeLists.txt b/src/CosseratPlugin/Binding/CMakeLists.txt new file mode 100644 index 00000000..15e91927 --- /dev/null +++ b/src/CosseratPlugin/Binding/CMakeLists.txt @@ -0,0 +1,28 @@ +project(CosseratBindings) + +set(SOURCE_FILES + ${CMAKE_CURRENT_SOURCE_DIR}/Module_CosseratPlugin.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/Binding_PointsManager.cpp +) + +set(HEADER_FILES + ${CMAKE_CURRENT_SOURCE_DIR}/Binding_PointsManager.h +) + +if (NOT TARGET SofaPython3::Plugin) + find_package(SofaPython3 REQUIRED COMPONENTS Plugin Bindings.Sofa) +endif() + +sofa_find_package(Sofa.GL QUIET) + +SP3_add_python_module( + TARGET ${PROJECT_NAME} + PACKAGE Bindings + MODULE CosseratPlugin + DESTINATION Sofa + SOURCES ${SOURCE_FILES} + HEADERS ${HEADER_FILES} + DEPENDS SofaPython3::Plugin SofaPython3::Bindings Sofa.GL CosseratPlugin + +) +message("-- SofaPython3 bindings for CosseratPlugin will be created.") diff --git a/src/CosseratPlugin/Binding/Module_CosseratPlugin.cpp b/src/CosseratPlugin/Binding/Module_CosseratPlugin.cpp new file mode 100644 index 00000000..f823c023 --- /dev/null +++ b/src/CosseratPlugin/Binding/Module_CosseratPlugin.cpp @@ -0,0 +1,35 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2021 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#include +#include "Binding_PointsManager.h" + + +namespace py { using namespace pybind11; } + +namespace sofapython3 +{ + +PYBIND11_MODULE(CosseratPlugin, m) +{ + moduleAddPointsManager(m); +} + +} // namespace sofapython3 diff --git a/src/CosseratPlugin/Binding/testScene.py b/src/CosseratPlugin/Binding/testScene.py new file mode 100644 index 00000000..9bbbf0ea --- /dev/null +++ b/src/CosseratPlugin/Binding/testScene.py @@ -0,0 +1,55 @@ +import Sofa +import Sofa.Core +import Sofa.CosseratPlugin +import numpy as np +from cosserat.cosseratObject import Cosserat, cosserat_config + + +def createScene(root): + root.gravity = [0, -9.81, 0] + + root.addObject("DefaultAnimationLoop") + root.addObject("DefaultVisualManagerLoop") + root.addObject("RequiredPlugin", name="Sofa.Component.Topology.Container.Dynamic") + root.addObject('VisualStyle', displayFlags='showMechanicalMappings') + needle = root.addChild( + Cosserat(parent=root, cosseratGeometry=cosserat_config, name="needle", radius=0.15)) + constraintPointsNode = needle.addChild("constraintPointsNode") + + slidingPoint = needle.addSlidingPoints() + pathToSlidingMo = slidingPoint.getLinkPath() + str("/slidingPointMO") + + print(f'------> The save path is : {pathToSlidingMo}') + + container = constraintPointsNode.addObject("PointSetTopologyContainer", points=[]) + modifier = constraintPointsNode.addObject("PointSetTopologyModifier") + state = constraintPointsNode.addObject("MechanicalObject", template="Vec3d", position=[], showObject=True, + showObjectScale=10, listening="True") + pointManager = constraintPointsNode.addObject('PointsManager', name="pointsManager", listening="True", + beamPath="/needle/rigidBase/cosseratInSofaFrameNode/slidingPoint/slidingPointMO") + + root.addObject(PointController(pointManager=pointManager, state=state, baseState=needle)) + + +class PointController(Sofa.Core.Controller): + def __init__(self, pointManager, state, baseState): + super().__init__() + self.pointManager = pointManager + self.state = state + self.baseState = baseState + + def onKeypressedEvent(self, event): + if event["key"] == "M": + print("Add 10 points") + self.pointManager.addNewPointToState() + + print(f" The size using array: {len(self.state.position.array())}") + print(f'=====> The state using array : {self.state.position.array()}') + # print("Before setting positions", self.state.position.array()) + + elif event["key"] == "D": + print("Remove a point") + self.pointManager.removeLastPointfromState() + + elif event["key"] == "B": + print(f" The size of t{len(self.state.position.array())}") diff --git a/src/CosseratPlugin/Bindings/Binding_PointManager.cpp b/src/CosseratPlugin/Bindings/Binding_PointManager.cpp new file mode 100644 index 00000000..29c3781e --- /dev/null +++ b/src/CosseratPlugin/Bindings/Binding_PointManager.cpp @@ -0,0 +1,52 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture * + * (c) 2021 INRIA, USTL, UJF, CNRS, MGH * + * * + * This program is free software; you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as published by * + * the Free Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, but WITHOUT * + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * + * for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with this program. If not, see . * + ******************************************************************************* + * Contact information: contact@sofa-framework.org * + ******************************************************************************/ +#include +#include +#include +#include "Binding_PointsManager.h" +#include +#include + +typedef sofa::core::behavior::PointsManager PointsManager; + +namespace py +{ + using namespace pybind11; +} + +using namespace sofa::core::objectmodel; +using namespace sofa::core::topology; + +namespace sofapython3 +{ + + void moduleAddPointsManager(py::module &m) + { + py::class_> c(m, "PointsManager"); + + /// register the PointSetTopologyModifier binding in the downcasting subsystem + PythonFactory::registerType([](sofa::core::objectmodel::Base *object) + { return py::cast(dynamic_cast(object)); }); + + c.def("addNewPointToState", &PointsManager::addNewPointToState); + c.def("removeLastPointfromState", &PointsManager::removeLastPointfromState); + } + +} // namespace sofapython3 diff --git a/src/CosseratPlugin/Bindings/Binding_PointManager.h b/src/CosseratPlugin/Bindings/Binding_PointManager.h new file mode 100644 index 00000000..cf985960 --- /dev/null +++ b/src/CosseratPlugin/Bindings/Binding_PointManager.h @@ -0,0 +1,30 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture * + * (c) 2021 INRIA, USTL, UJF, CNRS, MGH * + * * + * This program is free software; you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as published by * + * the Free Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, but WITHOUT * + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * + * for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with this program. If not, see . * + ******************************************************************************* + * Contact information: contact@sofa-framework.org * + ******************************************************************************/ + +#pragma once + +#include + +namespace sofapython3 +{ + + void moduleAddPointsManager(pybind11::module &m); + +} // namespace sofapython3 diff --git a/src/CosseratPlugin/Bindings/CMakeLists.txt b/src/CosseratPlugin/Bindings/CMakeLists.txt new file mode 100644 index 00000000..eee6099c --- /dev/null +++ b/src/CosseratPlugin/Bindings/CMakeLists.txt @@ -0,0 +1,28 @@ +project(CosseratBindings) + +set(SOURCE_FILES + ${CMAKE_CURRENT_SOURCE_DIR}/Module_CosseratPlugin.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/Binding_PointsManager.cpp +) + +set(HEADER_FILES + ${CMAKE_CURRENT_SOURCE_DIR}/Binding_PointsManager.h +) + +if (NOT TARGET SofaPython3::Plugin) + find_package(SofaPython3 REQUIRED COMPONENTS Plugin Bindings.Sofa) +endif() + +sofa_find_package(Sofa.GL QUIET) + +SP3_add_python_module( + TARGET ${PROJECT_NAME} + PACKAGE Binding + MODULE CosseratPlugin + DESTINATION Sofa + SOURCES ${SOURCE_FILES} + HEADERS ${HEADER_FILES} + DEPENDS SofaPython3::Plugin SofaPython3::Bindings Sofa.GL CosseratPlugin + +) +message("-- SofaPython3 binding for CosseratPlugin will be created.") diff --git a/src/CMakeLists.txt b/src/CosseratPlugin/CMakeLists.txt similarity index 92% rename from src/CMakeLists.txt rename to src/CosseratPlugin/CMakeLists.txt index c47824ff..ceff3af7 100644 --- a/src/CMakeLists.txt +++ b/src/CosseratPlugin/CMakeLists.txt @@ -5,8 +5,8 @@ set(HEADER_FILES mapping/BaseCosserat.h mapping/BaseCosserat.inl - mapping/DiscreteCosseratMapping.h - mapping/DiscreteCosseratMapping.inl + mapping/DiscreteCosseratMapping.h + mapping/DiscreteCosseratMapping.inl mapping/DiscretDynamicCosseratMapping.h mapping/DiscreteDynamicCosseratMapping.inl mapping/ProjectionEngine.h @@ -25,7 +25,7 @@ set(HEADER_FILES forcefield/CosseratInternalActuation.inl forcefield/MyUniformVelocityDampingForceField.h forcefield/MyUniformVelocityDampingForceField.inl - + constraint/CosseratSlidingConstraint.h constraint/CosseratSlidingConstraint.inl constraint/QPSlidingConstraint.h @@ -54,11 +54,12 @@ set(SOURCE_FILES constraint/CosseratNeedleSlidingConstraint.cpp ) +# add_subdirectory(Binding) + add_executable(${PROJECT_NAME} ${SOURCE_FILES}) -target_include_directories(${PROJECT_NAME} PUBLIC +target_include_directories(${PROJECT_NAME} PUBLIC "$") -target_link_libraries(${PROJECT_NAME} SofaTest SofaGTestMain SofaCore +target_link_libraries(${PROJECT_NAME} SofaTest SofaGTestMain SofaCore SofaConstraint SofaBaseMechanics SofaUserInteraction) - diff --git a/src/initCosserat.h b/src/CosseratPlugin/config.h.in similarity index 87% rename from src/initCosserat.h rename to src/CosseratPlugin/config.h.in index a6f8c066..ba34b6d0 100644 --- a/src/initCosserat.h +++ b/src/CosseratPlugin/config.h.in @@ -27,17 +27,18 @@ * Contact information: https://project.inria.fr/softrobot/contact/ * * * ******************************************************************************/ -#ifndef SOFA_COMPONENT_COSSERA_MAPPING_INIT_H -#define SOFA_COMPONENT_COSSERA_MAPPING_INIT_H +#ifndef SOFA_COSSERATPLUGIN_INIT_H +#define SOFA_COSSERATPLUGIN_INIT_H #include +#define COSSERATPLUGIN_VERSION @PROJECT_VERSION@ -#ifdef SOFA_BUILD_COSSERAT -#define SOFA_TARGET CosseratPlugin -#define SOFA_COSSERAT_MAPPING_API SOFA_EXPORT_DYNAMIC_LIBRARY +#ifdef SOFA_BUILD_COSSERATPLUGIN +#define SOFA_TARGET @PROJECT_NAME@ +#define SOFA_COSSERATPLUGIN_API SOFA_EXPORT_DYNAMIC_LIBRARY #else -# define SOFA_COSSERAT_MAPPING_API SOFA_IMPORT_DYNAMIC_LIBRARY +# define SOFA_COSSERATPLUGIN_API SOFA_IMPORT_DYNAMIC_LIBRARY #endif //#cmakedefine COSSERAT_PYTHON @@ -46,4 +47,4 @@ This is the plugin for the Discret Cosserat Plugin */ -#endif /* SOFA_COMPONENT_COSSERA_MAPPING_INIT_H */ +#endif /* SOFA_COSSERATPLUGIN_INIT_H */ diff --git a/src/constraint/CosseratActuatorConstraint.cpp b/src/CosseratPlugin/constraint/CosseratActuatorConstraint.cpp similarity index 100% rename from src/constraint/CosseratActuatorConstraint.cpp rename to src/CosseratPlugin/constraint/CosseratActuatorConstraint.cpp diff --git a/src/constraint/CosseratActuatorConstraint.h b/src/CosseratPlugin/constraint/CosseratActuatorConstraint.h similarity index 100% rename from src/constraint/CosseratActuatorConstraint.h rename to src/CosseratPlugin/constraint/CosseratActuatorConstraint.h diff --git a/src/constraint/CosseratActuatorConstraint.inl b/src/CosseratPlugin/constraint/CosseratActuatorConstraint.inl similarity index 100% rename from src/constraint/CosseratActuatorConstraint.inl rename to src/CosseratPlugin/constraint/CosseratActuatorConstraint.inl diff --git a/src/constraint/CosseratNeedleSlidingConstraint.cpp b/src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.cpp similarity index 100% rename from src/constraint/CosseratNeedleSlidingConstraint.cpp rename to src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.cpp diff --git a/src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.h b/src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.h new file mode 100644 index 00000000..fef8e2d3 --- /dev/null +++ b/src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.h @@ -0,0 +1,149 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture * + * (c) 2020 INRIA, USTL, UJF, CNRS, MGH * + * * + * This library is free software; you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as published by * + * the Free Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, but WITHOUT * + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * + * for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with this library; if not, write to the Free Software Foundation, * + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * + ******************************************************************************* + * Plugin Cosserat v1.0 * + * * + * This plugin is also distributed under the GNU LGPL (Lesser General * + * Public License) license with the same conditions than SOFA. * + * * + * Contributors: Defrost team (INRIA, University of Lille, CNRS, * + * Ecole Centrale de Lille) * + * * + * Contact information: https://project.inria.fr/softrobot/contact/ * + * adagolodjo@protonmail.com * + ******************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace sofa::component::constraintset +{ + + using sofa::core::ConstraintParams; + using sofa::core::VecCoordId; + using sofa::core::behavior::Constraint; + using sofa::core::objectmodel::Data; + using sofa::core::visual::VisualParams; + using sofa::defaulttype::Vec3dTypes; + using sofa::defaulttype::Vec3fTypes; + using sofa::helper::ReadAccessor; + using sofa::linearalgebra::BaseVector; + using sofa::core::MultiVecDerivId ; + + /** + * This class contains common implementation of cable constraints + */ + template + class CosseratNeedleSlidingConstraint : public Constraint + { + public: + SOFA_CLASS(SOFA_TEMPLATE(CosseratNeedleSlidingConstraint, DataTypes), SOFA_TEMPLATE(Constraint, DataTypes)); + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename Coord::value_type Real; + typedef typename core::behavior::MechanicalState MechanicalState; + + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef type::vector SetIndexArray; + + public: + CosseratNeedleSlidingConstraint(MechanicalState *object = nullptr); + + ~CosseratNeedleSlidingConstraint() override; + + ////////////////////////// Inherited from BaseObject //////////////////// + void init() override; + void reinit() override; + void draw(const VisualParams *vparams) override; + ///////////////////////////////////////////////////////////////////////// + + ////////////////////////// Inherited from Actuator ////////////////////// + void buildConstraintMatrix(const ConstraintParams *cParams, + DataMatrixDeriv &cMatrix, + unsigned int &cIndex, + const DataVecCoord &x) override; + void getConstraintViolation(const ConstraintParams *cParams, + BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override; + // void getConstraintViolation(const ConstraintParams* cParams, + // BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) override; + void getConstraintResolution(const ConstraintParams *, + std::vector &resTab, + unsigned int &offset) override; + + ///////////////////////////////////////////////////////////////////////// + + ////////////////////////// Inherited from BaseConstraint //////////////// + // void storeLambda(const ConstraintParams* cParams, + // core::MultiVecDerivId res, + // const BaseVector* lambda) override; + ///////////////////////////////////////////////////////////////////////// + + void storeLambda(const ConstraintParams */*cParams*/, Data &result, const Data &jacobian, const sofa::linearalgebra::BaseVector *lambda) + { + auto res = sofa::helper::getWriteAccessor(result); + const MatrixDeriv &j = jacobian.getValue(); + j.multTransposeBaseVector(res, lambda); // lambda is a vector of scalar value so block size is one. + } + + void storeLambda(const ConstraintParams */*cParams*/, MultiVecDerivId res, const sofa::linearalgebra::BaseVector *lambda) override + { + // if (cParams) + // { + // storeLambda(cParams, *res[m_state1->getMstate()].write(), *cParams->readJ(m_state1->getMstate()), lambda); + // storeLambda(cParams, *res[m_state2->getMstate()].write(), *cParams->readJ(m_state2->getMstate()), lambda); + // } + } + + protected: + // Input data + Data> d_value; + Data d_valueIndex; + Data d_valueType; + Data> d_useDirections; + // displacement = the constraint will impose the displacement provided in data d_inputValue[d_iputIndex] + // force = the constraint will impose the force provided in data d_inputValue[d_iputIndex] + + protected: + ////////////////////////// Inherited attributes //////////////////////////// + /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html + /// Bring inherited attributes and function in the current lookup context. + /// otherwise any access to the base::attribute would require + /// the "this->" approach. + using Constraint::d_componentState; + //////////////////////////////////////////////////////////////////////////// + /// \brief internalInit + unsigned int m_nbLines; + unsigned int m_constraintId; + + void internalInit(); + }; + + // Declares template as extern to avoid the code generation of the template for + // each compilation unit. see: http://www.stroustrup.com/C++11FAQ.html#extern-templates + // extern template class CosseratNeedleSlidingConstraint; + +} // namespace sofa diff --git a/src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.inl b/src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.inl new file mode 100644 index 00000000..e5720089 --- /dev/null +++ b/src/CosseratPlugin/constraint/CosseratNeedleSlidingConstraint.inl @@ -0,0 +1,194 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture * + * (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * + * * + * This library is free software; you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as published by * + * the Free Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, but WITHOUT * + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * + * for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with this library; if not, write to the Free Software Foundation, * + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * + ******************************************************************************* + * Plugin Cosserat v1.0 * + * * + * This plugin is also distributed under the GNU LGPL (Lesser General * + * Public License) license with the same conditions than SOFA. * + * * + * Contributors: Defrost team (INRIA, University of Lille, CNRS, * + * Ecole Centrale de Lille) * + * * + * Contact information: https://project.inria.fr/softrobot/contact/ * + * adagolodjo@protonmail.com * + ******************************************************************************/ + +#pragma once + +#include +#include +#include +#include "CosseratNeedleSlidingConstraint.h" + +namespace sofa::component::constraintset +{ + + using sofa::core::objectmodel::ComponentState; + using sofa::helper::WriteAccessor; + + using sofa::core::objectmodel::ComponentState; + using sofa::core::visual::VisualParams; + using sofa::linearalgebra::BaseVector; + using sofa::helper::ReadAccessor; + using sofa::type::Vec4f; + using sofa::type::Vec3; + using sofa::type::vector; + using sofa::helper::OptionsGroup; + using sofa::component::constraint::lagrangian::model::BilateralConstraintResolution; + + template + CosseratNeedleSlidingConstraint::CosseratNeedleSlidingConstraint(MechanicalState *object) + : Inherit1(object), + d_value(initData(&d_value, "value", "Displacement or force to impose.\n")), + d_valueIndex(initData(&d_valueIndex, (unsigned int)0, "valueIndex", + "Index of the value (in InputValue vector) that we want to impose \n" + "If unspecified the default value is {0}")), + d_valueType(initData(&d_valueType, OptionsGroup(2, "displacement", "force"), "valueType", + "displacement = the contstraint will impose the displacement provided in data value[valueIndex] \n" + "force = the contstraint will impose the force provided in data value[valueIndex] \n" + "If unspecified, the default value is displacement")), + d_useDirections(initData(&d_useDirections, type::Vec<3, bool>(0, 1, 1), "useDirections", "Directions to constrain.\n")) + {} + + template + CosseratNeedleSlidingConstraint::~CosseratNeedleSlidingConstraint() + { + } + + template + void CosseratNeedleSlidingConstraint::init() + { + Inherit1::init(); + d_componentState.setValue(ComponentState::Valid); + + internalInit(); + } + + template + void CosseratNeedleSlidingConstraint::reinit() + { + internalInit(); + } + + template + void CosseratNeedleSlidingConstraint::internalInit() + { + if (d_value.getValue().size() == 0) + { + WriteAccessor>> value = d_value; + value.resize(1, 0.); + } + + // check for errors in the initialization + if (d_value.getValue().size() < d_valueIndex.getValue()) + { + msg_warning() << "Bad size for data value (size=" << d_value.getValue().size() << "), or wrong value for data valueIndex (valueIndex=" << d_valueIndex.getValue() << "). Set default valueIndex=0."; + d_valueIndex.setValue(0); + } + + } + + template + void CosseratNeedleSlidingConstraint::buildConstraintMatrix(const ConstraintParams *cParams, DataMatrixDeriv &cMatrix, unsigned int &cIndex, const DataVecCoord &x) + { + if (d_componentState.getValue() != ComponentState::Valid) + return; + + SOFA_UNUSED(cParams); + MatrixDeriv &matrix = *cMatrix.beginEdit(); + VecCoord positions = x.getValue(); + m_constraintId = cIndex; + + type::Vec<3, bool> use = d_useDirections.getValue(); + + for (unsigned int i = 0; i < positions.size(); i++) + { + if (use[1]) + { + MatrixDerivRowIterator c_it = matrix.writeLine(cIndex++); + c_it.addCol(i, Coord(0, 1, 0)); + } + if (use[2]) + { + MatrixDerivRowIterator c_it = matrix.writeLine(cIndex++); + c_it.addCol(i, Coord(0, 0, 1)); + } + } + cMatrix.endEdit(); + m_nbLines = cIndex - m_constraintId; + + } + + template + void CosseratNeedleSlidingConstraint::getConstraintViolation(const ConstraintParams *cParams, + BaseVector *resV, const DataVecCoord &x, const DataVecDeriv &v) + // void CosseratNeedleSlidingConstraint::getConstraintViolation(const ConstraintParams* cParams, + // BaseVector *resV, + // const BaseVector *Jdx) + { + if (d_componentState.getValue() != ComponentState::Valid) + return; + + SOFA_UNUSED(cParams); + SOFA_UNUSED(x); + SOFA_UNUSED(v); + //ReadAccessor> positions = this->mstate->readPositions(); + const VecCoord positions = x.getValue(); + type::Vec<3, bool> use = d_useDirections.getValue(); + + for (unsigned int i = 0; i < positions.size(); i++) + { + if (use[0]){ + Real dfree0 = positions[i][0]; + resV->set(m_constraintId + 2 * i, dfree0); + } + if (use[1]){ + Real dfree1 = positions[i][1]; + resV->set(m_constraintId + 2 * i, dfree1); + } + if (use[2]){ + Real dfree2 = positions[i][2]; + resV->set(m_constraintId + 2 * i + 1, dfree2); + } + } + } + + template + void CosseratNeedleSlidingConstraint::getConstraintResolution(const ConstraintParams *, + std::vector &resTab, + unsigned int &offset) + { + ReadAccessor> positions = this->mstate->readPositions(); + type::Vec<3, bool> use = d_useDirections.getValue(); + for (size_t i = 0; i < positions.size(); i++) + { + if (use[1]) + resTab[offset++] = new BilateralConstraintResolution(); + if (use[2]) + resTab[offset++] = new BilateralConstraintResolution(); + } + } + + template + void CosseratNeedleSlidingConstraint::draw(const VisualParams *vparams) + { + SOFA_UNUSED(vparams); + if (d_componentState.getValue() != ComponentState::Valid) + return; + } +} // namespace sofa::component::constraintset diff --git a/src/constraint/CosseratSlidingConstraint.cpp b/src/CosseratPlugin/constraint/CosseratSlidingConstraint.cpp similarity index 94% rename from src/constraint/CosseratSlidingConstraint.cpp rename to src/CosseratPlugin/constraint/CosseratSlidingConstraint.cpp index d6d6ddb3..861e9636 100644 --- a/src/constraint/CosseratSlidingConstraint.cpp +++ b/src/CosseratPlugin/constraint/CosseratSlidingConstraint.cpp @@ -19,13 +19,12 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#pragma once - +#define SOFA_COSSERATPLUGIN_CPP_CosseratSlidingConstraint #include "CosseratSlidingConstraint.inl" #include -#include #include +#include namespace sofa::component::constraintset { diff --git a/src/constraint/CosseratSlidingConstraint.h b/src/CosseratPlugin/constraint/CosseratSlidingConstraint.h similarity index 99% rename from src/constraint/CosseratSlidingConstraint.h rename to src/CosseratPlugin/constraint/CosseratSlidingConstraint.h index 0099484f..a7264505 100644 --- a/src/constraint/CosseratSlidingConstraint.h +++ b/src/CosseratPlugin/constraint/CosseratSlidingConstraint.h @@ -20,12 +20,14 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once +#include #include #include -#include #include +#include + namespace sofa::component::constraintset { diff --git a/src/constraint/CosseratSlidingConstraint.inl b/src/CosseratPlugin/constraint/CosseratSlidingConstraint.inl similarity index 99% rename from src/constraint/CosseratSlidingConstraint.inl rename to src/CosseratPlugin/constraint/CosseratSlidingConstraint.inl index 8709d9b3..3253995f 100644 --- a/src/constraint/CosseratSlidingConstraint.inl +++ b/src/CosseratPlugin/constraint/CosseratSlidingConstraint.inl @@ -20,8 +20,8 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once - #include "CosseratSlidingConstraint.h" + #include #include #include @@ -278,7 +278,7 @@ void CosseratSlidingConstraint::draw(const core::visual::VisualParams // else color = sofa::type::RGBAColor::magenta(); - std::vector vertices; + std::vector vertices; // vertices.push_back(DataTypes::getCPos((this->mstate1->read(core::ConstVecCoordId::position())->getValue())[d_m1.getValue()])); // vparams->drawTool()->drawPoints(vertices, 10, color); @@ -307,7 +307,7 @@ void CosseratSlidingConstraint::drawLinesBetweenPoints(const core::vi const VecCoord & positions = this->mstate2->read(core::ConstVecCoordId::position())->getValue(); sofa::type::RGBAColor color; color = sofa::type::RGBAColor::magenta(); - std::vector vertices; + std::vector vertices; for (unsigned int i=0; i +#include +#include + +#include "CosseratUnilateralInteractionConstraint.h" + +namespace sofa::component::constraintset +{ + + using sofa::core::objectmodel::ComponentState; + using sofa::core::visual::VisualParams; + using sofa::helper::OptionsGroup; + using sofa::helper::ReadAccessor; + using sofa::helper::WriteAccessor; + using sofa::linearalgebra::BaseVector; + using sofa::type::Vec4f; + using sofa::type::vector; + using sofa::type::Vector3; + + template + CosseratUnilateralInteractionConstraint::CosseratUnilateralInteractionConstraint(MechanicalState *object) + : Inherit1(object), + d_value(initData(&d_value, "value", "Displacement or force to impose.\n")), d_dampingCoefficient(initData(&d_dampingCoefficient, Real(0.1), "dampingCoefficient", + "The dumping coefficient to slow down the insertion speed.\n")), + d_valueIndex(initData(&d_valueIndex, (unsigned int)0, "valueIndex", + "Index of the value (in InputValue vector) that we want to impose \n" + "If unspecified the default value is {0}")), + d_vectorOfIndices(initData(&d_vectorOfIndices, "vectorOfIndices", + "vector of indices on witch we have to apply the constraint.\n")), + d_entryPoint(initData(&d_entryPoint, type::Vector3(24.95, 0., 0), "entryPoint", + "The predefined entry point, this point can also be determined automatically" + "but not implemented here.\n")), + d_direction(initData(&d_direction, "direction", + "direction of insertion, if this is not given " + "the insertion is direct along X.\n")) + { + } + + template + CosseratUnilateralInteractionConstraint::~CosseratUnilateralInteractionConstraint() = default; + + template + void CosseratUnilateralInteractionConstraint::init() + { + Inherit1::init(); + internalInit(); + UpdateList(); + } + + template + void CosseratUnilateralInteractionConstraint::reinit() + { + internalInit(); + UpdateList(); + } + + template + type::Vector3 CosseratUnilateralInteractionConstraint::findEntryPoint() + { + /// @todo:1- find the entry automatically, this is need in the case of needle insertion + /// can also be necessary when the volume is deforming + /// @todo:2- Build unitest function + + return type::Vector3(0.0f, 0.0f, 0.0f); + } + + template + bool CosseratUnilateralInteractionConstraint::UpdateList() + { + /// @todo:1- Update the list of points beyond the entry point + /// @todo:2- Build unitest function + /// @todo:3- use the insertion direction + + ReadAccessor> positions = m_state->readPositions(); + auto entryPoint = d_entryPoint.getValue(); + WriteAccessor>> indices = d_vectorOfIndices; + indices.clear(); + + unsigned int index = 0; + for (auto position : positions) + { + if (position[0] >= entryPoint[0]) + { + indices.push_back(index); + } + index++; + } + return true; + } + + template + void CosseratUnilateralInteractionConstraint::buildConstraintMatrix(const ConstraintParams *cParams, DataMatrixDeriv &cMatrix, unsigned int &cIndex, const DataVecCoord &x) + { + if (d_componentState.getValue() != ComponentState::Valid) + return; + UpdateList(); + + SOFA_UNUSED(cParams); + MatrixDeriv &matrix = *cMatrix.beginEdit(); + VecCoord positions = x.getValue(); + auto indices = d_vectorOfIndices.getValue(); + auto direction = d_direction.getValue(); + + m_constraintId = cIndex; + + // printf("================================= \n"); + if ((!direction.empty()) && (direction.size() == positions.size())) + { + type::Vector3 vx, vy, vz; + for (auto index : indices) + { + type::Quat q = direction[index]; + q.normalize(); + vx = q.rotate(Vector3(1., 0., 0)); + vx.normalize(); + vy = q.rotate(Vector3(0., 1., 0.)); + vy.normalize(); + vz = q.rotate(Vector3(0., 0., 1.)); + vz.normalize(); + + MatrixDerivRowIterator c_it_x = matrix.writeLine(cIndex++); + c_it_x.addCol(index, vx); + MatrixDerivRowIterator c_it_y = matrix.writeLine(cIndex++); + c_it_y.addCol(index, vy); + MatrixDerivRowIterator c_it_z = matrix.writeLine(cIndex++); + c_it_z.addCol(index, vz); + // std::cout <<"index : "<< index << std::endl; + // std::cout <<"vx : "<< vx << std::endl; + // std::cout <<"vy : "<< vy << std::endl; + // std::cout <<"vz : "<< vz << std::endl; + } + } + else + { + for (auto index : indices) + { + MatrixDerivRowIterator c_it_x = matrix.writeLine(cIndex++); + c_it_x.addCol(index, Coord(1., 0, 0)); + MatrixDerivRowIterator c_it_y = matrix.writeLine(cIndex++); + c_it_y.addCol(index, Coord(0, 1., 0)); + MatrixDerivRowIterator c_it_z = matrix.writeLine(cIndex++); + c_it_z.addCol(index, Coord(0, 0, 1.)); + } + } + // printf("================================= \n"); + cMatrix.endEdit(); + m_nbLines = cIndex - m_constraintId; + } + + template + void CosseratUnilateralInteractionConstraint::getConstraintViolation(const ConstraintParams *cParams, + BaseVector *resV, + const BaseVector *Jdx) + { + if (d_componentState.getValue() != ComponentState::Valid) + return; + SOFA_UNUSED(cParams); + ReadAccessor> positions = m_state->readPositions(); + ReadAccessor> freePositions = m_state->read(core::ConstVecCoordId::freePosition()); + ReadAccessor> velocity = m_state->read(core::ConstVecDerivId::velocity()); + auto indices = d_vectorOfIndices.getValue(); + + unsigned int iter = 0; + + for (auto index : indices) + { + //@todo this need to be recompute, use dfree = newPos-oldPos or posFree - pos or velocity + Coord dx = freePositions[index] - positions[index]; // This is also equal Jdx->element(3*iter +i); i=0,1,2 + // std::cout<<" ===> dx :"<< dx << std::endl; + // std::cout<<" ===> dv :"<< velocity[index] << std::endl; + Real dfree1 = dx[0]; + Real dfree2 = dx[1]; + Real dfree3 = dx[2]; + + resV->set(m_constraintId + 3 * iter + 0, dfree1); + resV->set(m_constraintId + 3 * iter + 1, dfree2); + resV->set(m_constraintId + 3 * iter + 2, dfree2); + iter++; + } + } + + template + void CosseratUnilateralInteractionConstraint::getConstraintResolution(const ConstraintParams *, + std::vector &resTab, + unsigned int &offset) + { + double dampingCoefficient = d_dampingCoefficient.getValue(); + for (unsigned int index : d_vectorOfIndices.getValue()) + { + resTab[offset + 0] = new MyUnilateralConstraintResolutionWithFriction(dampingCoefficient); + resTab[offset + 1] = new MyUnilateralConstraintResolutionWithFriction(dampingCoefficient); + resTab[offset + 2] = new MyUnilateralConstraintResolutionWithFriction(dampingCoefficient); + offset += 3; + } + } + + template + void CosseratUnilateralInteractionConstraint::draw(const VisualParams * /*vparams*/) + { + if (d_componentState.getValue() != ComponentState::Valid) + return; + } +} // namespace sofa diff --git a/src/constraint/DrawTrianglesComponent.cpp b/src/CosseratPlugin/constraint/DrawTrianglesComponent.cpp similarity index 100% rename from src/constraint/DrawTrianglesComponent.cpp rename to src/CosseratPlugin/constraint/DrawTrianglesComponent.cpp diff --git a/src/constraint/DrawTrianglesComponent.h b/src/CosseratPlugin/constraint/DrawTrianglesComponent.h similarity index 100% rename from src/constraint/DrawTrianglesComponent.h rename to src/CosseratPlugin/constraint/DrawTrianglesComponent.h diff --git a/src/CosseratPlugin/constraint/DrawTrianglesComponent.inl b/src/CosseratPlugin/constraint/DrawTrianglesComponent.inl new file mode 100644 index 00000000..c0cfa007 --- /dev/null +++ b/src/CosseratPlugin/constraint/DrawTrianglesComponent.inl @@ -0,0 +1,279 @@ +#ifndef DrawTrianglesComponent_INL +#define DrawTrianglesComponent_INL + +#include "DrawTrianglesComponent.h" +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::controller + +DrawTrianglesComponent::DrawTrianglesComponent() + : d_transparency(initData(&d_transparency, double(0.1), "transparency", "Scene scale")), + d_vecTriangles(initData(&d_vecTriangles, "triangles", "tetra index elements")), + d_vecTetra(initData(&d_vecTetra, "tetrahedra", "tetra index elements")), + d_maxStress(initData(&d_maxStress, (double)5e3, "maxStress", "max Stress")), + d_minStress(initData(&d_minStress, (double)0, "minStress", "min Stress")), + d_maxVonMisesPerNode(initData(&d_maxVonMisesPerNode, "maxVonMisesPerNode", "max Von Mises Per Node")), + d_draw(initData(&d_draw, true, "draw", "draw triangles")) +{ + this->f_listening.setValue(true); +} + +void DrawTrianglesComponent::init() +{ + this->getContext()->get(m_state); + this->getContext()->get(m_tetraForceField); + if (m_state == NULL) + { + serr << "Error cannot find the mstate" << sendl; + return; + } + if (m_tetraForceField == NULL) + { + serr << "Error cannot find the m_tetrahedron" << sendl; + return; + } + m_VonMisesColorMap.setColorScheme("Blue to Red"); + m_VonMisesColorMap.reinit(); + m_tetraForceField->updateVonMisesStress = true; + + m_minVM = d_minStress.getValue(); + m_maxVM = d_maxStress.getValue(); + + helper::ReadAccessor>> vMN = m_tetraForceField->_vonMisesPerNode; + helper::vector &vonMises = *d_maxVonMisesPerNode.beginEdit(); + vonMises.resize(vMN.size()); + for (size_t i = 0; i < vMN.size(); i++) + { + vonMises[i] = vMN[i]; + } + d_maxVonMisesPerNode.endEdit(); +} + +void DrawTrianglesComponent::handleEvent(sofa::core::objectmodel::Event *event) +{ + helper::ReadAccessor>> vMN = m_tetraForceField->_vonMisesPerNode; + if (dynamic_cast(event)) + { + helper::vector &vonMises = *d_maxVonMisesPerNode.beginEdit(); + for (size_t index = 0; index < vMN.size(); index++) + { + m_minVM = (vMN[index] < m_minVM) ? vMN[index] : m_minVM; + m_maxVM = (vMN[index] > m_maxVM) ? vMN[index] : m_maxVM; + if (vonMises[index] < vMN[index]) + { + vonMises[index] = vMN[index]; + } + } + d_maxVonMisesPerNode.endEdit(); + } + else if (sofa::core::objectmodel::KeypressedEvent *ev = dynamic_cast(event)) + { + if ((ev->getKey() == 'l') || (ev->getKey() == 'L')) + { + helper::vector &vonMises = *d_maxVonMisesPerNode.beginEdit(); + m_minVM = d_minStress.getValue(); + m_maxVM = d_maxStress.getValue(); + + for (size_t i = 0; i < vMN.size(); i++) + { + vonMises[i] = 0.0; + } + d_maxVonMisesPerNode.endEdit(); + } + } +} + +void DrawTrianglesComponent::draw(const core::visual::VisualParams *vparams) +{ + if (d_draw.getValue()) + if (!this->m_state) + return; + m_tetraForceField->updateVonMisesStress = true; + + const VecCoord &x = this->m_state->read(core::ConstVecCoordId::position())->getValue(); + helper::ReadAccessor>> vM = m_tetraForceField->_vonMisesPerElement; + helper::ReadAccessor>> vMN = m_tetraForceField->_vonMisesPerNode; + // helper::vector & vMN = *d_maxVonMisesPerNode.beginEdit(); + + helper::ColorMap::evaluator evalColor = m_VonMisesColorMap.getEvaluator(d_minStress.getValue(), d_maxStress.getValue()); + + glDisable(GL_LIGHTING); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + // glColor4f(1,1,1,1); + // if (m_textureImage!=TEXTURE_UNASSIGNED) { + // glEnable(GL_TEXTURE_2D); + // glBindTexture(GL_TEXTURE_2D, m_textureImage); + // glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); + // } + if (vparams->displayFlags().getShowWireFrame()) + { + glBegin(GL_LINES); + + int i = 0; + std::vector nodeColors(4); + for (auto it = d_vecTriangles.getValue().begin(); it != d_vecTriangles.getValue().end(); ++it, ++i) + { + Index a = (*it)[0]; + Index b = (*it)[1]; + Index c = (*it)[2]; + nodeColors[0] = evalColor(vMN[a]); + nodeColors[0][3] = d_transparency.getValue(); + nodeColors[1] = evalColor(vMN[b]); + nodeColors[1][3] = d_transparency.getValue(); + nodeColors[2] = evalColor(vMN[c]); + nodeColors[2][3] = d_transparency.getValue(); + + float *ca = &nodeColors[0][0]; + float *cb = &nodeColors[1][0]; + float *cc = &nodeColors[2][0]; + + Coord pa = x[a]; + Coord pb = x[b]; + Coord pc = x[c]; + + glColor4fv(ca); + helper::gl::glVertexT(pa); + glColor4fv(cb); + helper::gl::glVertexT(pb); + glColor4fv(cc); + helper::gl::glVertexT(pc); + } + glEnd(); + } + else + { + + glBegin(GL_TRIANGLES); + + // for (size_t nd = 0; nd < x.size(); nd++) { + // defaulttype::Vec4f col = evalColor(vMN[nd]); + + // glColor4f(col[0],col[1],col[2],1.0); + // glVertex3d(x[nd][0],x[nd][1],x[nd][2]); + // } + + // vparams->drawTool()->drawPoints(pts, 10, nodeColors); + + int i = 0; + std::vector nodeColors(4); + + i = 0; + for (auto it = d_vecTetra.getValue().begin(); it != d_vecTetra.getValue().end(); ++it, ++i) + { + Index a = (*it)[0]; + Index b = (*it)[1]; + Index c = (*it)[2]; + Index d = (*it)[3]; + nodeColors[0] = evalColor(vMN[a]); + nodeColors[0][3] = d_transparency.getValue(); + nodeColors[1] = evalColor(vMN[b]); + nodeColors[1][3] = d_transparency.getValue(); + nodeColors[2] = evalColor(vMN[c]); + nodeColors[2][3] = d_transparency.getValue(); + nodeColors[3] = evalColor(vMN[d]); + nodeColors[3][3] = d_transparency.getValue(); + float *ca = &nodeColors[0][0]; + float *cb = &nodeColors[1][0]; + float *cc = &nodeColors[2][0]; + float *cd = &nodeColors[3][0]; + + Coord center = (x[a] + x[b] + x[c] + x[d]) * 0.125; + Coord pa = (x[a] + center) * (Real)0.666667; + Coord pb = (x[b] + center) * (Real)0.666667; + Coord pc = (x[c] + center) * (Real)0.666667; + Coord pd = (x[d] + center) * (Real)0.666667; + + glColor4fv(ca); + helper::gl::glVertexT(pa); + glColor4fv(cb); + helper::gl::glVertexT(pb); + glColor4fv(cc); + helper::gl::glVertexT(pc); + + glColor4fv(ca); + helper::gl::glVertexT(pa); + glColor4fv(cb); + helper::gl::glVertexT(pb); + glColor4fv(cd); + helper::gl::glVertexT(pd); + + glColor4fv(ca); + helper::gl::glVertexT(pa); + glColor4fv(cc); + helper::gl::glVertexT(pc); + glColor4fv(cd); + helper::gl::glVertexT(pd); + + glColor4fv(cc); + helper::gl::glVertexT(pc); + glColor4fv(cb); + helper::gl::glVertexT(pb); + glColor4fv(cd); + helper::gl::glVertexT(pd); + } + + for (auto it = d_vecTriangles.getValue().begin(); it != d_vecTriangles.getValue().end(); ++it, ++i) + { + Index a = (*it)[0]; + Index b = (*it)[1]; + Index c = (*it)[2]; + nodeColors[0] = evalColor(vMN[a]); + nodeColors[0][3] = d_transparency.getValue(); + nodeColors[1] = evalColor(vMN[b]); + nodeColors[1][3] = d_transparency.getValue(); + nodeColors[2] = evalColor(vMN[c]); + nodeColors[2][3] = d_transparency.getValue(); + float *ca = &nodeColors[0][0]; + float *cb = &nodeColors[1][0]; + float *cc = &nodeColors[2][0]; + + Coord pa = x[a]; + Coord pb = x[b]; + Coord pc = x[c]; + + glColor4fv(ca); + helper::gl::glVertexT(pa); + glColor4fv(cb); + helper::gl::glVertexT(pb); + glColor4fv(cc); + helper::gl::glVertexT(pc); + } + + glEnd(); + } +} + +} // end namespace controller + +#endif diff --git a/src/constraint/QPSlidingConstraint.cpp b/src/CosseratPlugin/constraint/QPSlidingConstraint.cpp similarity index 100% rename from src/constraint/QPSlidingConstraint.cpp rename to src/CosseratPlugin/constraint/QPSlidingConstraint.cpp diff --git a/src/constraint/QPSlidingConstraint.h b/src/CosseratPlugin/constraint/QPSlidingConstraint.h similarity index 98% rename from src/constraint/QPSlidingConstraint.h rename to src/CosseratPlugin/constraint/QPSlidingConstraint.h index c63ee880..9bb68ef0 100644 --- a/src/constraint/QPSlidingConstraint.h +++ b/src/CosseratPlugin/constraint/QPSlidingConstraint.h @@ -35,7 +35,7 @@ #include #include -#include +#include namespace sofa::component::constraintset { diff --git a/src/constraint/QPSlidingConstraint.inl b/src/CosseratPlugin/constraint/QPSlidingConstraint.inl similarity index 98% rename from src/constraint/QPSlidingConstraint.inl rename to src/CosseratPlugin/constraint/QPSlidingConstraint.inl index 3c485f61..30b251cf 100644 --- a/src/constraint/QPSlidingConstraint.inl +++ b/src/CosseratPlugin/constraint/QPSlidingConstraint.inl @@ -32,7 +32,7 @@ #include #include -#include +#include #include "QPSlidingConstraint.h" @@ -46,7 +46,7 @@ using sofa::core::visual::VisualParams; using sofa::linearalgebra::BaseVector; using sofa::helper::ReadAccessor; using sofa::type::Vec4f; -using sofa::type::Vector3; +using sofa::type::Vec3; using sofa::type::vector; using sofa::helper::OptionsGroup; using sofa::component::constraint::lagrangian::model::BilateralConstraintResolution; diff --git a/src/CosseratPlugin/engine/CosseratState.cpp b/src/CosseratPlugin/engine/CosseratState.cpp new file mode 100644 index 00000000..782c082a --- /dev/null +++ b/src/CosseratPlugin/engine/CosseratState.cpp @@ -0,0 +1,70 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4 * +* (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS * +* * +* This library is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This library is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* SOFA :: Modules * +* * +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ + +#define SOFA_COMPONENT_CONTAINER_TEMPERATURESTATE_CPP +#include "TemperatureState.inl" +#include +#include + +namespace sofa +{ + +namespace component +{ + +namespace container +{ + +using namespace core::behavior; +using namespace defaulttype; + +SOFA_DECL_CLASS(TemperatureState) + +int TemperatureStateClass = core::RegisterObject("electrical state vectors") +.add< TemperatureState >(true) // default template +.add< TemperatureState >() +.add< TemperatureState >() +.add< TemperatureState >() +.add< TemperatureState >() +.add< TemperatureState >() + +; + +// template specialization must be in the same namespace as original namespace for GCC 4.1 +// g++ 4.1 requires template instantiations to be declared on a parent namespace from the template class. +template class TemperatureState; +template class TemperatureState; +template class TemperatureState; +template class TemperatureState; +template class TemperatureState; +template class TemperatureState; + + + +} + +} // namespace component + +} // namespace sofa diff --git a/src/CosseratPlugin/engine/CosseratState.h b/src/CosseratPlugin/engine/CosseratState.h new file mode 100644 index 00000000..9ade0c5d --- /dev/null +++ b/src/CosseratPlugin/engine/CosseratState.h @@ -0,0 +1,64 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4 * + * (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS * + * * + * This library is free software; you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as published by * + * the Free Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, but WITHOUT * + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * + * for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with this library; if not, write to the Free Software Foundation, * + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * + ******************************************************************************* + * SOFA :: Modules * + * * + * Authors: The SOFA Team and external contributors (see Authors.txt) * + * * + * Contact information: contact@sofa-framework.org * + ******************************************************************************/ + +#pragma once +#include +#include +#include + +namespace sofa::component::container +{ + + using namespace core::behavior; + using namespace core::objectmodel; + using namespace sofa::defaulttype; + using namespace sofa::core::topology; + using sofa::defaulttype::Vector3; + + template + class TemperatureState : public sofa::component::container::MechanicalObject + { + public: + SOFA_CLASS(SOFA_TEMPLATE(TemperatureState, DataTypes), SOFA_TEMPLATE(sofa::core::behavior::MechanicalState, DataTypes)); + + typedef sofa::core::behavior::MechanicalState Inherited; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + + /// assumes the mechanical object type (3D) + typedef Vec<3, Real> Vec3; + typedef StdVectorTypes MechanicalTypes; + + TemperatureState(); + + /// compute the bounding box of the current state + Data computeBoundingBox; + void computeBBox(const core::ExecParams *params); + }; + +} // namespace container diff --git a/src/CosseratPlugin/engine/CosseratState.inl b/src/CosseratPlugin/engine/CosseratState.inl new file mode 100644 index 00000000..2b96cf64 --- /dev/null +++ b/src/CosseratPlugin/engine/CosseratState.inl @@ -0,0 +1,69 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4 * +* (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS * +* * +* This library is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This library is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this library; if not, write to the Free Software Foundation, * +* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * +******************************************************************************* +* SOFA :: Modules * +* * +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#ifndef SOFA_COMPONENT_CONTAINER_TEMPERATURESTATE_INL +#define SOFA_COMPONENT_CONTAINER_TEMPERATURESTATE_INL + +#include "TemperatureState.h" + + +namespace sofa +{ + +namespace component +{ + +namespace container +{ +using namespace std; +using namespace sofa::core; +using namespace sofa::core::topology; +using namespace sofa::defaulttype; + +template +TemperatureState::TemperatureState() + : computeBoundingBox ( initData(&computeBoundingBox, (bool) true, "computeBoundingBox","Boolean to activate the computation of BoundingBox") ) +{ +} + + +template +void TemperatureState::computeBBox(const core::ExecParams* params) +{ + if ( computeBoundingBox.getValue()) + { + std::cout << "Compute BBox" << std::endl; + Inherited::computeBBox(params); + } +} + + + +} // namespace container + +} // namespace component + +} // namespace sofa + +#endif //SOFA_COMPONENT_CONTAINER_TEMPERATURESTATE_INL diff --git a/src/CosseratPlugin/engine/PointsManager.cpp b/src/CosseratPlugin/engine/PointsManager.cpp new file mode 100755 index 00000000..89e27686 --- /dev/null +++ b/src/CosseratPlugin/engine/PointsManager.cpp @@ -0,0 +1,19 @@ + +#include "PointsManager.inl" +#include +#include +#include +#include /* assert */ + +namespace sofa::core::behavior +{ + + using namespace sofa::defaulttype; + + SOFA_DECL_CLASS(PointsManager) + + int PointsManagerClass = core::RegisterObject("add and remove " + "points from the state will taking into account the changes inside the modifier and the container") + .add(); + +} // namespace sofa diff --git a/src/CosseratPlugin/engine/PointsManager.h b/src/CosseratPlugin/engine/PointsManager.h new file mode 100755 index 00000000..3e530452 --- /dev/null +++ b/src/CosseratPlugin/engine/PointsManager.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// #include +#include + +typedef sofa::component::topology::container::dynamic::PointSetTopologyModifier PointSetTopologyModifier; + +using sofa::core::objectmodel::KeypressedEvent; +namespace sofa::core::behavior +{ + class SOFA_COSSERATPLUGIN_API PointsManager : public sofa::core::objectmodel::BaseObject + { + + public: + SOFA_CLASS(PointsManager, sofa::core::objectmodel::BaseObject); + + typedef sofa::defaulttype::Vec3dTypes DataTypes; + typedef DataTypes::VecCoord VecCoord; + typedef DataTypes::Coord Coord; + typedef DataTypes::Real Real; + + public: + PointsManager(); + typedef type::Vec3 Vec3; + + Data d_beamTip; + Data d_radius; + Data d_color; + Data d_beamPath; + + PointSetTopologyModifier *m_modifier; + core::behavior::MechanicalState *m_beam; + + void init(); + void handleEvent(sofa::core::objectmodel::Event *event); + // void draw(const core::visual::VisualParams *vparams); + + void addNewPointToState(); + void removeLastPointfromState(); + + topology::TopologyContainer *getTopology() + { + return dynamic_cast(getContext()->getTopology()); + } + + sofa::core::behavior::MechanicalState *getMstate() + { + return dynamic_cast *>(getContext()->getMechanicalState()); + } + }; + +} // namespace sofa diff --git a/src/CosseratPlugin/engine/PointsManager.inl b/src/CosseratPlugin/engine/PointsManager.inl new file mode 100755 index 00000000..80b7f9c5 --- /dev/null +++ b/src/CosseratPlugin/engine/PointsManager.inl @@ -0,0 +1,133 @@ +#pragma once + +#include "PointsManager.h" +#include +#include +#include +#include + +namespace sofa::core::behavior +{ + + PointsManager::PointsManager() + : d_beamTip(initData(&d_beamTip, "beamTip", "The beam tip")), + d_radius(initData(&d_radius, double(1), "radius", "sphere radius")), + d_color(initData(&d_color, type::Vec4f(1, 0, 0, 1), "color", "Default color is (1,0,0,1)")), + d_beamPath(initData(&d_beamPath, "beamPath", "path to beam state")) + { + this->f_listening.setValue(true); + } + + void PointsManager::init() + { + if (getTopology() == NULL) + msg_error() << "Error cannot find the topology"; + + if (getMstate() == NULL) + msg_error() << "Error cannot find the topology"; + + this->getContext()->get(m_beam, d_beamPath.getValue()); + if (m_beam == nullptr) + msg_error() << "Cannot find the beam collision state : " << d_beamPath.getValue(); + + this->getContext()->get(m_modifier); + if (m_modifier == NULL) + { + msg_error() << " Error cannot find the EdgeSetTopologyModifier"; + return; + } + } + + void PointsManager::addNewPointToState() + { + helper::WriteAccessor> x = *this->getMstate()->write(core::VecCoordId::position()); + helper::WriteAccessor> xRest = *this->getMstate()->write(core::VecCoordId::restPosition()); + helper::WriteAccessor> xfree = *this->getMstate()->write(core::VecCoordId::freePosition()); + helper::WriteAccessor> xforce = *this->getMstate()->write(core::VecDerivId::force()); + const helper::ReadAccessor> &beam = m_beam->readPositions(); + unsigned nbPoints = this->getTopology()->getNbPoints(); // do not take the last point because there is a bug + + size_t beamSz = beam.size(); + m_modifier->addPoints(1, true); + + Vec3 pos = beam[beamSz - 1]; +// std::cout << "beam tip is =-----> " << pos << std::endl; +// std::cout << "nbPoints is equal :" << nbPoints << std::endl; +// std::cout << "x.size is equal :" << x.size() << std::endl; + + x.resize(nbPoints + 1); + xRest.resize(nbPoints + 1); + xfree.resize(nbPoints + 1); + xforce.resize(nbPoints + 1); + + x[nbPoints] = pos; + xRest[nbPoints] = pos; + xfree[nbPoints] = pos; + xforce[nbPoints] = Vec3(0, 0, 0); + + m_modifier->notifyEndingEvent(); +// std::cout << "End addNewPointToState " << std::endl; +// std::cout << "End notifyEndingEvent " << std::endl; + } + + void PointsManager::removeLastPointfromState() + { + helper::WriteAccessor> x = *this->getMstate()->write(core::VecCoordId::position()); + helper::WriteAccessor> xfree = *this->getMstate()->write(core::VecCoordId::freePosition()); + const helper::ReadAccessor> &beam = m_beam->readPositions(); + unsigned nbPoints = this->getTopology()->getNbPoints(); // do not take the last point because there is a bug + + size_t beamSz = beam.size(); + sofa::type::vector Indices; + if (nbPoints > 0) + { + Indices.push_back(nbPoints - 1); + m_modifier->removePoints(Indices, true); + x.resize(nbPoints - 1); + std::cout << "the size is equal :" << nbPoints << std::endl; + xfree.resize(nbPoints - 1); + } + else + { + msg_error() << "Error cannot remove the last point because there is no point in the state"; + } + m_modifier->notifyEndingEvent(); + } + + void PointsManager::handleEvent(sofa::core::objectmodel::Event *event) + { + if (KeypressedEvent::checkEventType(event)) + { + KeypressedEvent *ev = static_cast(event); + switch (ev->getKey()) + { + case 'S': + case 's': + printf("A point is created \n"); + addNewPointToState(); + break; + case 'L': + case 'l': + printf("Remove point from state \n"); + removeLastPointfromState(); + break; + } + } + } + + // void PointsManager::draw(const core::visual::VisualParams *vparams) + // { + + // helper::ReadAccessor> x = *this->getMstate()->read(core::VecCoordId::position()); + // if (!x.size()) + // return; // if no points return + // glDisable(GL_LIGHTING); + // for (unsigned int j = 0; j < x.size(); j++) + // { + // glColor3f(1.0, 1.0, 0.0); + // vparams->drawTool()->drawSphere(x[j], d_radius.getValue() * 0.001); + // } + // glEnable(GL_LIGHTING); + // } + +} // Sofa diff --git a/src/engine/engineQuat.cpp b/src/CosseratPlugin/engine/engineQuat.cpp similarity index 100% rename from src/engine/engineQuat.cpp rename to src/CosseratPlugin/engine/engineQuat.cpp diff --git a/src/engine/engineQuat.h b/src/CosseratPlugin/engine/engineQuat.h similarity index 100% rename from src/engine/engineQuat.h rename to src/CosseratPlugin/engine/engineQuat.h diff --git a/src/forcefield/BeamHookeLawForceField.cpp b/src/CosseratPlugin/forcefield/BeamHookeLawForceField.cpp similarity index 98% rename from src/forcefield/BeamHookeLawForceField.cpp rename to src/CosseratPlugin/forcefield/BeamHookeLawForceField.cpp index 0dae9e12..4f11fdf1 100644 --- a/src/forcefield/BeamHookeLawForceField.cpp +++ b/src/CosseratPlugin/forcefield/BeamHookeLawForceField.cpp @@ -27,7 +27,9 @@ * Contact information: https://project.inria.fr/softrobot/contact/ * * * ******************************************************************************/ +#define SOFA_COSSERATPLUGIN_CPP_BeamHookeLawForceField #include "BeamHookeLawForceField.inl" + #include namespace sofa::component::forcefield diff --git a/src/forcefield/BeamHookeLawForceField.h b/src/CosseratPlugin/forcefield/BeamHookeLawForceField.h similarity index 76% rename from src/forcefield/BeamHookeLawForceField.h rename to src/CosseratPlugin/forcefield/BeamHookeLawForceField.h index 44fc4f94..90012a77 100644 --- a/src/forcefield/BeamHookeLawForceField.h +++ b/src/CosseratPlugin/forcefield/BeamHookeLawForceField.h @@ -27,8 +27,9 @@ * Contact information: https://project.inria.fr/softrobot/contact/ * * * ******************************************************************************/ - #pragma once +#include + #include #include #include @@ -83,6 +84,27 @@ public : typedef typename CompressedRowSparseMatrix::BlockConstAccessor _3_3_BlockConstAccessor; typedef typename CompressedRowSparseMatrix::BlockAccessor _3_3_BlockAccessor; + /** \enum class DeformationRegime + * \brief Types of mechanical state depending of the strain level. The + * POSTPLASTIC state corresponds to points which underwent plastic strain, + * but on which constraints were released so that the plasticity process + * stopped. + */ + // TO DO: this class, together with the field m_sectionDeformationRegimes declared below, + // is only useful because of the existence of a derived class from BeamHookeLawForceField, + // implementing nonelastic behaviour (such as BeamPlasticLawForceField). When using a + // BeamHookeLawForceField component only, all beam elements will only experience elastic + // deformation. The two other enum types (PLASTIC and POSTPLASTIC) won't be used, and the + // m_sectionDeformationRegimes field won't change. However, this allows a nonelastic forcefield + // to beneficiate from polymorphism when used in other object methods (typically BaseCosserat, + // or DiscreteCosseratMapping). + // Is this the best implementation scenario ? + enum class DeformationRegime { + ELASTIC = 0, + PLASTIC = 1, + POSTPLASTIC = 2, + }; + public : BeamHookeLawForceField(); @@ -90,7 +112,7 @@ public : ////////////////////////// Inherited from BaseObject ///////////////////////// void init() override; - void reinit() ; + void reinit() override; /////////////////////////////////////////////////////////////////////////// ////////////////////////// Inherited from ForceField ///////////////////////// @@ -112,7 +134,13 @@ public : const DataVecCoord& x) const override; //////////////////////////////////////////////////////////////////////////// - Real getRadius(); + virtual bool isPlastic() const; + auto getSectionDeformationRegimes() -> vector&; + auto getCrossSectionShape() const -> helper::OptionsGroup; + auto getRadius() const -> Real; + auto getLengthY() const -> Real; + auto getLengthZ() const -> Real; + auto getBeamLengths() const -> type::vector; protected: Data d_crossSectionShape; @@ -123,26 +151,32 @@ public : /// Circular Cross Section Data d_radius; Data d_innerRadius; - /// Rectangular Cross Section Data d_lengthY; Data d_lengthZ; - - /// Cross-section area - Real m_crossSectionArea; - - //In case we have a beam with different propertise per section - Data d_varianteSections; /// bool to identify different beams sections + //In case we have a beam with different properties per section + Data d_variantSections; /// bool to identify different beams sections Data> d_youngModulusList; /// youngModulus Data> d_poissonRatioList; /// poissonRatio + /// If the inertia parameters are given by the user, there is no longer any need to use YG. + Data d_useInertiaParams; + Data d_GI; + Data d_GA; + Data d_EA; + Data d_EI; bool compute_df; Mat33 m_K_section; type::vector m_K_sectionList; + /// Cross-section area + Real m_crossSectionArea; + + /// List of mechanical states associated to the length sections + vector m_sectionDeformationRegimes; private : - + ////////////////////////// Inherited attributes //////////////////////////// /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html /// Bring inherited attributes and function in the current lookup context. diff --git a/src/forcefield/BeamHookeLawForceField.inl b/src/CosseratPlugin/forcefield/BeamHookeLawForceField.inl similarity index 74% rename from src/forcefield/BeamHookeLawForceField.inl rename to src/CosseratPlugin/forcefield/BeamHookeLawForceField.inl index 9cd58e4d..44e528ee 100644 --- a/src/forcefield/BeamHookeLawForceField.inl +++ b/src/CosseratPlugin/forcefield/BeamHookeLawForceField.inl @@ -28,21 +28,19 @@ * * ******************************************************************************/ #pragma once - #include "BeamHookeLawForceField.h" + #include #include - #include -using sofa::core::behavior::MechanicalState ; +#include // ?? +using sofa::core::behavior::MechanicalState ; using sofa::core::objectmodel::BaseContext ; using sofa::helper::ReadAccessor ; using sofa::helper::WriteAccessor ; using sofa::core::VecCoordId; -// ?? -#include #include using std::cout ; using std::endl ; @@ -71,14 +69,18 @@ namespace sofa::component::forcefield d_innerRadius( initData( &d_innerRadius, 0.0, "innerRadius", "internal radius of the cross section (if circular)")), d_lengthY( initData( &d_lengthY, 1.0, "lengthY", "side length of the cross section along local y axis (if rectangular)")), d_lengthZ( initData( &d_lengthZ, 1.0, "lengthZ", "side length of the cross section along local z axis (if rectangular)")), - d_varianteSections( initData( &d_varianteSections, false, "varianteSections", "In case we have variant beam sections this has to be set to true")), + d_variantSections(initData(&d_variantSections, false, "variantSections", "In case we have variant beam sections this has to be set to true")), d_youngModulusList(initData(&d_youngModulusList, "youngModulusList", "The list of Young modulus in case we have sections with variable physical properties")), - d_poissonRatioList(initData(&d_poissonRatioList, "poissonRatioList", "The list of poisson's ratio in case we have sections with variable physical properties")) + d_poissonRatioList(initData(&d_poissonRatioList, "poissonRatioList", "The list of poisson's ratio in case we have sections with variable physical properties")), + d_useInertiaParams(initData(&d_useInertiaParams, false, "useInertiaParams", "If the inertia parameters are given by the user, there is no longer any need to use @d_youngModulus and @d_poissonRatio.")), + d_GI(initData(&d_GI, "GI", "The inertia parameter, GI")), + d_GA(initData(&d_GA, "GA", "The inertia parameter, GA")), + d_EA(initData(&d_EA, "EA", "The inertia parameter, EA")), + d_EI(initData(&d_EI, "EI", "The inertia parameter, EI")) { compute_df=true; } - template BeamHookeLawForceField::~BeamHookeLawForceField() {} @@ -121,23 +123,32 @@ namespace sofa::component::forcefield } m_crossSectionArea = A; - if(!d_varianteSections.getValue()){ - - Real E= d_youngModulus.getValue(); - Real G= E/(2.0*(1.0+d_poissonRatio.getValue())); + if(!d_variantSections.getValue()){ + if(!d_useInertiaParams.getValue()){ + Real E= d_youngModulus.getValue(); + Real G= E/(2.0*(1.0+d_poissonRatio.getValue())); + + m_K_section[0][0] = G*J; + m_K_section[1][1] = E*Iy; + m_K_section[2][2] = E*Iz; + } else{ + msg_info("BeamHookeLawForceField")<< "Pre-calculated inertia parameters are used for the computation " + "of the stiffness matrix."; + m_K_section[0][0] = d_GI.getValue(); + m_K_section[1][1] = d_EI.getValue(); + m_K_section[2][2] = d_EI.getValue(); + } - m_K_section[0][0] = G*J; - m_K_section[1][1] = E*Iy; - m_K_section[2][2] = E*Iz; }else { - msg_info("BeamHookeLawForceField")<< "=====> Multi section"; + msg_info("BeamHookeLawForceField")<< "Multi section beam are used for the simulation!"; m_K_sectionList.clear(); size_t szYM = d_youngModulusList.getValue().size(); size_t szPR = d_poissonRatioList.getValue().size(); size_t szL = d_length.getValue().size(); if((szL != szPR)||(szL != szYM)){ - msg_error("BeamHookeLawForceField")<< "Please, lenght, youngModulusList and poissonRatioList should have the same size"; + msg_error("BeamHookeLawForceField")<< "Please the size of the data length, youngModulusList and " + "poissonRatioList should be the same !"; return; } @@ -151,7 +162,15 @@ namespace sofa::component::forcefield _m_K_section[2][2] = E*Iz; m_K_sectionList.push_back(_m_K_section); } + msg_info("BeamHookeLawForceField")<< "If you plan to use a multi section beam with (different " + "mechanical properties) and pre-calculated inertia parameters " + "(GI, GA, etc.), this is not yet supported."; } + + // Initialisation of the section mechanical states to ELASTIC + size_t nbSections = d_length.getValue().size(); + m_sectionDeformationRegimes.clear(); + m_sectionDeformationRegimes.resize(nbSections, DeformationRegime::ELASTIC); } template @@ -181,7 +200,7 @@ namespace sofa::component::forcefield return; } - if(!d_varianteSections.getValue()) + if(!d_variantSections.getValue()) for (unsigned int i=0; ikFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); df.resize(dx.size()); - if(!d_varianteSections.getValue()) + if(!d_variantSections.getValue()) for (unsigned int i=0; imstate->read(core::ConstVecCoordId::position())->getValue(); for (unsigned int n=0; nadd(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_section[i][j]*d_length.getValue()[n]); @@ -245,11 +264,46 @@ namespace sofa::component::forcefield } } + template + bool BeamHookeLawForceField::isPlastic() const + { + return false; + } + + template + auto BeamHookeLawForceField::getCrossSectionShape() const -> helper::OptionsGroup + { + return d_crossSectionShape.getValue(); + } template - typename BeamHookeLawForceField::Real BeamHookeLawForceField::getRadius() + auto BeamHookeLawForceField::getRadius() const -> Real { return d_radius.getValue(); } + template + auto BeamHookeLawForceField::getLengthY() const -> Real + { + return d_lengthY.getValue(); + } + + template + auto BeamHookeLawForceField::getLengthZ() const -> Real + { + return d_lengthZ.getValue(); + } + + template + auto BeamHookeLawForceField::getBeamLengths() const -> type::vector + { + return d_length.getValue(); + } + + template + auto BeamHookeLawForceField::getSectionDeformationRegimes() -> vector& + { + return m_sectionDeformationRegimes; + } + } // forcefield diff --git a/src/forcefield/BeamPlasticLawForceField.cpp b/src/CosseratPlugin/forcefield/BeamPlasticLawForceField.cpp similarity index 98% rename from src/forcefield/BeamPlasticLawForceField.cpp rename to src/CosseratPlugin/forcefield/BeamPlasticLawForceField.cpp index 8da0e562..66dc2e96 100644 --- a/src/forcefield/BeamPlasticLawForceField.cpp +++ b/src/CosseratPlugin/forcefield/BeamPlasticLawForceField.cpp @@ -19,8 +19,9 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ - +#define SOFA_COSSERATPLUGIN_CPP_BeamPlasticLawForceField #include "BeamPlasticLawForceField.inl" + #include namespace sofa::component::forcefield diff --git a/src/forcefield/BeamPlasticLawForceField.h b/src/CosseratPlugin/forcefield/BeamPlasticLawForceField.h similarity index 85% rename from src/forcefield/BeamPlasticLawForceField.h rename to src/CosseratPlugin/forcefield/BeamPlasticLawForceField.h index 5c799903..41a7be64 100644 --- a/src/forcefield/BeamPlasticLawForceField.h +++ b/src/CosseratPlugin/forcefield/BeamPlasticLawForceField.h @@ -19,10 +19,10 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ - #pragma once +#include -#include "BeamHookeLawForceField.h" +#include namespace sofa::component::forcefield { @@ -43,6 +43,7 @@ class BeamPlasticLawForceField : public BeamHookeLawForceField typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::VecDeriv VecDeriv; typedef typename DataTypes::Coord Coord; + typedef typename BeamHookeLawForceField::DeformationRegime DeformationRegime; typedef Data DataVecCoord; typedef Data DataVecDeriv; @@ -50,17 +51,6 @@ class BeamPlasticLawForceField : public BeamHookeLawForceField typedef Vec<3, Real> Vec3; typedef Mat<3, 3, Real> Mat33; - /** \enum class MechanicalState - * \brief Types of mechanical state depending of the strain level. The - * POSTPLASTIC state corresponds to points which underwent plastic strain, - * but on which constraints were released so that the plasticity process - * stopped. - */ - enum class MechanicalState { - ELASTIC = 0, - PLASTIC = 1, - POSTPLASTIC = 2, - }; public: @@ -85,15 +75,22 @@ class BeamPlasticLawForceField : public BeamHookeLawForceField const MultiMatrixAccessor* matrix) override; /////////////////////////////////////////////////////////////////////////// - const vector& getSectionMechanicalStates(); + // Inherited from BeamHookeLawForceField + bool isPlastic() const; protected: - Data d_initialYieldStress; + Data> d_initialYieldStresses; // TO DO: Plastic modulus should not be a constant. Is this approximation relevant ? // Otherwise a generic law such as Ramberg-Osgood should be use. (Cf Plugin BeamPlastic) - Data d_plasticModulus; ///Linearisation coefficient for the plastic behaviour law + Data> d_plasticModuli; ///Linearisation coefficient for the plastic behaviour law + + /// Coefficients to determine the porportion of isotropic and kinematic hardening + /// 0 = purely kinematic + /// 1 = purely isotropic + Data> d_mixedHardeningCoefficients; + // Threshold used to compare stress tensor norms to 0. See detailed explanation // at the computation of the threshold in the init() method. @@ -103,8 +100,6 @@ class BeamPlasticLawForceField : public BeamHookeLawForceField VecCoord m_lastStrain; /// List of the stress associated to each length sections, at the previous time step vector m_prevStress; - /// List of mechanical states associated to the length sections - vector m_sectionMechanicalStates; /// List of tangent stiffness matrices, taking into account the plastic deformation of the length sections vector m_Kt_sectionList; @@ -113,6 +108,10 @@ class BeamPlasticLawForceField : public BeamHookeLawForceField vector m_effectivePlasticStrain; vector m_backStress; /// Centre of the yield surface, in stress space (one for each length section) + // NB: the distinction between d_initialYieldStresses and m_yieldStress allows to handle the evolution of + // the yield stresses internally, without accessing any user data. The evolution of yield stress should + // depend entirely on the plasticity method, and it doesn't make sense for a user to have access to it + // (left aside for the parameter initialisation). vector m_yieldStress; /// Elastic limit, varying if plastic deformation occurs (one for each length section) /// Generalised Hooke's law, reduced to the considered strain and stress components diff --git a/src/forcefield/BeamPlasticLawForceField.inl b/src/CosseratPlugin/forcefield/BeamPlasticLawForceField.inl similarity index 67% rename from src/forcefield/BeamPlasticLawForceField.inl rename to src/CosseratPlugin/forcefield/BeamPlasticLawForceField.inl index d1245aa0..1f33dd49 100644 --- a/src/forcefield/BeamPlasticLawForceField.inl +++ b/src/CosseratPlugin/forcefield/BeamPlasticLawForceField.inl @@ -19,9 +19,7 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ - #pragma once - #include "BeamPlasticLawForceField.h" using sofa::helper::ReadAccessor; @@ -34,15 +32,106 @@ namespace sofa::component::forcefield template BeamPlasticLawForceField::BeamPlasticLawForceField() : Inherit1(), - d_initialYieldStress(initData(&d_initialYieldStress, "initialYieldStress", - "Yield stress of the considered material, prior to any elastic deformation")), - d_plasticModulus(initData(&d_plasticModulus, "plasticModulus", - "Approximation of the plastic modulus as a constant. Can be deduced from a generic law such as Ramberg-Osgood's")) + d_initialYieldStresses(initData(&d_initialYieldStresses, "initialYieldStresses", + "Vector of yield stresses of the considered material(s), prior to any elastic deformation. A value is expected for each beam element")), + d_plasticModuli(initData(&d_plasticModuli, "plasticModuli", + "Approximation of the plastic modulus as a constant. Can be deduced from a generic law such as Ramberg-Osgood's. A value is expected for each beam element")), + d_mixedHardeningCoefficients(initData(&d_mixedHardeningCoefficients, "mixedHardeningCoefficients", + "For each beam: coefficient determining the proportion of kinematic and isotropic hardening. 0 = purely kinematic, 1 = purely isotropic")) { - if (d_initialYieldStress.getValue() < 0) + size_t nbBeams = this->d_length.getValue().size(); + + // Checking the number of yield stress parameters + size_t nbYieldStressParameters = d_initialYieldStresses.getValue().size(); + if (nbYieldStressParameters != nbBeams) + { + // If only one value is provided, we use this value for all beam elements, by default + if (nbYieldStressParameters == 1) + { + msg_info() << "Only one value was provided for the material yield stresses." + << "This value will be used for all beam elements as if they were" + << "made in the same material. If your Cosserat beam model involves" + << "several materials, you should provide a list of yield stresses" + << "values instead."; + Real uniqueYieldStress = d_initialYieldStresses.getValue()[0]; + vector& initialYieldStresses = *d_initialYieldStresses.beginEdit(); + initialYieldStresses.resize(nbBeams, uniqueYieldStress); + d_initialYieldStresses.endEdit(); + } + else + { + msg_error() << "The number of yield stress parameters doesn't match the" + << "number of beam elements."; + } + } + + // Checking the number of plastic modulus parameters + size_t nbPlasticModulusParameters = d_plasticModuli.getValue().size(); + if (nbPlasticModulusParameters != nbBeams) + { + // If only one value is provided, we use this value for all beam elements, by default + if (nbPlasticModulusParameters == 1) + { + msg_info() << "Only one value was provided for the material plastic moduli." + << "This value will be used for all beam elements as if they were" + << "made in the same material. If your Cosserat beam model involves" + << "several materials, you should provide a list of plastic modulus" + << "values instead."; + Real uniquePlasticModulus = d_plasticModuli.getValue()[0]; + vector& plasticModuli = *d_plasticModuli.beginEdit(); + plasticModuli.resize(nbBeams, uniquePlasticModulus); + d_plasticModuli.endEdit(); + } + else + { + msg_error() << "The number of plastic modulus parameters doesn't match the" + << "number of beam elements."; + } + } + + // Checking the number of mixed hardening parameters + size_t nbMixedHardeningCoefficients = d_mixedHardeningCoefficients.getValue().size(); + if (nbMixedHardeningCoefficients != nbBeams) + { + // If only one value is provided, we use this value for all beam elements, by default + if (nbMixedHardeningCoefficients == 1) + { + msg_info() << "Only one value was provided for the material mixed hardening coefficients." + << "This value will be used for all beam elements as if they were" + << "made in the same material. If your Cosserat beam model involves" + << "several materials, you should provide a list of coefficients for" + << "mixed hardening instead."; + Real uniqueHardeningCoefficient = d_mixedHardeningCoefficients.getValue()[0]; + vector& mixedHardeningCoefficients = *d_mixedHardeningCoefficients.beginEdit(); + mixedHardeningCoefficients.resize(nbBeams, uniqueHardeningCoefficient); + d_mixedHardeningCoefficients.endEdit(); + } + else + { + msg_error() << "The number of hardening coefficient parameters doesn't match the" + << "number of beam elements."; + } + } + + // Checking the integrity of each parameter + // TO DO: there probably is a more efficient way to access vector component data + for (unsigned int beamId = 0; beamId < nbBeams; beamId++) { - msg_error() << "yield Stress should be positive. Please provide a positive yield" - << "stress value for the considered material"; + if (d_initialYieldStresses.getValue()[beamId] < 0) + { + msg_error() << "yield Stress should be positive. Please provide a positive yield" + << "stress value for the considered material"; + } + if (d_plasticModuli.getValue()[beamId] < 0) + { + msg_error() << "Plastic modulus should be positive. Please provide a positive plastic" + << "modulus value for the considered material"; + } + if (d_mixedHardeningCoefficients.getValue()[beamId] < 0 || d_mixedHardeningCoefficients.getValue()[beamId] > 1) + { + msg_error() << "Mixed hardening coefficients should be between 0 and 1 (0 = purely kinematic, 1 = purely isotropic)." + << "Please provide a value between 0 and 1 for the considered material"; + } } } @@ -67,13 +156,9 @@ void BeamPlasticLawForceField::reinit() m_prevStress.clear(); m_prevStress.resize(nbSections, Vec3()); - // Initialisaiton of the section mechanical states to ELASTIC - m_sectionMechanicalStates.clear(); - m_sectionMechanicalStates.resize(nbSections, MechanicalState::ELASTIC); - - // Initialisaiton of the tangent stiffness matrices with the elastic stiffness matrices + // Initialisation of the tangent stiffness matrices with the elastic stiffness matrices m_Kt_sectionList.clear(); - if (!this->d_varianteSections.getValue()) + if (!this->d_variantSections.getValue()) { m_Kt_sectionList.resize(nbSections, this->m_K_section); } @@ -87,7 +172,7 @@ void BeamPlasticLawForceField::reinit() // As we are working with only 3 components of the strain tensor, // the generalised Hooke's law is reduced to a 3x3 diagonal matrix // (instead of a 4th order tensor represented as a 9x9 matrix) - if (!this->d_varianteSections.getValue()) { + if (!this->d_variantSections.getValue()) { Real E = this->d_youngModulus.getValue(); Real nu = this->d_poissonRatio.getValue(); @@ -108,12 +193,21 @@ void BeamPlasticLawForceField::reinit() } } + // Initialisation of the tangent stiffness matrices + // At the beginning of the simulation, the tangent stiffness matrix is actually + // a linear elastic stifness matrix + m_Kt_sectionList.clear(); + m_Kt_sectionList.resize(nbSections); + for (unsigned int segmentId=0; segmentId < nbSections; segmentId++) + updateTangentStiffness(segmentId); + // Initialisation of plasticity parameters m_backStress.clear(); m_backStress.resize(nbSections, Vec3()); m_yieldStress.clear(); - m_yieldStress.resize(nbSections, d_initialYieldStress.getValue()); + for (unsigned int beamId=0; beamId < nbSections; beamId++) + m_yieldStress.push_back(d_initialYieldStresses.getValue()[beamId]); //By default, no plastic deformation => no history m_plasticStrain.clear(); @@ -131,8 +225,9 @@ void BeamPlasticLawForceField::reinit() // available precision limit (e.g. std::numeric_limits::epsilon()). // We rely on the value of the initial Yield stress, as we can expect plastic // deformation to occur inside a relatively small intervl of stresses around this value. - const int orderOfMagnitude = d_initialYieldStress.getValue(); //Should use std::abs, but d_initialYieldStress > 0 + const int orderOfMagnitude = d_initialYieldStresses.getValue()[0]; //Should use std::abs, but d_initialYieldStresses[i] > 0 m_stressComparisonThreshold = std::numeric_limits::epsilon() * orderOfMagnitude; +// std::cout << "Comparison threshold : " << m_stressComparisonThreshold << std::endl; } @@ -174,10 +269,10 @@ void BeamPlasticLawForceField::computeStressIncrement(unsigned int se /// corresponds to an associative flow rule (for plasticity). //// First step = computation of the elastic predictor, as if deformation was entirely elastic - const MechanicalState mechanicalState = m_sectionMechanicalStates[sectionId]; + const DeformationRegime deformationRegime = this->m_sectionDeformationRegimes[sectionId]; Vec3 elasticIncrement = Vec3(); - if (!this->d_varianteSections.getValue()) + if (!this->d_variantSections.getValue()) elasticIncrement = m_genHookesLaw * strainIncrement; else elasticIncrement = m_genHookesLawList[sectionId] * strainIncrement; @@ -194,14 +289,14 @@ void BeamPlasticLawForceField::computeStressIncrement(unsigned int se newStressPoint = elasticPredictorStress; // If the segment was initially plastic, we update its mechanical state - if (mechanicalState == MechanicalState::PLASTIC) - m_sectionMechanicalStates[sectionId] = MechanicalState::POSTPLASTIC; + if (deformationRegime == DeformationRegime::PLASTIC) + this->m_sectionDeformationRegimes[sectionId] = DeformationRegime::POSTPLASTIC; } else { // If the segment was initially elastic, we update its mechanical state - if (mechanicalState == MechanicalState::POSTPLASTIC || mechanicalState == MechanicalState::ELASTIC) - m_sectionMechanicalStates[sectionId] = MechanicalState::PLASTIC; + if (deformationRegime == DeformationRegime::POSTPLASTIC || deformationRegime == DeformationRegime::ELASTIC) + this->m_sectionDeformationRegimes[sectionId] = DeformationRegime::PLASTIC; // /!\ We here consider that we obtain a deviatoric stress tensor, as the diagonal components of a // complete stress tensor, representing the axial stresses, are ignored in the model. @@ -215,10 +310,10 @@ void BeamPlasticLawForceField::computeStressIncrement(unsigned int se Vec3 N = shiftedDeviatoricElasticPredictor / shiftDevElasticPredictorNorm; // Indicates the proportion of Kinematic vs isotropic hardening. beta=0 <=> kinematic, beta=1 <=> isotropic - const Real beta = 0.5; + const Real beta = d_mixedHardeningCoefficients.getValue()[sectionId]; Real E, nu = 0; - if (!this->d_varianteSections.getValue()) + if (!this->d_variantSections.getValue()) { E = this->d_youngModulus.getValue(); nu = this->d_poissonRatio.getValue(); @@ -231,7 +326,7 @@ void BeamPlasticLawForceField::computeStressIncrement(unsigned int se const Real mu = E / (2 * (1 + nu)); // Lame coefficient // Plastic modulus - const Real H = d_plasticModulus.getValue(); + const Real H = d_plasticModuli.getValue()[sectionId]; // Computation of the plastic multiplier const double sqrt2 = helper::rsqrt(2.0); @@ -265,29 +360,25 @@ void BeamPlasticLawForceField::updateTangentStiffness(unsigned int se // TO DO: better way to handle the two cases? Mat33 C = Mat33(); Real E, nu = 0; - if (!this->d_varianteSections.getValue()) + if (!this->d_variantSections.getValue()) { E = this->d_youngModulus.getValue(); nu = this->d_poissonRatio.getValue(); C = m_genHookesLaw; } else - { - E = this->d_youngModulusList.getValue()[sectionId]; - nu = this->d_poissonRatioList.getValue()[sectionId]; C = m_genHookesLawList[sectionId]; - } Vec3 currentStressPoint = m_prevStress[sectionId]; //Updated in computeStressIncrement - Real H = d_plasticModulus.getValue(); + Real H = d_plasticModuli.getValue()[sectionId]; Mat33 Cep = Mat33(); // Cep Vec3 gradient = vonMisesGradient(currentStressPoint); if (correctedNorm(gradient) < m_stressComparisonThreshold - || m_sectionMechanicalStates[sectionId] != MechanicalState::PLASTIC) + || this->m_sectionDeformationRegimes[sectionId] != DeformationRegime::PLASTIC) Cep = C; //TO DO: is that correct ? else { @@ -299,20 +390,20 @@ void BeamPlasticLawForceField::updateTangentStiffness(unsigned int se matCN[i][0] = CN[i]; // NtC = (NC)t because of C symmetry Cep = C - (2 * matCN * matCN.transposed()) / (2.0 * N * CN + (2.0 / 3.0) * H); //TO DO: check that * operator is actually dot product - - // /!\ Warning on the computation above /!\ - // Terms CNNtC in the numerator and NtCN in the denominator are multiplied by 2 - // in order to account for the fact that we are using a reduced notation. - // In the same way that the generalised Hooke's law we use in m_genHookesLaw - // differs from the 'complete' 9x9 generalised Hooke's Law components by a - // factor 2, we multiply Cep components by 2 to account for the fact that - // multiplication of this matrix by a vector representing a symmetric tensor - // makes each non-diagonal terms appear twice. - // NB: this takes into account the fact that C is already expressed here with - // a factor 2. - // /!\ This should be used with serious caution in all computations, as it - // always has to be coherent with the actual complete tensor computation. - // TO DO: better way to implement this ? + /* /!\ Warning on the computation above /!\ + * Terms CNNtC in the numerator and NtCN in the denominator are multiplied by 2 + * in order to account for the fact that we are using a reduced notation. + * In the same way that the generalised Hooke's law we use in m_genHookesLaw + * differs from the 'complete' 9x9 generalised Hooke's Law components by a + * factor 2, we multiply Cep components by 2 to account for the fact that + * multiplication of this matrix by a vector representing a symmetric tensor + * makes each non-diagonal terms appear twice. + * NB: this takes into account the fact that C is already expressed here with + * a factor 2. + * /!\ This should be used with serious caution in all computations, as it + * always has to be coherent with the actual complete tensor computation. + * TO DO: better way to implement this ? + */ } // Integration step, consisting only in multiplication by the element volume, as @@ -363,7 +454,9 @@ void BeamPlasticLawForceField::addForce(const MechanicalParams* mpara computeStressIncrement(i, strainIncrement, newStressPoint); // If the beam is in plastic state, we update the tangent stiffness matrix - updateTangentStiffness(i); + const DeformationRegime deformationRegime = this->m_sectionDeformationRegimes[i]; + if (deformationRegime == DeformationRegime::PLASTIC) + updateTangentStiffness(i); // Computation of internal forces from stress // As stress and strain are uniform over the segment, spatial integration reduces to the segment volume @@ -457,11 +550,10 @@ typename BeamPlasticLawForceField::Real BeamPlasticLawForceField -const sofa::type::vector::MechanicalState>& BeamPlasticLawForceField::getSectionMechanicalStates() +template +bool BeamPlasticLawForceField::isPlastic() const { - return m_sectionMechanicalStates; + return true; } } // sofa::component::forcefield diff --git a/src/forcefield/CosseratInternalActuation.cpp b/src/CosseratPlugin/forcefield/CosseratInternalActuation.cpp similarity index 98% rename from src/forcefield/CosseratInternalActuation.cpp rename to src/CosseratPlugin/forcefield/CosseratInternalActuation.cpp index 5fcc0d0a..f23cb552 100644 --- a/src/forcefield/CosseratInternalActuation.cpp +++ b/src/CosseratPlugin/forcefield/CosseratInternalActuation.cpp @@ -27,7 +27,9 @@ * Contact information: https://project.inria.fr/softrobot/contact/ * * * ******************************************************************************/ +#define SOFA_COSSERATPLUGIN_CPP_CosseratInternalActuation #include "CosseratInternalActuation.inl" + #include namespace sofa::component::forcefield diff --git a/src/forcefield/CosseratInternalActuation.h b/src/CosseratPlugin/forcefield/CosseratInternalActuation.h similarity index 98% rename from src/forcefield/CosseratInternalActuation.h rename to src/CosseratPlugin/forcefield/CosseratInternalActuation.h index a320415a..c2cfb4a0 100644 --- a/src/forcefield/CosseratInternalActuation.h +++ b/src/CosseratPlugin/forcefield/CosseratInternalActuation.h @@ -28,6 +28,7 @@ * * ******************************************************************************/ #pragma once +#include #include #include @@ -36,10 +37,9 @@ #include #include #include -#include #include - #include +#include namespace sofa::component::forcefield { @@ -77,7 +77,7 @@ public : typedef Mat<3, 3, Real> Mat33; typedef CompressedRowSparseMatrix CSRMat33B66; - typedef type::Vector3 vector3; + typedef type::Vec3 vector3; typedef typename CompressedRowSparseMatrix::ColBlockConstIterator _3_3_ColBlockConstIterator; typedef typename CompressedRowSparseMatrix::RowBlockConstIterator _3_3_RowBlockConstIterator; diff --git a/src/forcefield/CosseratInternalActuation.inl b/src/CosseratPlugin/forcefield/CosseratInternalActuation.inl similarity index 99% rename from src/forcefield/CosseratInternalActuation.inl rename to src/CosseratPlugin/forcefield/CosseratInternalActuation.inl index 85c9b62d..3a75bc87 100644 --- a/src/forcefield/CosseratInternalActuation.inl +++ b/src/CosseratPlugin/forcefield/CosseratInternalActuation.inl @@ -29,12 +29,14 @@ ******************************************************************************/ #pragma once #include "CosseratInternalActuation.h" + #include #include #include +#include + #include #include -#include #include using sofa::core::behavior::MechanicalState ; @@ -48,7 +50,7 @@ using std::endl ; namespace sofa::component::forcefield { -using sofa::component::linearsolver::DefaultMultiMatrixAccessor ; +using sofa::core::behavior::DefaultMultiMatrixAccessor ; using sofa::core::behavior::MultiMatrixAccessor ; using sofa::core::behavior::BaseMechanicalState ; using sofa::helper::WriteAccessor ; diff --git a/src/forcefield/MyUniformVelocityDampingForceField.cpp b/src/CosseratPlugin/forcefield/MyUniformVelocityDampingForceField.cpp similarity index 100% rename from src/forcefield/MyUniformVelocityDampingForceField.cpp rename to src/CosseratPlugin/forcefield/MyUniformVelocityDampingForceField.cpp diff --git a/src/forcefield/MyUniformVelocityDampingForceField.h b/src/CosseratPlugin/forcefield/MyUniformVelocityDampingForceField.h similarity index 98% rename from src/forcefield/MyUniformVelocityDampingForceField.h rename to src/CosseratPlugin/forcefield/MyUniformVelocityDampingForceField.h index d94fc2d9..664a69ec 100644 --- a/src/forcefield/MyUniformVelocityDampingForceField.h +++ b/src/CosseratPlugin/forcefield/MyUniformVelocityDampingForceField.h @@ -81,7 +81,7 @@ class MyUniformVelocityDampingForceField : public core::behavior::ForceField; extern template class SOFA_SOFABOUNDARYCONDITION_API MyUniformVelocityDampingForceField; extern template class SOFA_SOFABOUNDARYCONDITION_API MyUniformVelocityDampingForceField; diff --git a/src/forcefield/MyUniformVelocityDampingForceField.inl b/src/CosseratPlugin/forcefield/MyUniformVelocityDampingForceField.inl similarity index 100% rename from src/forcefield/MyUniformVelocityDampingForceField.inl rename to src/CosseratPlugin/forcefield/MyUniformVelocityDampingForceField.inl diff --git a/src/initCosserat.cpp b/src/CosseratPlugin/initCosseratPlugin.cpp similarity index 80% rename from src/initCosserat.cpp rename to src/CosseratPlugin/initCosseratPlugin.cpp index a4ec2273..3631a9f3 100644 --- a/src/initCosserat.cpp +++ b/src/CosseratPlugin/initCosseratPlugin.cpp @@ -24,26 +24,25 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#include "initCosserat.h" -#include -#include +#include + #include #include using sofa::helper::system::PluginManager; +#include +#include -namespace sofa +namespace sofa::component { -namespace component -{ extern "C" { -SOFA_COSSERAT_MAPPING_API void initExternalModule(); -SOFA_COSSERAT_MAPPING_API const char* getModuleLicense(); -SOFA_COSSERAT_MAPPING_API const char* getModuleName(); -SOFA_COSSERAT_MAPPING_API const char* getModuleVersion(); -SOFA_COSSERAT_MAPPING_API const char* getModuleDescription(); -SOFA_COSSERAT_MAPPING_API const char* getModuleComponentList(); +SOFA_COSSERATPLUGIN_API void initExternalModule(); +SOFA_COSSERATPLUGIN_API const char* getModuleLicense(); +SOFA_COSSERATPLUGIN_API const char* getModuleName(); +SOFA_COSSERATPLUGIN_API const char* getModuleVersion(); +SOFA_COSSERATPLUGIN_API const char* getModuleDescription(); +SOFA_COSSERATPLUGIN_API const char* getModuleComponentList(); } //Here are just several convenient functions to help user to know what contains the plugin @@ -72,12 +71,12 @@ const char* getModuleLicense() const char* getModuleName() { - return "CosseratPlugin"; + return sofa_tostring(SOFA_TARGET); } const char* getModuleVersion() { - return "1.0"; + return sofa_tostring(COSSERATPLUGIN_VERSION); } const char* getModuleDescription() @@ -87,13 +86,9 @@ const char* getModuleDescription() const char* getModuleComponentList() { - return "BaseCosserat \n" - "DiscretCosseratMapping \n" - "DifferenceMultiMapping \n" - "DiscretDynamicCosseratMapping \n" - ; + // string containing the names of the classes provided by the plugin + static std::string classes = sofa::core::ObjectFactory::getInstance()->listClassesFromTarget(sofa_tostring(SOFA_TARGET)); + return classes.c_str(); } -} -} - +} // namespace sofa::component diff --git a/src/mapping/BaseCosserat.cpp b/src/CosseratPlugin/mapping/BaseCosserat.cpp similarity index 87% rename from src/mapping/BaseCosserat.cpp rename to src/CosseratPlugin/mapping/BaseCosserat.cpp index f6efc8d9..14c2554b 100644 --- a/src/mapping/BaseCosserat.cpp +++ b/src/CosseratPlugin/mapping/BaseCosserat.cpp @@ -19,9 +19,9 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#pragma once - +#define SOFA_COSSERATPLUGIN_CPP_BaseCosserat #include "BaseCosserat.inl" + #include #include #include @@ -39,7 +39,7 @@ int BaseCosseratClass = core::RegisterObject("Set the positions and velocities o ; -template class SOFA_COSSERAT_MAPPING_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; -template class SOFA_COSSERAT_MAPPING_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; +template class SOFA_COSSERATPLUGIN_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; +template class SOFA_COSSERATPLUGIN_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; } // namespace sofa. diff --git a/src/mapping/BaseCosserat.h b/src/CosseratPlugin/mapping/BaseCosserat.h similarity index 93% rename from src/mapping/BaseCosserat.h rename to src/CosseratPlugin/mapping/BaseCosserat.h index 758eb877..8d0e47c7 100644 --- a/src/mapping/BaseCosserat.h +++ b/src/CosseratPlugin/mapping/BaseCosserat.h @@ -19,18 +19,18 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ - #pragma once +#include + #include #include #include -#include "../initCosserat.h" #include #include #include -#include #include +#include namespace sofa::component::mapping { @@ -110,6 +110,9 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject [[maybe_unused]] core::State* m_fromModel2; core::State* m_toModel; + // Threshold for curvilinear abscissas (float) comparison + double m_comparisonThreshold; + public: /*===========COSSERAT VECTORS ======================*/ unsigned int m_index_input; @@ -154,14 +157,14 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject protected: /**********************COSSERAT METHODS**************************/ - void computeExponentialSE3(const double &x, const type::Vector3& k, Transform & Trans); + void computeExponentialSE3(const double &x, const type::Vec3& k, Transform & Trans); void computeAdjoint(const Transform & frame, Mat6x6 &adjoint); void compute_coAdjoint(const Transform & frame, Mat6x6 &co_adjoint); void compute_adjointVec6(const Vec6 & frame, Mat6x6 &adjoint); void update_ExponentialSE3(const In1VecCoord & inDeform); void update_TangExpSE3(const In1VecCoord & inDeform, const type::vector &curv_abs_section , const type::vector &curv_abs_frames ); - void compute_Tang_Exp(double & x, const Vector3& k, Mat6x6 & TgX); + void compute_Tang_Exp(double & x, const type::Vec3& k, Mat6x6 & TgX); [[maybe_unused]] type::Vec6 compute_eta(const Vec6 & baseEta, const In1VecDeriv & k_dot, double abs_input); type::Matrix4 computeLogarithm(const double & x, const Mat4x4 &gX); @@ -216,7 +219,7 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject return P; } - Matrix3 getTildeMatrix(const Vector3 & u){ + Matrix3 getTildeMatrix(const type::Vec3 & u){ type::Matrix3 tild; tild[0][1] = -u[2]; tild[0][2] = u[1]; @@ -255,7 +258,7 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject Matrix4 convertTransformToMatrix4x4(const Transform & T){ Matrix4 M; M.identity(); Matrix3 R = extract_rotMatrix(T); - Vector3 trans = T.getOrigin(); + type::Vec3 trans = T.getOrigin(); for (unsigned int i = 0; i < 3; i++) { for (unsigned int j=0; j<3; j++){ @@ -269,9 +272,9 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject }; -#if !defined(SOFA_COMPONENT_MAPPING_POE_MAPING_CPP) -extern template class SOFA_COSSERAT_MAPPING_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; -extern template class SOFA_COSSERAT_MAPPING_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; +#if !defined(SOFA_COSSERATPLUGIN_CPP_BaseCosserat) +extern template class SOFA_COSSERATPLUGIN_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; +extern template class SOFA_COSSERATPLUGIN_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; #endif } // namespace sofa diff --git a/src/mapping/BaseCosserat.inl b/src/CosseratPlugin/mapping/BaseCosserat.inl similarity index 82% rename from src/mapping/BaseCosserat.inl rename to src/CosseratPlugin/mapping/BaseCosserat.inl index 500b64b8..5c68f5d5 100644 --- a/src/mapping/BaseCosserat.inl +++ b/src/CosseratPlugin/mapping/BaseCosserat.inl @@ -19,20 +19,19 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ - #pragma once +#include "BaseCosserat.h" #include -#include "BaseCosserat.h" #include #include -#include #include - #include #include #include -#include "sofa/type/Quat.h" +#include + +#include namespace sofa::component::mapping @@ -80,7 +79,7 @@ void BaseCosserat::reinit() template -void BaseCosserat::computeExponentialSE3(const double & x, const type::Vector3& k, Transform & Trans){ +void BaseCosserat::computeExponentialSE3(const double & x, const type::Vec3& k, Transform & Trans){ Matrix4 I4; I4.identity(); double theta = k.norm(); @@ -116,7 +115,7 @@ void BaseCosserat::computeExponentialSE3(const double & x, con // msg_info("BaseCosserat: ")<< "Sub matrix M : "<< g_X; sofa::type::Quat R ; R.fromMatrix(M); - Vector3 T = Vector3(g_X(0,3),g_X(1,3),g_X(2,3)); + type::Vec3 T = type::Vec3(g_X(0,3),g_X(1,3),g_X(2,3)); Trans = Transform(T,R); } @@ -138,7 +137,7 @@ void BaseCosserat::update_ExponentialSE3(const In1VecCoord & i for (size_t i = 0; i < sz; i++) { Transform T ; - const Vector3 k = inDeform[m_indicesVectors[i]-1]; + const type::Vec3 k = inDeform[m_indicesVectors[i]-1]; const double x = m_framesLengthVectors[i]; computeExponentialSE3(x,k,T); m_framesExponentialSE3Vectors.push_back(T); @@ -152,10 +151,18 @@ void BaseCosserat::update_ExponentialSE3(const In1VecCoord & i //Compute the exponential at the nodes - m_nodesExponentialSE3Vectors.push_back(Transform(Vector3(0.0,0.0,0.0),type::Quat(0.,0.,0.,1.))); + m_nodesExponentialSE3Vectors.push_back(Transform(type::Vec3(0.0,0.0,0.0),type::Quat(0.,0.,0.,1.))); - for (unsigned int j = 0; j < inDeform.size(); j++) { - Vector3 k = inDeform[j]; + // Check on the number of beams compared to the size of the associated MechanicalObject + const unsigned int nbBeams = m_BeamLengthVectors.size(); + if (nbBeams > inDeform.size()) + { + msg_error("BaseCosserat") << "The number of beams defined in the Cosserat mapping should be inferior " + << "(or equal) to the size of the associated MechanicalObject.\n"; + } + for (unsigned int j = 0; j < nbBeams; j++) + { + type::Vec3 k = inDeform[j]; double x = m_BeamLengthVectors[j]; Transform T; computeExponentialSE3(x,k,T) ; m_nodesExponentialSE3Vectors.push_back(T); @@ -180,7 +187,7 @@ template void BaseCosserat:: computeAdjoint(const Transform & frame, Mat6x6 &adjoint) { Matrix3 R = extract_rotMatrix(frame); - Vector3 u = frame.getOrigin(); + type::Vec3 u = frame.getOrigin(); Matrix3 tilde_u = getTildeMatrix(u); Matrix3 tilde_u_R = tilde_u * R; buildAdjoint(R, tilde_u_R, adjoint); @@ -190,7 +197,7 @@ template void BaseCosserat:: compute_coAdjoint(const Transform & frame, Mat6x6 &co_adjoint) { Matrix3 R = extract_rotMatrix(frame); - Vector3 u = frame.getOrigin(); + type::Vec3 u = frame.getOrigin(); Matrix3 tilde_u = getTildeMatrix(u); Matrix3 tilde_u_R = tilde_u * R; build_coaAdjoint(R, tilde_u_R, co_adjoint); @@ -199,18 +206,18 @@ void BaseCosserat:: compute_coAdjoint(const Transform & frame, template void BaseCosserat::compute_adjointVec6(const Vec6& eta, Mat6x6 &adjoint) { - Matrix3 tildeMat = getTildeMatrix(Vector3(eta[0], eta[1], eta[2])); + Matrix3 tildeMat = getTildeMatrix(type::Vec3(eta[0], eta[1], eta[2])); adjoint.setsub(0, 0, tildeMat); adjoint.setsub(3, 3, tildeMat); - adjoint.setsub(3, 0, getTildeMatrix(Vector3(eta[3], eta[4], eta[5]))); + adjoint.setsub(3, 0, getTildeMatrix(type::Vec3(eta[3], eta[4], eta[5]))); } template -void BaseCosserat::compute_Tang_Exp(double & x, const type::Vector3& k, Mat6x6 & TgX){ +void BaseCosserat::compute_Tang_Exp(double & x, const type::Vec3& k, Mat6x6 & TgX){ double theta = k.norm(); - Matrix3 tilde_p = getTildeMatrix(Vector3(1.0, 0.0, 0.0)); + Matrix3 tilde_p = getTildeMatrix(type::Vec3(1.0, 0.0, 0.0)); Matrix3 tilde_k = getTildeMatrix(k); Mat6x6 ad_Xi ; @@ -271,7 +278,7 @@ Matrix4 BaseCosserat::computeLogarithm(const double & x, const // for (std::size_t i = 0; i < sz; i++) { // Mat6x6 temp ; -// Vector3 k = inDeform[m_indicesVectors[i]-1]; +// type::Vec3 k = inDeform[m_indicesVectors[i]-1]; // double x = m_framesLengthVectors[i]; // compute_Tang_Exp(x,k,temp) ; // m_framesTangExpVectors.push_back(temp); @@ -295,7 +302,7 @@ void BaseCosserat::update_TangExpSE3(const In1VecCoord & inDef for (unsigned int i = 0; i < sz; i++) { Mat6x6 temp ; - Vector3 k = inDeform[m_indicesVectors[i]-1]; + type::Vec3 k = inDeform[m_indicesVectors[i]-1]; double x = m_framesLengthVectors[i]; compute_Tang_Exp(x,k,temp) ; m_framesTangExpVectors.push_back(temp); @@ -313,7 +320,7 @@ void BaseCosserat::update_TangExpSE3(const In1VecCoord & inDef m_nodesTangExpVectors.push_back(tangExpO); for (size_t j = 1; j < curv_abs_section.size(); j++) { - Vector3 k = inDeform[j-1]; + type::Vec3 k = inDeform[j-1]; double x = m_BeamLengthVectors[j - 1]; Mat6x6 temp; temp.clear(); compute_Tang_Exp(x,k,temp); @@ -373,6 +380,16 @@ void BaseCosserat::initialize() helper::ReadAccessor>> curv_abs_section = d_curv_abs_section; helper::ReadAccessor>> curv_abs_frames = d_curv_abs_frames; + // Initialisation of the threshold for float comparison + // Implementation of the Cosserat mapping heavily relies on the use of curvilinear + // abscissas, which are computed over the total length of the Cosserat beam + // elements. Comparison between these curvilinear abscissas is a classic problem + // of float to float comparison, which may lead to rounding errors in some cases. + // To standardise float comparison in the Cosserat mapping, we use a threshold + // based both on the total Cosserat beam length, and the epsilon value from std. + const double totalBeamLength = curv_abs_section[curv_abs_section.size()-1] - curv_abs_section[0]; + m_comparisonThreshold = std::numeric_limits::epsilon() * totalBeamLength; + size_t sz = d_curv_abs_frames.getValue().size(); if(d_debug.getValue()) @@ -386,19 +403,31 @@ void BaseCosserat::initialize() m_indicesVectorsDraw.clear(); size_t input_index = 1; + const size_t nbBeams = d_curv_abs_section.getValue().size(); for (size_t i=0; i < sz; i++) { - if (curv_abs_section[input_index] > curv_abs_frames[i]) { + if (curv_abs_section[input_index] > curv_abs_frames[i] + m_comparisonThreshold) { m_indicesVectors.push_back(input_index); m_indicesVectorsDraw.push_back(input_index); } - else if(curv_abs_section[input_index] == curv_abs_frames[i]){ + else if(std::abs(curv_abs_section[input_index] - curv_abs_frames[i]) < m_comparisonThreshold){ m_indicesVectors.push_back(input_index); input_index++; m_indicesVectorsDraw.push_back(input_index); } else { - input_index++; + // In this case, curv_abs_frames[i] is srictly superior to the end + // curvilinear abscissa of the current beam. We increase the beam + // index until we find the beam on which is the frame, or until we + // reach the last beam. In this last case, the frame curvlinear abscissa + // is actually 'outside' of the beam component, so we arbitrarily + // assign the frame to the last beam. + // NB: most of the time, the while loop below contains only one + // iteration (to reach the next beam). Several iterations are + // necessary only if 0-length beams are created (for instance in + // an instrument navigation scenario) + while (curv_abs_frames[i] > curv_abs_section[input_index] + m_comparisonThreshold && input_index < nbBeams-1) + input_index++; m_indicesVectors.push_back(input_index); m_indicesVectorsDraw.push_back(input_index); } @@ -426,4 +455,3 @@ void BaseCosserat::draw(const core::visual::VisualParams* vpar if(!d_debug.getValue()) return; } } - diff --git a/src/mapping/DifferenceMultiMapping.cpp b/src/CosseratPlugin/mapping/DifferenceMultiMapping.cpp similarity index 97% rename from src/mapping/DifferenceMultiMapping.cpp rename to src/CosseratPlugin/mapping/DifferenceMultiMapping.cpp index ebba7177..e062062f 100644 --- a/src/mapping/DifferenceMultiMapping.cpp +++ b/src/CosseratPlugin/mapping/DifferenceMultiMapping.cpp @@ -19,9 +19,9 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define COSSERAT_PLUGIN_DIFFERENCEMULTIMAPPING_CPP - +#define SOFA_COSSERATPLUGIN_CPP_DifferenceMultiMapping #include "DifferenceMultiMapping.inl" + #include #include diff --git a/src/mapping/DifferenceMultiMapping.h b/src/CosseratPlugin/mapping/DifferenceMultiMapping.h similarity index 89% rename from src/mapping/DifferenceMultiMapping.h rename to src/CosseratPlugin/mapping/DifferenceMultiMapping.h index adf53c36..8954e55e 100644 --- a/src/mapping/DifferenceMultiMapping.h +++ b/src/CosseratPlugin/mapping/DifferenceMultiMapping.h @@ -20,13 +20,15 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once +#include + +#include + #include #include #include -#include "../initCosserat.h" #include #include -#include "BaseCosserat.h" namespace sofa::component::mapping @@ -35,7 +37,7 @@ using sofa::defaulttype::SolidTypes ; using sofa::core::objectmodel::BaseContext ; using sofa::type::Matrix3; using sofa::type::Matrix4; -using sofa::type::Vector3; +using sofa::type::Vec3; using sofa::type::Vec6; using std::get; using type::vector; @@ -95,12 +97,18 @@ class DifferenceMultiMapping : public core::Multi2Mapping //, typedef Data OutDataVecDeriv; typedef Data OutDataMatrixDeriv; -// typedef MultiLink, sofa::core::State< In1 >, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> LinkFromModels1; -// typedef MultiLink, sofa::core::State< In2 >, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> LinkFromModels2; -// typedef MultiLink, sofa::core::State< Out >, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> LinkToModels; - typedef typename SolidTypes::Transform Transform ; +public: + /********************** The component Data **************************/ + //Input data + Data> d_direction; + Data> d_indices; + Data d_radius; + Data d_color; + Data d_drawArrows; + Data d_lastPointIsFixed; + protected: core::State* m_fromModel1; core::State* m_fromModel2; @@ -110,14 +118,14 @@ class DifferenceMultiMapping : public core::Multi2Mapping //, /// Constructor DifferenceMultiMapping() ; /// Destructor - ~DifferenceMultiMapping() override {} + ~DifferenceMultiMapping() override = default; public: /**********************SOFA METHODS**************************/ void init() override; - virtual void bwdInit() override; // get the points - virtual void reset() override; - virtual void reinit() override; + void bwdInit() override; // get the points + void reset() override; + void reinit() override; void draw(const core::visual::VisualParams* vparams) override; /**********************MAPPING METHODS**************************/ @@ -140,7 +148,7 @@ class DifferenceMultiMapping : public core::Multi2Mapping //, void applyDJT(const core::MechanicalParams* /*mparams*/, core::MultiVecDerivId /*inForce*/, core::ConstMultiVecDerivId /*outForce*/) override{} /// This method must be reimplemented by all mappings if they need to support constraints. - virtual void applyJT( + void applyJT( const core::ConstraintParams* cparams , const type::vector< In1DataMatrixDeriv*>& dataMatOut1Const , const type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , const type::vector& dataMatInConst) override; @@ -155,15 +163,6 @@ class DifferenceMultiMapping : public core::Multi2Mapping //, void addPointProcess(){ msg_warning("DifferenceMultiMapping")<< "The point you are adding is :"; //<< pointPos ; } -public: - /********************** The component Data **************************/ - //Input data - Data> d_direction; - Data> d_indices; - Data d_radius; - Data d_color; - Data d_drawArrows; - Data d_lastPointIsFixed; private: @@ -180,7 +179,6 @@ class DifferenceMultiMapping : public core::Multi2Mapping //, } Constraint; type::vector m_constraints; - }; } // sofa::component::mapping diff --git a/src/CosseratPlugin/mapping/DifferenceMultiMapping.inl b/src/CosseratPlugin/mapping/DifferenceMultiMapping.inl new file mode 100644 index 00000000..a210312e --- /dev/null +++ b/src/CosseratPlugin/mapping/DifferenceMultiMapping.inl @@ -0,0 +1,817 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture, development version * + * (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH * + * * + * This program is free software; you can redistribute it and/or modify it * + * under the terms of the GNU Lesser General Public License as published by * + * the Free Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, but WITHOUT * + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * + * for more details. * + * * + * You should have received a copy of the GNU Lesser General Public License * + * along with this program. If not, see . * + ******************************************************************************* + * Authors: The SOFA Team and external contributors (see Authors.txt) * + * * + * Contact information: contact@sofa-framework.org * + ******************************************************************************/ +#pragma once +#include "DifferenceMultiMapping.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sofa::component::mapping +{ + using sofa::core::objectmodel::BaseContext; + using sofa::helper::AdvancedTimer; + using sofa::helper::WriteAccessor; + using sofa::type::RGBAColor; + + template + DifferenceMultiMapping::DifferenceMultiMapping() + : d_direction(initData(&d_direction, "direction", "The list of directions of fix points .\n")), + d_indices(initData(&d_indices, "indices", "Indices of fixe points of the cable")), + d_radius(initData(&d_radius, 2.0, "radius", "The size of the cable")), + d_color(initData(&d_color, type::Vec4f(1, 0, 0, 1), "color", "The color of the cable")), + d_drawArrows(initData(&d_drawArrows, false, "drawArrows", "The color of the cable")), + d_lastPointIsFixed(initData(&d_lastPointIsFixed, true, "lastPointIsFixed", "This select the last point as fixed of not," + "one.")), + m_fromModel1(NULL), m_fromModel2(NULL), m_toModel(NULL) + { + } + + template + void DifferenceMultiMapping::initiateTopologies() + { + m_toModel = this->getToModels()[0]; + if (!m_toModel) + { + std::cout << " No output mechanical state found. Consider setting the " + << this->toModels.getName() << " attribute." << std::endl; + return; + } + + if (!d_direction.isSet()) + msg_warning() << "No direction nor indices is given."; + + } + + // _________________________________________________________________________________________ + + template + void DifferenceMultiMapping::init() + { + if (this->getFromModels1().empty()) + { + msg_error() << "Error while initializing ; input getFromModels1 not found"; + return; + } + + if (this->getFromModels2().empty()) + { + msg_error() << "Error while initializing ; output getFromModels2 not found"; + return; + } + + if (this->getToModels().empty()) + { + msg_error() << "Error while initializing ; output Model not found"; + // return; + } + m_fromModel1 = this->getFromModels1()[0]; + m_fromModel2 = this->getFromModels2()[0]; + m_toModel = this->getToModels()[0]; + + m_toModel = m_fromModel1; + + initiateTopologies(); + + printf("init\n"); + } + + template + void DifferenceMultiMapping::bwdInit() + { + } + + template + void DifferenceMultiMapping::reinit() + { + } + + template + void DifferenceMultiMapping::reset() + { + reinit(); + } + + template + void DifferenceMultiMapping::computeProximity(const In1VecCoord &x1, const In2VecCoord &x2) + { + + In1VecCoord from = x1; + In2VecCoord dst = x2; + m_constraints.clear(); + + size_t szFrom = from.size(); + size_t szDst = dst.size(); + type::vector direction = d_direction.getValue(); + + /// get the last rigid direction, the main goal is to use it for the + /// 3D bilateral constraint i.e the fix point of the cable in the robot structure + // Rigid direction = d_direction.getValue()[szDst-1]; + + // For each point in the FEM find the closest edge of the cable + for (size_t i = 0; i < szFrom; i++) + { + Coord2 P = from[i]; + Constraint constraint; + + // find the min distance between a from mstate point, and it's projection on each edge of the cable (destination mstate) + Real min_dist = std::numeric_limits::max(); + for (size_t j = 0; j < szDst - 1; j++) + { + Coord1 Q1 = dst[j]; + Coord1 Q2 = dst[j + 1]; + // the axis + Coord1 dirAxe = Q2 - Q1; + Real length = dirAxe.norm(); + Real fact_v = dot(P - Q1, dirAxe) / dot(dirAxe, dirAxe); + + if (std::abs(fact_v) < min_dist) + { + // if(fact_v < min_dist){ + min_dist = std::abs(fact_v); + + // define the constraint variables + Deriv1 proj; // distVec; + Real alpha; // dist; + + /// To solve the case that the closest node is + /// not the node 0 but the node 1 of the beam + if (fact_v < 0.0 && j != 0 && std::abs(fact_v) > 1e-8) + { + // if fact_v < 0.0 that means the last beam is the good beam + // printf("if fact_v < 0.0 that means the last beam is the good beam \n"); + Q1 = dst[j - 1]; + dirAxe = dst[j] - Q1; + length = dirAxe.norm(); + fact_v = dot(P - Q1, dirAxe) / dot(dirAxe, dirAxe); + dirAxe.normalize(); + alpha = (P - Q1) * dirAxe; + + proj = Q1 + dirAxe * alpha; + // distVec = P - proj; // violation vector + // dist = (P - proj).norm(); // constraint violation + constraint.eid = j - 1; + // The direction of the axe or the beam + constraint.dirAxe = dirAxe; + // the node contribution to the constraint which is 1-coeff + alpha = alpha / length; // normalize, ensure that <1.0 + if (alpha < 1e-8) + constraint.alpha = 1.0; + else + constraint.alpha = 1.0 - alpha; + + // The projection on the axe + constraint.proj = proj; + constraint.Q = from[i]; + + ///// + length = (dst[j] - Q1).norm(); + constraint.Q1Q2 = length; + constraint.r2 = fact_v; + + // We move the constraint point onto the projection + Deriv1 t1 = P - proj; // violation vector + constraint.dist = t1.norm(); // constraint violation + t1.normalize(); // direction of the constraint + + //// First method compute normals using projections + // if(t1.norm()<1.0e-1 && dirAxe[2] < 0.99){ + // type::Vec3 temp = type::Vec3(dirAxe[0],dirAxe[1],dirAxe[2]+50.0); + // t1 = cross(dirAxe,temp); + // t1.normalize(); + // constraint.t1 = t1; + // } + // if(t1.norm()<1.0e-1){ + // type::Vec3 temp = type::Vec3(dirAxe[0],dirAxe[1]+50.0,dirAxe[2]); + // t1 = cross(dirAxe,temp); + // t1.normalize(); + // constraint.t1 = t1; + // } + + // if(t1.norm()<1.0e-1) + // { + + //// Second method compute normals using frames directions + Rigid dir = direction[constraint.eid]; + type::Vec3 vY = type::Vec3(0., 1., 0.); + type::Quat ori = dir.getOrientation(); + vY = ori.rotate(vY); + vY.normalize(); + t1 = vY; + // } + + constraint.t1 = t1; + // tangential 2 + Deriv1 t2 = cross(t1, dirAxe); t2.normalize(); + constraint.t2 = t2; + + if (i == szFrom - 1) + { + /// This handle the fix point constraint the last point of + /// of cstr points indeed here we have + /// 3D bilateral constraint and alpha=1.0 + // We use the given direction of fill H + + if (!direction.empty()) + { + type::Quat _ori = direction[szDst - 1].getOrientation(); + type::Vec3 _vY = _ori.rotate(type::Vec3(0., 1., 0.)); _vY.normalize(); + type::Vec3 _vZ = _ori.rotate(type::Vec3(0., 0., 1.)); _vZ.normalize(); + + constraint.t1 = _vY; + constraint.t2 = _vZ; + } + constraint.proj = dst[szDst - 1]; + constraint.eid = szDst - 2; + constraint.alpha = 1.0; + constraint.dist = (dst[szDst - 1] - from[szFrom - 1]).norm(); + } + } + else + { + // compute needs for constraint + dirAxe.normalize(); + alpha = (P - Q1) * dirAxe; + + proj = Q1 + dirAxe * alpha; + // distVec = P - proj; // violation vector + // dist = (P - proj).norm(); // constraint violation + constraint.eid = j; + // The direction of the axe or the beam + constraint.dirAxe = dirAxe; + // the node contribution to the constraint which is 1-coeff + alpha = alpha / length; // normalize, ensure that <1.0 + if (alpha < 1e-8) + constraint.alpha = 1.0; + else + constraint.alpha = 1.0 - alpha; + + // The projection on the axe + constraint.proj = proj; + constraint.Q = from[i]; + + ///// + constraint.Q1Q2 = length; + constraint.r2 = fact_v; + + // We move the constraint point onto the projection + Deriv1 t1 = P - proj; // violation vector + constraint.dist = t1.norm(); // constraint violation + t1.normalize(); // direction of the constraint + + /// If the violation is very small t1 is close to zero + /// + //// First method compute normals using projections + // if(t1.norm()<1.0e-1 && dirAxe[2] < 0.99){ + // type::Vec3 temp = type::Vec3(dirAxe[0],dirAxe[1],dirAxe[2]+50.0); + // t1 = cross(dirAxe,temp); + // t1.normalize(); + // constraint.t1 = t1; + // } + // if(t1.norm()<1.0e-1){ + // type::Vec3 temp = type::Vec3(dirAxe[0],dirAxe[1]+50.0,dirAxe[2]); + // t1 = cross(dirAxe,temp); + // t1.normalize(); + // constraint.t1 = t1; + // } + + //// Second method compute normals using frames directions + type::Vec3 vY = type::Vec3(0., 1., 0.); + type::Quat ori = (direction[szDst - 1]).getOrientation(); + vY = ori.rotate(vY); vY.normalize(); + t1 = vY; + // } + constraint.t1 = t1; + // tangential 2 + Deriv1 t2 = cross(t1, dirAxe); + t2.normalize(); + constraint.t2 = t2; + + /// This is need because we are applying the a + /// bilateral constraint on the last node of the mstate + if (i == szFrom - 1) + { + /// This handle the fix point constraint the last point of + /// of cstr points indeed here we have + /// 3D bilateral constraint and alpha=1.0 + // We use the given direction of fill H + if (!d_direction.getValue().empty()) + { + type::Quat _ori = (direction[szDst - 1]).getOrientation(); + type::Vec3 _vY = _ori.rotate(type::Vec3(0., 1., 0.)); _vY.normalize(); + type::Vec3 _vZ = _ori.rotate(type::Vec3(0., 0., 1.)); _vZ.normalize(); + + constraint.t1 = _vY; + constraint.t2 = _vZ; + } + constraint.proj = dst[szDst - 1]; + constraint.eid = szDst - 2; + constraint.alpha = 1.0; + constraint.dist = (dst[szDst - 1] - from[szFrom - 1]).norm(); + } + } + } + } + m_constraints.push_back(constraint); + } + } + + template + void DifferenceMultiMapping::computeNeedleProximity(const In1VecCoord &x1, const In2VecCoord &x2) + { + + In1VecCoord from = x1; + In2VecCoord dst = x2; + m_constraints.clear(); + + size_t szFrom = from.size(); + size_t szDst = dst.size(); + type::vector direction = d_direction.getValue(); + + /// get the last rigid direction, the main goal is to use it for the + /// 3D bilateral constraint i.e the fix point of the cable in the robot structure + // Rigid direction = d_direction.getValue()[szDst-1]; + + // For each point in the FEM find the closest edge of the cable + for (size_t i = 0; i < szFrom; i++) + { + Coord2 P = from[i]; + Constraint constraint; + + // find the min distance between a from mstate point, and it's projection on each edge of the cable (destination mstate) + Real min_dist = std::numeric_limits::max(); + for (size_t j = 0; j < szDst - 1; j++) + { + Coord1 Q1 = dst[j]; + Coord1 Q2 = dst[j + 1]; + // the axis + Coord1 dirAxe = Q2 - Q1; + Real length = dirAxe.norm(); + Real fact_v = dot(P - Q1, dirAxe) / dot(dirAxe, dirAxe); + + if (std::abs(fact_v) < min_dist) + { + // if(fact_v < min_dist){ + min_dist = std::abs(fact_v); + + // define the constraint variables + Deriv1 proj; + Real alpha; + + /// To solve the case that the closest node is + /// not the node 0 but the node 1 of the beam + if (fact_v < 0.0 && j != 0 && std::abs(fact_v) > 1e-8) + { + // if fact_v < 0.0 that means the last beam is the good beam + // printf("if fact_v < 0.0 that means the last beam is the good beam \n"); + Q1 = dst[j - 1]; + dirAxe = dst[j] - Q1; + length = dirAxe.norm(); + fact_v = dot(P - Q1, dirAxe) / dot(dirAxe, dirAxe); + dirAxe.normalize(); + alpha = (P - Q1) * dirAxe; + + proj = Q1 + dirAxe * alpha; + // distVec = P - proj; // violation vector + // dist = (P - proj).norm(); // constraint violation + constraint.eid = j - 1; + // The direction of the axe or the beam + constraint.dirAxe = dirAxe; + // the node contribution to the constraint which is 1-coeff + alpha = alpha / length; // normalize, ensure that <1.0 + if (alpha < 1e-8) + constraint.alpha = 1.0; + else + constraint.alpha = 1.0 - alpha; + + // The projection on the axe + constraint.proj = proj; + constraint.Q = from[i]; + + ///// + length = (dst[j] - Q1).norm(); + constraint.Q1Q2 = length; + constraint.r2 = fact_v; + + // We move the constraint point onto the projection + Deriv1 t1 = P - proj; // violation vector + constraint.dist = t1.norm(); // constraint violation + t1.normalize(); // direction of the constraint + + //// Second method compute normals using frames directions + Rigid dir = direction[constraint.eid]; + type::Vec3 vY = type::Vec3(0., 1., 0.); + type::Quat ori = dir.getOrientation(); + vY = ori.rotate(vY); vY.normalize(); + t1 = vY; + // } + + constraint.t1 = t1; + // tangential 2 + Deriv1 t2 = cross(t1, dirAxe); + t2.normalize(); + constraint.t2 = t2; + } + else + { + // compute needs for constraint + dirAxe.normalize(); + alpha = (P - Q1) * dirAxe; + + proj = Q1 + dirAxe * alpha; + // distVec = P - proj; // violation vector + // dist = (P - proj).norm(); // constraint violation + constraint.eid = j; + // The direction of the axe or the beam + constraint.dirAxe = dirAxe; + // the node contribution to the constraint which is 1-coeff + alpha = alpha / length; // normalize, ensure that <1.0 + if (alpha < 1e-8) + constraint.alpha = 1.0; + else + constraint.alpha = 1.0 - alpha; + + // The projection on the axe + constraint.proj = proj; + constraint.Q = from[i]; + + ///// + constraint.Q1Q2 = length; + constraint.r2 = fact_v; + + // We move the constraint point onto the projection + Deriv1 t1 = P - proj; // violation vector + constraint.dist = t1.norm(); // constraint violation + t1.normalize(); // direction of the constraint + + //// Second method compute normals using frames directions + Rigid dir = direction[constraint.eid]; + type::Vec3 vY = type::Vec3(0., 1., 0.); + type::Quat ori = dir.getOrientation(); + vY = ori.rotate(vY); vY.normalize(); + t1 = vY; + // } + constraint.t1 = t1; + // tangential 2 + Deriv1 t2 = cross(t1, dirAxe); t2.normalize(); + constraint.t2 = t2; + } + } + } + m_constraints.push_back(constraint); + } + } + + template + void DifferenceMultiMapping::apply( + const core::MechanicalParams * /* mparams */, const type::vector &dataVecOutPos, + const type::vector &dataVecIn1Pos, + const type::vector &dataVecIn2Pos) + { + + if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) + return; + + // printf("///Do Apply//We need only one input In model and input Root model (if present) \n"); + const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); + const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); + + OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); + + if (d_lastPointIsFixed.getValue()) + { + computeProximity(in1, in2); + + // auto out = sofa::helper::writeOnly(*dataVecOutPos[0]); + size_t sz = m_constraints.size(); + out.resize(sz); + + for (unsigned int i = 0; i < sz; i++) + { + Constraint &c = m_constraints[i]; + if (i < sz - 1) + { + out[i][0] = 0.0; + out[i][1] = c.t1 * (in1[i] - c.proj); // c.dist; + out[i][2] = c.t2 * (in1[i] - c.proj); // 0.0 + } + else + { + out[sz - 1][0] = c.dirAxe * (in1[in1.size() - 1] - in2[in2.size() - 1]); + out[sz - 1][1] = c.t1 * (in1[in1.size() - 1] - in2[in2.size() - 1]); // std::abs(in2[in2.size()-1][1] - in1[in1.size()-1][1]); + out[sz - 1][2] = c.t2 * (in1[in1.size() - 1] - in2[in2.size() - 1]); // std::abs(in2[in2.size()-1][2] - in1[in1.size()-1][2]); + } + } + } + else + { + computeNeedleProximity(in1, in2); + + // auto out = sofa::helper::writeOnly(*dataVecOutPos[0]); + size_t sz = m_constraints.size(); + out.resize(sz); + + for (unsigned int i = 0; i < sz; i++) + { + Constraint &c = m_constraints[i]; + out[i][0] = 0.0; + out[i][1] = c.t1 * (in1[i] - c.proj); // c.dist; + out[i][2] = c.t2 * (in1[i] - c.proj); // 0.0 + } + } + dataVecOutPos[0]->endEdit(); + } + + template + void DifferenceMultiMapping::applyJ( + const core::MechanicalParams * /* mparams */, const type::vector &dataVecOutVel, + const type::vector &dataVecIn1Vel, + const type::vector &dataVecIn2Vel) + { + if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) + return; + const In1VecDeriv &in1 = dataVecIn1Vel[0]->getValue(); + const In2VecDeriv &in2 = dataVecIn2Vel[0]->getValue(); + OutVecDeriv &outVel = *dataVecOutVel[0]->beginEdit(); + + // const OutVecCoord& out = m_toModel->read(core::ConstVecCoordId::position())->getValue(); + size_t sz = m_constraints.size(); + outVel.resize(sz); + + if (d_lastPointIsFixed.getValue()) + { + for (size_t i = 0; i < sz; i++) + { + Constraint &c = m_constraints[i]; + int ei1 = c.eid; + int ei2 = c.eid + 1; + if (i < sz - 1) + { + Real v0 = c.dirAxe * (in1[i] - c.alpha * in2[ei1] - (1 - c.alpha) * in2[ei2]); + Real v1 = c.t1 * (in1[i] - c.alpha * in2[ei1] - (1 - c.alpha) * in2[ei2]); + Real v2 = c.t2 * (in1[i] - c.alpha * in2[ei1] - (1 - c.alpha) * in2[ei2]); + outVel[i] = OutDeriv(v0, v1, v2); + } + else + { + Real v0 = c.dirAxe * (in1[i] - in2[ei2]); + Real v1 = c.t1 * (in1[i] - in2[ei2]); + Real v2 = c.t2 * (in1[i] - in2[ei2]); + outVel[i] = OutDeriv(v0, v1, v2); + } + } + } + else + { + for (size_t i = 0; i < sz; i++) + { + Constraint &c = m_constraints[i]; + + int ei1 = c.eid; + int ei2 = c.eid + 1; + Real v0 = c.dirAxe * (in1[i] - c.alpha * in2[ei1] - (1 - c.alpha) * in2[ei2]); + Real v1 = c.t1 * (in1[i] - c.alpha * in2[ei1] - (1 - c.alpha) * in2[ei2]); + Real v2 = c.t2 * (in1[i] - c.alpha * in2[ei1] - (1 - c.alpha) * in2[ei2]); + + outVel[i] = OutDeriv(v0, v1, v2); + } + } + dataVecOutVel[0]->endEdit(); + } + + template + void DifferenceMultiMapping::applyJT( + const core::MechanicalParams * /*mparams*/, const type::vector &dataVecOut1Force, + const type::vector &dataVecOut2Force, + const type::vector &dataVecInForce) + { + if (dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) + return; + + const OutVecDeriv &in = dataVecInForce[0]->getValue(); + if (in.empty()) + return; + + In1VecDeriv &out1 = *dataVecOut1Force[0]->beginEdit(); + In2VecDeriv &out2 = *dataVecOut2Force[0]->beginEdit(); + // Compute output forces + size_t sz = m_constraints.size(); + if (d_lastPointIsFixed.getValue()) + { + for (size_t i = 0; i < sz; i++) + { + Constraint &c = m_constraints[i]; + int ei1 = c.eid; + int ei2 = c.eid + 1; + OutDeriv f = in[i]; + if (i < sz - 1) + { + Deriv2 f1 = (f[0] * c.dirAxe) + (f[1] * c.t1) + (f[2] * c.t2); + Deriv1 f2_1 = (c.alpha * f[0] * c.dirAxe) + (c.alpha * f[1] * c.t1) + (c.alpha * f[2] * c.t2); + Deriv1 f2_2 = ((1 - c.alpha) * f[0] * c.dirAxe) + ((1 - c.alpha) * f[1] * c.t1) + ((1 - c.alpha) * f[2] * c.t2); + out1[i] += f1; out2[ei1] -= f2_1; out2[ei2] -= f2_2; + } + else + { + Deriv2 f1 = (f[0] * c.dirAxe) + (f[1] * c.t1) + (f[2] * c.t2); + out1[i] += f1; out2[ei2] -= f1; + } + } + } + else + { + for (size_t i = 0; i < sz; i++) + { + Constraint &c = m_constraints[i]; + int ei1 = c.eid; + int ei2 = c.eid + 1; + OutDeriv f = in[i]; + Deriv2 f1 = (f[0] * c.dirAxe) + (f[1] * c.t1) + (f[2] * c.t2); + Deriv1 f2_1 = (c.alpha * f[0] * c.dirAxe) + (c.alpha * f[1] * c.t1) + (c.alpha * f[2] * c.t2); + Deriv1 f2_2 = ((1 - c.alpha) * f[0] * c.dirAxe) + ((1 - c.alpha) * f[1] * c.t1) + ((1 - c.alpha) * f[2] * c.t2); + out1[i] += f1; + out2[ei1] -= f2_1; + out2[ei2] -= f2_2; + } + } + dataVecOut1Force[0]->endEdit(); + dataVecOut2Force[0]->endEdit(); + } + + //___________________________________________________________________________ + template + void DifferenceMultiMapping::applyJT( + const core::ConstraintParams * /*cparams*/, const type::vector &dataMatOut1Const, + const type::vector &dataMatOut2Const, + const type::vector &dataMatInConst) + { + if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty()) + return; + + // We need only one input In model and input Root model (if present) + In1MatrixDeriv &out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the FEM cable points + In2MatrixDeriv &out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the frames cable points + const OutMatrixDeriv &in = dataMatInConst[0]->getValue(); // input constraints defined on the mapped point + const In1DataVecCoord *x1fromData = m_fromModel1->read(core::ConstVecCoordId::position()); + const In1VecCoord x1from = x1fromData->getValue(); + + typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); + typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + + for (rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) + { + typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); + typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); + + // Creates a constraints if the input constraint is not empty. + if (colIt == colItEnd) + { + continue; + } + typename In1MatrixDeriv::RowIterator o1 = out1.writeLine(rowIt.index()); // we store the constraint number + typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); + + if (d_lastPointIsFixed.getValue()) + { + if ((rowIt.index() / 2) < (x1from.size() - 1)) + { + while (colIt != colItEnd) + { + int childIndex = colIt.index(); + Constraint c = m_constraints[childIndex]; + const OutDeriv h = colIt.val(); + int indexBeam = c.eid; + + Deriv2 h1 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2); + Deriv1 h2_1 = (c.alpha * h[0] * c.dirAxe) + (c.alpha * h[1] * c.t1) + (c.alpha * h[2] * c.t2); + Deriv1 h2_2 = ((1.0 - c.alpha) * h[0] * c.dirAxe) + ((1.0 - c.alpha) * h[1] * c.t1) + ((1.0 - c.alpha) * h[2] * c.t2); + + o1.addCol(childIndex, h1); + o2.addCol(indexBeam, -h2_1); + o2.addCol(indexBeam + 1, -h2_2); + + colIt++; + } + } + else + { + while (colIt != colItEnd) + { + int childIndex = colIt.index(); + Constraint c = m_constraints[childIndex]; + const OutDeriv h = colIt.val(); + int indexBeam = c.eid; + + Deriv2 h1 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2); + Deriv1 h2 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2); + + o1.addCol(childIndex, h1); + o2.addCol(indexBeam + 1, -h2); + colIt++; + } + } + } + else + { + while (colIt != colItEnd) + { + int childIndex = colIt.index(); + Constraint c = m_constraints[childIndex]; + const OutDeriv h = colIt.val(); + int indexBeam = c.eid; + + Deriv2 h1 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2); + Deriv1 h2_1 = (c.alpha * h[0] * c.dirAxe) + (c.alpha * h[1] * c.t1) + (c.alpha * h[2] * c.t2); + Deriv1 h2_2 = ((1.0 - c.alpha) * h[0] * c.dirAxe) + ((1.0 - c.alpha) * h[1] * c.t1) + ((1.0 - c.alpha) * h[2] * c.t2); + + o1.addCol(childIndex, h1); + o2.addCol(indexBeam, -h2_1); + o2.addCol(indexBeam + 1, -h2_2); + + colIt++; + } + } + } + dataMatOut1Const[0]->endEdit(); + dataMatOut2Const[0]->endEdit(); + } + + template + void DifferenceMultiMapping::draw(const core::visual::VisualParams *vparams) + { + /// draw cable + if (!vparams->displayFlags().getShowInteractionForceFields()) + return; + + typedef sofa::type::RGBAColor RGBAColor; + vparams->drawTool()->saveLastState(); + vparams->drawTool()->disableLighting(); + + std::vector vertices; + RGBAColor color = RGBAColor::magenta(); + + if (d_drawArrows.getValue() && d_lastPointIsFixed.getValue()) + { + for (size_t i = 0; i < m_constraints.size(); i++) + { + color = RGBAColor::green(); + vertices.push_back(m_constraints[i].proj); + vertices.push_back(m_constraints[i].Q); + vparams->drawTool()->drawLines(vertices, 4.0, color); + if (i == (m_constraints.size() - 1)) + { + Coord2 P1 = m_constraints[i].Q; + Real radius_arrow = 0.30; + Coord2 x = m_constraints[i].dirAxe * 5.0; + Coord2 y = m_constraints[i].t1 * 5.0; + Coord2 z = m_constraints[i].t2 * 5.0; + + vparams->drawTool()->drawArrow(P1, P1 + x, radius_arrow, RGBAColor::red()); + vparams->drawTool()->drawArrow(P1, P1 + y, radius_arrow, RGBAColor::green()); + vparams->drawTool()->drawArrow(P1, P1 + z, radius_arrow, RGBAColor::blue()); + } + else + { + Coord2 P1 = m_constraints[i].Q; + Real radius_arrow = 0.30; + // Coord2 x = m_constraints[i].dirAxe * 5.0; + Coord2 y = m_constraints[i].t1 * 5.0; + Coord2 z = m_constraints[i].t2 * 5.0; + vparams->drawTool()->drawArrow(P1, P1 + y, radius_arrow, RGBAColor::blue()); + vparams->drawTool()->drawArrow(P1, P1 + z, radius_arrow, RGBAColor::blue()); + } + } + const In1DataVecDeriv *xDestData = m_fromModel1->read(core::ConstVecCoordId::position()); + const In1VecCoord &fromPos = xDestData[0].getValue(); + vparams->drawTool()->draw3DText_Indices(fromPos, 6, RGBAColor(0.0, 1.0, 0.0, 1.0)); + } + vparams->drawTool()->restoreLastState(); + } + +} // namespace sofa \ No newline at end of file diff --git a/src/mapping/DiscreteCosseratMapping.cpp b/src/CosseratPlugin/mapping/DiscreteCosseratMapping.cpp similarity index 90% rename from src/mapping/DiscreteCosseratMapping.cpp rename to src/CosseratPlugin/mapping/DiscreteCosseratMapping.cpp index f23a9e02..fe7315aa 100644 --- a/src/mapping/DiscreteCosseratMapping.cpp +++ b/src/CosseratPlugin/mapping/DiscreteCosseratMapping.cpp @@ -19,9 +19,9 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -//#define SOFA_COMPONENT_MAPPING_POE_MAPING_CPP -#pragma once +#define SOFA_COSSERATPLUGIN_CPP_DiscreteCosseratMapping #include "DiscreteCosseratMapping.inl" + #include #include #include @@ -34,6 +34,6 @@ using namespace sofa::defaulttype; int DiscreteCosseratMappingClass = core::RegisterObject("Set the positions and velocities of points attached to a rigid parent") .add< DiscreteCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >(true) ; -template class SOFA_COSSERAT_MAPPING_API DiscreteCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; +template class SOFA_COSSERATPLUGIN_API DiscreteCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; } // namespace sofa::component::mapping diff --git a/src/mapping/DiscreteCosseratMapping.h b/src/CosseratPlugin/mapping/DiscreteCosseratMapping.h similarity index 92% rename from src/mapping/DiscreteCosseratMapping.h rename to src/CosseratPlugin/mapping/DiscreteCosseratMapping.h index cb5dc611..c07a0d78 100644 --- a/src/mapping/DiscreteCosseratMapping.h +++ b/src/CosseratPlugin/mapping/DiscreteCosseratMapping.h @@ -20,28 +20,26 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once +#include + +#include +#include #include #include #include -#include "../initCosserat.h" #include #include -#include "BaseCosserat.h" #include -#include "../forcefield/BeamPlasticLawForceField.h" -#include "../forcefield/BeamHookeLawForceField.h" - namespace sofa::component::mapping { - using sofa::component::forcefield::BeamPlasticLawForceField; using sofa::component::forcefield::BeamHookeLawForceField; using sofa::defaulttype::SolidTypes ; using sofa::core::objectmodel::BaseContext ; using sofa::type::Matrix3; using sofa::type::Matrix4; - using sofa::type::Vector3; + using sofa::type::Vec3; using sofa::type::Vec6; using std::get; @@ -76,7 +74,7 @@ class DiscreteCosseratMapping : public core::Multi2Mapping, pu typedef Data In1DataVecCoord; typedef Data In1DataVecDeriv; typedef Data In1DataMatrixDeriv; - + typedef typename In2::Coord::value_type Real2; typedef typename In2::Coord Coord2 ; typedef typename In2::Deriv Deriv2 ; @@ -107,6 +105,7 @@ class DiscreteCosseratMapping : public core::Multi2Mapping, pu Data d_min; Data d_radius ; Data d_drawMapBeam ; + Data d_drawBeamSegments; Data d_color; Data > d_index; Data d_baseIndex; @@ -119,7 +118,7 @@ class DiscreteCosseratMapping : public core::Multi2Mapping, pu typedef MultiLink, sofa::core::State< In2 >, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> LinkFromModels2; typedef MultiLink, sofa::core::State< Out >, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> LinkToModels; - typedef SingleLink, BeamPlasticLawForceField, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> LinkToPlasticForceField; + typedef SingleLink, BeamHookeLawForceField, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> LinkToBeamForceField; @@ -146,10 +145,10 @@ class DiscreteCosseratMapping : public core::Multi2Mapping, pu using BaseCosserat::m_indicesVectorsDraw; // Link with Cosserat force field, for visualisation purposes - LinkToPlasticForceField l_fromPlasticForceField; + LinkToBeamForceField l_fromBeamForceField; protected: - /// Constructor + /// Constructor DiscreteCosseratMapping() ; /// Destructor ~DiscreteCosseratMapping() override {} @@ -157,6 +156,7 @@ class DiscreteCosseratMapping : public core::Multi2Mapping, pu public: /**********************SOFA METHODS**************************/ void init() override; + void reinit() override; void draw(const core::visual::VisualParams* vparams) override; /**********************MAPPING METHODS**************************/ @@ -183,15 +183,14 @@ class DiscreteCosseratMapping : public core::Multi2Mapping, pu const core::ConstraintParams* cparams , const type::vector< In1DataMatrixDeriv*>& dataMatOut1Const , const type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , const type::vector& dataMatInConst) override; - + void computeBBox(const core::ExecParams* params, bool onlyVisible) override; protected: helper::ColorMap m_colorMap; }; -#if !defined(SOFA_COMPONENT_MAPPING_POE_MAPING_CPP) -extern template class SOFA_COSSERAT_MAPPING_API DiscreteCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; +#if !defined(SOFA_COSSERATPLUGIN_CPP_DiscreteCosseratMapping) +extern template class SOFA_COSSERATPLUGIN_API DiscreteCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; #endif -} // namespace sofa::componenet::mapping - //SOFA_COMPONENT_MAPPING_POEMAPING_H +} // namespace sofa::component::mapping diff --git a/src/mapping/DiscreteCosseratMapping.inl b/src/CosseratPlugin/mapping/DiscreteCosseratMapping.inl similarity index 70% rename from src/mapping/DiscreteCosseratMapping.inl rename to src/CosseratPlugin/mapping/DiscreteCosseratMapping.inl index 73b49d36..0b090fd7 100644 --- a/src/mapping/DiscreteCosseratMapping.inl +++ b/src/CosseratPlugin/mapping/DiscreteCosseratMapping.inl @@ -20,18 +20,19 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once +#include "DiscreteCosseratMapping.h" #include -#include "DiscreteCosseratMapping.h" #include #include -#include #include #include #include #include #include -#include "sofa/type/Quat.h" +#include + +#include namespace sofa::component::mapping @@ -55,10 +56,15 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() "the maximum of the deformation.\n")) , d_min(initData(&d_min, (Real2)0.0, "min", "the minimum of the deformation.\n")) - , d_radius(initData(&d_radius, (Real2)0.005, "radius", + , d_radius(initData(&d_radius, (Real2)0.05, "radius", "the axis in which we want to show the deformation.\n")) , d_drawMapBeam(initData(&d_drawMapBeam, true,"nonColored", "if this parameter is false, you draw the beam with " "color according to the force apply to each beam")) + , d_drawBeamSegments(initData(&d_drawBeamSegments, false, "drawBeamSegments", "if true, a visual representation of " + "the beam segments will be displayed, according to the " + "cross-section shape and dimension. The color of each beam " + "represents its mechanical state: red = elastic, " + "blue = plastic, green = post-plastic")) , d_color(initData(&d_color, type::Vec4f (40/255.0, 104/255.0, 137/255.0, 0.8) ,"color", "The default beam color")) , d_index(initData(&d_index, "index", "if this parameter is false, you draw the beam with color " "according to the force apply to each beam")) @@ -66,10 +72,17 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() "base of Cosserat models, 0 by default this can" "take another value if the rigid base is given " "by another body.")) - , l_fromPlasticForceField(initLink("forcefield","Path to the Cosserat force field component in scene")) -{} - - + , l_fromBeamForceField(initLink("forcefield","Path to the Cosserat force field component in scene")) +{ + this->addUpdateCallback("updateFrames", {&d_curv_abs_section, &d_curv_abs_frames, &d_debug}, [this](const core::DataTracker& t) + { + SOFA_UNUSED(t); + this->initialize(); + const In1VecCoord& inDeform = m_fromModel1->read(core::ConstVecCoordId::position())->getValue(); + this->update_ExponentialSE3(inDeform); + return sofa::core::objectmodel::ComponentState::Valid; + }, {}); +} template void DiscreteCosseratMapping::init() @@ -79,42 +92,54 @@ void DiscreteCosseratMapping::init() msg_error() << "Error while initializing ; input getFromModels1/getFromModels2/output not found" ; return; } + reinit(); + + if(!l_fromBeamForceField) + msg_info() << "No Cosserat force field found, no visual representation of such forcefield will be displayed."; - if(!l_fromPlasticForceField) - msg_info() << "No Cosserat plastic force field found, no visual representation of such forcefield will be displayed."; m_fromModel1 = this->getFromModels1()[0]; // Cosserat deformations (torsion and bending), in local frame m_fromModel2 = this->getFromModels2()[0]; // Cosserat base, in global frame m_toModel = this->getToModels()[0]; // Cosserat rigid frames, in global frame + m_colorMap.setColorScheme("Blue to Red"); + m_colorMap.reinit(); +} - // Fill the initial vector - const OutDataVecCoord* xFromData = m_toModel->read(core::ConstVecCoordId::position()); - const OutVecCoord xFrom = xFromData->getValue(); +template +void DiscreteCosseratMapping::reinit() +{ + m_fromModel1 = this->getFromModels1()[0]; // Cosserat deformations (torsion and bending), in local frame + m_fromModel2 = this->getFromModels2()[0]; // Cosserat base, in global frame + m_toModel = this->getToModels()[0]; // Cosserat rigid frames, in global frame - m_vecTransform.clear(); - for (unsigned int i = 0; i < xFrom.size(); i++) { - m_vecTransform.push_back(xFrom[i]); - } + // Fill the initial vectorvoid init() override; + const OutDataVecCoord* xFromData = m_toModel->read(core::ConstVecCoordId::position()); + const OutVecCoord xFrom = xFromData->getValue(); - if(d_debug.getValue()) - msg_info("DiscreteCosseratMapping")<< " m_vecTransform : "<< m_vecTransform; + m_vecTransform.clear(); + for (unsigned int i = 0; i < xFrom.size(); i++) { + m_vecTransform.push_back(xFrom[i]); + } - this->initialize(); + if(d_debug.getValue()) + msg_info("DiscreteCosseratMapping")<< " m_vecTransform : "<< m_vecTransform; - m_colorMap.setColorScheme("Blue to Red"); - m_colorMap.reinit(); + this->initialize(); } - template void DiscreteCosseratMapping::apply( const core::MechanicalParams* /* mparams */, const type::vector& dataVecOutPos, const type::vector& dataVecIn1Pos , const type::vector& dataVecIn2Pos) { - if(dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) return; + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_curv_abs_section and d_curv_abs_frames) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + ///Do Apply //We need only one input In model and input Root model (if present) const In1VecCoord& in1 = dataVecIn1Pos[0]->getValue(); @@ -125,10 +150,10 @@ void DiscreteCosseratMapping::apply( out.resize(sz); const auto baseIndex = d_baseIndex.getValue(); - // update the Exponential Matrices according to new deformation + // update the Exponential matrices according to new deformation // Here we update m_framesExponentialSE3Vectors & m_nodesExponentialSE3Vectors this->update_ExponentialSE3(in1); - /* Go from Cossserat to SOFA frame*/ + /* from cossserat to SOFA frame*/ Transform frame0 = Transform(In2::getCPos(in2[baseIndex]),In2::getCRot(in2[baseIndex])); for(unsigned int i=0; i::apply( } frame *= m_framesExponentialSE3Vectors[i]; - Vector3 v = frame.getOrigin(); + type::Vec3 v = frame.getOrigin(); type::Quat q = frame.getOrientation(); out[i] = OutCoord(v,q); } - // @todo do this another place + // @todo m_index_input = 0; dataVecOutPos[0]->endEdit(); } @@ -185,7 +210,7 @@ void DiscreteCosseratMapping::apply( // for (std::size_t i = 0; i < sz; i++) { // Mat6x6 temp ; -// Vector3 k = inDeform[m_indicesVectors[i]-1]; +// type::Vec3 k = inDeform[m_indicesVectors[i]-1]; // double x = m_framesLengthVectors[i]; // compute_Tang_Exp(x,k,temp) ; // m_framesTangExpVectors.push_back(temp); @@ -207,6 +232,12 @@ void DiscreteCosseratMapping:: applyJ( if(dataVecOutVel.empty() || dataVecIn1Vel.empty() ||dataVecIn2Vel.empty() ) return; + + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_curv_abs_section and d_curv_abs_frames) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + const In1VecDeriv& in1 = dataVecIn1Vel[0]->getValue(); const In2VecDeriv& in2_vecDeriv = dataVecIn2Vel[0]->getValue(); OutVecDeriv& outVel = *dataVecOutVel[0]->beginEdit(); @@ -244,7 +275,7 @@ void DiscreteCosseratMapping:: applyJ( Mat6x6 Adjoint; Adjoint.clear(); this->computeAdjoint(Trans, Adjoint); - type::Vec6 Xi_dot = Vec6(in1[i-1],Vector3(0.0,0.0,0.0)) ; + type::Vec6 Xi_dot = Vec6(in1[i-1],type::Vec3(0.0,0.0,0.0)) ; Vec6 temp = Adjoint * (m_nodesVelocityVectors[i-1] + m_nodesTangExpVectors[i] * Xi_dot ); m_nodesVelocityVectors.push_back(temp); if(d_debug.getValue()) @@ -259,7 +290,7 @@ void DiscreteCosseratMapping:: applyJ( Mat6x6 Adjoint; Adjoint.clear(); this->computeAdjoint(Trans, Adjoint); - type::Vec6 Xi_dot = Vec6(in1[m_indicesVectors[i]-1],Vector3(0.0,0.0,0.0)) ; + type::Vec6 Xi_dot = Vec6(in1[m_indicesVectors[i]-1],type::Vec3(0.0,0.0,0.0)) ; Vec6 temp = Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i]-1] + m_framesTangExpVectors[i] * Xi_dot ); // eta auto T = Transform(out[i].getCenter(), out[i].getOrientation()); @@ -284,6 +315,11 @@ void DiscreteCosseratMapping:: applyJT( if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) return; + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_curv_abs_section and d_curv_abs_frames) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + const OutVecDeriv& in = dataVecInForce[0]->getValue(); In1VecDeriv& out1 = *dataVecOut1Force[0]->beginEdit(); @@ -328,9 +364,9 @@ void DiscreteCosseratMapping:: applyJT( Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; Mat6x6 temp = m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in applyJ (here we transpose) temp.transpose(); - Vector3 f = matB_trans * temp * node_F_Vec; + type::Vec3 f = matB_trans * temp * node_F_Vec; - if(index != m_indicesVectors[s]){ // TODO to be replaced by while + if(index != m_indicesVectors[s]){ index--; //bring F_tot to the reference of the new beam this->compute_coAdjoint(m_nodesExponentialSE3Vectors[index],coAdjoint); //m_nodesExponentialSE3Vectors computed in apply @@ -338,7 +374,7 @@ void DiscreteCosseratMapping:: applyJT( Mat6x6 temp = m_nodesTangExpVectors[index]; temp.transpose(); //apply F_tot to the new beam - Vector3 temp_f = matB_trans * temp * F_tot; + type::Vec3 temp_f = matB_trans * temp * F_tot; out1[index-1] += temp_f; } if(d_debug.getValue()) @@ -373,6 +409,11 @@ void DiscreteCosseratMapping::applyJT( if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty() ) return; + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_curv_abs_section and d_curv_abs_frames) were changed + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + //We need only one input In model and input Root model (if present) In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space (reduced coordinate) In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the reference frame (base frame) @@ -434,7 +475,7 @@ void DiscreteCosseratMapping::applyJT( type::Vec6 local_F = coAdjoint * P_trans * valueConst; // constraint direction in local frame of the beam. - Vector3 f = matB_trans * temp * local_F; // constraint direction in the strain space. + type::Vec3 f = matB_trans * temp * local_F; // constraint direction in the strain space. o1.addCol(indexBeam-1, f); std::tuple test = std::make_tuple(indexBeam, local_F); @@ -509,7 +550,7 @@ void DiscreteCosseratMapping::applyJT( // transfer to strain space (local coordinates) Mat6x6 temp = m_nodesTangExpVectors[i-1]; temp.transpose(); - Vector3 temp_f = matB_trans * temp * CumulativeF; + type::Vec3 temp_f = matB_trans * temp * CumulativeF; if(i>1) o1.addCol(i-2, temp_f); i--; @@ -528,6 +569,26 @@ void DiscreteCosseratMapping::applyJT( dataMatOut2Const[0]->endEdit(); } +template +void DiscreteCosseratMapping::computeBBox(const core::ExecParams*, bool) +{ +// const VecCoord& x = getVertices(); //m_vertices.getValue(); + const OutVecCoord& x = m_toModel->read(core::ConstVecCoordId::position())->getValue(); + + SReal minBBox[3] = {std::numeric_limits::max(),std::numeric_limits::max(),std::numeric_limits::max()}; + SReal maxBBox[3] = {-std::numeric_limits::max(),-std::numeric_limits::max(),-std::numeric_limits::max()}; + for (std::size_t i = 0; i < x.size(); i++) + { + const OutCoord& p = x[i]; + for (int c=0; c<3; c++) + { + if (p[c] > maxBBox[c]) maxBBox[c] = p[c]; + if (p[c] < minBBox[c]) minBBox[c] = p[c]; + } + } + this->f_bbox.setValue(sofa::type::TBoundingBox(minBBox,maxBBox)); +} + template void DiscreteCosseratMapping::draw(const core::visual::VisualParams* vparams) @@ -536,11 +597,10 @@ void DiscreteCosseratMapping::draw(const core::visual::VisualP // draw cable typedef RGBAColor RGBAColor; - typedef typename BeamPlasticLawForceField::MechanicalState MechanicalState; const OutDataVecCoord* xfromData = m_toModel->read(core::ConstVecCoordId::position()); const OutVecCoord xData = xfromData->getValue(); - type::vector positions; + type::vector positions; type::vector> Orientation; positions.clear(); Orientation.clear(); @@ -550,36 +610,114 @@ void DiscreteCosseratMapping::draw(const core::visual::VisualP Orientation.push_back(xData[i].getOrientation()); } - //Get access articulated + const auto beamCurvAbs = d_curv_abs_section.getValue(); + const auto nbBeams = beamCurvAbs.size()-1; // beamCurvAbs also contains the curvilinear abscissa for the end of the last beam + + //Get access articulated (Vec3 strain DoFs, defined usually in rateAngularDeform) const In1DataVecCoord* artiData = m_fromModel1->read(core::ConstVecCoordId::position()); const In1VecCoord xPos = artiData->getValue(); - // Drawing a beam representation to display plastic behaviour - if (l_fromPlasticForceField) - { - auto radius = l_fromPlasticForceField->getRadius(); - auto sectionMechanicalStates = l_fromPlasticForceField->getSectionMechanicalStates(); - auto nbSections = sectionMechanicalStates.size(); + // Retrieve the Cosserat base coordinates (necessary to get the beam nodes positions, for display) + const auto baseIndex = d_baseIndex.getValue(); + const In2VecCoord& xfrom2Data = m_fromModel2->read(core::ConstVecCoordId::position())->getValue(); + Transform frame0 = Transform(In2::getCPos(xfrom2Data[baseIndex]),In2::getCRot(xfrom2Data[baseIndex])); + - for (unsigned int sectionId=0; sectionId < nbSections; sectionId++) + // Drawing cylinders between the input (beam) frames + + if (d_drawBeamSegments.getValue()) + { + if(!l_fromBeamForceField) { - RGBAColor drawColor = RGBAColor::gray(); - if(sectionMechanicalStates[sectionId] == MechanicalState::ELASTIC) - drawColor = RGBAColor(191/255.0, 37/255.0, 42/255.0, 0.8); // Red - else if(sectionMechanicalStates[sectionId] == MechanicalState::PLASTIC) - drawColor = RGBAColor(40/255.0, 104/255.0, 137/255.0, 0.8); // Blue - else // MechanicalState::POSTPLASTIC - drawColor = RGBAColor(76/255.0, 154/255.0, 50/255.0, 0.8);; // Green - - for (unsigned int i=0; idrawTool()->drawCylinder(positions[i], positions[i+1], radius, drawColor); + msg_info() << "No Cosserat force field found, no visual representation of such forcefield will be displayed."; } - } + else + { + const auto beamLengths = l_fromBeamForceField->getBeamLengths(); + + //Initialisation, with the begin frame of the first beam + double localBeginNodeCurvAbs = 0.; + Vector3 beamStrainDoFs = xPos[0]; + Transform beginFrameLocalTransform; + this->computeExponentialSE3(localBeginNodeCurvAbs,beamStrainDoFs,beginFrameLocalTransform); + Transform beginFrameGlobalTransform = frame0 * beginFrameLocalTransform; + Vector3 beginFramePos = beginFrameGlobalTransform.getOrigin(); + type::Quat beginFrameQuat = beginFrameGlobalTransform.getOrientation(); + + // Variables for the beam end frame + double localEndNodeCurvAbs = 0.0; + Transform endFrameLocalTransform = Transform(); + Transform endFrameGlobalTransform = Transform(); + Vector3 endFramePos = Vector3(); + type::Quat endFrameQuat = type::Quat(0., 0., 0., 1.); + + for (unsigned int beamId=0; beamId < nbBeams; beamId++) + { + if (beamLengths[beamId] < 1.0e-6) //TO DO: use section dimensions as threshold + continue; + + // Retrieve the end frame of the current beam + beamStrainDoFs = xPos[beamId]; + localEndNodeCurvAbs = beamLengths[beamId]; + + this->computeExponentialSE3(localEndNodeCurvAbs,beamStrainDoFs,endFrameLocalTransform); + + // TO DO: is there a way to not loop, but take advantage of the precedent local/global Transform ? + // NB: at least, it is not possible to take advantage of the computation done for the frames, + // as they can be placed anywhere on the beams by the user (with d_curv_abs_output) + endFrameGlobalTransform = frame0; + for (unsigned int beamSubId=0; beamSubId < beamId+1; beamSubId++) + endFrameGlobalTransform *= this->m_nodesExponentialSE3Vectors[beamSubId]; + endFrameGlobalTransform *= endFrameLocalTransform; + + endFramePos = endFrameGlobalTransform.getOrigin(); + endFrameQuat = endFrameGlobalTransform.getOrientation(); + + + //Draw a beam, which colour indicates the mechanical state + + const auto sectionDeformationRegimes = l_fromBeamForceField->getSectionDeformationRegimes(); + RGBAColor drawColor = RGBAColor::gray(); + if (l_fromBeamForceField->isPlastic()) + { + if(sectionDeformationRegimes[beamId] == BeamHookeLawForceField::DeformationRegime::ELASTIC) + drawColor = RGBAColor(191/255.0, 37/255.0, 42/255.0, 0.8); // Red + else if(sectionDeformationRegimes[beamId] == BeamHookeLawForceField::DeformationRegime::PLASTIC) + drawColor = RGBAColor(40/255.0, 104/255.0, 137/255.0, 0.8); // Blue + else // DeformationRegime::POSTPLASTIC + drawColor = RGBAColor(76/255.0, 154/255.0, 50/255.0, 0.8); // Green + } + else + { + drawColor = d_color.getValue(); + } + + auto crossSectionShape = l_fromBeamForceField->getCrossSectionShape(); + if (crossSectionShape.getSelectedItem() == "rectangular") + { + const auto lengthY = l_fromBeamForceField->getLengthY(); + const auto lengthZ = l_fromBeamForceField->getLengthZ(); + //TO DO: change for an actual parallelepiped + vparams->drawTool()->drawCylinder(beginFramePos, endFramePos, std::min(lengthY, lengthZ), drawColor); + } + else // crossSectionShape.getSelectedItem() == "circular" + { + const auto radius = l_fromBeamForceField->getRadius(); + vparams->drawTool()->drawCylinder(beginFramePos, endFramePos, radius, drawColor); + } + + //Update begin frame for next step + beginFramePos = endFramePos; + } + } // if l_fromBeamForceField + } // if d_drawBeamSegments else { - RGBAColor drawColor = d_color.getValue(); + // Drawing cylinders between the output frames + RGBAColor drawFrameColor = d_color.getValue(); for (unsigned int i=0; idrawTool()->drawCylinder(positions[i], positions[i+1], d_radius.getValue(), drawColor); + vparams->drawTool()->drawCylinder(positions[i], positions[i+1], + d_radius.getValue(), drawFrameColor); } diff --git a/src/mapping/DiscreteDynamicCosseratMapping.cpp b/src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.cpp similarity index 90% rename from src/mapping/DiscreteDynamicCosseratMapping.cpp rename to src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.cpp index 8a22be9c..5c61fcd6 100644 --- a/src/mapping/DiscreteDynamicCosseratMapping.cpp +++ b/src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.cpp @@ -19,8 +19,9 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#pragma once //SOFA_COMPONENT_MAPPING_DYNAMIC_COSSERAT_DISCRETE_CPP +#define SOFA_COSSERATPLUGIN_CPP_DiscreteDynamicCosseratMapping #include "DiscreteDynamicCosseratMapping.inl" + #include #include #include @@ -35,6 +36,6 @@ int DiscretDynamicCosseratMappingClass = core::RegisterObject("Set the positions .add< DiscreteDynamicCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >() ; -template class SOFA_COSSERAT_MAPPING_API DiscreteDynamicCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; +template class SOFA_COSSERATPLUGIN_API DiscreteDynamicCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; } // namespace sofa::component::mapping diff --git a/src/mapping/DiscreteDynamicCosseratMapping.h b/src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.h similarity index 94% rename from src/mapping/DiscreteDynamicCosseratMapping.h rename to src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.h index 5da45364..e4e3dd5d 100644 --- a/src/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.h @@ -19,16 +19,15 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#pragma once SOFA_COMPONENT_MAPPING_DYNAMIC_COSSERAT_DISCRET_H +#pragma once +#include + +#include #include #include -#include "../initCosserat.h" #include #include -#include "BaseCosserat.h" - - namespace sofa::component::mapping { @@ -71,7 +70,7 @@ class DiscreteDynamicCosseratMapping : public core::Multi2Mapping In1DataVecCoord; typedef Data In1DataVecDeriv; typedef Data In1DataMatrixDeriv; - + typedef typename In2::Coord::value_type Real ; typedef typename In2::Coord Coord2 ; typedef typename In2::Deriv Deriv2 ; @@ -181,17 +180,14 @@ class DiscreteDynamicCosseratMapping : public core::Multi2Mapping &J_i, const type::Vec6 &etaFrame, type::vector &J_dot_i); }; -//extern template class SOFA_POE_MAPPING_API DiscretDynamicCosseratMapping; - - -#if !defined(SOFA_COMPONENT_MAPPING_DYNAMIC_COSSERAT_DISCRET_CPP) -extern template class SOFA_COSSERAT_MAPPING_API DiscreteDynamicCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; +#if !defined(SOFA_COSSERATPLUGIN_CPP_DiscreteDynamicCosseratMapping) +extern template class SOFA_COSSERATPLUGIN_API DiscreteDynamicCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; #endif } // namespace sofa::componenet::mapping diff --git a/src/mapping/DiscreteDynamicCosseratMapping.inl b/src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.inl similarity index 91% rename from src/mapping/DiscreteDynamicCosseratMapping.inl rename to src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.inl index dc97f6f2..18b32df1 100644 --- a/src/mapping/DiscreteDynamicCosseratMapping.inl +++ b/src/CosseratPlugin/mapping/DiscreteDynamicCosseratMapping.inl @@ -19,19 +19,19 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#pragma once //SOFA_COMPONENT_MAPPING_POEMAPING_INL +#pragma once +#include "DiscreteDynamicCosseratMapping.h" #include -#include "DiscreteDynamicCosseratMapping.h" #include #include -#include #include - #include #include #include -#include "sofa/type/Quat.h" +#include + +#include namespace sofa::component::mapping @@ -147,7 +147,7 @@ void DiscreteDynamicCosseratMapping::apply( } frame *= m_framesExponentialSE3Vectors[i]; - Vector3 v = frame.getOrigin(); + type::Vec3 v = frame.getOrigin(); type::Quat q = frame.getOrientation(); out[i] = OutCoord(v,q); } @@ -208,14 +208,16 @@ void DiscreteDynamicCosseratMapping:: applyJ( Transform t= m_nodesExponentialSE3Vectors[i].inversed(); Mat6x6 Adjoint; Adjoint.clear(); this->computeAdjoint(t,Adjoint); - m_nodeAdjointVectors.push_back(Adjoint); //Add this line because need for the jacobian computation - - //Compute velocity (eta) at node i != 0 eq.(13) paper - type::Vec6 Xi_dot = Vec6(in1[i-1],Vector3(0.0,0.0,0.0)) ; - Vec6 temp = Adjoint * (m_nodesVelocityVectors[i-1] + m_nodesTangExpVectors[i] * Xi_dot ); - m_nodesVelocityVectors.push_back(temp); - if(debug) - std::cout<< "Node velocity : "<< i << " = " << temp<< std::endl; + //Add this line because need for the jacobian computation + m_nodeAdjointVectors.push_back(Adjoint); + + //Compute velocity (eta) at node i != 0 eq.(13) paper + type::Vec6 Xi_dot = Vec6(in1[i-1],type::Vec3(0.0,0.0,0.0)) ; + Vec6 temp = Adjoint * (m_nodesVelocityVectors[i-1] + + m_nodesTangExpVectors[i] * Xi_dot ); + m_nodesVelocityVectors.push_back(temp); + if(debug) + std::cout<< "Node velocity : "<< i << " = " << temp<< std::endl; } const OutVecCoord& out = m_toModel->read(core::ConstVecCoordId::position())->getValue(); @@ -226,24 +228,26 @@ void DiscreteDynamicCosseratMapping:: applyJ( Mat6x6 Adjoint; Adjoint.clear(); this->computeAdjoint(t,Adjoint); - type::Vec6 Xi_dot = Vec6(in1[m_indicesVectors[i]-1],Vector3(0.0,0.0,0.0)) ; - Vec6 etaFrame = Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i]-1] + m_framesTangExpVectors[i] * Xi_dot ); // eta + type::Vec6 Xi_dot = Vec6(in1[m_indicesVectors[i]-1], + type::Vec3(0.0,0.0,0.0)) ; + // eta + Vec6 etaFrame = Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i]-1] + + m_framesTangExpVectors[i] * Xi_dot ); - //Compute here jacobien and jacobien_dot + //Compute here jacobien and jacobien_dot type::vector J_i, J_dot_i; - computeJ_Jdot_i(Adjoint, i, J_i, etaFrame, J_dot_i); - m_frameJacobienVector.push_back(J_i); - m_frameJacobienVector.push_back(J_dot_i); - - //Convert from Federico node to Sofa node - Transform _T = Transform(out[i].getCenter(),out[i].getOrientation()); - Mat6x6 _P = this->build_projector(_T); - //std::cout<< "Eta local : "<< eta << std::endl; - - outVel[i] = _P * etaFrame; - - if(debug) - std::cout<< "Frame velocity : "<< i << " = " << etaFrame<< std::endl; + computeJ_Jdot_i(Adjoint, i, J_i, etaFrame, J_dot_i); + m_frameJacobienVector.push_back(J_i); + m_frameJacobienVector.push_back(J_dot_i); + + //Convert from Federico node to Sofa node + Transform _T = Transform(out[i].getCenter(),out[i].getOrientation()); + Mat6x6 _P = this->build_projector(_T); + //std::cout<< "Eta local : "<< eta << std::endl; + + outVel[i] = _P * etaFrame; + if(debug) + std::cout<< "Frame velocity : "<< i << " = " << etaFrame<< std::endl; } // std::cout << "Inside the apply J, outVel after computation : "<< outVel << std::endl; dataVecOutVel[0]->endEdit(); @@ -268,7 +272,7 @@ void DiscreteDynamicCosseratMapping::computeJ_Jdot_i(const Mat bool reachNode = false; for (unsigned int i = 1; i < sz; i++) { M = Adjoint; - int u = m_indicesVectors[frameId]; + unsigned int u = m_indicesVectors[frameId]; //std::cout << "frame : "<< frameId << " ==> section :" << i << " ==> u :"<< u << std::endl; if(i < u ){ for (int j = u; j>0; j--) { @@ -351,7 +355,7 @@ void DiscreteDynamicCosseratMapping:: applyJT( //Compute output forces size_t sz = m_indicesVectors.size(); - int index = m_indicesVectors[sz-1]; + unsigned int index = m_indicesVectors[sz-1]; m_totalBeamForceVectors.clear(); m_totalBeamForceVectors.resize(sz); @@ -367,7 +371,7 @@ void DiscreteDynamicCosseratMapping:: applyJT( Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; Mat6x6 temp = m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in applyJ (here we transpose) temp.transpose(); - Vector3 f = matB_trans * temp * node_F_Vec; + type::Vec3 f = matB_trans * temp * node_F_Vec; if(index!=m_indicesVectors[s]){ // TODO to be replaced by while index--; @@ -377,7 +381,7 @@ void DiscreteDynamicCosseratMapping:: applyJT( Mat6x6 temp = m_nodesTangExpVectors[index]; temp.transpose(); //apply F_tot to the new beam - Vector3 temp_f = matB_trans * temp * F_tot; + type::Vec3 temp_f = matB_trans * temp * F_tot; out1[index-1] += temp_f; } if(d_debug.getValue()) @@ -479,7 +483,7 @@ void DiscreteDynamicCosseratMapping::applyJT( type::Vec6 local_F = coAdjoint * P_trans * valueConst; // constraint direction in local frame of the beam. - Vector3 f = matB_trans * temp * local_F; // constraint direction in the strain space. + type::Vec3 f = matB_trans * temp * local_F; // constraint direction in the strain space. o1.addCol(indexBeam-1, f); @@ -563,7 +567,7 @@ void DiscreteDynamicCosseratMapping::applyJT( // transfer to strain space (local coordinates) Mat6x6 temp = m_nodesTangExpVectors[i-1]; temp.transpose(); - Vector3 temp_f = matB_trans * temp * CumulativeF; + type::Vec3 temp_f = matB_trans * temp * CumulativeF; if(i>1) o1.addCol(i-2, temp_f); diff --git a/src/mapping/LegendrePolynomialsMapping.cpp b/src/CosseratPlugin/mapping/LegendrePolynomialsMapping.cpp similarity index 75% rename from src/mapping/LegendrePolynomialsMapping.cpp rename to src/CosseratPlugin/mapping/LegendrePolynomialsMapping.cpp index 0ce1b02c..1484e2f5 100644 --- a/src/mapping/LegendrePolynomialsMapping.cpp +++ b/src/CosseratPlugin/mapping/LegendrePolynomialsMapping.cpp @@ -1,9 +1,9 @@ // // Created by younes on 17/11/2021. // - -#define SOFA_COMPONENT_MAPPING_LEGENDREPOLYNOMIALSMAPPING_CPP +#define SOFA_COSSERATPLUGIN_CPP_LegendrePolynomialsMapping #include "LegendrePolynomialsMapping.inl" + #include #include #include @@ -15,5 +15,5 @@ namespace sofa::component::mapping // Register in the Factory int LegendrePolynomialsMappingClass = core::RegisterObject("Set the positions and velocities of points attached to a rigid parent") .add< LegendrePolynomialsMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types > >() ; - template class SOFA_COSSERAT_MAPPING_API LegendrePolynomialsMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; + template class SOFA_COSSERATPLUGIN_API LegendrePolynomialsMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; } // namespace sofa::component::mapping diff --git a/src/mapping/LegendrePolynomialsMapping.h b/src/CosseratPlugin/mapping/LegendrePolynomialsMapping.h similarity index 90% rename from src/mapping/LegendrePolynomialsMapping.h rename to src/CosseratPlugin/mapping/LegendrePolynomialsMapping.h index c6bb7134..c144c09e 100644 --- a/src/mapping/LegendrePolynomialsMapping.h +++ b/src/CosseratPlugin/mapping/LegendrePolynomialsMapping.h @@ -1,16 +1,17 @@ // // Created by younes on 17/11/2021. // - #pragma once +#include + #include #include #include -#include "../initCosserat.h" #include #include #include #include + #include @@ -81,9 +82,9 @@ class LegendrePolynomialsMapping : public core::Mapping void draw(const core::visual::VisualParams* vparams) override; }; -#if !defined(SOFA_COMPONENT_MAPPING_LEGENDREPOLYNOMIALSMAPPING_CPP) - extern template class SOFA_SOFARIGID_API LegendrePolynomialsMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; + +#if !defined(SOFA_COSSERATPLUGIN_CPP_LegendrePolynomialsMapping) +extern template class SOFA_SOFARIGID_API LegendrePolynomialsMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Vec3Types >; #endif } -//#endif //SOFA_LEGENDREPOLYNOMIALSMAPPING_H diff --git a/src/mapping/LegendrePolynomialsMapping.inl b/src/CosseratPlugin/mapping/LegendrePolynomialsMapping.inl similarity index 98% rename from src/mapping/LegendrePolynomialsMapping.inl rename to src/CosseratPlugin/mapping/LegendrePolynomialsMapping.inl index f46209f3..517023da 100644 --- a/src/mapping/LegendrePolynomialsMapping.inl +++ b/src/CosseratPlugin/mapping/LegendrePolynomialsMapping.inl @@ -1,24 +1,20 @@ // // Created by younes on 17/11/2021. // - #pragma once - #include "LegendrePolynomialsMapping.h" -#include -#include +#include #include #include - #include #include - #include #include #include #include #include +#include namespace sofa::component::mapping { @@ -78,7 +74,7 @@ namespace sofa::component::mapping { // std::cout<< "Apply : in " << in[0] <::draw(const core::visual::VisualParam } -} \ No newline at end of file +} diff --git a/src/mapping/ProjectionEngine.cpp b/src/CosseratPlugin/mapping/ProjectionEngine.cpp similarity index 95% rename from src/mapping/ProjectionEngine.cpp rename to src/CosseratPlugin/mapping/ProjectionEngine.cpp index b2250333..e26183bf 100644 --- a/src/mapping/ProjectionEngine.cpp +++ b/src/CosseratPlugin/mapping/ProjectionEngine.cpp @@ -19,12 +19,11 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COMPONENT_CONSTRAINTSET_ProjectionEngine_CPP - +#define SOFA_COSSERATPLUGIN_CPP_ProjectionEngine #include "ProjectionEngine.inl" #include -#include +#include #include namespace sofa diff --git a/src/mapping/ProjectionEngine.h b/src/CosseratPlugin/mapping/ProjectionEngine.h similarity index 93% rename from src/mapping/ProjectionEngine.h rename to src/CosseratPlugin/mapping/ProjectionEngine.h index ece911e6..52f82d96 100644 --- a/src/mapping/ProjectionEngine.h +++ b/src/CosseratPlugin/mapping/ProjectionEngine.h @@ -19,16 +19,20 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#ifndef SOFA_COMPONENT_CONSTRAINTSET_ProjectionEngine_H -#define SOFA_COMPONENT_CONSTRAINTSET_ProjectionEngine_H +#ifndef COSSERATPLUGIN_ProjectionEngine_H +#define COSSERATPLUGIN_ProjectionEngine_H + +#include #include #include -#include #include #include #include +#include + + namespace sofa { namespace component @@ -84,7 +88,7 @@ class ProjectionEngine : public core::DataEngine void init() override; void reinit() override; - void handleEvent(core::objectmodel::Event *ev); + void handleEvent(core::objectmodel::Event *ev) override; void computeProximity(); @@ -120,7 +124,7 @@ class ProjectionEngine : public core::DataEngine }; -//#if !defined(SOFA_COMPONENT_CONSTRAINTSET_ProjectionEngine_CPP) +//#if !defined(SOFA_COSSERATPLUGIN_CPP_ProjectionEngine) //extern template class SOFA_CONSTRAINT_API ProjectionEngine< sofa::defaulttype::Vec3Types >; //#endif @@ -131,4 +135,4 @@ class ProjectionEngine : public core::DataEngine } // namespace sofa -#endif // SOFA_COMPONENT_CONSTRAINTSET_ProjectionEngine_H +#endif // COSSERATPLUGIN_ProjectionEngine_H diff --git a/src/mapping/ProjectionEngine.inl b/src/CosseratPlugin/mapping/ProjectionEngine.inl similarity index 97% rename from src/mapping/ProjectionEngine.inl rename to src/CosseratPlugin/mapping/ProjectionEngine.inl index 27c7b8b9..b9735d8c 100644 --- a/src/mapping/ProjectionEngine.inl +++ b/src/CosseratPlugin/mapping/ProjectionEngine.inl @@ -19,14 +19,17 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#ifndef SOFA_COMPONENT_CONSTRAINTSET_ProjectionEngine_INL -#define SOFA_COMPONENT_CONSTRAINTSET_ProjectionEngine_INL +#ifndef COSSERATPLUGIN_ProjectionEngine_INL +#define COSSERATPLUGIN_ProjectionEngine_INL #include "ProjectionEngine.h" + #include #include #include #include + + namespace sofa { @@ -229,7 +232,7 @@ void ProjectionEngine::draw(const core::visual::VisualParams* vparams // else color = sofa::type::RGBAColor::magenta(); - std::vector vertices; + std::vector vertices; // vertices.push_back(DataTypes::getCPos((this->mstate1->read(core::ConstVecCoordId::position())->getValue())[d_m1.getValue()])); // vparams->drawTool()->drawPoints(vertices, 10, color); @@ -258,7 +261,7 @@ void ProjectionEngine::drawLinesBetweenPoints(const core::visual::Vis const VecCoord & positions = d_dest.getValue(); // this->mstate2->read(core::ConstVecCoordId::position())->getValue(); sofa::type::RGBAColor color; color = sofa::type::RGBAColor::magenta(); - std::vector vertices; + std::vector vertices; for (unsigned int i=0; i #include #include -namespace sofa +namespace sofa::component::mapping { -namespace component -{ - -namespace mapping -{ using namespace sofa::defaulttype; // Register in the Factory @@ -40,11 +35,6 @@ int RigidDistanceMappingClass = core::RegisterObject("Set the positions and velo .add< RigidDistanceMapping< sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >() ; -template class SOFA_COSSERAT_MAPPING_API RigidDistanceMapping< sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; - - -} // namespace mapping - -} // namespace component +template class SOFA_COSSERATPLUGIN_API RigidDistanceMapping< sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; } // namespace sofa. diff --git a/src/mapping/RigidDistanceMapping.h b/src/CosseratPlugin/mapping/RigidDistanceMapping.h similarity index 92% rename from src/mapping/RigidDistanceMapping.h rename to src/CosseratPlugin/mapping/RigidDistanceMapping.h index 915cc4fa..8edeb4d1 100644 --- a/src/mapping/RigidDistanceMapping.h +++ b/src/CosseratPlugin/mapping/RigidDistanceMapping.h @@ -19,15 +19,16 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#pragma once //SOFA_COMPONENT_RIGID_RIGID_MAPPING_CPP +#pragma once +#include + +#include -#include "../initCosserat.h" #include #include #include #include #include -#include "BaseCosserat.h" #include namespace sofa::component::mapping @@ -36,7 +37,7 @@ using sofa::defaulttype::SolidTypes ; using sofa::core::objectmodel::BaseContext ; using sofa::type::Matrix3; using sofa::type::Matrix4; -using type::Vector3; +using type::Vec3; using type::Vec6; using std::get; @@ -96,14 +97,14 @@ class RigidDistanceMapping : public core::Multi2Mapping typedef typename SolidTypes< Real>::SpatialVector SpatialVector ; protected: - Data > d_index1 ; - Data > d_index2 ; - Data d_max ; - Data d_min ; - Data d_radius ; - Data d_color; - Data > d_index; - Data d_debug ; + Data > d_index1 ; + Data > d_index2 ; + Data d_max ; + Data d_min ; + Data d_radius ; + Data d_color; + Data > d_index; + Data d_debug ; core::State* m_toModel; diff --git a/src/mapping/RigidDistanceMapping.inl b/src/CosseratPlugin/mapping/RigidDistanceMapping.inl similarity index 97% rename from src/mapping/RigidDistanceMapping.inl rename to src/CosseratPlugin/mapping/RigidDistanceMapping.inl index 02eee4c6..c2f27155 100644 --- a/src/mapping/RigidDistanceMapping.inl +++ b/src/CosseratPlugin/mapping/RigidDistanceMapping.inl @@ -20,19 +20,18 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once +#include "RigidDistanceMapping.h" #include #include #include -#include #include #include #include #include +#include -#include "sofa/type/Quat.h" -#include "BaseCosserat.inl" -#include "RigidDistanceMapping.h" +#include namespace sofa::component::mapping @@ -70,8 +69,8 @@ void RigidDistanceMapping::init() return; } - const type::vector &m1Indices = d_index1.getValue(); - const type::vector &m2Indices = d_index2.getValue(); + const type::vector &m1Indices = d_index1.getValue(); + const type::vector &m2Indices = d_index2.getValue(); m_minInd = std::min(m1Indices.size(), m2Indices.size()); if (m_minInd == 0) { @@ -105,7 +104,7 @@ void RigidDistanceMapping::apply( for (sofa::Index pid=0; pid::applyJT( for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) { typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); +// typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); typename In1MatrixDeriv::RowIterator o1 = out1.writeLine(rowIt.index()); // we store the constraint number diff --git a/src/config.h b/src/config.h deleted file mode 100644 index 93fb2d01..00000000 --- a/src/config.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include - -// Used to disable warnings on external project headers (MSVC only since gcc and clang already do it) -#ifdef _MSC_VER -#define DISABLE_ALL_WARNINGS_BEGIN __pragma(warning(push, 0)) -#define DISABLE_ALL_WARNINGS_END __pragma(warning(pop)) -#else -#define DISABLE_ALL_WARNINGS_BEGIN -#define DISABLE_ALL_WARNINGS_END -#endif \ No newline at end of file diff --git a/src/config.h.in b/src/config.h.in deleted file mode 100644 index 93fb2d01..00000000 --- a/src/config.h.in +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include - -// Used to disable warnings on external project headers (MSVC only since gcc and clang already do it) -#ifdef _MSC_VER -#define DISABLE_ALL_WARNINGS_BEGIN __pragma(warning(push, 0)) -#define DISABLE_ALL_WARNINGS_END __pragma(warning(pop)) -#else -#define DISABLE_ALL_WARNINGS_BEGIN -#define DISABLE_ALL_WARNINGS_END -#endif \ No newline at end of file diff --git a/src/constraint/CosseratNeedleSlidingConstraint.h b/src/constraint/CosseratNeedleSlidingConstraint.h deleted file mode 100644 index 8db6522a..00000000 --- a/src/constraint/CosseratNeedleSlidingConstraint.h +++ /dev/null @@ -1,152 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2020 INRIA, USTL, UJF, CNRS, MGH * -* * -* This library is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. * -* * -* This library is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this library; if not, write to the Free Software Foundation, * -* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * -******************************************************************************* -* Plugin Cosserat v1.0 * -* * -* This plugin is also distributed under the GNU LGPL (Lesser General * -* Public License) license with the same conditions than SOFA. * -* * -* Contributors: Defrost team (INRIA, University of Lille, CNRS, * -* Ecole Centrale de Lille) * -* * -* Contact information: https://project.inria.fr/softrobot/contact/ * -* adagolodjo@protonmail.com * -******************************************************************************/ - -#pragma once - -#include -#include - -#include -#include -#include "QPSlidingConstraint.h" - -namespace sofa::component::constraintset -{ - -using sofa::core::behavior::SoftRobotsConstraint ; -using sofa::core::visual::VisualParams ; -using sofa::core::objectmodel::Data ; -using sofa::defaulttype::Vec3dTypes ; -using sofa::defaulttype::Vec3fTypes ; -using sofa::linearalgebra::BaseVector ; -using sofa::core::ConstraintParams ; -using sofa::helper::ReadAccessor ; -using sofa::core::VecCoordId ; - -/** - * This class contains common implementation of cable constraints -*/ -template< class DataTypes > -class CosseratNeedleSlidingConstraint : public CableModel -{ -public: - - SOFA_CLASS(SOFA_TEMPLATE(CosseratNeedleSlidingConstraint,DataTypes), SOFA_TEMPLATE(CableModel,DataTypes)); - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename Coord::value_type Real; - typedef typename core::behavior::MechanicalState MechanicalState; - - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef type::vector SetIndexArray; - - -public: - CosseratNeedleSlidingConstraint(MechanicalState* object = nullptr); - - ~CosseratNeedleSlidingConstraint() override; - - ////////////////////////// Inherited from BaseObject //////////////////// - void init() override; - void reinit() override; - void draw(const VisualParams* vparams) override; - ///////////////////////////////////////////////////////////////////////// - - ////////////////////////// Inherited from Actuator ////////////////////// - void buildConstraintMatrix(const ConstraintParams* cParams, - DataMatrixDeriv &cMatrix, - unsigned int &cIndex, - const DataVecCoord &x) override; - - void getConstraintViolation(const ConstraintParams* cParams, - BaseVector *resV, - const BaseVector *Jdx) override; - void getConstraintResolution(const ConstraintParams*, - std::vector& resTab, - unsigned int& offset) override; - - ///////////////////////////////////////////////////////////////////////// - - ////////////////////////// Inherited from BaseConstraint //////////////// - // void storeLambda(const ConstraintParams* cParams, - // core::MultiVecDerivId res, - // const BaseVector* lambda) override; - ///////////////////////////////////////////////////////////////////////// - -protected: - //Input data - Data > d_value; - Data d_valueIndex; - Data d_valueType; - // displacement = the constraint will impose the displacement provided in data d_inputValue[d_iputIndex] - // force = the constraint will impose the force provided in data d_inputValue[d_iputIndex] - - -protected: - - ////////////////////////// Inherited attributes //////////////////////////// - /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html - /// Bring m_state in the current lookup context. - /// otherwise any access to the base::attribute would require - /// using the "this->" approach. - using SoftRobotsConstraint::m_state ; - ////////////////////////// Inherited attributes //////////////////////////// - /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html - /// Bring inherited attributes and function in the current lookup context. - /// otherwise any access to the base::attribute would require - /// the "this->" approach. - using CableModel::d_maxDispVariation ; - using CableModel::d_maxPositiveDisplacement ; - using CableModel::d_maxNegativeDisplacement ; - using CableModel::d_maxForce ; - using CableModel::d_minForce ; - using CableModel::d_displacement ; - using CableModel::d_componentState ; - //////////////////////////////////////////////////////////////////////////// - /// \brief internalInit - using SoftRobotsConstraint::m_nbLines ; - using SoftRobotsConstraint::m_constraintId ; - - void internalInit(); -}; - -// Declares template as extern to avoid the code generation of the template for -// each compilation unit. see: http://www.stroustrup.com/C++11FAQ.html#extern-templates -//extern template class CosseratNeedleSlidingConstraint; - -} // namespace sofa - - diff --git a/src/constraint/CosseratNeedleSlidingConstraint.inl b/src/constraint/CosseratNeedleSlidingConstraint.inl deleted file mode 100644 index a8fc976b..00000000 --- a/src/constraint/CosseratNeedleSlidingConstraint.inl +++ /dev/null @@ -1,181 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * -* * -* This library is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. * -* * -* This library is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this library; if not, write to the Free Software Foundation, * -* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * -******************************************************************************* -* Plugin Cosserat v1.0 * -* * -* This plugin is also distributed under the GNU LGPL (Lesser General * -* Public License) license with the same conditions than SOFA. * -* * -* Contributors: Defrost team (INRIA, University of Lille, CNRS, * -* Ecole Centrale de Lille) * -* * -* Contact information: https://project.inria.fr/softrobot/contact/ * -* adagolodjo@protonmail.com * -******************************************************************************/ - -#pragma once - -#include -#include -#include -#include "CosseratNeedleSlidingConstraint.h" - -namespace sofa::component::constraintset -{ - -using sofa::core::objectmodel::ComponentState; -using sofa::helper::WriteAccessor; - -using sofa::core::objectmodel::ComponentState; -using sofa::core::visual::VisualParams; -using sofa::linearalgebra::BaseVector; -using sofa::helper::ReadAccessor; -using sofa::type::Vec4f; -using sofa::type::Vector3; -using sofa::type::vector; -using sofa::helper::OptionsGroup; -using sofa::component::constraint::lagrangian::model::BilateralConstraintResolution; - -template -CosseratNeedleSlidingConstraint::CosseratNeedleSlidingConstraint(MechanicalState* object) - : Inherit1(object) - - , d_value(initData(&d_value, "value", - "Displacement or force to impose.\n")) - - , d_valueIndex(initData(&d_valueIndex, (unsigned int) 0, "valueIndex", - "Index of the value (in InputValue vector) that we want to impose \n" - "If unspecified the default value is {0}")) - - , d_valueType(initData(&d_valueType, OptionsGroup(2,"displacement","force"), "valueType", - "displacement = the contstraint will impose the displacement provided in data value[valueIndex] \n" - "force = the contstraint will impose the force provided in data value[valueIndex] \n" - "If unspecified, the default value is displacement")) -{ -} - -template -CosseratNeedleSlidingConstraint::~CosseratNeedleSlidingConstraint() -{ -} - -template -void CosseratNeedleSlidingConstraint::init() -{ - Inherit1::init(); - - internalInit(); -} - -template -void CosseratNeedleSlidingConstraint::reinit() -{ - internalInit(); -} - -template -void CosseratNeedleSlidingConstraint::internalInit() -{ - if(d_value.getValue().size()==0) - { - WriteAccessor>> value = d_value; - value.resize(1,0.); - } - - // check for errors in the initialization - if(d_value.getValue().size() -void CosseratNeedleSlidingConstraint::buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &cMatrix, unsigned int &cIndex, const DataVecCoord &x) -{ - if(d_componentState.getValue() != ComponentState::Valid) - return ; - - SOFA_UNUSED(cParams); - MatrixDeriv& matrix = *cMatrix.beginEdit(); - VecCoord positions = x.getValue(); - m_constraintId= cIndex; - - for (unsigned int i=0; i -void CosseratNeedleSlidingConstraint::getConstraintViolation(const ConstraintParams* cParams, - BaseVector *resV, - const BaseVector *Jdx) -{ - if(d_componentState.getValue() != ComponentState::Valid) - return ; - - SOFA_UNUSED(cParams); - ReadAccessor> positions = m_state->readPositions(); - - if(Jdx->size()==0){ - for (unsigned int i = 0; i < positions.size(); i++){ - Real dfree1 = positions[i][1]; - Real dfree2 = positions[i][2]; - - resV->set(m_constraintId + 2*i , dfree1); - resV->set(m_constraintId + 2*i +1, dfree2); - } - }else{ - for (unsigned int i = 0; i < positions.size(); i++){ - Real dfree1 = Jdx->element(2*i) + positions[i][1]; - Real dfree2 = Jdx->element(2*i+1) + positions[i][2]; - resV->set(m_constraintId + 2*i , dfree1); - resV->set(m_constraintId + 2*i +1, dfree2); - } - } -} - -template -void CosseratNeedleSlidingConstraint::getConstraintResolution(const ConstraintParams*, - std::vector& resTab, - unsigned int& offset) -{ - ReadAccessor> positions = m_state->readPositions(); - double imposedValue= 1.0; - for (size_t i = 0; i < positions.size(); i++){ - - resTab[offset++] = new BilateralConstraintResolution(); - resTab[offset++] = new BilateralConstraintResolution(); - } -} - -template -void CosseratNeedleSlidingConstraint::draw(const VisualParams* vparams) -{ - if(d_componentState.getValue() != ComponentState::Valid) - return ; -} -} // namespace sofa::component::constraintset - diff --git a/src/constraint/CosseratUnilateralInteractionConstraint.inl b/src/constraint/CosseratUnilateralInteractionConstraint.inl deleted file mode 100644 index d101ceb0..00000000 --- a/src/constraint/CosseratUnilateralInteractionConstraint.inl +++ /dev/null @@ -1,238 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture * -* (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * -* * -* This library is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. * -* * -* This library is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this library; if not, write to the Free Software Foundation, * -* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * -******************************************************************************* -* Plugin Cosserat v1.0 * -* * -* This plugin is also distributed under the GNU LGPL (Lesser General * -* Public License) license with the same conditions than SOFA. * -* * -* Contributors: Defrost team (INRIA, University of Lille, CNRS, * -* Ecole Centrale de Lille) * -* * -* Contact information: https://project.inria.fr/softrobot/contact/ * -* adagolodjo_AT_protonmail.com * -******************************************************************************/ - -#pragma once - -#include -#include -#include - -#include "CosseratUnilateralInteractionConstraint.h" - -namespace sofa::component::constraintset -{ - - using sofa::core::objectmodel::ComponentState; - using sofa::helper::WriteAccessor; - - using sofa::core::objectmodel::ComponentState; - using sofa::core::visual::VisualParams; - using sofa::linearalgebra::BaseVector; - using sofa::helper::ReadAccessor; - using sofa::type::Vec4f; - using sofa::type::Vector3; - using sofa::type::vector; - using sofa::helper::OptionsGroup; - - template - CosseratUnilateralInteractionConstraint::CosseratUnilateralInteractionConstraint(MechanicalState* object) - : Inherit1(object) - - , d_value(initData(&d_value, "value","Displacement or force to impose.\n")) - , d_dampingCoefficient(initData(&d_dampingCoefficient, Real(0.1), "dampingCoefficient", - "The dumping coefficient to slow down the insertion speed.\n")) - , d_valueIndex(initData(&d_valueIndex, (unsigned int) 0, "valueIndex", - "Index of the value (in InputValue vector) that we want to impose \n" - "If unspecified the default value is {0}")) - , d_vectorOfIndices(initData(&d_vectorOfIndices, "vectorOfIndices", - "vector of indices on witch we have to apply the constraint.\n")) - , d_entryPoint(initData(&d_entryPoint, type::Vector3(24.95, 0., 0), "entryPoint", - "The predefined entry point, this point can also be determined automatically" - "but not implemented here.\n")) - , d_direction(initData(&d_direction, "direction", - "direction of insertion, if this is not given " - "the insertion is direct along X.\n")) - {} - - template - CosseratUnilateralInteractionConstraint::~CosseratUnilateralInteractionConstraint() - = default; - - - template - void CosseratUnilateralInteractionConstraint::init() - { - Inherit1::init(); - internalInit(); - UpdateList(); - } - - - template - void CosseratUnilateralInteractionConstraint::reinit() - { - internalInit(); - UpdateList(); - } - - template - type::Vector3 CosseratUnilateralInteractionConstraint::findEntryPoint() - { - /// @todo:1- find the entry automatically, this is need in the case of needle insertion - /// can also be necessary when the volume is deforming - /// @todo:2- Build unitest function - - return type::Vector3(0.0f,0.0f,0.0f); - } - - template - bool CosseratUnilateralInteractionConstraint::UpdateList() - { - /// @todo:1- Update the list of points beyond the entry point - /// @todo:2- Build unitest function - /// @todo:3- use the insertion direction - - ReadAccessor> positions = m_state->readPositions(); - auto entryPoint = d_entryPoint.getValue(); - WriteAccessor>> indices = d_vectorOfIndices; - indices.clear(); - - unsigned int index=0; - for(auto position : positions){ - if (position[0] >= entryPoint[0] ){ - indices.push_back(index); - } - index++; - } - return true; - } - - - template - void CosseratUnilateralInteractionConstraint::buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &cMatrix, unsigned int &cIndex, const DataVecCoord &x) - { - if(d_componentState.getValue() != ComponentState::Valid) - return ; - UpdateList(); - - SOFA_UNUSED(cParams); - MatrixDeriv& matrix = *cMatrix.beginEdit(); - VecCoord positions = x.getValue(); - auto indices = d_vectorOfIndices.getValue(); - auto direction = d_direction.getValue(); - - m_constraintId = cIndex; - - //printf("================================= \n"); - if ((!direction.empty()) && (direction.size() == positions.size())){ - type::Vector3 vx, vy, vz; - for (auto index : indices) - { - type::Quat q =direction[index]; q.normalize(); - vx = q.rotate(Vector3(1.,0.,0)); vx.normalize(); - vy = q.rotate(Vector3(0.,1.,0.)); vy.normalize(); - vz = q.rotate(Vector3(0.,0.,1.)); vz.normalize(); - - MatrixDerivRowIterator c_it_x = matrix.writeLine(cIndex++); - c_it_x.addCol(index, vx); - MatrixDerivRowIterator c_it_y = matrix.writeLine(cIndex++); - c_it_y.addCol(index, vy); - MatrixDerivRowIterator c_it_z = matrix.writeLine(cIndex++); - c_it_z.addCol(index, vz); - // std::cout <<"index : "<< index << std::endl; - // std::cout <<"vx : "<< vx << std::endl; - // std::cout <<"vy : "<< vy << std::endl; - // std::cout <<"vz : "<< vz << std::endl; - - } - } else{ - for (auto index : indices) - { - MatrixDerivRowIterator c_it_x = matrix.writeLine(cIndex++); - c_it_x.addCol(index, Coord(1.,0,0)); - MatrixDerivRowIterator c_it_y = matrix.writeLine(cIndex++); - c_it_y.addCol(index, Coord(0,1.,0)); - MatrixDerivRowIterator c_it_z = matrix.writeLine(cIndex++); - c_it_z.addCol(index, Coord(0,0,1.)); - } - } - //printf("================================= \n"); - cMatrix.endEdit(); - m_nbLines = cIndex - m_constraintId; - } - - - template - void CosseratUnilateralInteractionConstraint::getConstraintViolation(const ConstraintParams* cParams, - BaseVector *resV, - const BaseVector *Jdx) - { - if(d_componentState.getValue() != ComponentState::Valid) - return ; - SOFA_UNUSED(cParams); - ReadAccessor> positions = m_state->readPositions(); - ReadAccessor> freePositions = m_state->read(core::ConstVecCoordId::freePosition()); - ReadAccessor> velocity = m_state->read(core::ConstVecDerivId::velocity()); - auto indices = d_vectorOfIndices.getValue(); - - unsigned int iter = 0; - - for (auto index : indices){ - //@todo this need to be recompute, use dfree = newPos-oldPos or posFree - pos or velocity - Coord dx = freePositions[index] - positions[index]; //This is also equal Jdx->element(3*iter +i); i=0,1,2 - //std::cout<<" ===> dx :"<< dx << std::endl; - // std::cout<<" ===> dv :"<< velocity[index] << std::endl; - Real dfree1 = dx[0]; - Real dfree2 = dx[1]; - Real dfree3 = dx[2]; - - resV->set(m_constraintId + 3*iter+0, dfree1); - resV->set(m_constraintId + 3*iter+1, dfree2); - resV->set(m_constraintId + 3*iter+2, dfree2); - iter++; - } - } - - - template - void CosseratUnilateralInteractionConstraint::getConstraintResolution(const ConstraintParams*, - std::vector& resTab, - unsigned int& offset) - { - double dampingCoefficient = d_dampingCoefficient.getValue(); - for (unsigned int index : d_vectorOfIndices.getValue()){ - resTab[offset+0] = new MyUnilateralConstraintResolutionWithFriction(dampingCoefficient); - resTab[offset+1] = new MyUnilateralConstraintResolutionWithFriction(dampingCoefficient); - resTab[offset+2] = new MyUnilateralConstraintResolutionWithFriction(dampingCoefficient); - offset += 3; - } - } - - - template - void CosseratUnilateralInteractionConstraint::draw(const VisualParams* /*vparams*/) - { - if(d_componentState.getValue() != ComponentState::Valid) - return ; - - } - -} // namespace sofa - diff --git a/src/constraint/DrawTrianglesComponent.inl b/src/constraint/DrawTrianglesComponent.inl deleted file mode 100644 index 30c81b0d..00000000 --- a/src/constraint/DrawTrianglesComponent.inl +++ /dev/null @@ -1,263 +0,0 @@ -#ifndef DrawTrianglesComponent_INL -#define DrawTrianglesComponent_INL - -#include "DrawTrianglesComponent.h" -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sofa { - -namespace component { - -namespace controller { - -DrawTrianglesComponent::DrawTrianglesComponent() - : d_transparency( initData(&d_transparency, double(0.1),"transparency", "Scene scale")) - , d_vecTriangles( initData(&d_vecTriangles, "triangles", "tetra index elements")) - , d_vecTetra( initData(&d_vecTetra, "tetrahedra", "tetra index elements")) - , d_maxStress( initData(&d_maxStress,(double)5e3, "maxStress", "max Stress")) - , d_minStress( initData(&d_minStress, (double)0,"minStress", "min Stress")) - , d_maxVonMisesPerNode( initData(&d_maxVonMisesPerNode, "maxVonMisesPerNode", "max Von Mises Per Node")) - , d_draw( initData(&d_draw, true,"draw", "draw triangles")) -{ - this->f_listening.setValue(true); -} - -void DrawTrianglesComponent::init() -{ - this->getContext()->get(m_state); - this->getContext()->get(m_tetraForceField); - if (m_state == NULL) { - serr << "Error cannot find the mstate" << sendl; - return; - } - if (m_tetraForceField== NULL) { - serr << "Error cannot find the m_tetrahedron" << sendl; - return; - } - m_VonMisesColorMap.setColorScheme("Blue to Red"); - m_VonMisesColorMap.reinit(); - m_tetraForceField->updateVonMisesStress = true; - - m_minVM = d_minStress.getValue(); - m_maxVM = d_maxStress.getValue(); - - helper::ReadAccessor > > vMN = m_tetraForceField->_vonMisesPerNode; - helper::vector & vonMises = *d_maxVonMisesPerNode.beginEdit(); - vonMises.resize(vMN.size()); - for(size_t i =0; i < vMN.size(); i++){ - vonMises[i] = vMN[i]; - } - d_maxVonMisesPerNode.endEdit(); -} - -void DrawTrianglesComponent::handleEvent(sofa::core::objectmodel::Event* event) -{ - helper::ReadAccessor > > vMN = m_tetraForceField->_vonMisesPerNode; - if (dynamic_cast(event)) { - helper::vector & vonMises = *d_maxVonMisesPerNode.beginEdit(); - for (size_t index = 0; index < vMN.size(); index++) { - m_minVM = (vMN[index] < m_minVM) ? vMN[index] : m_minVM; - m_maxVM = (vMN[index] > m_maxVM) ? vMN[index] : m_maxVM; - if (vonMises[index] < vMN[index]) { - vonMises[index] = vMN[index]; - } - } - d_maxVonMisesPerNode.endEdit(); - } - else if (sofa::core::objectmodel::KeypressedEvent* ev = dynamic_cast(event)) { - if ((ev->getKey() == 'l') || (ev->getKey() == 'L')) - { - helper::vector & vonMises = *d_maxVonMisesPerNode.beginEdit(); - m_minVM = d_minStress.getValue() ; - m_maxVM = d_maxStress.getValue(); - - for(size_t i =0; i < vMN.size(); i++){ - vonMises[i] = 0.0; - } - d_maxVonMisesPerNode.endEdit(); - } - } -} - - - -void DrawTrianglesComponent::draw(const core::visual::VisualParams* vparams) -{ - if(d_draw.getValue()) - if (!this->m_state) return; - m_tetraForceField->updateVonMisesStress = true; - - const VecCoord& x = this->m_state->read(core::ConstVecCoordId::position())->getValue(); - helper::ReadAccessor > > vM = m_tetraForceField->_vonMisesPerElement; - helper::ReadAccessor > > vMN = m_tetraForceField->_vonMisesPerNode; - //helper::vector & vMN = *d_maxVonMisesPerNode.beginEdit(); - - helper::ColorMap::evaluator evalColor = m_VonMisesColorMap.getEvaluator(d_minStress.getValue(),d_maxStress.getValue()); - - glDisable(GL_LIGHTING); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - - // glColor4f(1,1,1,1); - - // if (m_textureImage!=TEXTURE_UNASSIGNED) { - // glEnable(GL_TEXTURE_2D); - // glBindTexture(GL_TEXTURE_2D, m_textureImage); - // glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); - // } - if (vparams->displayFlags().getShowWireFrame()) { - glBegin(GL_LINES); - - int i = 0; - std::vector nodeColors(4); - for(auto it = d_vecTriangles.getValue().begin(); it !=d_vecTriangles.getValue().end() ; ++it, ++i) - { - Index a = (*it)[0]; - Index b = (*it)[1]; - Index c = (*it)[2]; - nodeColors[0] = evalColor(vMN[a]); - nodeColors[0][3] = d_transparency.getValue(); - nodeColors[1] = evalColor(vMN[b]); - nodeColors[1][3] = d_transparency.getValue(); - nodeColors[2] = evalColor(vMN[c]); - nodeColors[2][3] = d_transparency.getValue(); - - float* ca = &nodeColors[0][0]; - float* cb = &nodeColors[1][0]; - float* cc = &nodeColors[2][0]; - - Coord pa = x[a]; - Coord pb = x[b]; - Coord pc = x[c]; - - glColor4fv(ca); helper::gl::glVertexT(pa); - glColor4fv(cb); helper::gl::glVertexT(pb); - glColor4fv(cc); helper::gl::glVertexT(pc); - } - glEnd(); - - }else { - - glBegin(GL_TRIANGLES); - - - // for (size_t nd = 0; nd < x.size(); nd++) { - // defaulttype::Vec4f col = evalColor(vMN[nd]); - - // glColor4f(col[0],col[1],col[2],1.0); - // glVertex3d(x[nd][0],x[nd][1],x[nd][2]); - // } - - //vparams->drawTool()->drawPoints(pts, 10, nodeColors); - - int i = 0; - std::vector nodeColors(4); - - - i=0; - for(auto it = d_vecTetra.getValue().begin(); it !=d_vecTetra.getValue().end() ; ++it, ++i) - { - Index a = (*it)[0]; - Index b = (*it)[1]; - Index c = (*it)[2]; - Index d = (*it)[3]; - nodeColors[0] = evalColor(vMN[a]);nodeColors[0][3] = d_transparency.getValue(); - nodeColors[1] = evalColor(vMN[b]);nodeColors[1][3] = d_transparency.getValue(); - nodeColors[2] = evalColor(vMN[c]);nodeColors[2][3] = d_transparency.getValue(); - nodeColors[3] = evalColor(vMN[d]);nodeColors[3][3] = d_transparency.getValue(); - float* ca = &nodeColors[0][0]; - float* cb = &nodeColors[1][0]; - float* cc = &nodeColors[2][0]; - float* cd = &nodeColors[3][0]; - - Coord center = (x[a]+x[b]+x[c]+x[d])*0.125; - Coord pa = (x[a]+center)*(Real)0.666667; - Coord pb = (x[b]+center)*(Real)0.666667; - Coord pc = (x[c]+center)*(Real)0.666667; - Coord pd = (x[d]+center)*(Real)0.666667; - - glColor4fv(ca); helper::gl::glVertexT(pa); - glColor4fv(cb); helper::gl::glVertexT(pb); - glColor4fv(cc); helper::gl::glVertexT(pc); - - glColor4fv(ca); helper::gl::glVertexT(pa); - glColor4fv(cb); helper::gl::glVertexT(pb); - glColor4fv(cd); helper::gl::glVertexT(pd); - - glColor4fv(ca); helper::gl::glVertexT(pa); - glColor4fv(cc); helper::gl::glVertexT(pc); - glColor4fv(cd); helper::gl::glVertexT(pd); - - glColor4fv(cc); helper::gl::glVertexT(pc); - glColor4fv(cb); helper::gl::glVertexT(pb); - glColor4fv(cd); helper::gl::glVertexT(pd); - - } - - - for(auto it = d_vecTriangles.getValue().begin(); it !=d_vecTriangles.getValue().end() ; ++it, ++i) - { - Index a = (*it)[0]; - Index b = (*it)[1]; - Index c = (*it)[2]; - nodeColors[0] = evalColor(vMN[a]);nodeColors[0][3] = d_transparency.getValue(); - nodeColors[1] = evalColor(vMN[b]);nodeColors[1][3] = d_transparency.getValue(); - nodeColors[2] = evalColor(vMN[c]);nodeColors[2][3] = d_transparency.getValue(); - float* ca = &nodeColors[0][0]; - float* cb = &nodeColors[1][0]; - float* cc = &nodeColors[2][0]; - - Coord pa = x[a]; - Coord pb = x[b]; - Coord pc = x[c]; - - glColor4fv(ca); helper::gl::glVertexT(pa); - glColor4fv(cb); helper::gl::glVertexT(pb); - glColor4fv(cc); helper::gl::glVertexT(pc); - } - - - - - - glEnd(); - } - -} - - - -} //end namespace controller -} //end namespace component -} //end namespace sofa - -#endif diff --git a/src/mapping/DifferenceMultiMapping.inl b/src/mapping/DifferenceMultiMapping.inl deleted file mode 100644 index 8962fb35..00000000 --- a/src/mapping/DifferenceMultiMapping.inl +++ /dev/null @@ -1,794 +0,0 @@ -/****************************************************************************** -* SOFA, Simulation Open-Framework Architecture, development version * -* (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH * -* * -* This program is free software; you can redistribute it and/or modify it * -* under the terms of the GNU Lesser General Public License as published by * -* the Free Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. * -* * -* This program is distributed in the hope that it will be useful, but WITHOUT * -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * -* for more details. * -* * -* You should have received a copy of the GNU Lesser General Public License * -* along with this program. If not, see . * -******************************************************************************* -* Authors: The SOFA Team and external contributors (see Authors.txt) * -* * -* Contact information: contact@sofa-framework.org * -******************************************************************************/ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "DifferenceMultiMapping.h" - -namespace sofa::component::mapping -{ -using sofa::core::objectmodel::BaseContext ; -using sofa::helper::AdvancedTimer; -using sofa::helper::WriteAccessor; -using sofa::type::RGBAColor ; - -template -DifferenceMultiMapping::DifferenceMultiMapping() - : d_direction(initData(&d_direction, "direction","The list of directions of fix points .\n")) - , d_indices(initData(&d_indices, "indices","Indices of fixe points of the cable")) - , d_radius(initData(&d_radius, 2.0, "radius", "The size of the cable")) - , d_color(initData(&d_color,type::Vec4f(1,0,0,1), "color","The color of the cable")) - , d_drawArrows(initData(&d_drawArrows,false, "drawArrows","The color of the cable")) - , d_lastPointIsFixed(initData(&d_lastPointIsFixed,true, "lastPointIsFixed","This select the last point as fixed of not," - "When the model is used to model a cable we need to fix the " - "last point. Otherwise, when it's used as needle no need to fix" - "one.")) - , m_fromModel1(NULL) - , m_fromModel2(NULL) - , m_toModel(NULL) -{ - -} - - -template -void DifferenceMultiMapping::initiateTopologies() -{ - m_toModel = this->getToModels()[0]; - if (! m_toModel) { - std::cout << " No output mechanical state found. Consider setting the " - << this->toModels.getName() << " attribute."<< std::endl; - return; - } - - if (!d_direction.isSet()) - msg_warning()<<"No direction nor indices is given."; -} - - -// _________________________________________________________________________________________ - -template -void DifferenceMultiMapping::init() -{ - if(this->getFromModels1().empty()) - { - msg_error() << "Error while initializing ; input getFromModels1 not found" ; - return; - } - - if(this->getFromModels2().empty()) - { - msg_error() << "Error while initializing ; output getFromModels2 not found" ; - return; - } - - if(this->getToModels().empty()) - { - msg_error() << "Error while initializing ; output Model not found" ; - //return; - } - m_fromModel1 = this->getFromModels1()[0]; - m_fromModel2 = this->getFromModels2()[0]; - m_toModel = this->getToModels()[0]; - - m_toModel = m_fromModel1; - - initiateTopologies(); -} - - -template -void DifferenceMultiMapping::bwdInit() -{ - -} - -template -void DifferenceMultiMapping::reinit() -{ - -} - -template -void DifferenceMultiMapping::reset() -{ - reinit(); -} - - -template -void DifferenceMultiMapping::computeProximity(const In1VecCoord &x1, const In2VecCoord &x2){ - - In1VecCoord from = x1; - In2VecCoord dst = x2; - m_constraints.clear(); - - size_t szFrom = from.size(); - size_t szDst = dst.size(); - type::vector direction = d_direction.getValue(); - - ///get the last rigid direction, the main goal is to use it for the - /// 3D bilateral constraint i.e the fix point of the cable in the robot structure - //Rigid direction = d_direction.getValue()[szDst-1]; - - //For each point in the FEM find the closest edge of the cable - for (size_t i = 0 ; i < szFrom; i++) { - Coord2 P = from[i]; - Constraint constraint; - - // find the min distance between a from mstate point and it's projection on each edge of the cable (destination mstate) - Real min_dist = std::numeric_limits::max(); - for (size_t j = 0; j < szDst-1; j++) { - Coord1 Q1 = dst[j]; - Coord1 Q2 = dst[j+1]; - //the axis - Coord1 dirAxe = Q2 -Q1; - Real length = dirAxe.norm(); - Real fact_v = dot(P-Q1,dirAxe) / dot(dirAxe,dirAxe) ; - - if(std::abs(fact_v) < min_dist){ - //if(fact_v < min_dist){ - min_dist = std::abs(fact_v) ; - - //define the constraint variables - Deriv1 proj; // distVec; - Real alpha; // dist; - - ///To solve the case that the closest node is - /// not the node 0 but the node 1 of the beam - if(fact_v<0.0 && j!=0 && std::abs(fact_v) > 1e-8){ - //if fact_v < 0.0 that means the last beam is the good beam - //printf("if fact_v < 0.0 that means the last beam is the good beam \n"); - Q1 = dst[j-1] ; - dirAxe = dst[j] - Q1; - length = dirAxe.norm(); - fact_v = dot(P-Q1,dirAxe) / dot(dirAxe,dirAxe) ; - dirAxe.normalize(); - alpha = (P-Q1) * dirAxe; - - proj = Q1 + dirAxe * alpha; - //distVec = P - proj; // violation vector - //dist = (P - proj).norm(); // constraint violation - constraint.eid = j-1; - //The direction of the axe or the beam - constraint.dirAxe = dirAxe; - //the node contribution to the constraint which is 1-coeff - alpha = alpha / length; //normalize, ensure that <1.0 - if (alpha < 1e-8)constraint.alpha = 1.0 ; - else constraint.alpha = 1.0 - alpha; - - //The projection on the axe - constraint.proj = proj; - constraint.Q = from[i]; - - ///// - length = (dst[j] - Q1).norm(); - constraint.Q1Q2 = length; - constraint.r2 = fact_v; - - // We move the constraint point onto the projection - Deriv1 t1 = P - proj; // violation vector - constraint.dist = t1.norm(); // constraint violation - t1.normalize(); // direction of the constraint - - //// First method compute normals using projections - // if(t1.norm()<1.0e-1 && dirAxe[2] < 0.99){ - // Vector3 temp = Vector3(dirAxe[0],dirAxe[1],dirAxe[2]+50.0); - // t1 = cross(dirAxe,temp); - // t1.normalize(); - // constraint.t1 = t1; - // } - // if(t1.norm()<1.0e-1){ - // Vector3 temp = Vector3(dirAxe[0],dirAxe[1]+50.0,dirAxe[2]); - // t1 = cross(dirAxe,temp); - // t1.normalize(); - // constraint.t1 = t1; - // } - - // if(t1.norm()<1.0e-1) - // { - - //// Second method compute normals using frames directions - Rigid dir = direction[constraint.eid]; - Vector3 vY = Vector3(0.,1.,0.); - type::Quat ori = dir.getOrientation() ; - vY = ori.rotate(vY); vY.normalize(); - t1 = vY ; - // } - - constraint.t1 = t1; - //tangential 2 - Deriv1 t2 = cross(t1, dirAxe); t2.normalize(); - constraint.t2 = t2; - - - if(i == szFrom-1){ - ///This handle the fix point constraint the last point of - /// of cstr points indeed here we have - /// 3D bilateral constraint and alpha=1.0 - // We use the given direction of fill H - - if (!direction.empty()){ - Rigid _dir = direction[szDst-1]; - type::Quat _ori = _dir.getOrientation() ; - - Vector3 _vY = _ori.rotate(Vector3(0.,1.,0.)); _vY.normalize(); - Vector3 _vZ = _ori.rotate(Vector3(0.,0.,1.)); _vZ.normalize(); - - constraint.t1 = _vY ; - constraint.t2 = _vZ ; - } - constraint.proj = dst[szDst-1]; - constraint.eid = szDst-2; - constraint.alpha = 1.0; - constraint.dist = (dst[szDst-1] - from[szFrom-1]).norm(); - } - }else{ - //compute needs for constraint - dirAxe.normalize(); - alpha = (P-Q1) * dirAxe; - - proj = Q1 + dirAxe * alpha; - //distVec = P - proj; // violation vector - //dist = (P - proj).norm(); // constraint violation - constraint.eid = j; - //The direction of the axe or the beam - constraint.dirAxe = dirAxe; - //the node contribution to the constraint which is 1-coeff - alpha = alpha / length; //normalize, ensure that <1.0 - if (alpha < 1e-8)constraint.alpha = 1.0 ; - else constraint.alpha = 1.0 - alpha; - - //The projection on the axe - constraint.proj = proj; - constraint.Q = from[i]; - - ///// - constraint.Q1Q2 = length; - constraint.r2 = fact_v; - - // We move the constraint point onto the projection - Deriv1 t1 = P - proj; // violation vector - constraint.dist = t1.norm(); // constraint violation - t1.normalize(); // direction of the constraint - - /// If the violation is very small t1 is close to zero - /// - //// First method compute normals using projections - // if(t1.norm()<1.0e-1 && dirAxe[2] < 0.99){ - // Vector3 temp = Vector3(dirAxe[0],dirAxe[1],dirAxe[2]+50.0); - // t1 = cross(dirAxe,temp); - // t1.normalize(); - // constraint.t1 = t1; - // } - // if(t1.norm()<1.0e-1){ - // Vector3 temp = Vector3(dirAxe[0],dirAxe[1]+50.0,dirAxe[2]); - // t1 = cross(dirAxe,temp); - // t1.normalize(); - // constraint.t1 = t1; - // } - - //// Second method compute normals using frames directions - Rigid dir = direction[constraint.eid]; - Vector3 vY = Vector3(0.,1.,0.); - type::Quat ori = dir.getOrientation() ; - vY = ori.rotate(vY); vY.normalize(); - t1 = vY ; - // } - constraint.t1 = t1; - //tangential 2 - Deriv1 t2 = cross(t1, dirAxe); t2.normalize(); - constraint.t2 = t2; - - ///This is need because we are applying the a - /// bilateral constraint on the last node of the mstate - if(i == szFrom-1){ - ///This handle the fix point constraint the last point of - /// of cstr points indeed here we have - /// 3D bilateral constraint and alpha=1.0 - // We use the given direction of fill H - if (!d_direction.getValue().empty()){ - Rigid _dir = direction[szDst-1]; - type::Quat _ori = (direction[szDst-1]).getOrientation() ; - Vector3 _vY = _ori.rotate(Vector3(0.,1.,0.)); - _vY.normalize(); - Vector3 _vZ = _ori.rotate(Vector3(0.,0.,1.)); - _vZ.normalize(); - - constraint.t1 = _vY ; - constraint.t2 = _vZ ; - } - constraint.proj = dst[szDst-1]; - constraint.eid = szDst-2; - constraint.alpha = 1.0; - constraint.dist = (dst[szDst-1] - from[szFrom-1]).norm(); - } - } - } - } - //printf("______________________________________________________________\n"); - //std::cout << "i :" << i << " ; eid:" << constraint.eid << " alpha : " << constraint.alpha << " ; dist :"<< constraint.dist << std::endl; - //std::cout<<" fact_v :"<< constraint.r2 << i << " ; n :"<< constraint.dirAxe << "; t1:" << constraint.t1 << "; t2 :"<< constraint.t2 << std::endl; - //printf("______________________________________________________________\n"); - m_constraints.push_back(constraint); - } -} - -template -void DifferenceMultiMapping::computeNeedleProximity(const In1VecCoord &x1, const In2VecCoord &x2){ - - In1VecCoord from = x1; - In2VecCoord dst = x2; - m_constraints.clear(); - - size_t szFrom = from.size(); - size_t szDst = dst.size(); - type::vector direction = d_direction.getValue(); - - ///get the last rigid direction, the main goal is to use it for the - /// 3D bilateral constraint i.e the fix point of the cable in the robot structure - //Rigid direction = d_direction.getValue()[szDst-1]; - - //For each point in the FEM find the closest edge of the cable - for (size_t i = 0 ; i < szFrom; i++) { - Coord2 P = from[i]; - Constraint constraint; - - // find the min distance between a from mstate point and it's projection on each edge of the cable (destination mstate) - Real min_dist = std::numeric_limits::max(); - for (size_t j = 0; j < szDst-1; j++) { - Coord1 Q1 = dst[j]; - Coord1 Q2 = dst[j+1]; - //the axis - Coord1 dirAxe = Q2 -Q1; - Real length = dirAxe.norm(); - Real fact_v = dot(P-Q1,dirAxe) / dot(dirAxe,dirAxe) ; - - if(std::abs(fact_v) < min_dist){ - //if(fact_v < min_dist){ - min_dist = std::abs(fact_v) ; - - //define the constraint variables - Deriv1 proj; - Real alpha; - - ///To solve the case that the closest node is - /// not the node 0 but the node 1 of the beam - if(fact_v<0.0 && j!=0 && std::abs(fact_v) > 1e-8){ - //if fact_v < 0.0 that means the last beam is the good beam - //printf("if fact_v < 0.0 that means the last beam is the good beam \n"); - Q1 = dst[j-1] ; - dirAxe = dst[j] - Q1; - length = dirAxe.norm(); - fact_v = dot(P-Q1,dirAxe) / dot(dirAxe,dirAxe) ; - dirAxe.normalize(); - alpha = (P-Q1) * dirAxe; - - proj = Q1 + dirAxe * alpha; - //distVec = P - proj; // violation vector - //dist = (P - proj).norm(); // constraint violation - constraint.eid = j-1; - //The direction of the axe or the beam - constraint.dirAxe = dirAxe; - //the node contribution to the constraint which is 1-coeff - alpha = alpha / length; //normalize, ensure that <1.0 - if (alpha < 1e-8)constraint.alpha = 1.0 ; - else constraint.alpha = 1.0 - alpha; - - //The projection on the axe - constraint.proj = proj; - constraint.Q = from[i]; - - ///// - length = (dst[j] - Q1).norm(); - constraint.Q1Q2 = length; - constraint.r2 = fact_v; - - // We move the constraint point onto the projection - Deriv1 t1 = P - proj; // violation vector - constraint.dist = t1.norm(); // constraint violation - t1.normalize(); // direction of the constraint - - //// Second method compute normals using frames directions - Rigid dir = direction[constraint.eid]; - Vector3 vY = Vector3(0.,1.,0.); - type::Quat ori = dir.getOrientation() ; - vY = ori.rotate(vY); vY.normalize(); - t1 = vY ; - // } - - constraint.t1 = t1; - //tangential 2 - Deriv1 t2 = cross(t1, dirAxe); t2.normalize(); - constraint.t2 = t2; - }else{ - //compute needs for constraint - dirAxe.normalize(); - alpha = (P-Q1) * dirAxe; - - proj = Q1 + dirAxe * alpha; - //distVec = P - proj; // violation vector - //dist = (P - proj).norm(); // constraint violation - constraint.eid = j; - //The direction of the axe or the beam - constraint.dirAxe = dirAxe; - //the node contribution to the constraint which is 1-coeff - alpha = alpha / length; //normalize, ensure that <1.0 - if (alpha < 1e-8)constraint.alpha = 1.0 ; - else constraint.alpha = 1.0 - alpha; - - //The projection on the axe - constraint.proj = proj; - constraint.Q = from[i]; - - ///// - constraint.Q1Q2 = length; - constraint.r2 = fact_v; - - // We move the constraint point onto the projection - Deriv1 t1 = P - proj; // violation vector - constraint.dist = t1.norm(); // constraint violation - t1.normalize(); // direction of the constraint - - //// Second method compute normals using frames directions - Rigid dir = direction[constraint.eid]; - Vector3 vY = Vector3(0.,1.,0.); - type::Quat ori = dir.getOrientation() ; - vY = ori.rotate(vY); vY.normalize(); - t1 = vY ; - // } - constraint.t1 = t1; - //tangential 2 - Deriv1 t2 = cross(t1, dirAxe); t2.normalize(); - constraint.t2 = t2; - } - } - } - /*printf("______________________________________________________________\n"); - std::cout << "i :" << i << " ; eid:" << constraint.eid << " alpha : " << constraint.alpha << " ; dist :"<< constraint.dist << std::endl; - std::cout<<" fact_v :"<< constraint.r2 << i << " ; n :"<< constraint.dirAxe << "; t1:" << constraint.t1 << "; t2 :"<< constraint.t2 << std::endl; - printf("______________________________________________________________\n");*/ - m_constraints.push_back(constraint); - } -} - - -template -void DifferenceMultiMapping::apply( - const core::MechanicalParams* /* mparams */, const type::vector& dataVecOutPos, - const type::vector& dataVecIn1Pos , - const type::vector& dataVecIn2Pos) -{ - - if(dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) - return; - - //printf("///Do Apply//We need only one input In model and input Root model (if present) \n"); - const In1VecCoord& in1 = dataVecIn1Pos[0]->getValue(); - const In2VecCoord& in2 = dataVecIn2Pos[0]->getValue(); - - OutVecCoord& out = *dataVecOutPos[0]->beginEdit(); - - if (d_lastPointIsFixed.getValue()){ - computeProximity(in1,in2); - - //auto out = sofa::helper::writeOnly(*dataVecOutPos[0]); - size_t sz = m_constraints.size(); - out.resize(sz); - - for(unsigned int i=0; iendEdit(); -} - - - -template -void DifferenceMultiMapping:: applyJ( - const core::MechanicalParams* /* mparams */, const type::vector< OutDataVecDeriv*>& dataVecOutVel, - const type::vector& dataVecIn1Vel, - const type::vector& dataVecIn2Vel) { - if(dataVecOutVel.empty() || dataVecIn1Vel.empty() ||dataVecIn2Vel.empty() ) - return; - const In1VecDeriv& in1 = dataVecIn1Vel[0]->getValue(); - const In2VecDeriv& in2 = dataVecIn2Vel[0]->getValue(); - OutVecDeriv& outVel = *dataVecOutVel[0]->beginEdit(); - - //const OutVecCoord& out = m_toModel->read(core::ConstVecCoordId::position())->getValue(); - size_t sz = m_constraints.size(); - outVel.resize(sz); - if (d_lastPointIsFixed.getValue()){ - for (size_t i = 0 ; i < sz; i++) { - Constraint& c = m_constraints[i]; - int ei1 = c.eid; - int ei2 = c.eid+1; - if(i < sz-1){ - // std::cout << " ei1 : " << ei1 << " ei2 : "<< ei2 << std::endl; - Real v0 = c.dirAxe * (in1[i] - c.alpha * in2[ei1] - (1-c.alpha) * in2[ei2] ); - Real v1 = c.t1 * (in1[i] - c.alpha * in2[ei1] - (1-c.alpha) * in2[ei2] ); - Real v2 = c.t2 * (in1[i] - c.alpha * in2[ei1] - (1-c.alpha) * in2[ei2] ); - outVel[i] = OutDeriv(v0,v1,v2); - }else { - //std::cout << " i : " << i << " ei2 : "<< ei2 << std::endl; - Real v0 = c.dirAxe * (in1[i] - in2[ei2]); - Real v1 = c.t1 * (in1[i] - in2[ei2]); - Real v2 = c.t2 * (in1[i] - in2[ei2]); - outVel[i] = OutDeriv(v0,v1,v2); - } - } - }else{ - for (size_t i = 0 ; i < sz; i++) { - Constraint& c = m_constraints[i]; - int ei1 = c.eid; - int ei2 = c.eid+1; - - // std::cout << " ei1 : " << ei1 << " ei2 : "<< ei2 << std::endl; - Real v0 = c.dirAxe * (in1[i] - c.alpha * in2[ei1] - (1-c.alpha) * in2[ei2] ); - Real v1 = c.t1 * (in1[i] - c.alpha * in2[ei1] - (1-c.alpha) * in2[ei2] ); - Real v2 = c.t2 * (in1[i] - c.alpha * in2[ei1] - (1-c.alpha) * in2[ei2] ); - outVel[i] = OutDeriv(v0,v1,v2); - } - } - - dataVecOutVel[0]->endEdit(); -} - - -template -void DifferenceMultiMapping::applyJT( - const core::MechanicalParams* /*mparams*/, const type::vector< In1DataVecDeriv*>& dataVecOut1Force, - const type::vector< In2DataVecDeriv*>& dataVecOut2Force, - const type::vector& dataVecInForce) { - - if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) - return; - - const OutVecDeriv& in = dataVecInForce[0]->getValue(); - - In1VecDeriv& out1 = *dataVecOut1Force[0]->beginEdit(); - In2VecDeriv& out2 = *dataVecOut2Force[0]->beginEdit(); - - //Compute output forces - size_t sz = m_constraints.size(); - if (d_lastPointIsFixed.getValue()){ - for (size_t i = 0 ; i < sz; i++) { - Constraint& c = m_constraints[i]; - int ei1 = c.eid; - int ei2 = c.eid+1; - OutDeriv f = in[i]; - //std::cout << " ================+++++++++>>>>> The force : " << f << std::endl; - if(i < sz-1){ - Deriv2 f1 = (f[0] * c.dirAxe) + (f[1] * c.t1) + (f[2] * c.t2) ; - Deriv1 f2_1 = (c.alpha * f[0]*c.dirAxe) + (c.alpha* f[1] * c.t1) + (c.alpha * f[2] * c.t2); - Deriv1 f2_2 = ((1-c.alpha) * f[0]*c.dirAxe )+ ((1-c.alpha) * f[1]*c.t1) + ((1-c.alpha) * f[2]*c.t2); - - //std::cout << " f1 : " << f1 << " f&_1: " << f2_1 << " ; f2 : "<< f2 << std::endl; - out1[i] += f1; - out2[ei1] -= f2_1; - out2[ei2] -= f2_2; - }else{ - Deriv2 f = (f[0] * c.dirAxe) + (f[1] * c.t1) + (f[2] * c.t2) ; - out1[i] += f; - out2[ei2] -= f; - } - } - }else{ - for (size_t i = 0 ; i < sz; i++) { - Constraint& c = m_constraints[i]; - int ei1 = c.eid; - int ei2 = c.eid+1; - OutDeriv f = in[i]; - //std::cout << " ================+++++++++>>>>> The force : " << f << std::endl; - Deriv2 f1 = (f[0] * c.dirAxe) + (f[1] * c.t1) + (f[2] * c.t2) ; - Deriv1 f2_1 = (c.alpha * f[0]*c.dirAxe) + (c.alpha* f[1] * c.t1) + (c.alpha * f[2] * c.t2); - Deriv1 f2_2 = ((1-c.alpha) * f[0]*c.dirAxe )+ ((1-c.alpha) * f[1]*c.t1) + ((1-c.alpha) * f[2]*c.t2); - - out1[i] += f1; - out2[ei1] -= f2_1; - out2[ei2] -= f2_2; - - } - } - dataVecOut1Force[0]->endEdit(); - dataVecOut2Force[0]->endEdit(); -} - -//___________________________________________________________________________ -template -void DifferenceMultiMapping::applyJT( - const core::ConstraintParams*/*cparams*/ , const type::vector< In1DataMatrixDeriv*>& dataMatOut1Const, - const type::vector< In2DataMatrixDeriv*>& dataMatOut2Const , - const type::vector& dataMatInConst) -{ - if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty() ) - return; - - - //We need only one input In model and input Root model (if present) - In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the FEM cable points - In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the frames cable points - const OutMatrixDeriv& in = dataMatInConst[0]->getValue(); // input constraints defined on the mapped point - const In1DataVecCoord* x1fromData = m_fromModel1->read(core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); - - typename OutMatrixDeriv::RowConstIterator rowIt = in.begin() ; - typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); - - for (rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) - { - typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); - - // Creates a constraints if the input constraint is not empty. - if (colIt == colItEnd) - { - continue; - } - typename In1MatrixDeriv::RowIterator o1 = out1.writeLine(rowIt.index()); // we store the constraint number - typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); - - if (d_lastPointIsFixed.getValue()){ - if((rowIt.index()/2) < (x1from.size() -1)){ - while (colIt != colItEnd) - { - int childIndex = colIt.index(); - Constraint c = m_constraints[childIndex]; - const OutDeriv h = colIt.val(); - int indexBeam = c.eid; - - Deriv2 h1 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2) ; - Deriv1 h2_1 = (c.alpha * h[0]*c.dirAxe) + (c.alpha* h[1] * c.t1) + (c.alpha * h[2] * c.t2); - Deriv1 h2_2 = ((1.0-c.alpha) * h[0]*c.dirAxe )+ ((1.0-c.alpha) * h[1]*c.t1) + ((1.0-c.alpha) * h[2]*c.t2); - - o1.addCol(childIndex, h1); - o2.addCol(indexBeam, -h2_1); - o2.addCol(indexBeam+1, -h2_2); - - colIt++; - } - }else{ - while (colIt != colItEnd) - { - int childIndex = colIt.index(); - Constraint c = m_constraints[childIndex]; - const OutDeriv h = colIt.val(); - int indexBeam = c.eid; - - Deriv2 h1 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2) ; - Deriv1 h2 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2); - - o1.addCol(childIndex, h1); - o2.addCol(indexBeam+1, -h2); - colIt++; - } - } - }else { - while (colIt != colItEnd) - { - int childIndex = colIt.index(); - Constraint c = m_constraints[childIndex]; - const OutDeriv h = colIt.val(); - int indexBeam = c.eid; - - Deriv2 h1 = (h[0] * c.dirAxe) + (h[1] * c.t1) + (h[2] * c.t2) ; - Deriv1 h2_1 = (c.alpha * h[0]*c.dirAxe) + (c.alpha* h[1] * c.t1) + (c.alpha * h[2] * c.t2); - Deriv1 h2_2 = ((1.0-c.alpha) * h[0]*c.dirAxe )+ ((1.0-c.alpha) * h[1]*c.t1) + ((1.0-c.alpha) * h[2]*c.t2); - - o1.addCol(childIndex, h1); - o2.addCol(indexBeam, -h2_1); - o2.addCol(indexBeam+1, -h2_2); - - colIt++; - } - } - } - dataMatOut1Const[0]->endEdit(); - dataMatOut2Const[0]->endEdit(); -} - - -template -void DifferenceMultiMapping::draw(const core::visual::VisualParams* vparams) -{ - ///draw cable - if (!vparams->displayFlags().getShowInteractionForceFields()) - return; - - typedef sofa::type::RGBAColor RGBAColor; - vparams->drawTool()->saveLastState(); - vparams->drawTool()->disableLighting(); - - std::vector vertices; - RGBAColor color = RGBAColor::magenta(); - - if(d_drawArrows.getValue() && d_lastPointIsFixed.getValue()){ - for (size_t i =0 ; i < m_constraints.size(); i++) { - color = RGBAColor::green(); - vertices.push_back(m_constraints[i].proj); - vertices.push_back(m_constraints[i].Q); - vparams->drawTool()->drawLines(vertices, 4.0, color); - if(i==(m_constraints.size()-1)){ - Coord2 P1 = m_constraints[i].Q; - Real radius_arrow = 0.30; - Coord2 x = m_constraints[i].dirAxe * 5.0; - Coord2 y = m_constraints[i].t1 * 5.0; - Coord2 z = m_constraints[i].t2 * 5.0; - - vparams->drawTool()->drawArrow(P1,P1 + x, radius_arrow, RGBAColor::red()); - vparams->drawTool()->drawArrow(P1,P1 + y, radius_arrow, RGBAColor::green()); - vparams->drawTool()->drawArrow(P1,P1 + z, radius_arrow, RGBAColor::blue()); - - }else{ - Coord2 P1 = m_constraints[i].Q; - Real radius_arrow = 0.30; - //Coord2 x = m_constraints[i].dirAxe * 5.0; - Coord2 y = m_constraints[i].t1 * 5.0; - Coord2 z = m_constraints[i].t2 * 5.0; - vparams->drawTool()->drawArrow(P1,P1 + y, radius_arrow, RGBAColor::blue()); - vparams->drawTool()->drawArrow(P1,P1 + z, radius_arrow, RGBAColor::blue()); - } - } - const In1DataVecDeriv* xDestData = m_fromModel1->read(core::ConstVecCoordId::position()); - const In1VecCoord& fromPos = xDestData[0].getValue(); - vparams->drawTool()->draw3DText_Indices(fromPos,6,RGBAColor(0.0,1.0,0.0,1.0)); - } - vparams->drawTool()->restoreLastState(); -} - -} // namespace sofa