diff --git a/CMakeLists.txt b/CMakeLists.txt index 39e243a..a2065de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,6 +158,17 @@ if(COSSERAT_BUILD_TESTS) add_subdirectory(Tests) endif() +# Benchmarks +# Only build benchmarks if Google Benchmark is available +find_package(benchmark QUIET) +if(benchmark_FOUND) + message(STATUS "Google Benchmark found, enabling benchmark support") + cmake_dependent_option(COSSERAT_BUILD_BENCHMARKS "Compile the benchmarks" ON "COSSERAT_BUILD_TESTS" OFF) +else() + message(STATUS "Google Benchmark not found, benchmarks will be disabled") + set(COSSERAT_BUILD_BENCHMARKS OFF CACHE BOOL "Compile the benchmarks" FORCE) +endif() + # Config files and install rules for pythons scripts sofa_install_pythonscripts(PLUGIN_NAME ${PROJECT_NAME} PYTHONSCRIPTS_SOURCE_DIR "examples/python3/") diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index d15db72..8c89bd5 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -1,42 +1,38 @@ cmake_minimum_required(VERSION 3.12) -set(This Cosserat_test) +project(Cosserat_test) -project(${This} C CXX) +if (UNIT_TEST) -#find_package(Cosserat REQUIRED) -find_package(Sofa.Testing REQUIRED) + enable_testing() -enable_testing() + set(HEADER_FILES + ) -set(HEADER_FILES - Example.h - constraint/Constraint.h - ) -set(SOURCE_FILES - Example.cpp - constraint/ExampleTest.cpp -# constraint/CosseratUnilateralInteractionConstraintTest.cpp + set(SOURCE_FILES + engine/GeometricStiffnessEngineTest.cpp forcefield/BeamHookeLawForceFieldTest.cpp + mapping/AdaptiveBeamMappingTest.cpp + mapping/CosseratNonLinearMapping2DTest.cpp + liegroups/RealSpaceTest.cpp + liegroups/SO2Test.cpp + liegroups/SE2Test.cpp ) + add_executable(${PROJECT_NAME} ${HEADER_FILES} ${SOURCE_FILES}) + target_link_libraries(${PROJECT_NAME} Cosserat_SRC) + target_link_libraries(${PROJECT_NAME} gtest gtest_main) + target_link_libraries(${PROJECT_NAME} SofaGTestMain) -add_executable(${This} ${SOURCE_FILES} ${HEADER_FILES}) - -target_link_libraries(${PROJECT_NAME} - Sofa.Testing - Cosserat -) - -target_include_directories(${This} - PUBLIC - "$" -) + if(APPLE) + target_link_libraries(${PROJECT_NAME} "-framework Cocoa") + endif() + add_test(NAME ${PROJECT_NAME} COMMAND ${PROJECT_NAME}) -add_test( - NAME ${This} - COMMAND ${This} -) +endif() -#[[add_subdirectory(constraint)]] +# Add benchmarks subdirectory if benchmarks are enabled +if(COSSERAT_BUILD_BENCHMARKS) + add_subdirectory(benchmarks) +endif() diff --git a/Tests/forcefield/BeamHookeLawForceFieldTest.cpp b/Tests/forcefield/BeamHookeLawForceFieldTest.cpp index c5921f3..a0f1fe1 100644 --- a/Tests/forcefield/BeamHookeLawForceFieldTest.cpp +++ b/Tests/forcefield/BeamHookeLawForceFieldTest.cpp @@ -17,6 +17,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -42,6 +46,8 @@ struct BeamHookeLawForceFieldTest : public testing::NumericTest<> { createObject(root, "DefaultAnimationLoop"); createObject(root, "DefaultVisualManagerLoop"); + sofa::simpleapi::importPlugin("Sofa.Component"); + sofa::simpleapi::importPlugin("Cosserat"); } @@ -52,8 +58,6 @@ struct BeamHookeLawForceFieldTest : public testing::NumericTest<> { void doSetUp() override { // initialization or some code to run before each test fprintf(stderr, "Starting up ! \n"); - sofa::simpleapi::importPlugin("Sofa.Component"); - sofa::simpleapi::importPlugin("Cosserat"); } // Tears down the test fixture. @@ -84,11 +88,13 @@ struct BeamHookeLawForceFieldTest : public testing::NumericTest<> { } /** + * * */ void basicAttributesTest(); - void triangle(); - + void testUniformSection(); + void testVariantSection(); + void testParallelPerformance(); // void addForceTest(const MechanicalParams* mparams, // DataVecDeriv& f , // const DataVecCoord& x , @@ -110,38 +116,406 @@ struct BeamHookeLawForceFieldTest : public testing::NumericTest<> { ///< Root of the scene graph, created by the constructor an re-used in the tests simulation::Node::SPtr root; + // Helper function to create a basic beam model + TheBeamHookeLawForceField* createBeamModel(bool variantSections, bool useMultiThreading); + + // Compare performance between single-threaded and multi-threaded force computation + void comparePerformance(int numElements, bool variantSections); + void testFonctionnel(); }; -template<> -void BeamHookeLawForceFieldTest::testFonctionnel() { - EXPECT_MSG_NOEMIT(Error, Warning) ; +template +typename BeamHookeLawForceFieldTest::TheBeamHookeLawForceField* +BeamHookeLawForceFieldTest::createBeamModel(bool variantSections, bool useMultiThreading) { + EXPECT_MSG_NOEMIT(Error, Warning); ASSERT_NE(root, nullptr); - createObject(root, "MechanicalObject", {{"position", "-1 0 1 1 0 1 -1 0 -1 1 0 -1 0 0 1 0 0 -1 -1 0 0 1 0 0 0 0 0"}}); - createObject(root, "TriangleSetTopologyContainer", {{"triangles", "7 5 8 8 2 6 4 6 0 1 8 4 7 3 5 8 5 2 4 8 6 1 7 8"}}); - - auto traction = dynamic_cast( - createObject(root, "BeamHookeLawForceField", {{"name", "beamHookeLaw"}, - {"crossSectionShape", "circular"}, - {"lengthY", "35e-5"}, - {"lengthZ", "1374e-5"}, - {"radius", "0.25"}, - {"variantSections", "true"}}).get() + + // Create mechanical object with positions for testing + sofa::component::statecontainer::MechanicalObject* mstate = dynamic_cast*>( + createObject(root, "MechanicalObject", { + {"name", "mstate"}, + {"position", "0 0 0 0.1 0 0 0.2 0 0 0.3 0 0 0.4 0 0 0.5 0 0 0.6 0 0 0.7 0 0 0.8 0 0 0.9 0 0"} + }).get() + ); + + // Set rest position to match initial position + mstate->writeRestPositions(mstate->x); + + // Create beam force field + std::vector> attributes = { + {"name", "beamForceField"}, + {"crossSectionShape", "circular"}, + {"youngModulus", "1e5"}, + {"poissonRatio", "0.3"}, + {"radius", "0.01"}, + {"variantSections", variantSections ? "true" : "false"}, + {"useMultiThreading", useMultiThreading ? "true" : "false"}, + {"length", "0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1"} + }; + + // Add variant section data if needed + if (variantSections) { + attributes.push_back({"youngModulusList", "1e5 1.1e5 1.2e5 1.3e5 1.4e5 1.5e5 1.6e5 1.7e5 1.8e5"}); + attributes.push_back({"poissonRatioList", "0.3 0.31 0.32 0.33 0.34 0.35 0.36 0.37 0.38"}); + } + + auto forcefield = dynamic_cast( + createObject(root, "BeamHookeLawForceField", attributes).get() ); + + EXPECT_NE(forcefield, nullptr); + root->init(sofa::core::execparams::defaultInstance()); + + return forcefield; +} - EXPECT_NE(traction, nullptr); - EXPECT_NE(root.get(), nullptr) ; - root->init(sofa::core::execparams::defaultInstance()) ; +template +void BeamHookeLawForceFieldTest::comparePerformance(int numElements, bool variantSections) { + // Reset the root node for this test + if (root) { + sofa::simulation::node::unload(root); + } + root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + createObject(root, "DefaultAnimationLoop"); + createObject(root, "DefaultVisualManagerLoop"); + + // Create a large mechanical object for performance testing + std::stringstream positionStr; + std::stringstream lengthStr; + std::stringstream youngModuliStr; + std::stringstream poissonRatioStr; + + for (int i = 0; i < numElements; i++) { + positionStr << i/10.0 << " 0 0 "; + if (i < numElements - 1) { + lengthStr << "0.1 "; + youngModuliStr << (1.0 + 0.1*i) << "e5 "; + poissonRatioStr << (0.3 + 0.01*i) << " "; + } + } + + // Create mechanical object + sofa::component::statecontainer::MechanicalObject* mstate = dynamic_cast*>( + createObject(root, "MechanicalObject", { + {"name", "mstate"}, + {"position", positionStr.str()} + }).get() + ); + + // Set rest positions + mstate->writeRestPositions(mstate->x); + + // Apply a small deformation to test forces + auto x = mstate->x.beginEdit(); + for (size_t i = 0; i < x->size(); i++) { + (*x)[i][0] += 0.001 * i; // Apply small deformation + } + mstate->x.endEdit(); + + // Run with single-threading + auto singleThreadForceField = dynamic_cast( + createObject(root, "BeamHookeLawForceField", { + {"name", "singleThreadFF"}, + {"crossSectionShape", "circular"}, + {"youngModulus", "1e5"}, + {"poissonRatio", "0.3"}, + {"radius", "0.01"}, + {"variantSections", variantSections ? "true" : "false"}, + {"useMultiThreading", "false"}, + {"length", lengthStr.str()} + }).get() + ); + + if (variantSections) { + singleThreadForceField->findData("youngModulusList")->read(youngModuliStr.str()); + singleThreadForceField->findData("poissonRatioList")->read(poissonRatioStr.str()); + } + + root->init(sofa::core::execparams::defaultInstance()); + + // Measure single-threaded performance + auto start_single = std::chrono::high_resolution_clock::now(); + for (int i = 0; i < 10; i++) { // Run multiple iterations for more accurate timing + sofa::simulation::node::animate(root.get(), 0.01); + } + auto end_single = std::chrono::high_resolution_clock::now(); + std::chrono::duration single_thread_time = end_single - start_single; + + // Remove single threaded force field + root->removeObject(singleThreadForceField); + + // Run with multi-threading + auto multiThreadForceField = dynamic_cast( + createObject(root, "BeamHookeLawForceField", { + {"name", "multiThreadFF"}, + {"crossSectionShape", "circular"}, + {"youngModulus", "1e5"}, + {"poissonRatio", "0.3"}, + {"radius", "0.01"}, + {"variantSections", variantSections ? "true" : "false"}, + {"useMultiThreading", "true"}, + {"length", lengthStr.str()} + }).get() + ); + + if (variantSections) { + multiThreadForceField->findData("youngModulusList")->read(youngModuliStr.str()); + multiThreadForceField->findData("poissonRatioList")->read(poissonRatioStr.str()); + } + + root->init(sofa::core::execparams::defaultInstance()); + + // Measure multi-threaded performance + auto start_multi = std::chrono::high_resolution_clock::now(); + for (int i = 0; i < 10; i++) { // Run multiple iterations for more accurate timing + sofa::simulation::node::animate(root.get(), 0.01); + } + auto end_multi = std::chrono::high_resolution_clock::now(); + std::chrono::duration multi_thread_time = end_multi - start_multi; + + // Calculate speedup + double speedup = single_thread_time.count() / multi_thread_time.count(); + + // Output performance results + std::cout << "Performance comparison for " << (variantSections ? "variant" : "uniform") + << " sections with " << numElements << " elements:" << std::endl; + std::cout << " Single-threaded time: " << single_thread_time.count() << " ms" << std::endl; + std::cout << " Multi-threaded time: " << multi_thread_time.count() << " ms" << std::endl; + std::cout << " Speedup factor: " << speedup << "x" << std::endl; + + // We expect some speedup with multithreading, though the exact amount depends on hardware + // For a reasonable number of elements, we should see at least some improvement + EXPECT_GT(speedup, 1.0) << "Multithreading should provide some speedup"; +} - auto total_load = dynamic_cast *>(traction->findData("lengthY")); - for (unsigned int step = 1; step <= 5; ++step) { - sofa::simulation::node::animate(root.get(), 1); - EXPECT_DOUBLE_EQ(total_load->getValue(), 4*step) << "Total load at time step " << step << " is incorrect."; +template +void BeamHookeLawForceFieldTest::testUniformSection() { + EXPECT_MSG_NOEMIT(Error, Warning); + + // Create models for testing - one single-threaded and one multi-threaded + if (root) { + sofa::simulation::node::unload(root); + } + root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + createObject(root, "DefaultAnimationLoop"); + createObject(root, "DefaultVisualManagerLoop"); + + // Create a force field with uniform sections using single threading + auto singleThreadFF = createBeamModel(false, false); + ASSERT_NE(singleThreadFF, nullptr); + + // Create a mechanical object and apply a known deformation + auto mstate = dynamic_cast*>( + root->getTreeObject("mstate")); + ASSERT_NE(mstate, nullptr); + + // Apply deformation + auto x = mstate->x.beginEdit(); + (*x)[4][0] += 0.01; // Apply deformation to middle node + mstate->x.endEdit(); + + // Compute forces with single-threading + sofa::simulation::node::animate(root.get(), 0.01); + + // Store the forces from single-threaded computation + const auto& singleThreadForces = mstate->f.getValue(); + + // Remove single-threaded force field + root->removeObject(singleThreadFF); + + // Create a force field with uniform sections using multi-threading + auto multiThreadFF = createBeamModel(false, true); + ASSERT_NE(multiThreadFF, nullptr); + + // Compute forces with multi-threading + sofa::simulation::node::animate(root.get(), 0.01); + + // Compare forces from single-threaded and multi-threaded computations + const auto& multiThreadForces = mstate->f.getValue(); + + // The forces should be identical regardless of threading + ASSERT_EQ(singleThreadForces.size(), multiThreadForces.size()); + for (size_t i = 0; i < singleThreadForces.size(); i++) { + for (size_t j = 0; j < 3; j++) { // Compare each component (assuming Vec3) + EXPECT_NEAR(singleThreadForces[i][j], multiThreadForces[i][j], 1e-10) + << "Force difference at index " << i << ", component " << j; + } + } +} + +template +void BeamHookeLawForceFieldTest::testVariantSection() { + EXPECT_MSG_NOEMIT(Error, Warning); + + // Create models for testing - one single-threaded and one multi-threaded + if (root) { + sofa::simulation::node::unload(root); + } + root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + createObject(root, "DefaultAnimationLoop"); + createObject(root, "DefaultVisualManagerLoop"); + + // Create a force field with variant sections using single threading + auto singleThreadFF = createBeamModel(true, false); + ASSERT_NE(singleThreadFF, nullptr); + + // Create a mechanical object and apply a known deformation + auto mstate = dynamic_cast*>( + root->getTreeObject("mstate")); + ASSERT_NE(mstate, nullptr); + + // Apply deformation + auto x = mstate->x.beginEdit(); + for (size_t i = 0; i < x->size(); i++) { + (*x)[i][0] += 0.001 * i; // Apply varying deformation + } + mstate->x.endEdit(); + + // Compute forces with single-threading + sofa::simulation::node::animate(root.get(), 0.01); + + // Store the forces from single-threaded computation + const auto& singleThreadForces = mstate->f.getValue(); + + // Remove single-threaded force field + root->removeObject(singleThreadFF); + + // Create a force field with variant sections using multi-threading + auto multiThreadFF = createBeamModel(true, true); + ASSERT_NE(multiThreadFF, nullptr); + + // Compute forces with multi-threading + sofa::simulation::node::animate(root.get(), 0.01); + + // Compare forces from single-threaded and multi-threaded computations + const auto& multiThreadForces = mstate->f.getValue(); + + // The forces should be identical regardless of threading + ASSERT_EQ(singleThreadForces.size(), multiThreadForces.size()); + for (size_t i = 0; i < singleThreadForces.size(); i++) { + for (size_t j = 0; j < 3; j++) { // Compare each component (assuming Vec3) + EXPECT_NEAR(singleThreadForces[i][j], multiThreadForces[i][j], 1e-10) + << "Force difference at index " << i << ", component " << j; + } } } +template +void BeamHookeLawForceFieldTest::testParallelPerformance() { + EXPECT_MSG_NOEMIT(Error, Warning); + + // Test performance with different numbers of elements + comparePerformance(100, false); // Uniform sections with 100 elements + comparePerformance(500, false); // Uniform sections with 500 elements + comparePerformance(100, true); // Variant sections with 100 elements + comparePerformance(500, true); // Variant sections with 500 elements +} +template +void BeamHookeLawForceFieldTest::testFonctionnel() { + EXPECT_MSG_NOEMIT(Error, Warning); + ASSERT_NE(root, nullptr); + + // Reset root for this test + if (root) { + sofa::simulation::node::unload(root); + } + root = sofa::simpleapi::createRootNode(sofa::simulation::getSimulation(), "root"); + createObject(root, "DefaultAnimationLoop"); + createObject(root, "DefaultVisualManagerLoop"); + + // Create a mechanical object with positions for beam simulation + sofa::component::statecontainer::MechanicalObject* mstate = dynamic_cast*>( + createObject(root, "MechanicalObject", { + {"name", "mstate"}, + {"position", "0 0 0 0.1 0 0 0.2 0 0 0.3 0 0 0.4 0 0"} + }).get() + ); + + // Set rest position to match initial position + mstate->writeRestPositions(mstate->x); + + // Create beam force field with both uniform and variant options for testing + auto forcefield = dynamic_cast( + createObject(root, "BeamHookeLawForceField", { + {"name", "beamForceField"}, + {"crossSectionShape", "circular"}, + {"youngModulus", "1e5"}, + {"poissonRatio", "0.3"}, + {"radius", "0.01"}, + {"variantSections", "false"}, + {"useMultiThreading", "true"}, + {"length", "0.1 0.1 0.1 0.1"} + }).get() + ); + + EXPECT_NE(forcefield, nullptr); + root->init(sofa::core::execparams::defaultInstance()); + + // Apply deformation and verify forces + auto x = mstate->x.beginEdit(); + (*x)[2][0] += 0.01; // Apply deformation to middle node + mstate->x.endEdit(); + + // Run one step of simulation + sofa::simulation::node::animate(root.get(), 0.01); + + // Check that forces are computed and are non-zero + const auto& forces = mstate->f.getValue(); + ASSERT_EQ(forces.size(), 5); + + // Verify that forces are computed correctly (non-zero at deformed points) + bool foundNonZeroForce = false; + for (size_t i = 0; i < forces.size(); i++) { + if (forces[i].norm() > 1e-10) { + foundNonZeroForce = true; + break; + } + } + + EXPECT_TRUE(foundNonZeroForce) << "Expected non-zero forces due to deformation"; + + // Now switch to variant sections and test again + root->removeObject(forcefield); + + auto variantForcefield = dynamic_cast( + createObject(root, "BeamHookeLawForceField", { + {"name", "beamForceField"}, + {"crossSectionShape", "circular"}, + {"youngModulus", "1e5"}, + {"poissonRatio", "0.3"}, + {"radius", "0.01"}, + {"variantSections", "true"}, + {"youngModulusList", "1e5 1.1e5 1.2e5 1.3e5"}, + {"poissonRatioList", "0.3 0.31 0.32 0.33"}, + {"useMultiThreading", "true"}, + {"length", "0.1 0.1 0.1 0.1"} + }).get() + ); + + EXPECT_NE(variantForcefield, nullptr); + root->init(sofa::core::execparams::defaultInstance()); + + // Run simulation with variant sections + sofa::simulation::node::animate(root.get(), 0.01); + + // Check that forces are computed and are non-zero with variant sections + const auto& variantForces = mstate->f.getValue(); + ASSERT_EQ(variantForces.size(), 5); + + // Verify that forces are computed correctly with variant sections + foundNonZeroForce = false; + for (size_t i = 0; i < variantForces.size(); i++) { + if (variantForces[i].norm() > 1e-10) { + foundNonZeroForce = true; + break; + } + } + + EXPECT_TRUE(foundNonZeroForce) << "Expected non-zero forces with variant sections"; +} template<> void BeamHookeLawForceFieldTest::basicAttributesTest(){ @@ -199,7 +573,6 @@ TYPED_TEST( BeamHookeLawForceFieldTest , basicAttributesTest ) ASSERT_NO_THROW (this->basicAttributesTest()); } -TYPED_TEST(BeamHookeLawForceFieldTest, DISABLED_testFonctionnel) { ASSERT_NO_THROW (this->testFonctionnel()); } diff --git a/Tests/liegroups/BundleTest.cpp b/Tests/liegroups/BundleTest.cpp new file mode 100644 index 0000000..ddc801c --- /dev/null +++ b/Tests/liegroups/BundleTest.cpp @@ -0,0 +1,267 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Test suite for Bundle Lie group implementation + */ +class BundleTest : public BaseTest +{ +protected: + // Define common types + using RealSpace3d = RealSpace; + using SO3d = SO3; + using SE3d = SE3; + using Vector3 = Eigen::Vector3d; + using Matrix3 = Eigen::Matrix3d; + using Quaternion = Eigen::Quaterniond; + + // Define test bundle types + using PoseVel = Bundle; // Pose + velocity + using MultiBody = Bundle; // Three rigid bodies + using ComplexSystem = Bundle; // Mixed system + + const double pi = M_PI; + const double eps = 1e-10; + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test identity element properties + */ +TEST_F(BundleTest, Identity) +{ + // Test PoseVel identity + PoseVel id_pv = PoseVel::identity(); + EXPECT_TRUE(id_pv.get<0>().isApprox(SE3d::identity())); + EXPECT_TRUE(id_pv.get<1>().isApprox(RealSpace3d::identity())); + + // Test MultiBody identity + MultiBody id_mb = MultiBody::identity(); + EXPECT_TRUE(id_mb.get<0>().isApprox(SE3d::identity())); + EXPECT_TRUE(id_mb.get<1>().isApprox(SE3d::identity())); + EXPECT_TRUE(id_mb.get<2>().isApprox(SE3d::identity())); + + // Test right and left identity + Vector3 axis = Vector3(1, 1, 1).normalized(); + PoseVel g(SE3d(SO3d(pi/4, axis), Vector3(1, 2, 3)), + RealSpace3d(Vector3(0.1, 0.2, 0.3))); + + EXPECT_TRUE((g * id_pv).isApprox(g)); + EXPECT_TRUE((id_pv * g).isApprox(g)); +} + +/** + * Test group operation (component-wise composition) + */ +TEST_F(BundleTest, GroupOperation) +{ + // Test PoseVel composition + Vector3 axis1(1, 0, 0), axis2(0, 1, 0); + Vector3 trans1(1, 0, 0), trans2(0, 1, 0); + Vector3 vel1(0.1, 0, 0), vel2(0, 0.2, 0); + + PoseVel g1(SE3d(SO3d(pi/2, axis1), trans1), RealSpace3d(vel1)); + PoseVel g2(SE3d(SO3d(pi/2, axis2), trans2), RealSpace3d(vel2)); + + PoseVel g12 = g1 * g2; + + // Verify component-wise composition + EXPECT_TRUE(g12.get<0>().isApprox(g1.get<0>() * g2.get<0>())); + EXPECT_TRUE(g12.get<1>().isApprox(g1.get<1>() * g2.get<1>())); + + // Test MultiBody composition + MultiBody mb1(SE3d(SO3d(pi/2, axis1), trans1), + SE3d(SO3d(pi/3, axis1), trans1), + SE3d(SO3d(pi/4, axis1), trans1)); + + MultiBody mb2(SE3d(SO3d(pi/2, axis2), trans2), + SE3d(SO3d(pi/3, axis2), trans2), + SE3d(SO3d(pi/4, axis2), trans2)); + + MultiBody mb12 = mb1 * mb2; + + // Verify all bodies composed correctly + EXPECT_TRUE(mb12.get<0>().isApprox(mb1.get<0>() * mb2.get<0>())); + EXPECT_TRUE(mb12.get<1>().isApprox(mb1.get<1>() * mb2.get<1>())); + EXPECT_TRUE(mb12.get<2>().isApprox(mb1.get<2>() * mb2.get<2>())); +} + +/** + * Test inverse operation + */ +TEST_F(BundleTest, Inverse) +{ + // Test PoseVel inverse + Vector3 axis = Vector3(1, 1, 0).normalized(); + Vector3 trans(1, 2, 3); + Vector3 vel(0.1, 0.2, 0.3); + + PoseVel g(SE3d(SO3d(pi/3, axis), trans), RealSpace3d(vel)); + PoseVel inv = g.inverse(); + + // Test inverse properties + EXPECT_TRUE((g * inv).isApprox(PoseVel::identity())); + EXPECT_TRUE((inv * g).isApprox(PoseVel::identity())); + + // Verify component-wise inverse + EXPECT_TRUE(inv.get<0>().isApprox(g.get<0>().inverse())); + EXPECT_TRUE(inv.get<1>().isApprox(g.get<1>().inverse())); +} + +/** + * Test exponential and logarithm maps + */ +TEST_F(BundleTest, ExpLog) +{ + // Test PoseVel exp/log + Vector3 axis = Vector3(1, 2, 3).normalized(); + Vector3 trans(4, 5, 6); + Vector3 vel(0.1, 0.2, 0.3); + + PoseVel g(SE3d(SO3d(pi/4, axis), trans), RealSpace3d(vel)); + auto xi = g.log(); + PoseVel g2 = PoseVel().exp(xi); + + EXPECT_TRUE(g.isApprox(g2)); + + // Test MultiBody exp/log + MultiBody mb(SE3d(SO3d(pi/3, axis), trans), + SE3d(SO3d(pi/4, axis), trans), + SE3d(SO3d(pi/6, axis), trans)); + + auto xi_mb = mb.log(); + MultiBody mb2 = MultiBody().exp(xi_mb); + + EXPECT_TRUE(mb.isApprox(mb2)); +} + +/** + * Test adjoint representation + */ +TEST_F(BundleTest, Adjoint) +{ + // Test PoseVel adjoint (should be block diagonal) + Vector3 axis = Vector3(0, 0, 1); + Vector3 trans(1, 2, 0); + Vector3 vel(0.1, 0.2, 0); + + PoseVel g(SE3d(SO3d(pi/2, axis), trans), RealSpace3d(vel)); + auto Ad = g.adjoint(); + + // Verify block diagonal structure + auto Ad1 = g.get<0>().adjoint(); + auto Ad2 = g.get<1>().adjoint(); + + EXPECT_TRUE(Ad.block(0, 0, Ad1.rows(), Ad1.cols()).isApprox(Ad1)); + EXPECT_TRUE(Ad.block(Ad1.rows(), Ad1.cols(), Ad2.rows(), Ad2.cols()).isApprox(Ad2)); +} + +/** + * Test group action + */ +TEST_F(BundleTest, Action) +{ + // Test PoseVel action + Vector3 axis = Vector3(0, 0, 1); + Vector3 trans(1, 0, 0); + Vector3 vel(0.1, 0, 0); + + PoseVel g(SE3d(SO3d(pi/2, axis), trans), RealSpace3d(vel)); + + // Create point and velocity + Vector3 p(1, 0, 0); + Vector3 v(0.1, 0, 0); + Eigen::VectorXd state(6); + state << p, v; + + // Test action + auto result = g.act(state); + + // Verify components transformed correctly + Vector3 p_new = result.head<3>(); + Vector3 v_new = result.tail<3>(); + + EXPECT_TRUE(p_new.isApprox(g.get<0>().act(p))); + EXPECT_TRUE(v_new.isApprox(v + vel)); +} + +/** + * Test interpolation + */ +TEST_F(BundleTest, Interpolation) +{ + // Test PoseVel interpolation + Vector3 axis = Vector3(1, 1, 1).normalized(); + PoseVel start = PoseVel::identity(); + PoseVel end(SE3d(SO3d(pi/2, axis), Vector3(2, 0, 0)), + RealSpace3d(Vector3(0.2, 0, 0))); + + // Test midpoint + PoseVel mid = interpolate(start, end, 0.5); + + // Verify component-wise interpolation + EXPECT_TRUE(mid.get<0>().isApprox(interpolate(start.get<0>(), end.get<0>(), 0.5))); + EXPECT_TRUE(mid.get<1>().isApprox(interpolate(start.get<1>(), end.get<1>(), 0.5))); + + // Test boundary conditions + EXPECT_TRUE(interpolate(start, end, 0.0).isApprox(start)); + EXPECT_TRUE(interpolate(start, end, 1.0).isApprox(end)); +} + +/** + * Test mixed bundle operations + */ +TEST_F(BundleTest, ComplexSystem) +{ + // Create a complex system with different types + Vector3 axis = Vector3(1, 1, 1).normalized(); + ComplexSystem sys( + SE3d(SO3d(pi/4, axis), Vector3(1, 2, 3)), // Rigid body + SO3d(pi/3, Vector3(0, 0, 1)), // Pure rotation + RealSpace3d(Vector3(0.1, 0.2, 0.3)) // Vector space + ); + + // Test identity composition + EXPECT_TRUE((sys * ComplexSystem::identity()).isApprox(sys)); + + // Test inverse + auto inv = sys.inverse(); + EXPECT_TRUE((sys * inv).isApprox(ComplexSystem::identity())); + + // Test exp/log + auto xi = sys.log(); + auto sys2 = ComplexSystem().exp(xi); + EXPECT_TRUE(sys.isApprox(sys2)); +} + +} // namespace sofa::component::cosserat::liegroups::testing diff --git a/Tests/liegroups/LieGroupBenchmark.cpp b/Tests/liegroups/LieGroupBenchmark.cpp new file mode 100644 index 0000000..a34d25b --- /dev/null +++ b/Tests/liegroups/LieGroupBenchmark.cpp @@ -0,0 +1,254 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::benchmark { + +using Vector3 = Eigen::Vector3d; +using Matrix3 = Eigen::Matrix3d; +using Quaternion = Eigen::Quaterniond; + +// Helper for random generation +class RandomGenerator { +private: + std::mt19937 gen{std::random_device{}()}; + std::uniform_real_distribution angle_dist{0, 2*M_PI}; + std::normal_distribution vec_dist{0, 1.0}; + +public: + Vector3 randomVector() { + return Vector3(vec_dist(gen), vec_dist(gen), vec_dist(gen)); + } + + Vector3 randomUnitVector() { + Vector3 v = randomVector(); + return v.normalized(); + } + + double randomAngle() { + return angle_dist(gen); + } +}; + +/** + * Benchmark SO3 operations + */ +static void BM_SO3_Operations(benchmark::State& state) { + RandomGenerator rng; + SO3 rot(rng.randomAngle(), rng.randomUnitVector()); + Vector3 point = rng.randomVector(); + + for (auto _ : state) { + // Test common operations + auto result1 = rot.act(point); + auto result2 = rot.inverse(); + auto result3 = rot.log(); + auto result4 = rot.adjoint(); + benchmark::DoNotOptimize(result1); + benchmark::DoNotOptimize(result2); + benchmark::DoNotOptimize(result3); + benchmark::DoNotOptimize(result4); + } +} +BENCHMARK(BM_SO3_Operations); + +/** + * Benchmark SE3 operations + */ +static void BM_SE3_Operations(benchmark::State& state) { + RandomGenerator rng; + SE3 transform( + SO3(rng.randomAngle(), rng.randomUnitVector()), + rng.randomVector() + ); + Vector3 point = rng.randomVector(); + + for (auto _ : state) { + // Test common operations + auto result1 = transform.act(point); + auto result2 = transform.inverse(); + auto result3 = transform.log(); + auto result4 = transform.adjoint(); + benchmark::DoNotOptimize(result1); + benchmark::DoNotOptimize(result2); + benchmark::DoNotOptimize(result3); + benchmark::DoNotOptimize(result4); + } +} +BENCHMARK(BM_SE3_Operations); + +/** + * Benchmark SE_2(3) operations + */ +static void BM_SE23_Operations(benchmark::State& state) { + RandomGenerator rng; + SE23 extended_pose( + SE3( + SO3(rng.randomAngle(), rng.randomUnitVector()), + rng.randomVector() + ), + rng.randomVector() + ); + Vector3 point = rng.randomVector(); + + for (auto _ : state) { + // Test common operations + auto result1 = extended_pose.act(point); + auto result2 = extended_pose.inverse(); + auto result3 = extended_pose.log(); + auto result4 = extended_pose.adjoint(); + benchmark::DoNotOptimize(result1); + benchmark::DoNotOptimize(result2); + benchmark::DoNotOptimize(result3); + benchmark::DoNotOptimize(result4); + } +} +BENCHMARK(BM_SE23_Operations); + +/** + * Benchmark Bundle operations + */ +static void BM_Bundle_Operations(benchmark::State& state) { + RandomGenerator rng; + using PoseVel = Bundle, RealSpace>; + + PoseVel bundle( + SE3( + SO3(rng.randomAngle(), rng.randomUnitVector()), + rng.randomVector() + ), + RealSpace(rng.randomVector()) + ); + + for (auto _ : state) { + // Test common operations + auto result1 = bundle.inverse(); + auto result2 = bundle.log(); + auto result3 = bundle.adjoint(); + benchmark::DoNotOptimize(result1); + benchmark::DoNotOptimize(result2); + benchmark::DoNotOptimize(result3); + } +} +BENCHMARK(BM_Bundle_Operations); + +/** + * Benchmark Cosserat rod operations + */ +static void BM_CosseratRod_Operations(benchmark::State& state) { + RandomGenerator rng; + const int num_segments = state.range(0); + using RodSegment = Bundle, RealSpace>; + std::vector segments; + + // Initialize rod segments + for (int i = 0; i < num_segments; ++i) { + segments.push_back(RodSegment( + SE3( + SO3(rng.randomAngle(), rng.randomUnitVector()), + rng.randomVector() + ), + RealSpace(rng.randomVector()) + )); + } + + for (auto _ : state) { + // Simulate rod deformation + for (int i = 1; i < num_segments; ++i) { + auto rel_transform = segments[i-1].inverse() * segments[i]; + auto strain = rel_transform.log(); + benchmark::DoNotOptimize(strain); + } + } +} +BENCHMARK(BM_CosseratRod_Operations) + ->RangeMultiplier(2) + ->Range(8, 128); + +/** + * Benchmark exponential map implementations + */ +static void BM_ExpMap_Operations(benchmark::State& state) { + RandomGenerator rng; + Vector3 omega = rng.randomVector(); + + for (auto _ : state) { + // SO3 exponential + auto rot = SO3().exp(omega); + + // SE3 exponential + Vector3 v = rng.randomVector(); + auto transform = SE3().exp( + (Eigen::Matrix() << v, omega).finished() + ); + + benchmark::DoNotOptimize(rot); + benchmark::DoNotOptimize(transform); + } +} +BENCHMARK(BM_ExpMap_Operations); + +/** + * Benchmark interpolation operations + */ +static void BM_Interpolation_Operations(benchmark::State& state) { + RandomGenerator rng; + + // Create random transformations + SE3 T1( + SO3(rng.randomAngle(), rng.randomUnitVector()), + rng.randomVector() + ); + SE3 T2( + SO3(rng.randomAngle(), rng.randomUnitVector()), + rng.randomVector() + ); + + const int num_steps = state.range(0); + std::vector times(num_steps); + for (int i = 0; i < num_steps; ++i) { + times[i] = static_cast(i) / (num_steps - 1); + } + + for (auto _ : state) { + // Interpolate between transformations + for (double t : times) { + auto result = interpolate(T1, T2, t); + benchmark::DoNotOptimize(result); + } + } +} +BENCHMARK(BM_Interpolation_Operations) + ->RangeMultiplier(2) + ->Range(8, 128); + +} // namespace sofa::component::cosserat::liegroups::benchmark + +BENCHMARK_MAIN(); diff --git a/Tests/liegroups/LieGroupIntegrationTest.cpp b/Tests/liegroups/LieGroupIntegrationTest.cpp new file mode 100644 index 0000000..17387b5 --- /dev/null +++ b/Tests/liegroups/LieGroupIntegrationTest.cpp @@ -0,0 +1,307 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Integration tests demonstrating real-world applications of Lie groups + */ +class LieGroupIntegrationTest : public BaseTest +{ +protected: + // Define common types + using Vector3 = Eigen::Vector3d; + using Vector6 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix3d; + using Matrix4 = Eigen::Matrix4d; + using Quaternion = Eigen::Quaterniond; + + // Basic Lie groups + using SO3d = SO3; + using SE3d = SE3; + using SE23d = SE23; + using SGal3d = SGal3; + using RealSpace3d = RealSpace; + + // Specialized bundle types for Cosserat mechanics + using CosseratSection = Bundle; // Position, strain + using CosseratNode = Bundle; // Configuration with velocity + using CosseratRod = Bundle; // Position, strain, stress + + const double pi = M_PI; + const double eps = 1e-10; + + /** + * Helper to create a rotation from axis-angle + */ + SO3d makeRotation(double angle, const Vector3& axis) { + return SO3d(angle, axis.normalized()); + } + + /** + * Helper to create a material frame + */ + SE3d makeMaterialFrame(const Vector3& position, const Vector3& tangent, + const Vector3& normal) { + // Create rotation that aligns z-axis with tangent + Vector3 z_axis(0, 0, 1); + Vector3 rot_axis = z_axis.cross(tangent); + double rot_angle = std::acos(z_axis.dot(tangent)); + SO3d R(rot_angle, rot_axis.normalized()); + + // Additional rotation around tangent to align normal + Vector3 current_normal = R.act(Vector3(1, 0, 0)); + Vector3 target_normal = normal - normal.dot(tangent) * tangent; + target_normal.normalize(); + + double twist_angle = std::acos(current_normal.dot(target_normal)); + if (std::abs(twist_angle) > eps) { + SO3d twist(twist_angle, tangent); + R = twist * R; + } + + return SE3d(R, position); + } + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test Cosserat rod centerline representation + */ +TEST_F(LieGroupIntegrationTest, CosseratCenterline) +{ + // Create a circular arc centerline + const int num_points = 50; + const double radius = 1.0; + const double arc_angle = pi; // Half circle + + std::vector frames; + frames.reserve(num_points); + + for (int i = 0; i < num_points; ++i) { + double t = static_cast(i) / (num_points - 1); + double theta = t * arc_angle; + + // Position on circle + Vector3 position(radius * std::cos(theta), radius * std::sin(theta), 0); + + // Tangent vector (normalized derivative) + Vector3 tangent(-std::sin(theta), std::cos(theta), 0); + + // Normal vector (points inward) + Vector3 normal(-std::cos(theta), -std::sin(theta), 0); + + frames.push_back(makeMaterialFrame(position, tangent, normal)); + } + + // Verify geometric properties + for (size_t i = 1; i < frames.size(); ++i) { + // Get relative transform between consecutive frames + SE3d rel = frames[i-1].inverse() * frames[i]; + + // Extract curvature from relative rotation + Vector3 omega = rel.rotation().log(); + double curvature = omega.norm() * (num_points - 1) / arc_angle; + + // Should be constant and equal to 1/radius + EXPECT_NEAR(curvature, 1.0/radius, 0.1); + } +} + +/** + * Test Cosserat rod dynamics + */ +TEST_F(LieGroupIntegrationTest, CosseratDynamics) +{ + // Create a rod configuration with velocity + Vector3 position(0, 0, 0); + Vector3 tangent(0, 0, 1); + Vector3 normal(1, 0, 0); + + // Initial material frame + SE3d frame = makeMaterialFrame(position, tangent, normal); + + // Initial velocity state (linear and angular velocity) + Vector3 linear_vel(0.1, 0, 0); // Moving in x direction + Vector3 angular_vel(0, 0.5, 0); // Rotating around y axis + + // Create extended pose with velocity + SE23d state(frame, linear_vel); + + // Initial strain and stress + Vector3 strain(0, 0, 1); // Unit stretch + Vector3 stress(0, 0, 0); // No initial stress + + // Create full Cosserat state + CosseratRod rod(frame, strain, stress); + + // Simulate simple time evolution + const double dt = 0.01; + const int steps = 100; + + for (int i = 0; i < steps; ++i) { + // Update position and orientation using current velocity + Vector6 twist; + twist << linear_vel, angular_vel; + frame = frame * SE3d().exp(dt * twist); + + // Update strain based on deformation + strain = frame.rotation().inverse().act(tangent); + + // Simple linear stress response + stress = 100.0 * (strain - Vector3(0, 0, 1)); // Hook's law + + // Store new state + rod = CosseratRod(frame, strain, stress); + + // Verify physical constraints + EXPECT_NEAR(strain.norm(), 1.0, 0.1); // Length preservation + EXPECT_TRUE(frame.rotation().matrix().determinant() > 0); // Proper rotation + } +} + +/** + * Test multi-body articulation + */ +TEST_F(LieGroupIntegrationTest, ArticulatedSystem) +{ + // Create a simple 3-link articulated system + using ThreeLink = Bundle; + + // Initial configuration (vertical stack) + std::vector links; + const double link_length = 1.0; + + for (int i = 0; i < 3; ++i) { + Vector3 position(0, 0, i * link_length); + SO3d orientation = SO3d::identity(); + links.push_back(SE3d(orientation, position)); + } + + ThreeLink system(links[0], links[1], links[2]); + + // Apply joint motions + const double joint_angle = pi/4; // 45 degrees + + // Rotate first joint around y-axis + SO3d R1(joint_angle, Vector3(0, 1, 0)); + links[0] = SE3d(R1, links[0].translation()); + + // Rotate second joint around x-axis + SO3d R2(joint_angle, Vector3(1, 0, 0)); + SE3d T1 = links[0]; // Transform from first link + links[1] = T1 * SE3d(R2, Vector3(0, 0, link_length)); + + // Rotate third joint around y-axis + SO3d R3(-joint_angle, Vector3(0, 1, 0)); + SE3d T2 = links[1]; // Transform from second link + links[2] = T2 * SE3d(R3, Vector3(0, 0, link_length)); + + // Update system state + system = ThreeLink(links[0], links[1], links[2]); + + // Verify kinematic chain properties + for (int i = 1; i < 3; ++i) { + // Check link connections + Vector3 parent_end = links[i-1].translation() + + links[i-1].rotation().act(Vector3(0, 0, link_length)); + Vector3 child_start = links[i].translation(); + + EXPECT_TRUE((parent_end - child_start).norm() < eps); + } +} + +/** + * Test time-varying trajectories + */ +TEST_F(LieGroupIntegrationTest, TimeVaryingTrajectory) +{ + // Create a helix trajectory using SGal(3) + const double radius = 1.0; + const double pitch = 0.5; + const double angular_vel = 1.0; + const double vertical_vel = pitch * angular_vel; + + std::vector trajectory; + const int num_points = 50; + + for (int i = 0; i < num_points; ++i) { + double t = static_cast(i) / (num_points - 1); + double theta = t * 2 * pi; + + // Position on helix + Vector3 position(radius * std::cos(theta), + radius * std::sin(theta), + pitch * theta); + + // Tangent vector + Vector3 tangent(-radius * std::sin(theta), + radius * std::cos(theta), + pitch); + tangent.normalize(); + + // Create frame from position and tangent + SE3d frame = makeMaterialFrame(position, tangent, Vector3(0, 0, 1)); + + // Velocity components + Vector3 linear_vel(-radius * angular_vel * std::sin(theta), + radius * angular_vel * std::cos(theta), + vertical_vel); + + // Create Galilean transformation + trajectory.push_back(SGal3d(frame, linear_vel, t)); + } + + // Verify trajectory properties + for (size_t i = 1; i < trajectory.size(); ++i) { + const auto& g1 = trajectory[i-1]; + const auto& g2 = trajectory[i]; + + // Time should increase monotonically + EXPECT_GT(g2.time(), g1.time()); + + // Velocity should be constant in magnitude + double vel_mag1 = g1.velocity().norm(); + double vel_mag2 = g2.velocity().norm(); + EXPECT_NEAR(vel_mag1, vel_mag2, eps); + + // Position should follow helix equation + Vector3 pos = g1.pose().translation(); + double theta = std::atan2(pos.y(), pos.x()); + double expected_z = pitch * theta; + EXPECT_NEAR(pos.z(), expected_z, 0.1); + } +} + +} // namespace sofa::component::cosserat::liegroups::testing diff --git a/Tests/liegroups/SE23Test.cpp b/Tests/liegroups/SE23Test.cpp new file mode 100644 index 0000000..0642487 --- /dev/null +++ b/Tests/liegroups/SE23Test.cpp @@ -0,0 +1,263 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Test suite for SE_2(3) Lie group implementation + */ +class SE23Test : public BaseTest +{ +protected: + using SE23d = SE23; + using SE3d = SE3; + using SO3d = SO3; + using Vector3 = Eigen::Vector3d; + using Vector9 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix3d; + using Matrix4 = Eigen::Matrix4d; + using Matrix9 = Eigen::Matrix; + using Quaternion = Eigen::Quaterniond; + + const double pi = M_PI; + const double eps = 1e-10; + + // Helper function to create twist vector with acceleration + Vector9 twist(const Vector3& v, const Vector3& omega, const Vector3& a) { + Vector9 xi; + xi << v, omega, a; + return xi; + } + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test identity element properties + */ +TEST_F(SE23Test, Identity) +{ + SE23d id = SE23d::identity(); + + // Test identity components + EXPECT_TRUE(id.pose().rotation().quaternion().isApprox(Quaternion::Identity())); + EXPECT_TRUE(id.pose().translation().isZero()); + EXPECT_TRUE(id.velocity().isZero()); + + // Test identity matrix + EXPECT_TRUE(id.matrix().isIdentity()); + + // Test composition with identity + Vector3 axis = Vector3(1, 1, 1).normalized(); + Vector3 trans(1, 2, 3); + Vector3 vel(0.1, 0.2, 0.3); + SE23d g(SE3d(SO3d(pi/4, axis), trans), vel); + EXPECT_TRUE((g * id).isApprox(g)); + EXPECT_TRUE((id * g).isApprox(g)); +} + +/** + * Test group operation (extended pose composition) + */ +TEST_F(SE23Test, GroupOperation) +{ + // Create transformations with different rotations, translations, and velocities + Vector3 axis1 = Vector3(1, 0, 0); // X-axis + Vector3 axis2 = Vector3(0, 1, 0); // Y-axis + Vector3 t1(1, 0, 0); // X-translation + Vector3 t2(0, 1, 0); // Y-translation + Vector3 v1(0.1, 0, 0); // X-velocity + Vector3 v2(0, 0.2, 0); // Y-velocity + + SE23d g1(SE3d(SO3d(pi/2, axis1), t1), v1); + SE23d g2(SE3d(SO3d(pi/2, axis2), t2), v2); + + // Test composition + SE23d g12 = g1 * g2; + + // Verify pose composition + EXPECT_TRUE(g12.pose().isApprox(g1.pose() * g2.pose())); + + // Verify velocity transformation + Vector3 expected_vel = v1 + g1.pose().rotation().act(v2); + EXPECT_TRUE(g12.velocity().isApprox(expected_vel)); + + // Test non-commutativity + SE23d g21 = g2 * g1; + EXPECT_FALSE(g12.isApprox(g21)); +} + +/** + * Test inverse operation + */ +TEST_F(SE23Test, Inverse) +{ + Vector3 axis = Vector3(1, 1, 0).normalized(); + Vector3 trans(1, 2, 3); + Vector3 vel(0.1, 0.2, 0.3); + SE23d g(SE3d(SO3d(pi/3, axis), trans), vel); + SE23d inv = g.inverse(); + + // Test inverse properties + EXPECT_TRUE((g * inv).isApprox(SE23d::identity())); + EXPECT_TRUE((inv * g).isApprox(SE23d::identity())); + + // Test inverse pose + EXPECT_TRUE(inv.pose().isApprox(g.pose().inverse())); + + // Test inverse velocity + Vector3 expected_vel = -g.pose().rotation().inverse().act(vel); + EXPECT_TRUE(inv.velocity().isApprox(expected_vel)); +} + +/** + * Test exponential and logarithm maps + */ +TEST_F(SE23Test, ExpLog) +{ + // Test exp(log(g)) = g + Vector3 axis = Vector3(1, 2, 3).normalized(); + Vector3 trans(4, 5, 6); + Vector3 vel(0.1, 0.2, 0.3); + SE23d g(SE3d(SO3d(pi/4, axis), trans), vel); + Vector9 xi = g.log(); + SE23d g2 = SE23d().exp(xi); + EXPECT_TRUE(g.isApprox(g2)); + + // Test pure translation + Vector9 xi_trans = twist(Vector3(1, 2, 3), Vector3::Zero(), Vector3::Zero()); + SE23d g_trans = SE23d().exp(xi_trans); + EXPECT_TRUE(g_trans.pose().rotation().isApprox(SO3d::identity())); + EXPECT_TRUE(g_trans.pose().translation().isApprox(Vector3(1, 2, 3))); + EXPECT_TRUE(g_trans.velocity().isZero()); + + // Test pure velocity + Vector9 xi_vel = twist(Vector3::Zero(), Vector3::Zero(), Vector3(0.1, 0.2, 0.3)); + SE23d g_vel = SE23d().exp(xi_vel); + EXPECT_TRUE(g_vel.pose().isApprox(SE3d::identity())); + EXPECT_TRUE(g_vel.velocity().isApprox(Vector3(0.1, 0.2, 0.3))); + + // Test small motion approximation + Vector9 xi_small = twist(Vector3(0.001, 0.001, 0.001), + Vector3(0.001, 0.001, 0.001), + Vector3(0.001, 0.001, 0.001)); + SE23d g_small = SE23d().exp(xi_small); + Matrix4 expected_pose = Matrix4::Identity(); + expected_pose.block<3,3>(0,0) = Matrix3::Identity() + SO3d::hat(xi_small.segment<3>(3)); + expected_pose.block<3,1>(0,3) = xi_small.head<3>(); + EXPECT_TRUE(g_small.pose().matrix().isApprox(expected_pose, 1e-6)); + EXPECT_TRUE(g_small.velocity().isApprox(xi_small.tail<3>(), 1e-6)); +} + +/** + * Test adjoint representation + */ +TEST_F(SE23Test, Adjoint) +{ + Vector3 axis = Vector3(0, 0, 1); // Z-axis + Vector3 trans(1, 2, 0); + Vector3 vel(0.1, 0.2, 0); + SE23d g(SE3d(SO3d(pi/2, axis), trans), vel); + Matrix9 Ad = g.adjoint(); + + // Test adjoint structure + Matrix3 R = g.pose().rotation().matrix(); + Matrix3 t_hat = SO3d::hat(trans); + Matrix3 v_hat = SO3d::hat(vel); + + EXPECT_TRUE(Ad.block<3,3>(0,0).isApprox(R)); + EXPECT_TRUE(Ad.block<3,3>(0,3).isApprox(t_hat * R)); + EXPECT_TRUE(Ad.block<3,3>(0,6).isApprox(v_hat * R)); + EXPECT_TRUE(Ad.block<3,3>(3,0).isZero()); + EXPECT_TRUE(Ad.block<3,3>(3,3).isApprox(R)); + EXPECT_TRUE(Ad.block<3,3>(6,6).isApprox(R)); + + // Test adjoint action + Vector9 xi = twist(Vector3(1, 0, 0), Vector3(0, 0, 1), Vector3(0.1, 0, 0)); + Vector9 xi_transformed = Ad * xi; + + // Verify transformation matches conjugation + SE23d h = SE23d().exp(xi); + SE23d h_transformed = g * h * g.inverse(); + EXPECT_TRUE(h_transformed.isApprox(SE23d().exp(xi_transformed))); +} + +/** + * Test group action on points with velocity + */ +TEST_F(SE23Test, Action) +{ + // 90° rotation around Z-axis + translation in X + velocity + SE23d g(SE3d(SO3d(pi/2, Vector3(0, 0, 1)), Vector3(1, 0, 0)), + Vector3(0.1, 0.2, 0)); + + // Point with velocity + Vector3 p(1, 0, 0); + Vector3 v(0.1, 0, 0); + Vector9 state; + state << p, v, Vector3::Zero(); + + // Test transformation + Vector9 transformed = g.act(state); + Vector3 p_new = transformed.head<3>(); + Vector3 v_new = transformed.segment<3>(3); + + // Check position transformation + EXPECT_NEAR(p_new[0], 1.0, eps); // Original x-translation + point x + EXPECT_NEAR(p_new[1], 1.0, eps); // Rotated point x + EXPECT_NEAR(p_new[2], 0.0, eps); + + // Check velocity transformation + Vector3 expected_vel = g.pose().rotation().act(v) + g.velocity(); + EXPECT_TRUE(v_new.isApprox(expected_vel)); +} + +/** + * Test interpolation + */ +TEST_F(SE23Test, Interpolation) +{ + // Create start and end states + Vector3 axis = Vector3(1, 1, 1).normalized(); + SE23d start = SE23d::identity(); + SE23d end(SE3d(SO3d(pi/2, axis), Vector3(2, 0, 0)), + Vector3(0.2, 0, 0)); + + // Test midpoint interpolation + SE23d mid = interpolate(start, end, 0.5); + + // Verify midpoint properties + EXPECT_TRUE(mid.pose().rotation().isApprox(SO3d(pi/4, axis))); + EXPECT_TRUE(mid.pose().translation().isApprox(Vector3(1, 0, 0))); + EXPECT_TRUE(mid.velocity().isApprox(Vector3(0.1, 0, 0))); + + // Test boundary conditions + EXPECT_TRUE(interpolate(start, end, 0.0).isApprox(start)); + EXPECT_TRUE(interpolate(start, end, 1.0).isApprox(end)); +} + +} // namespace sofa::component::cosserat::liegroups::testing diff --git a/Tests/liegroups/SE3Test.cpp b/Tests/liegroups/SE3Test.cpp new file mode 100644 index 0000000..4c64ea3 --- /dev/null +++ b/Tests/liegroups/SE3Test.cpp @@ -0,0 +1,259 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Test suite for SE3 Lie group implementation + */ +class SE3Test : public BaseTest +{ +protected: + using SE3d = SE3; + using SO3d = SO3; + using Vector3 = Eigen::Vector3d; + using Vector6 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix3d; + using Matrix4 = Eigen::Matrix4d; + using Matrix6 = Eigen::Matrix; + using Quaternion = Eigen::Quaterniond; + + const double pi = M_PI; + const double eps = 1e-10; + + // Helper function to create twist vector + Vector6 twist(const Vector3& v, const Vector3& omega) { + Vector6 xi; + xi << v, omega; + return xi; + } + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test identity element properties + */ +TEST_F(SE3Test, Identity) +{ + SE3d id = SE3d::identity(); + + // Test identity components + EXPECT_TRUE(id.rotation().quaternion().isApprox(Quaternion::Identity())); + EXPECT_TRUE(id.translation().isZero()); + + // Test identity matrix + EXPECT_TRUE(id.matrix().isApprox(Matrix4::Identity())); + + // Test composition with identity + Vector3 axis = Vector3(1, 1, 1).normalized(); + Vector3 trans(1, 2, 3); + SE3d g(SO3d(pi/4, axis), trans); // 45° rotation + translation + EXPECT_TRUE((g * id).isApprox(g)); + EXPECT_TRUE((id * g).isApprox(g)); +} + +/** + * Test group operation (rigid transformation composition) + */ +TEST_F(SE3Test, GroupOperation) +{ + // Create transformations with different rotations and translations + Vector3 axis1 = Vector3(1, 0, 0); // X-axis + Vector3 axis2 = Vector3(0, 1, 0); // Y-axis + Vector3 t1(1, 0, 0); // X-translation + Vector3 t2(0, 1, 0); // Y-translation + + SE3d g1(SO3d(pi/2, axis1), t1); // 90° around X + X-translation + SE3d g2(SO3d(pi/2, axis2), t2); // 90° around Y + Y-translation + + // Test composition + SE3d g12 = g1 * g2; + + // Verify using homogeneous matrices + Matrix4 T1 = g1.matrix(); + Matrix4 T2 = g2.matrix(); + Matrix4 T12 = g12.matrix(); + EXPECT_TRUE((T1 * T2).isApprox(T12)); + + // Test non-commutativity + SE3d g21 = g2 * g1; + EXPECT_FALSE(g12.isApprox(g21)); +} + +/** + * Test inverse operation + */ +TEST_F(SE3Test, Inverse) +{ + Vector3 axis = Vector3(1, 1, 0).normalized(); + Vector3 trans(1, 2, 3); + SE3d g(SO3d(pi/3, axis), trans); // 60° rotation + translation + SE3d inv = g.inverse(); + + // Test inverse properties + EXPECT_TRUE((g * inv).isApprox(SE3d::identity())); + EXPECT_TRUE((inv * g).isApprox(SE3d::identity())); + + // Test matrix inverse + EXPECT_TRUE(inv.matrix().isApprox(g.matrix().inverse())); + + // Test inverse translation + Vector3 expected_trans = -(g.rotation().inverse().act(trans)); + EXPECT_TRUE(inv.translation().isApprox(expected_trans)); +} + +/** + * Test exponential and logarithm maps + */ +TEST_F(SE3Test, ExpLog) +{ + // Test exp(log(g)) = g + Vector3 axis = Vector3(1, 2, 3).normalized(); + Vector3 trans(4, 5, 6); + SE3d g(SO3d(pi/4, axis), trans); + Vector6 xi = g.log(); + SE3d g2 = SE3d().exp(xi); + EXPECT_TRUE(g.isApprox(g2)); + + // Test pure translation + Vector6 xi_trans = twist(Vector3(1, 2, 3), Vector3::Zero()); + SE3d g_trans = SE3d().exp(xi_trans); + EXPECT_TRUE(g_trans.rotation().isApprox(SO3d::identity())); + EXPECT_TRUE(g_trans.translation().isApprox(Vector3(1, 2, 3))); + + // Test pure rotation + Vector6 xi_rot = twist(Vector3::Zero(), Vector3(0, 0, pi/2)); + SE3d g_rot = SE3d().exp(xi_rot); + EXPECT_TRUE(g_rot.translation().isZero()); + EXPECT_TRUE(g_rot.rotation().isApprox(SO3d(pi/2, Vector3(0, 0, 1)))); + + // Test small motion approximation + Vector6 xi_small = twist(Vector3(0.001, 0.001, 0.001), + Vector3(0.001, 0.001, 0.001)); + SE3d g_small = SE3d().exp(xi_small); + Matrix4 expected = Matrix4::Identity(); + expected.block<3,3>(0,0) = Matrix3::Identity() + SO3d::hat(xi_small.tail<3>()); + expected.block<3,1>(0,3) = xi_small.head<3>(); + EXPECT_TRUE(g_small.matrix().isApprox(expected, 1e-6)); +} + +/** + * Test adjoint representation + */ +TEST_F(SE3Test, Adjoint) +{ + Vector3 axis = Vector3(0, 0, 1); // Z-axis + Vector3 trans(1, 2, 0); + SE3d g(SO3d(pi/2, axis), trans); // 90° around Z + translation + Matrix6 Ad = g.adjoint(); + + // Test adjoint structure + Matrix3 R = g.rotation().matrix(); + Matrix3 t_hat = SO3d::hat(trans); + + EXPECT_TRUE(Ad.block<3,3>(0,0).isApprox(R)); + EXPECT_TRUE(Ad.block<3,3>(0,3).isApprox(t_hat * R)); + EXPECT_TRUE(Ad.block<3,3>(3,0).isZero()); + EXPECT_TRUE(Ad.block<3,3>(3,3).isApprox(R)); + + // Test adjoint action on twists + Vector6 xi = twist(Vector3(1, 0, 0), Vector3(0, 0, 1)); + Vector6 xi_transformed = Ad * xi; + + // Verify transformation matches conjugation + SE3d h = SE3d().exp(xi); + SE3d h_transformed = g * h * g.inverse(); + EXPECT_TRUE(h_transformed.isApprox(SE3d().exp(xi_transformed))); +} + +/** + * Test group action on points + */ +TEST_F(SE3Test, Action) +{ + // 90° rotation around Z-axis + translation in X + SE3d g(SO3d(pi/2, Vector3(0, 0, 1)), Vector3(1, 0, 0)); + Vector3 p(1, 0, 0); // Point on x-axis + + // Should map (1,0,0) to (1,1,0) + Vector3 q = g.act(p); + EXPECT_NEAR(q[0], 1.0, eps); + EXPECT_NEAR(q[1], 1.0, eps); + EXPECT_NEAR(q[2], 0.0, eps); + + // Test that action matches homogeneous transformation + Vector4 p_hom; + p_hom << p, 1.0; + Vector4 q_hom = g.matrix() * p_hom; + EXPECT_TRUE(q.isApprox(q_hom.head<3>())); +} + +/** + * Test interpolation + */ +TEST_F(SE3Test, Interpolation) +{ + // Create start and end transformations + Vector3 axis = Vector3(1, 1, 1).normalized(); + SE3d start = SE3d::identity(); + SE3d end(SO3d(pi/2, axis), Vector3(2, 0, 0)); + + // Test midpoint interpolation + SE3d mid = interpolate(start, end, 0.5); + + // Verify midpoint properties + EXPECT_TRUE(mid.rotation().isApprox(SO3d(pi/4, axis))); + EXPECT_TRUE(mid.translation().isApprox(Vector3(1, 0, 0))); + + // Test boundary conditions + EXPECT_TRUE(interpolate(start, end, 0.0).isApprox(start)); + EXPECT_TRUE(interpolate(start, end, 1.0).isApprox(end)); +} + +/** + * Test conversion between different representations + */ +TEST_F(SE3Test, Conversions) +{ + // Create transformation from rotation and translation + Vector3 axis = Vector3(1, 0, 0); // X-axis + double angle = pi/3; // 60° + Vector3 trans(1, 2, 3); + SE3d g(SO3d(angle, axis), trans); + + // Test conversion to/from homogeneous matrix + Matrix4 T = Matrix4::Identity(); + T.block<3,3>(0,0) = Eigen::AngleAxisd(angle, axis).toRotationMatrix(); + T.block<3,1>(0,3) = trans; + + SE3d g2(T); + EXPECT_TRUE(g.isApprox(g2)); + EXPECT_TRUE(g.matrix().isApprox(T)); +} + +} // namespace sofa::component::cosserat::liegroups::testing diff --git a/Tests/liegroups/SGal3Test.cpp b/Tests/liegroups/SGal3Test.cpp new file mode 100644 index 0000000..a4f68e2 --- /dev/null +++ b/Tests/liegroups/SGal3Test.cpp @@ -0,0 +1,268 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Test suite for SGal(3) Lie group implementation + */ +class SGal3Test : public BaseTest +{ +protected: + using SGal3d = SGal3; + using SE3d = SE3; + using SO3d = SO3; + using Vector3 = Eigen::Vector3d; + using Vector10 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix3d; + using Matrix4 = Eigen::Matrix4d; + using Matrix10 = Eigen::Matrix; + using Quaternion = Eigen::Quaterniond; + + const double pi = M_PI; + const double eps = 1e-10; + + // Helper function to create Galilean twist vector + Vector10 twist(const Vector3& v, const Vector3& omega, const Vector3& beta, double tau) { + Vector10 xi; + xi << v, omega, beta, tau; + return xi; + } + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test identity element properties + */ +TEST_F(SGal3Test, Identity) +{ + SGal3d id = SGal3d::identity(); + + // Test identity components + EXPECT_TRUE(id.pose().rotation().quaternion().isApprox(Quaternion::Identity())); + EXPECT_TRUE(id.pose().translation().isZero()); + EXPECT_TRUE(id.velocity().isZero()); + EXPECT_NEAR(id.time(), 0.0, eps); + + // Test composition with identity + Vector3 axis = Vector3(1, 1, 1).normalized(); + Vector3 trans(1, 2, 3); + Vector3 vel(0.1, 0.2, 0.3); + double t = 1.0; + SGal3d g(SE3d(SO3d(pi/4, axis), trans), vel, t); + EXPECT_TRUE((g * id).isApprox(g)); + EXPECT_TRUE((id * g).isApprox(g)); +} + +/** + * Test group operation (Galilean transformation composition) + */ +TEST_F(SGal3Test, GroupOperation) +{ + // Create transformations with different components + Vector3 axis1 = Vector3(1, 0, 0); // X-axis + Vector3 axis2 = Vector3(0, 1, 0); // Y-axis + Vector3 t1(1, 0, 0); // X-translation + Vector3 t2(0, 1, 0); // Y-translation + Vector3 v1(0.1, 0, 0); // X-velocity + Vector3 v2(0, 0.2, 0); // Y-velocity + double time1 = 1.0; + double time2 = 2.0; + + SGal3d g1(SE3d(SO3d(pi/2, axis1), t1), v1, time1); + SGal3d g2(SE3d(SO3d(pi/2, axis2), t2), v2, time2); + + // Test composition + SGal3d g12 = g1 * g2; + + // Verify pose composition + EXPECT_TRUE(g12.pose().isApprox(g1.pose() * g2.pose())); + + // Verify velocity transformation + Vector3 expected_vel = v1 + g1.pose().rotation().act(v2); + EXPECT_TRUE(g12.velocity().isApprox(expected_vel)); + + // Verify time addition + EXPECT_NEAR(g12.time(), time1 + time2, eps); + + // Test non-commutativity + SGal3d g21 = g2 * g1; + EXPECT_FALSE(g12.isApprox(g21)); +} + +/** + * Test inverse operation + */ +TEST_F(SGal3Test, Inverse) +{ + Vector3 axis = Vector3(1, 1, 0).normalized(); + Vector3 trans(1, 2, 3); + Vector3 vel(0.1, 0.2, 0.3); + double time = 1.5; + SGal3d g(SE3d(SO3d(pi/3, axis), trans), vel, time); + SGal3d inv = g.inverse(); + + // Test inverse properties + EXPECT_TRUE((g * inv).isApprox(SGal3d::identity())); + EXPECT_TRUE((inv * g).isApprox(SGal3d::identity())); + + // Test inverse pose + EXPECT_TRUE(inv.pose().isApprox(g.pose().inverse())); + + // Test inverse velocity + Vector3 expected_vel = -g.pose().rotation().inverse().act(vel); + EXPECT_TRUE(inv.velocity().isApprox(expected_vel)); + + // Test inverse time + EXPECT_NEAR(inv.time(), -time, eps); +} + +/** + * Test exponential and logarithm maps + */ +TEST_F(SGal3Test, ExpLog) +{ + // Test exp(log(g)) = g + Vector3 axis = Vector3(1, 2, 3).normalized(); + Vector3 trans(4, 5, 6); + Vector3 vel(0.1, 0.2, 0.3); + double time = 1.0; + SGal3d g(SE3d(SO3d(pi/4, axis), trans), vel, time); + Vector10 xi = g.log(); + SGal3d g2 = SGal3d().exp(xi); + EXPECT_TRUE(g.isApprox(g2)); + + // Test pure translation + Vector10 xi_trans = twist(Vector3(1, 2, 3), Vector3::Zero(), Vector3::Zero(), 0); + SGal3d g_trans = SGal3d().exp(xi_trans); + EXPECT_TRUE(g_trans.pose().rotation().isApprox(SO3d::identity())); + EXPECT_TRUE(g_trans.pose().translation().isApprox(Vector3(1, 2, 3))); + EXPECT_TRUE(g_trans.velocity().isZero()); + EXPECT_NEAR(g_trans.time(), 0.0, eps); + + // Test pure velocity and time + Vector10 xi_vel = twist(Vector3::Zero(), Vector3::Zero(), Vector3(0.1, 0.2, 0.3), 1.0); + SGal3d g_vel = SGal3d().exp(xi_vel); + EXPECT_TRUE(g_vel.pose().isApprox(SE3d::identity())); + EXPECT_TRUE(g_vel.velocity().isApprox(Vector3(0.1, 0.2, 0.3))); + EXPECT_NEAR(g_vel.time(), 1.0, eps); +} + +/** + * Test adjoint representation + */ +TEST_F(SGal3Test, Adjoint) +{ + Vector3 axis = Vector3(0, 0, 1); // Z-axis + Vector3 trans(1, 2, 0); + Vector3 vel(0.1, 0.2, 0); + double time = 1.0; + SGal3d g(SE3d(SO3d(pi/2, axis), trans), vel, time); + Matrix10 Ad = g.adjoint(); + + // Test adjoint structure + Matrix3 R = g.pose().rotation().matrix(); + Matrix3 t_hat = SO3d::hat(trans); + Matrix3 v_hat = SO3d::hat(vel); + + // Verify block structure + EXPECT_TRUE(Ad.block<3,3>(0,0).isApprox(R)); + EXPECT_TRUE(Ad.block<3,3>(0,3).isApprox(t_hat * R)); + EXPECT_TRUE(Ad.block<3,3>(0,6).isApprox(v_hat * R)); + EXPECT_TRUE(Ad.block<3,3>(3,0).isZero()); + EXPECT_TRUE(Ad.block<3,3>(3,3).isApprox(R)); + EXPECT_TRUE(Ad.block<3,3>(6,6).isApprox(R)); + EXPECT_NEAR(Ad(9,9), 1.0, eps); +} + +/** + * Test group action on points with velocity and time + */ +TEST_F(SGal3Test, Action) +{ + // Create a Galilean transformation + SGal3d g(SE3d(SO3d(pi/2, Vector3(0, 0, 1)), Vector3(1, 0, 0)), + Vector3(0.1, 0.2, 0), 1.0); + + // Point with velocity and time + Vector3 p(1, 0, 0); + Vector3 v(0.1, 0, 0); + Vector3 boost(0.01, 0, 0); + double t = 0.5; + Vector10 state; + state << p, v, boost, t; + + // Test transformation + Vector10 transformed = g.act(state); + Vector3 p_new = transformed.head<3>(); + Vector3 v_new = transformed.segment<3>(3); + Vector3 boost_new = transformed.segment<3>(6); + double t_new = transformed[9]; + + // Check position transformation with time evolution + Vector3 expected_pos = g.pose().act(p) + g.velocity() * t; + EXPECT_TRUE(p_new.isApprox(expected_pos)); + + // Check velocity transformation + Vector3 expected_vel = g.pose().rotation().act(v) + g.velocity(); + EXPECT_TRUE(v_new.isApprox(expected_vel)); + + // Check boost transformation + Vector3 expected_boost = g.pose().rotation().act(boost); + EXPECT_TRUE(boost_new.isApprox(expected_boost)); + + // Check time transformation + EXPECT_NEAR(t_new, t + g.time(), eps); +} + +/** + * Test interpolation + */ +TEST_F(SGal3Test, Interpolation) +{ + // Create start and end states + Vector3 axis = Vector3(1, 1, 1).normalized(); + SGal3d start = SGal3d::identity(); + SGal3d end(SE3d(SO3d(pi/2, axis), Vector3(2, 0, 0)), + Vector3(0.2, 0, 0), 2.0); + + // Test midpoint interpolation + SGal3d mid = interpolate(start, end, 0.5); + + // Verify midpoint properties + EXPECT_TRUE(mid.pose().rotation().isApprox(SO3d(pi/4, axis))); + EXPECT_TRUE(mid.pose().translation().isApprox(Vector3(1, 0, 0))); + EXPECT_TRUE(mid.velocity().isApprox(Vector3(0.1, 0, 0))); + EXPECT_NEAR(mid.time(), 1.0, eps); + + // Test boundary conditions + EXPECT_TRUE(interpolate(start, end, 0.0).isApprox(start)); + EXPECT_TRUE(interpolate(start, end, 1.0).isApprox(end)); +} + +} // namespace sofa::component::cosserat::liegroups::testing diff --git a/Tests/liegroups/SO2Test.cpp b/Tests/liegroups/SO2Test.cpp new file mode 100644 index 0000000..9287665 --- /dev/null +++ b/Tests/liegroups/SO2Test.cpp @@ -0,0 +1,170 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Test suite for SO2 Lie group implementation + */ +class SO2Test : public BaseTest +{ +protected: + using SO2d = SO2; + using Vector2 = Eigen::Vector2d; + using Matrix2 = Eigen::Matrix2d; + + const double pi = M_PI; + const double eps = 1e-10; + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test identity element properties + */ +TEST_F(SO2Test, Identity) +{ + SO2d id = SO2d::identity(); + EXPECT_NEAR(id.angle(), 0.0, eps); + + // Test right and left identity + SO2d rot(pi/4); // 45-degree rotation + EXPECT_TRUE((rot * id).isApprox(rot)); + EXPECT_TRUE((id * rot).isApprox(rot)); + + // Test identity matrix + EXPECT_TRUE(id.matrix().isApprox(Matrix2::Identity())); +} + +/** + * Test group operation (rotation composition) + */ +TEST_F(SO2Test, GroupOperation) +{ + SO2d a(pi/4); // 45 degrees + SO2d b(pi/3); // 60 degrees + SO2d c = a * b; // 105 degrees + + EXPECT_NEAR(c.angle(), pi/4 + pi/3, eps); + + // Test that composition matches matrix multiplication + Matrix2 Ra = a.matrix(); + Matrix2 Rb = b.matrix(); + Matrix2 Rc = c.matrix(); + EXPECT_TRUE((Ra * Rb).isApprox(Rc)); +} + +/** + * Test inverse operation + */ +TEST_F(SO2Test, Inverse) +{ + SO2d rot(pi/3); // 60-degree rotation + SO2d inv = rot.inverse(); + + // Test that inverse rotation has opposite angle + EXPECT_NEAR(inv.angle(), -pi/3, eps); + + // Test that rot * inv = inv * rot = identity + EXPECT_TRUE((rot * inv).isApprox(SO2d::identity())); + EXPECT_TRUE((inv * rot).isApprox(SO2d::identity())); + + // Test that inverse matches matrix inverse + EXPECT_TRUE(inv.matrix().isApprox(rot.matrix().inverse())); +} + +/** + * Test exponential and logarithm maps + */ +TEST_F(SO2Test, ExpLog) +{ + // Test exp(log(g)) = g + SO2d rot(pi/6); // 30-degree rotation + auto angle = rot.log(); + auto rot2 = SO2d().exp(angle); + EXPECT_TRUE(rot.isApprox(rot2)); + + // Test log(exp(w)) = w + double w = pi/4; // Angular velocity + auto rot3 = SO2d().exp(Vector2::Constant(w)); + EXPECT_NEAR(rot3.log()[0], w, eps); +} + +/** + * Test adjoint representation + */ +TEST_F(SO2Test, Adjoint) +{ + SO2d rot(pi/4); // 45-degree rotation + + // For SO(2), adjoint should always be identity + EXPECT_TRUE(rot.adjoint().isApprox(Matrix2::Identity())); +} + +/** + * Test group action on points + */ +TEST_F(SO2Test, Action) +{ + SO2d rot(pi/2); // 90-degree rotation + Vector2 p(1.0, 0.0); // Point on x-axis + + // 90-degree rotation should map (1,0) to (0,1) + Vector2 q = rot.act(p); + EXPECT_NEAR(q[0], 0.0, eps); + EXPECT_NEAR(q[1], 1.0, eps); + + // Test that action matches matrix multiplication + EXPECT_TRUE(q.isApprox(rot.matrix() * p)); +} + +/** + * Test angle normalization + */ +TEST_F(SO2Test, AngleNormalization) +{ + // Test that angles are normalized to [-π, π] + SO2d rot1(3*pi/2); // 270 degrees + SO2d rot2(-3*pi/2); // -270 degrees + + EXPECT_NEAR(rot1.angle(), -pi/2, eps); + EXPECT_NEAR(rot2.angle(), pi/2, eps); +} + +/** + * Test interpolation + */ +TEST_F(SO2Test, Interpolation) +{ + SO2d start(0); // 0 degrees + SO2d end(pi/2); // 90 degrees + SO2d mid = slerp(start, end, 0.5); + + // Midpoint should be 45-degree rotation + EXPECT_NEAR(mid.angle(), pi/4, eps); +} + +} // namespace sofa::component::cosserat::liegroups::testing diff --git a/Tests/liegroups/SO3Test.cpp b/Tests/liegroups/SO3Test.cpp new file mode 100644 index 0000000..3ede165 --- /dev/null +++ b/Tests/liegroups/SO3Test.cpp @@ -0,0 +1,248 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Test suite for SO3 Lie group implementation + */ +class SO3Test : public BaseTest +{ +protected: + using SO3d = SO3; + using Vector3 = Eigen::Vector3d; + using Matrix3 = Eigen::Matrix3d; + using Quaternion = Eigen::Quaterniond; + + const double pi = M_PI; + const double eps = 1e-10; + + // Helper function to create rotation vector + Vector3 rotationVector(double angle, const Vector3& axis) { + return axis.normalized() * angle; + } + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test identity element properties + */ +TEST_F(SO3Test, Identity) +{ + SO3d id = SO3d::identity(); + + // Test identity quaternion properties + EXPECT_NEAR(id.quaternion().w(), 1.0, eps); + EXPECT_NEAR(id.quaternion().x(), 0.0, eps); + EXPECT_NEAR(id.quaternion().y(), 0.0, eps); + EXPECT_NEAR(id.quaternion().z(), 0.0, eps); + + // Test identity matrix + EXPECT_TRUE(id.matrix().isApprox(Matrix3::Identity())); + + // Test composition with identity + Vector3 axis = Vector3(1, 1, 1).normalized(); + SO3d rot(pi/4, axis); // 45-degree rotation around (1,1,1) + EXPECT_TRUE((rot * id).isApprox(rot)); + EXPECT_TRUE((id * rot).isApprox(rot)); +} + +/** + * Test group operation (rotation composition) + */ +TEST_F(SO3Test, GroupOperation) +{ + // Create two rotations with different axes + Vector3 axis1 = Vector3(1, 0, 0); // X-axis + Vector3 axis2 = Vector3(0, 1, 0); // Y-axis + SO3d rx(pi/2, axis1); // 90° around X + SO3d ry(pi/2, axis2); // 90° around Y + + // Test composition + SO3d rxy = rx * ry; + + // Verify using matrix multiplication + Matrix3 Rx = rx.matrix(); + Matrix3 Ry = ry.matrix(); + Matrix3 Rxy = rxy.matrix(); + EXPECT_TRUE((Rx * Ry).isApprox(Rxy)); + + // Test non-commutativity + SO3d ryx = ry * rx; + EXPECT_FALSE(rxy.isApprox(ryx)); +} + +/** + * Test inverse operation + */ +TEST_F(SO3Test, Inverse) +{ + Vector3 axis = Vector3(1, 1, 0).normalized(); + SO3d rot(pi/3, axis); // 60° rotation + SO3d inv = rot.inverse(); + + // Test inverse properties + EXPECT_TRUE((rot * inv).isApprox(SO3d::identity())); + EXPECT_TRUE((inv * rot).isApprox(SO3d::identity())); + + // Test matrix inverse + EXPECT_TRUE(inv.matrix().isApprox(rot.matrix().inverse())); + + // Test quaternion conjugate + EXPECT_TRUE(inv.quaternion().coeffs().isApprox(rot.quaternion().conjugate().coeffs())); +} + +/** + * Test exponential and logarithm maps + */ +TEST_F(SO3Test, ExpLog) +{ + // Test exp(log(g)) = g + Vector3 axis = Vector3(1, 2, 3).normalized(); + SO3d rot(pi/4, axis); // 45° rotation + Vector3 omega = rot.log(); + SO3d rot2 = SO3d().exp(omega); + EXPECT_TRUE(rot.isApprox(rot2)); + + // Test small rotation approximation + Vector3 small_omega = Vector3(0.001, 0.001, 0.001); + SO3d small_rot = SO3d().exp(small_omega); + Matrix3 expected = Matrix3::Identity() + SO3d::hat(small_omega); + EXPECT_TRUE(small_rot.matrix().isApprox(expected, 1e-6)); + + // Test rotation vector recovery + Vector3 rot_vec = rotationVector(pi/3, Vector3(1,0,0)); + SO3d g = SO3d().exp(rot_vec); + EXPECT_TRUE(g.log().isApprox(rot_vec)); +} + +/** + * Test adjoint representation + */ +TEST_F(SO3Test, Adjoint) +{ + Vector3 axis = Vector3(0, 0, 1); // Z-axis + SO3d rot(pi/2, axis); // 90° around Z + Matrix3 Ad = rot.adjoint(); + + // Adjoint should be the rotation matrix itself for SO(3) + EXPECT_TRUE(Ad.isApprox(rot.matrix())); + + // Test adjoint action on vectors + Vector3 v(1, 0, 0); + EXPECT_TRUE((Ad * v).isApprox(rot.act(v))); +} + +/** + * Test group action on points + */ +TEST_F(SO3Test, Action) +{ + // 90° rotation around Z-axis + SO3d rot(pi/2, Vector3(0, 0, 1)); + Vector3 p(1, 0, 0); // Point on x-axis + + // Should map (1,0,0) to (0,1,0) + Vector3 q = rot.act(p); + EXPECT_NEAR(q[0], 0.0, eps); + EXPECT_NEAR(q[1], 1.0, eps); + EXPECT_NEAR(q[2], 0.0, eps); + + // Test that action preserves length + EXPECT_NEAR(p.norm(), q.norm(), eps); + + // Test that action matches matrix multiplication + EXPECT_TRUE(q.isApprox(rot.matrix() * p)); +} + +/** + * Test hat and vee operators + */ +TEST_F(SO3Test, HatVee) +{ + Vector3 omega(1, 2, 3); + Matrix3 Omega = SO3d::hat(omega); + + // Test skew-symmetry + EXPECT_TRUE(Omega.transpose().isApprox(-Omega)); + + // Test vee operator (inverse of hat) + EXPECT_TRUE(SO3d::vee(Omega).isApprox(omega)); + + // Test that hat(omega) * v = omega × v + Vector3 v(4, 5, 6); + EXPECT_TRUE((Omega * v).isApprox(omega.cross(v))); +} + +/** + * Test interpolation + */ +TEST_F(SO3Test, Interpolation) +{ + // Create start and end rotations + Vector3 axis = Vector3(1, 1, 1).normalized(); + SO3d start = SO3d::identity(); + SO3d end(pi/2, axis); // 90° rotation + + // Test midpoint interpolation + SO3d mid = interpolate(start, end, 0.5); + + // Midpoint should be 45° rotation around same axis + SO3d expected(pi/4, axis); + EXPECT_TRUE(mid.isApprox(expected)); + + // Test boundary conditions + EXPECT_TRUE(interpolate(start, end, 0.0).isApprox(start)); + EXPECT_TRUE(interpolate(start, end, 1.0).isApprox(end)); +} + +/** + * Test conversion between different rotation representations + */ +TEST_F(SO3Test, Conversions) +{ + // Create rotation from angle-axis + Vector3 axis = Vector3(1, 0, 0); // X-axis + double angle = pi/3; // 60° + SO3d rot(angle, axis); + + // Test conversion to/from quaternion + Quaternion quat(Eigen::AngleAxisd(angle, axis)); + EXPECT_TRUE(rot.quaternion().coeffs().isApprox(quat.coeffs())); + + // Test conversion to/from rotation matrix + Matrix3 R = quat.toRotationMatrix(); + EXPECT_TRUE(rot.matrix().isApprox(R)); + + // Test conversion to/from angle-axis + Eigen::AngleAxisd aa = rot.angleAxis(); + EXPECT_NEAR(aa.angle(), angle, eps); + EXPECT_TRUE(aa.axis().isApprox(axis)); +} + +} // namespace sofa::component::cosserat::liegroups::testing diff --git a/src/Cosserat/CMakeLists.txt b/src/Cosserat/CMakeLists.txt index fb8ef70..ec4c6f8 100644 --- a/src/Cosserat/CMakeLists.txt +++ b/src/Cosserat/CMakeLists.txt @@ -1,71 +1,73 @@ -project(Cosserat.src VERSION 21.06.99) - -set(HEADER_FILES - config.h - - mapping/BaseCosserat.h - mapping/BaseCosserat.inl - mapping/DiscreteCosseratMapping.h - mapping/DiscreteCosseratMapping.inl - mapping/DiscretDynamicCosseratMapping.h - mapping/DiscreteDynamicCosseratMapping.inl - mapping/DifferenceMultiMapping.h - mapping/DifferenceMultiMapping.inl - mapping/RigidDistanceMapping.h - mapping/RigidDistanceMapping.inl - mapping/LegendrePolynomialsMapping.h - mapping/LegendrePolynomialsMapping.inl - - engine/ProjectionEngine.h - engine/ProjectionEngine.inl - +cmake_minimum_required(VERSION 3.12) +project(Cosserat_SRC) +set(HEADER_FILES + config.h.in + fwd.h + types.h + initCosserat.cpp + # Lie group implementations + liegroups/Types.h + liegroups/LieGroupBase.h + liegroups/LieGroupBase.inl + liegroups/RealSpace.h + liegroups/RealSpace.inl + liegroups/SO2.h + liegroups/SO2.inl + liegroups/SE2.h + liegroups/SE2.inl + liegroups/SO3.h + liegroups/SO3.inl + liegroups/SE3.h + liegroups/SE3.inl + liegroups/SE23.h + liegroups/SE23.inl + liegroups/SGal3.h + liegroups/SGal3.inl + liegroups/Bundle.h + liegroups/Bundle.inl + # Existing components + engine/GeometricStiffnessEngine.h + engine/GeometricStiffnessEngine.inl forcefield/BeamHookeLawForceField.h forcefield/BeamHookeLawForceField.inl - forcefield/BeamHookeLawForceFieldRigid.h - forcefield/BeamHookeLawForceFieldRigid.inl - forcefield/CosseratInternalActuation.h - forcefield/CosseratInternalActuation.inl - forcefield/MyUniformVelocityDampingForceField.h - forcefield/MyUniformVelocityDampingForceField.inl - - constraint/CosseratSlidingConstraint.h - constraint/CosseratSlidingConstraint.inl - constraint/QPSlidingConstraint.h - constraint/QPSlidingConstraint.inl - constraint/CosseratNeedleSlidingConstraint.h - constraint/CosseratNeedleSlidingConstraint.inl - ) + forcefield/LinearForceField.h + forcefield/LinearForceField.inl + mapping/AdaptiveBeamMapping.h + mapping/AdaptiveBeamMapping.inl + mapping/CosseratNonLinearMapping2D.h + mapping/CosseratNonLinearMapping2D.inl + mapping/DifferenceMultiMapping.h + mapping/DifferenceMultiMapping.inl + constraint/UnilateralPlaneConstraint.h + constraint/UnilateralPlaneConstraint.inl +) set(SOURCE_FILES initCosserat.cpp - - mapping/BaseCosserat.cpp - mapping/DiscreteCosseratMapping.cpp - mapping/DiscreteDynamicCosseratMapping.cpp - mapping/DifferenceMultiMapping.cpp - mapping/RigidDistanceMapping.cpp - mapping/LegendrePolynomialsMapping.cpp - - engine/ProjectionEngine.cpp - + engine/GeometricStiffnessEngine.cpp forcefield/BeamHookeLawForceField.cpp - forcefield/BeamHookeLawForceFieldRigid.cpp - forcefield/CosseratInternalActuation.cpp - forcefield/MyUniformVelocityDampingForceField.cpp - - constraint/CosseratSlidingConstraint.cpp - constraint/QPSlidingConstraint.cpp - constraint/CosseratNeedleSlidingConstraint.cpp - ) - -# add_subdirectory(Binding) + forcefield/LinearForceField.cpp + mapping/AdaptiveBeamMapping.cpp + mapping/CosseratNonLinearMapping2D.cpp + mapping/DifferenceMultiMapping.cpp + constraint/UnilateralPlaneConstraint.cpp +) -add_executable(${PROJECT_NAME} ${SOURCE_FILES}) +find_package(SofaFramework REQUIRED) +find_package(SofaBase REQUIRED) +find_package(SofaOpenglVisual REQUIRED) +find_package(Geometric_stiffness REQUIRED) +find_package(Eigen3 REQUIRED) -target_include_directories(${PROJECT_NAME} PUBLIC - "$") +add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES}) +target_link_libraries(${PROJECT_NAME} PUBLIC + SofaBase + SofaOpenglVisual + Geometric_stiffness +) +target_include_directories(${PROJECT_NAME} PUBLIC "$") +target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR}) -target_link_libraries(${PROJECT_NAME} SofaTest SofaGTestMain SofaCore - SofaConstraint SofaBaseMechanics SofaUserInteraction) +set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DSOFA_BUILD_COSSERAT") diff --git a/src/Cosserat/constraint/CosseratActuatorConstraint.h b/src/Cosserat/constraint/CosseratActuatorConstraint.h index 7513e07..b279452 100644 --- a/src/Cosserat/constraint/CosseratActuatorConstraint.h +++ b/src/Cosserat/constraint/CosseratActuatorConstraint.h @@ -176,6 +176,7 @@ class CosseratActuatorConstraint : public CableModel using CableModel::d_componentState ; using CableModel::m_nbLines ; using CableModel::d_constraintIndex ; + using CableModel::m_state ; using CableModel::d_indices ; using CableModel::d_pullPoint; diff --git a/src/Cosserat/forcefield/BaseBeamHookeLawForceField.h b/src/Cosserat/forcefield/BaseBeamHookeLawForceField.h new file mode 100644 index 0000000..b6ffbfa --- /dev/null +++ b/src/Cosserat/forcefield/BaseBeamHookeLawForceField.h @@ -0,0 +1,346 @@ +/****************************************************************************** +* 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/ * +* * +******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace sofa::component::forcefield +{ + +/** + * @brief Base class for beam force fields using Hooke's law + * + * This force field simulates the mechanical behavior of beam elements using Hooke's law. + * It provides core functionality for both uniform sections (same properties throughout the beam) + * and variant sections (different properties for different segments). + * + * The class handles both circular and rectangular cross-sections, and supports + * computation of beam properties either from material properties (Young's modulus, + * Poisson ratio) or from directly specified inertia parameters. + */ +template +class BaseBeamHookeLawForceField : public sofa::core::behavior::ForceField +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(BaseBeamHookeLawForceField, DataTypes), SOFA_TEMPLATE(sofa::core::behavior::ForceField, DataTypes)); + + typedef sofa::core::behavior::ForceField Inherit1; + typedef typename DataTypes::Real Real; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef sofa::core::behavior::MultiMatrixAccessor MultiMatrixAccessor; + typedef sofa::linearalgebra::BaseMatrix BaseMatrix; + typedef sofa::type::vector Vector; + + // For task scheduling in multi-threaded computation + typedef sofa::simulation::TaskScheduler::Task Task; + + // Matrix types for stiffness representation + typedef sofa::type::Mat<3, 3, Real> Mat33; + typedef sofa::type::Mat<6, 6, Real> Mat66; + typedef sofa::type::vector VecMat33; + typedef sofa::type::vector VecMat66; + +protected: + // Default constructor + BaseBeamHookeLawForceField(); + + // Destructor + virtual ~BaseBeamHookeLawForceField(); + + // Cross-section inertia values + Real Iy, Iz, J; + + // Stiffness matrices + Mat33 m_K_section; // 3x3 stiffness matrix for rotational DOFs + Mat66 m_K_section66; // 6x6 stiffness matrix for rotational and translational DOFs + VecMat33 m_K_sectionList; // List of stiffness matrices for variant sections + VecMat66 m_K_section66List; // List of 6x6 stiffness matrices for variant sections + + // Flag to track whether to compute derivative forces + bool compute_df; + + // Flag to track initialization state + bool m_initialized; + + // Cross-section area + Real m_crossSectionArea; + +public: + ////////////////////////////////////////////////////////////////////////// + // DATA MEMBERS + ////////////////////////////////////////////////////////////////////////// + + /** + * @brief Shape of the cross-section + * Can be "circular" (tube with external radius being radius and internal radius + * being innerRadius) or "rectangular" (lengthY and lengthZ) + */ + Data d_crossSectionShape; + + /// Young's modulus - describes the stiffness of the material + Data d_youngModulus; + + /// Poisson's ratio - describes the compressibility of the material + Data d_poissonRatio; + + /// List of lengths of the different beam's sections + Data d_length; + + /// External radius of the cross section (if circular) + Data d_radius; + + /// Internal radius of the cross section (if circular) + Data d_innerRadius; + + /// Side length of the cross section along local y axis (if rectangular) + Data d_lengthY; + + /// Side length of the cross section along local z axis (if rectangular) + Data d_lengthZ; + + /// Set to true if we have variant beam sections + Data d_variantSections; + + /// List of Young's modulus values for variant sections + Data d_youngModulusList; + + /// List of Poisson's ratio values for variant sections + Data d_poissonRatioList; + + /// Set to true to use provided inertia parameters instead of Young's modulus and Poisson ratio + Data d_useInertiaParams; + + /// The inertia parameter GI (torsional rigidity) + Data d_GI; + + /// The inertia parameter GA (shear rigidity) + Data d_GA; + + /// The inertia parameter EA (axial rigidity) + Data d_EA; + + /// The inertia parameter EI (bending rigidity) + Data d_EI; + + /// The inertia parameter EIy (bending rigidity around y-axis) + Data d_EIy; + + /// The inertia parameter EIz (bending rigidity around z-axis) + Data d_EIz; + + /// Enable parallel processing for force computation + Data d_useMultiThreading; + + ////////////////////////////////////////////////////////////////////////// + // METHODS + ////////////////////////////////////////////////////////////////////////// + + /** + * @brief Initializes the force field + * + * This method is called during initialization to set up the force field parameters. + * It validates input data and computes cross-section properties. + */ + virtual void init() override; + + /** + * @brief Reinitializes the force field + * + * Recalculates cross-section properties and stiffness matrices based on current parameters. + */ + virtual void reinit() override; + + /** + * @brief Validates input data for force computation + * + * Checks that the input data is valid for force computation, including: + * - The mechanical state exists + * - Position vectors are not empty and have matching sizes + * - The length vector has the correct size + * - For variant sections, the section lists have the correct sizes + * + * @param f Force vector + * @param x Position vector + * @param x0 Rest position vector + * @return true if data is valid, false otherwise + */ + virtual bool validateInputData(const DataVecDeriv& f, const DataVecCoord& x, const DataVecCoord& x0) const; + + /** + * @brief Validates material parameters + * + * Ensures that Young's modulus and Poisson ratio have physically valid values. + * + * @return true if parameters are valid, false otherwise + */ + virtual bool validateMaterialParameters() const; + + /** + * @brief Validates cross-section parameters + * + * Ensures that cross-section dimensions are physically valid. + * + * @return true if parameters are valid, false otherwise + */ + virtual bool validateSectionParameters() const; + + /** + * @brief Computes cross-section properties + * + * Calculates moment of inertia, cross-section area, and stiffness matrices + * based on the cross-section shape and material properties. + */ + virtual void computeSectionProperties(); + + /** + * @brief Adds forces for uniform section beams + * + * Computes and adds forces to the force vector for beams with uniform cross-section + * properties. Supports both single-threaded and multi-threaded computation. + * + * @param f Force vector + * @param x Position vector + * @param x0 Rest position vector + * @param lengths Vector of beam section lengths + */ + virtual void addForceUniformSection(DataVecDeriv& f, const DataVecCoord& x, const DataVecCoord& x0, + const type::vector& lengths); + + /** + * @brief Adds forces for variant section beams + * + * Computes and adds forces to the force vector for beams with varying cross-section + * properties. Supports both single-threaded and multi-threaded computation. + * + * @param f Force vector + * @param x Position vector + * @param x0 Rest position vector + * @param lengths Vector of beam section lengths + */ + virtual void addForceVariantSection(DataVecDeriv& f, const DataVecCoord& x, const DataVecCoord& x0, + const type::vector& lengths); + + /** + * @brief Main method for force computation + * + * This method dispatches the force computation to either addForceUniformSection or + * addForceVariantSection based on the beam configuration. + * + * @param mparams Mechanical parameters + * @param f Force vector + * @param x Position vector + * @param v Velocity vector + */ + virtual void addForce(const sofa::core::MechanicalParams* mparams, DataVecDeriv& f, + const DataVecCoord& x, const DataVecDeriv& v) override; + + /** + * @brief Computes the product of the stiffness matrix and a displacement vector + * + * This method is used for implicit integration schemes to compute the change in force + * due to a displacement. + * + * @param mparams Mechanical parameters + * @param df Differential force vector + * @param dx Differential displacement vector + */ + virtual void addDForce(const sofa::core::MechanicalParams* mparams, DataVecDeriv& df, + const DataVecDeriv& dx) override; + + /** + * @brief Adds the stiffness matrix contribution to the global matrix + * + * This method is called during the assembly of the global stiffness matrix to add + * the contribution of this force field. + * + * @param mparams Mechanical parameters + * @param matrix Multi-matrix accessor + */ + virtual void addKToMatrix(const sofa::core::MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) override; + + /** + * @brief Template implementation to add stiffness to matrix + * + * Implementation details for adding stiffness to the global matrix. + * This is specialized in derived classes for different data types. + * + * @param mat Base matrix + * @param offset Matrix offset + * @param pos Position vector + * @param lengths Vector of beam section lengths + * @param kFact Stiffness factor + */ + template + void addKToMatrixImpl(MatrixType* mat, unsigned int offset, const VecCoord& pos, + const Vector& lengths, Real kFact); + + /** + * @brief Computes the potential energy of the beam + * + * This method returns the strain energy stored in the beam due to deformation. + * + * @param mparams Mechanical parameters + * @param x Position vector + * @return Potential energy + */ + virtual double getPotentialEnergy(const sofa::core::MechanicalParams* mparams, + const DataVecCoord& x) const override; + + /** + * @brief Returns the external radius of the beam + * + * Utility method to access the beam's external radius. + * + * @return The external radius of the beam + */ + virtual Real getRadius(); +}; + +#if !defined(SOFA_COMPONENT_FORCEFIELD_BASEBEAMHOOKELAWFORCEFIELD_CPP) +extern template class SOFA_COSSERAT_API BaseBeamHookeLawForceField; +extern template class SOFA_COSSERAT_API BaseBeamHookeLawForceField; +#endif + +} // namespace sofa::component::forcefield + diff --git a/src/Cosserat/forcefield/BaseBeamHookeLawForceField.inl b/src/Cosserat/forcefield/BaseBeamHookeLawForceField.inl new file mode 100644 index 0000000..7fa284e --- /dev/null +++ b/src/Cosserat/forcefield/BaseBeamHookeLawForceField.inl @@ -0,0 +1,1047 @@ +/****************************************************************************** +* 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/ * +* * +******************************************************************************/ +#pragma once +#include + +#include +#include +#include +#include + +// Standard includes +#include +#include +#include + +// Using declarations for common types +using sofa::core::behavior::MechanicalState; +using sofa::core::objectmodel::BaseContext; +using sofa::helper::ReadAccessor; +using sofa::helper::WriteAccessor; +using sofa::core::VecCoordId; +using std::cout; +using std::endl; + +namespace sofa::component::forcefield +{ + +using sofa::core::behavior::MultiMatrixAccessor; +using sofa::core::behavior::BaseMechanicalState; +using sofa::helper::WriteAccessor; +/** + * @brief Get the current frame transformation for a specific cross-section + * + * @param index Cross-section index + * @return Rigid transformation (SE3) representing the cross-section frame + */ +template +typename BaseBeamHookeLawForceField::SE3Type +BaseBeamHookeLawForceField::getFrame(size_t index) const +{ + assert(index < m_currentFrames.size()); + return m_currentFrames[index]; +} + +/** + * @brief Get the reference frame transformation for a specific cross-section + * + * @param index Cross-section index + * @return Rigid transformation (SE3) representing the reference cross-section frame + */ +template +typename BaseBeamHookeLawForceField::SE3Type +BaseBeamHookeLawForceField::getReferenceFrame(size_t index) const +{ + assert(index < m_referenceFrames.size()); + return m_referenceFrames[index]; +} + +/** + * @brief Get the relative rotation between consecutive cross-sections + * + * @param index Segment index (between cross-sections index and index+1) + * @return Rotation (SO3) representing the relative orientation + */ +template +typename BaseBeamHookeLawForceField::SO3Type +BaseBeamHookeLawForceField::getRelativeRotation(size_t index) const +{ + assert(index < m_relativeRotations.size()); + return m_relativeRotations[index]; +} + +} // namespace sofa::component::forcefield + * (different properties for different segments). + */ + +/****************************************************************************** +* CONSTRUCTORS / DESTRUCTORS * +******************************************************************************/ +ttemplate +void BaseBeamHookeLawForceField::init() +{ + Inherit1::init(); + if (!validateMaterialParameters()) { + msg_error("BaseBeamHookeLawForceField") << "Invalid material parameters detected during initialization."; + return; + } + + if (!validateSectionParameters()) { + msg_error("BaseBeamHookeLawForceField") << "Invalid cross-section parameters detected during initialization."; + return; + } + + computeSectionProperties(); + + // Initialize reference frames + const VecCoord& x0 = this->mstate->read(sofa::core::vec_id::read_access::restPosition)->getValue(); + const size_t numNodes = x0.size(); + + m_referenceFrames.resize(numNodes); + m_currentFrames.resize(numNodes); + m_relativeRotations.resize(numNodes > 0 ? numNodes - 1 : 0); + + // Set up initial reference frames + for (size_t i = 0; i < numNodes; ++i) { + // Extract position and rotation from x0[i] + Vector3 position = getPosition(x0[i]); + SO3Type rotation = getRotation(x0[i]); + + // Create an SE3 transformation + m_referenceFrames[i] = SE3Type(rotation, position); + m_currentFrames[i] = m_referenceFrames[i]; // Initially, current frames match reference frames + } + + // Compute initial relative rotations + computeRelativeRotations(); + + m_initialized = true; +} +// For variant section + localForce = -(m_K_sectionList[i] * strains[i]) * lengths[i]; + } + + // Transform to global frame and apply to nodes + Deriv globalForce = transformWrenchToGlobal(localForce, i); + + // Apply forces to both nodes of the segment + forces[i] += globalForce; + forces[i+1] -= globalForce; + } + + d_f.endEdit(); + } + else { + // Call the original specialized force calculation method + if (!d_variantSections.getValue()) { + addForceUniformSection(d_f, d_x, d_x0, lengths); + } else { + addForceVariantSection(d_f, d_x, d_x0, lengths); + } + } +} +* INITIALIZATION METHODS * +******************************************************************************/ + +/** + * @brief Initialize the force field, setting up parameters and cross-section properties + * + * This method is called during initialization to set up the force field. + * It validates parameters and calculates cross-section properties. + */ +template +void BaseBeamHookeLawForceField::init() +{ + Inherit1::init(); + if (!validateMaterialParameters()) { + msg_error("BaseBeamHookeLawForceField") << "Invalid material parameters detected during initialization."; + return; + } + + if (!validateSectionParameters()) { + msg_error("BaseBeamHookeLawForceField") << "Invalid cross-section parameters detected during initialization."; + return; + } + + computeSectionProperties(); + m_initialized = true; +} +/** + * @brief Main method for force computation + * + * This method dispatches the force computation to either addForceUniformSection or + * addForceVariantSection based on the beam configuration. + * + * @param mparams Mechanical parameters + * @param d_f Force vector + * @param d_x Position vector + * @param d_v Velocity vector (unused) + */ + +/** + * @brief Validates input data for force computation + * + * Checks that the input data is valid for force computation, including: + * - The mechanical state exists + * - Position vectors are not empty and have matching sizes + * - The length vector has the correct size + * - For variant sections, the section lists have the correct sizes + * + * @param d_f Force vector + * @param d_x Position vector + * @param d_x0 Rest position vector + * @return true if data is valid, false otherwise + */ +template +bool BaseBeamHookeLawForceField::validateInputData(const DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecCoord& d_x0) const +{ + if (!this->getMState()) { + msg_info("BaseBeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; + return false; + } + + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = d_x0.getValue(); + + if (x.empty() || x0.empty()) { + msg_warning("BaseBeamHookeLawForceField") << "Empty input vectors, no force will be computed" << "\n"; + return false; + } + + if (x.size() != x0.size()) { + msg_warning("BaseBeamHookeLawForceField") << "Position vector size (" << x.size() + << ") doesn't match rest position size (" << x0.size() << ")" << "\n"; + return false; + } + + unsigned int sz = d_length.getValue().size(); + if (x.size() != sz) { + msg_warning("BaseBeamHookeLawForceField") << "Length vector size (" << sz + << ") should have the same size as position vector (" << x.size() << ")" << "\n"; + return false; + } + + if (d_variantSections.getValue()) { + if (d_youngModulusList.getValue().size() != x.size()) { + msg_warning("BaseBeamHookeLawForceField") << "Using variant sections but youngModulusList size (" + << d_youngModulusList.getValue().size() + << ") doesn't match position vector size (" << x.size() << ")" << "\n"; + return false; + } + + if (d_poissonRatioList.getValue().size() != x.size()) { + msg_warning("BaseBeamHookeLawForceField") << "Using variant sections but poissonRatioList size (" + << d_poissonRatioList.getValue().size() + << ") doesn't match position vector size (" << x.size() << ")" << "\n"; + return false; + } + + if (m_K_sectionList.size() != x.size()) { + msg_warning("BaseBeamHookeLawForceField") << "Using variant sections but section list size (" + << m_K_sectionList.size() + << ") doesn't match position vector size (" << x.size() << ")" << "\n"; + return false; + } + } + + return true; +} + +/** + * @brief Validates material parameters + * + * Ensures that Young's modulus and Poisson ratio have physically valid values. + * + * @return true if parameters are valid, false otherwise + */ +template +bool BaseBeamHookeLawForceField::validateMaterialParameters() const +{ + // Check Young's modulus (must be positive) + if (d_youngModulus.getValue() <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "Young's modulus must be positive: " << d_youngModulus.getValue(); + return false; + } + + // Check Poisson ratio (theoretical limits: -1.0 < nu < 0.5) + if (d_poissonRatio.getValue() <= -1.0 || d_poissonRatio.getValue() >= 0.5) { + msg_error("BaseBeamHookeLawForceField") << "Poisson ratio must be in range (-1.0, 0.5): " << d_poissonRatio.getValue(); + return false; + } + + // For variant sections, check all values + if (d_variantSections.getValue()) { + const Vector& youngModulusList = d_youngModulusList.getValue(); + const Vector& poissonRatioList = d_poissonRatioList.getValue(); + + for (size_t i = 0; i < youngModulusList.size(); ++i) { + if (youngModulusList[i] <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "Young's modulus in list at index " << i << " must be positive: " << youngModulusList[i]; + return false; + } + } + + for (size_t i = 0; i < poissonRatioList.size(); ++i) { + if (poissonRatioList[i] <= -1.0 || poissonRatioList[i] >= 0.5) { + msg_error("BaseBeamHookeLawForceField") << "Poisson ratio in list at index " << i << " must be in range (-1.0, 0.5): " << poissonRatioList[i]; + return false; + } + } + } + + // If using inertia parameters directly, validate them + if (d_useInertiaParams.getValue()) { + // Only check these if they're actually provided (not default initialized) + if (d_EIy.isSet() && d_EIy.getValue() <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "EIy parameter must be positive: " << d_EIy.getValue(); + return false; + } + + if (d_EIz.isSet() && d_EIz.getValue() <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "EIz parameter must be positive: " << d_EIz.getValue(); + return false; + } + } + + return true; +} + +/** + * @brief Validates cross-section parameters + * + * Ensures that cross-section dimensions are physically valid. + * + * @return true if parameters are valid, false otherwise + */ +template +bool BaseBeamHookeLawForceField::validateSectionParameters() const +{ + // Get cross-section shape + const std::string& shape = d_crossSectionShape.getValue().getSelectedItem(); + + if (shape == "circular") { + // Check radius (must be positive) + if (d_radius.getValue() <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "External radius must be positive: " << d_radius.getValue(); + return false; + } + + // Check inner radius (must be non-negative and less than external radius) + if (d_innerRadius.getValue() < 0.0) { + msg_error("BaseBeamHookeLawForceField") << "Inner radius cannot be negative: " << d_innerRadius.getValue(); + return false; + } + + if (d_innerRadius.getValue() >= d_radius.getValue()) { + msg_error("BaseBeamHookeLawForceField") << "Inner radius (" << d_innerRadius.getValue() + << ") must be less than external radius (" << d_radius.getValue() << ")"; + return false; + } + } + else if (shape == "rectangular") { + // Check rectangular dimensions (must be positive) + if (d_lengthY.getValue() <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "Side length Y must be positive: " << d_lengthY.getValue(); + return false; + } + + if (d_lengthZ.getValue() <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "Side length Z must be positive: " << d_lengthZ.getValue(); + return false; + } + } + else { + msg_error("BaseBeamHookeLawForceField") << "Unknown cross-section shape: " << shape; + return false; + } + + // Check section lengths + const Vector& lengths = d_length.getValue(); + for (size_t i = 0; i < lengths.size(); ++i) { + if (lengths[i] <= 0.0) { + msg_error("BaseBeamHookeLawForceField") << "Section length at index " << i << " must be positive: " << lengths[i]; + return false; + } + } + + return true; +} + +/** + * @brief Computes cross-section properties + * + * Calculates moment of inertia, cross-section area, and stiffness matrices + * based on the cross-section shape and material properties. + */ +template +void BaseBeamHookeLawForceField::computeSectionProperties() +{ + // Initialize matrices to zero + m_K_section66.fill(0); + m_K_sectionList.clear(); + m_K_section66List.clear(); + + // Calculate moments of inertia based on cross-section shape + if (d_crossSectionShape.getValue().getSelectedItem() == "rectangular") { + // Rectangular cross-section + const Real Ly = d_lengthY.getValue(); + const Real Lz = d_lengthZ.getValue(); + + const Real LyLzLzLz = Ly * Lz * Lz * Lz; + const Real LzLyLyLy = Lz * Ly * Ly * Ly; + + Iy = LyLzLzLz / 12.0; + Iz = LzLyLyLy / 12.0; + J = Iy + Iz; + m_crossSectionArea = Ly * Lz; + } + else { + // Circular cross-section + const Real r = d_radius.getValue(); + const Real rInner = d_innerRadius.getValue(); + const Real r4 = r * r * r * r; + const Real rInner4 = rInner * rInner * rInner * rInner; + + Iy = M_PI * (r4 - rInner4) / 4.0; + Iz = Iy; + J = Iy + Iz; + m_crossSectionArea = M_PI * (r * r - rInner * rInner); + } + + // Calculate stiffness matrices + if (!d_variantSections.getValue()) { + // Uniform section + if (!d_useInertiaParams.getValue()) { + // Calculate from material properties + const Real E = d_youngModulus.getValue(); + const Real G = E / (2.0 * (1.0 + d_poissonRatio.getValue())); + + // 3x3 stiffness matrix for rotational DOFs + m_K_section[0][0] = G * J; + m_K_section[1][1] = E * Iy; + m_K_section[2][2] = E * Iz; + + // 6x6 stiffness matrix if needed for more complex beam formulations + if (DataTypes::spatial_dimensions > 3) { + m_K_section66[0][0] = E * m_crossSectionArea; // Axial stiffness + m_K_section66[1][1] = G * m_crossSectionArea; // Shear stiffness y + m_K_section66[2][2] = G * m_crossSectionArea; // Shear stiffness z + m_K_section66[3][3] = G * J; // Torsional stiffness + m_K_section66[4][4] = E * Iy; // Bending stiffness y + m_K_section66[5][5] = E * Iz; // Bending stiffness z + } + } + else { + // Use user-provided inertia parameters + msg_info("BaseBeamHookeLawForceField") << "Using pre-calculated inertia parameters for stiffness matrix."; + + // 3x3 stiffness matrix + m_K_section[0][0] = d_GI.getValue(); + + // Use specific EIy/EIz if provided, otherwise use general EI + if (d_EIy.isSet()) { + m_K_section[1][1] = d_EIy.getValue(); + } else { + m_K_section[1][1] = d_EI.getValue(); + } + + if (d_EIz.isSet()) { + m_K_section[2][2] = d_EIz.getValue(); + } else { + m_K_section[2][2] = d_EI.getValue(); + } + + // 6x6 stiffness matrix if needed + if (DataTypes::spatial_dimensions > 3) { + m_K_section66[0][0] = d_EA.getValue(); // Axial stiffness + m_K_section66[1][1] = d_GA.getValue(); // Shear stiffness y + m_K_section66[2][2] = d_GA.getValue(); // Shear stiffness z + m_K_section66[3][3] = d_GI.getValue(); // Torsional stiffness + + // Use specific EIy/EIz if provided, otherwise use general EI + if (d_EIy.isSet()) { + m_K_section66[4][4] = d_EIy.getValue(); + } else { + m_K_section66[4][4] = d_EI.getValue(); + } + + if (d_EIz.isSet()) { + m_K_section66[5][5] = d_EIz.getValue(); + } else { + m_K_section66[5][5] = d_EI.getValue(); + } + } + } + } + else { + // Variant sections + msg_info("BaseBeamHookeLawForceField") << "Computing properties for variant beam sections."; + + const auto szL = d_length.getValue().size(); + if ((szL != d_poissonRatioList.getValue().size()) || (szL != d_youngModulusList.getValue().size())) { + msg_error("BaseBeamHookeLawForceField") << "The size of data 'length', 'youngModulusList', and 'poissonRatioList' should be the same!"; + return; + } + + for (auto k = 0; k < szL; k++) { + // 3x3 stiffness matrix for each section + Mat33 _m_K_section; + const Real E = d_youngModulusList.getValue()[k]; + const Real G = E / (2.0 * (1.0 + d_poissonRatioList.getValue()[k])); + + _m_K_section[0][0] = G * J; + _m_K_section[1][1] = E * Iy; + _m_K_section[2][2] = E * Iz; + m_K_sectionList.push_back(_m_K_section); + + // 6x6 stiffness matrix if needed + if (DataTypes::spatial_dimensions > 3) { + Mat66 _m_K_section66; + _m_K_section66[0][0] = E * m_crossSectionArea; // Axial stiffness + _m_K_section66[1][1] = G * m_crossSectionArea; // Shear stiffness y + _m_K_section66[2][2] = G * m_crossSectionArea; // Shear stiffness z + _m_K_section66[3][3] = G * J; // Torsional stiffness + _m_K_section66[4][4] = E * Iy; // Bending stiffness y + _m_K_section66[5][5] = E * Iz; // Bending stiffness z + m_K_section66List.push_back(_m_K_section66); + } + } + + if (d_useInertiaParams.getValue()) { + msg_warning("BaseBeamHookeLawForceField") << "Using variant sections with pre-calculated inertia parameters is not yet supported."; + } + } +} + +/****************************************************************************** +* FORCE COMPUTATION METHODS * +******************************************************************************/ + +/** + * @brief Adds forces for uniform section beams + * + * Computes and adds forces to the force vector for beams with uniform cross-section + * properties. Supports both single-threaded and multi-threaded computation. + * + * @param d_f Force vector + * @param d_x Position vector + * @param d_x0 Rest position vector + * @param lengths Vector of beam section lengths + */ +template +void BaseBeamHookeLawForceField::addForceUniformSection(DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecCoord& d_x0, + const type::vector& lengths) +{ + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = d_x0.getValue(); + const size_t size = x.size(); + sofa::helper::ScopedAdvancedTimer timer("UniformSection"); + + if (d_useMultiThreading.getValue() && size > 1) { + sofa::simulation::TaskScheduler::Task::Status status; + sofa::simulation::TaskScheduler& scheduler = *(sofa::simulation::TaskScheduler::getInstance()); + // Define a lambda for parallel execution + auto calcForce = [this, &f, &x, &x0, &lengths](size_t start, size_t end) { + for (size_t i = start; i < end; ++i) { + f[i] -= (m_K_section * (x[i] - x0[i])) * lengths[i]; + } + }; + + // Determine chunk size for parallel processing + const size_t chunk_size = std::max(1, size / (scheduler.getThreadCount() * 2)); + size_t start = 0; + + // Create and queue tasks for parallel execution + while (start < size) { + size_t end = std::min(start + chunk_size, size); + Task* task = new Task(std::bind(calcForce, start, end), &status); + scheduler.addTask(task); + start = end; + } + + // Wait for all tasks to complete + scheduler.workUntilDone(&status); + } + else { + // Single-threaded fallback + for (size_t i = 0; i < size; ++i) { + f[i] -= (m_K_section * (x[i] - x0[i])) * lengths[i]; + } + } + + d_f.endEdit(); +} +/** + * @brief Adds forces for variant section beams + * + * Computes and adds forces to the force vector for beams with varying cross-section + * properties. Supports both single-threaded and multi-threaded computation. + * + * @param d_f Force vector + * @param d_x Position vector + * @param d_x0 Rest position vector + * @param lengths Vector of beam section lengths + */ +template +void BaseBeamHookeLawForceField::addForceVariantSection(DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecCoord& d_x0, + const type::vector& lengths) +{ + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = d_x0.getValue(); + const size_t size = x.size(); + sofa::helper::ScopedAdvancedTimer timer("VariantSection"); + + if (d_useMultiThreading.getValue() && size > 1) { + sofa::simulation::TaskScheduler::Task::Status status; + sofa::simulation::TaskScheduler& scheduler = *(sofa::simulation::TaskScheduler::getInstance()); + + // Define a lambda for parallel execution + auto calcForce = [this, &f, &x, &x0, &lengths](size_t start, size_t end) { + for (size_t i = start; i < end; ++i) { + f[i] -= (m_K_sectionList[i] * (x[i] - x0[i])) * lengths[i]; + } + }; + + // Determine chunk size for parallel processing + const size_t chunk_size = std::max(1, size / (scheduler.getThreadCount() * 2)); + size_t start = 0; + + // Create and queue tasks for parallel execution + while (start < size) { + size_t end = std::min(start + chunk_size, size); + Task* task = new Task(std::bind(calcForce, start, end), &status); + scheduler.addTask(task); + start = end; + } + + // Wait for all tasks to complete + scheduler.workUntilDone(&status); + } + else { + // Single-threaded fallback + for (size_t i = 0; i < size; ++i) { + f[i] -= (m_K_sectionList[i] * (x[i] - x0[i])) * lengths[i]; + } + } + + d_f.endEdit(); +} +template +void BaseBeamHookeLawForceField::addForce(const sofa::core::MechanicalParams* mparams, + DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecDeriv& d_v) +{ + SOFA_UNUSED(d_v); + SOFA_UNUSED(mparams); + + sofa::helper::ScopedAdvancedTimer timer("BaseBeamHookeLawForceField::addForce"); + + // Get rest position + const DataVecCoord& d_x0 = *this->mstate->read(sofa::core::vec_id::read_access::restPosition); + + // Validate input data + if (!validateInputData(d_f, d_x, d_x0)) { + compute_df = false; + return; + } + + // Prepare force vector + VecDeriv& f = *d_f.beginEdit(); + f.resize(d_x.getValue().size()); + d_f.endEdit(); + + const type::vector& lengths = d_length.getValue(); + + // Call the appropriate specialized force calculation method + if (!d_variantSections.getValue()) { + addForceUniformSection(d_f, d_x, d_x0, lengths); + } else { + addForceVariantSection(d_f, d_x, d_x0, lengths); + } +} + +/** + * @brief Computes the product of the stiffness matrix and a displacement vector + * + * This method is used for implicit integration schemes to compute the change in force + * due to a displacement. + * + * @param mparams Mechanical parameters + * @param d_df Differential force vector + * @param d_dx Differential displacement vector + */ +template +void BaseBeamHookeLawForceField::addDForce(const sofa::core::MechanicalParams* mparams, + DataVecDeriv& d_df, + const DataVecDeriv& d_dx) +{ + if (!compute_df) + return; + + WriteAccessor df = d_df; + ReadAccessor dx = d_dx; + const Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + + df.resize(dx.size()); + if (!d_variantSections.getValue()) { + // For uniform section beams + for (unsigned int i = 0; i < dx.size(); i++) { + df[i] -= (m_K_section * dx[i]) * kFactor * d_length.getValue()[i]; + } + } + else { + // For variant section beams + for (unsigned int i = 0; i < dx.size(); i++) { + df[i] -= (m_K_sectionList[i] * dx[i]) * kFactor * d_length.getValue()[i]; + } + } +} + +/****************************************************************************** +* MATRIX COMPUTATION METHODS * +******************************************************************************/ + +/** + * @brief Adds the stiffness matrix contribution to the global matrix + * + * This method is called during the assembly of the global stiffness matrix to add + * the contribution of this force field. + * + * @param mparams Mechanical parameters + * @param matrix Multi-matrix accessor + */ +template +void BaseBeamHookeLawForceField::addKToMatrix(const sofa::core::MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) +{ + MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); + BaseMatrix* mat = mref.matrix; + unsigned int offset = mref.offset; + Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + const VecCoord& pos = this->mstate->read(sofa::core::VecCoordId::position())->getValue(); + addKToMatrixImpl(mat, offset, pos, d_length.getValue(), kFact); +} + +/** + * @brief Template implementation to add stiffness to matrix + * + * Implementation details for adding stiffness to the global matrix. + * This is specialized in derived classes for different data types. + * + * @param mat Base matrix + * @param offset Matrix offset + * @param pos Position vector + * @param lengths Vector of beam section lengths + * @param kFact Stiffness factor + */ +template +template +void BaseBeamHookeLawForceField::addKToMatrixImpl(MatrixType* mat, + unsigned int offset, + const VecCoord& pos, + const Vector& lengths, + Real kFact) +{ + for (unsigned int n = 0; n < pos.size(); n++) + { + if (!d_variantSections.getValue()) { + // For uniform section beams + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + mat->add(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_section[i][j] * lengths[n]); + } + } + } + else { + // For variant section beams + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + mat->add(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_sectionList[n][i][j] * lengths[n]); + } + } + } + } +} + +/****************************************************************************** +* UTILITY METHODS * +******************************************************************************/ + +/** + * @brief Computes the potential energy of the beam + * + * This method returns the strain energy stored in the beam due to deformation. + * The strain energy is calculated as 0.5 * displacement^T * K * displacement. + * + * @param mparams Mechanical parameters + * @param d_x Position vector + * @return Potential energy + */ +template +double BaseBeamHookeLawForceField::getPotentialEnergy(const sofa::core::MechanicalParams* mparams, + const DataVecCoord& d_x) const +{ + SOFA_UNUSED(mparams); + + double energy = 0.0; + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = this->mstate->read(sofa::core::VecCoordId::restPosition())->getValue(); + const type::vector& lengths = d_length.getValue(); + + // Make sure we have valid data + if (x.size() != x0.size() || x.size() != lengths.size()) { + return 0.0; + } + + // Calculate the potential energy + if (!d_variantSections.getValue()) { + // For uniform section beams + for (unsigned int i = 0; i < x.size(); i++) { + Deriv delta = x[i] - x0[i]; + energy += 0.5 * (delta * (m_K_section * delta)) * lengths[i]; + } + } + else { + // For variant section beams + for (unsigned int i = 0; i < x.size(); i++) { + Deriv delta = x[i] - x0[i]; + energy += 0.5 * (delta * (m_K_sectionList[i] * delta)) * lengths[i]; + } + } + + return energy; +} + +/** + * @brief Returns the external radius of the beam + * + * Utility method to access the beam's external radius. + * + * @return The external radius of the beam + */ +template +typename BaseBeamHookeLawForceField::Real BaseBeamHookeLawForceField::getRadius() +{ + return d_radius.getValue(); +} + +/** + * @brief Compute relative rotations between beam cross-sections + * + * Updates m_relativeRotations based on the current frames. + */ +template +void BaseBeamHookeLawForceField::computeRelativeRotations() +{ + const size_t numSegments = m_currentFrames.size() - 1; + + for (size_t i = 0; i < numSegments; ++i) { + // Compute the relative rotation from frame i to frame i+1 + m_relativeRotations[i] = m_currentFrames[i].rotation().inverse() * + m_currentFrames[i+1].rotation(); + } +} + +/** + * @brief Update current frames based on positions and rotations + * + * @param positions Current positions of the beam nodes + * @param rotations Current rotations of the beam cross-sections + */ +template +void BaseBeamHookeLawForceField::updateFrames( + const VecCoord& positions, + const std::vector& rotations) +{ + const size_t numNodes = positions.size(); + assert(rotations.size() == numNodes); + + for (size_t i = 0; i < numNodes; ++i) { + // Extract position + Vector3 position = getPosition(positions[i]); + + // Update current frame + m_currentFrames[i] = SE3Type(rotations[i], position); + } + + // Update relative rotations + computeRelativeRotations(); +} + +/** + * @brief Get local frame axis vectors + * @param frameIndex Index of the frame + * @return Tuple of local x, y, z axes as Vector3 + */ +template +std::tuple::Vector3, + typename BaseBeamHookeLawForceField::Vector3, + typename BaseBeamHookeLawForceField::Vector3> +BaseBeamHookeLawForceField::getLocalAxes(size_t frameIndex) const +{ + using Matrix3 = typename SO3Type::Matrix; + + // Get rotation matrix + const Matrix3 R = m_currentFrames[frameIndex].rotation().matrix(); + + // Extract column vectors + Vector3 xAxis(R(0,0), R(1,0), R(2,0)); + Vector3 yAxis(R(0,1), R(1,1), R(2,1)); + Vector3 zAxis(R(0,2), R(1,2), R(2,2)); + + return std::make_tuple(xAxis, yAxis, zAxis); +} + +/** + * @brief Compute the strain tensor between two frames + * @param frame1 First frame + * @param frame2 Second frame + * @return Strain tensor in se(3) + */ +template +typename BaseBeamHookeLawForceField::SE3TangentType +BaseBeamHookeLawForceField::computeStrainTensor( + const SE3Type& frame1, + const SE3Type& frame2) const +{ + // Compute relative transformation + SE3Type relativeTransform = frame1.inverse() * frame2; + + // Extract strain using logarithm mapping to the Lie algebra + return relativeTransform.log(); +} + +/** + * @brief Transform a local force/moment to the global frame + * + * @param localWrench Local force/moment in the cross-section frame + * @param frameIndex Index of the cross-section frame + * @return Global force/moment + */ +template +typename BaseBeamHookeLawForceField::Deriv +BaseBeamHookeLawForceField::transformWrenchToGlobal( + const Deriv& localWrench, + size_t frameIndex) const +{ + // Extract force and moment components + Vector3 localForce = getForce(localWrench); + Vector3 localMoment = getMoment(localWrench); + + // Transform to global frame + Vector3 globalForce = m_currentFrames[frameIndex].rotation().act(localForce); + Vector3 globalMoment = m_currentFrames[frameIndex].rotation().act(localMoment); + + // Reconstruct Deriv + return createDeriv(globalForce, globalMoment); +} + +/** + * @brief Transform a global force/moment to a local frame + * + * @param globalWrench Global force/moment + * @param frameIndex Index of the cross-section frame + * @return Local force/moment + */ +template +typename BaseBeamHookeLawForceField::Deriv +BaseBeamHookeLawForceField::transformWrenchToLocal( + const Deriv& globalWrench, + size_t frameIndex) const +{ + // Extract force and moment components + Vector3 globalForce = getForce(globalWrench); + Vector3 globalMoment = getMoment(globalWrench); + + // Transform to local frame + Vector3 localForce = m_currentFrames[frameIndex].rotation().inverse().act(globalForce); + Vector3 localMoment = m_currentFrames[frameIndex].rotation().inverse().act(globalMoment); + + // Reconstruct Deriv + return createDeriv(localForce, localMoment); +} + +/** + * @brief Extract strains using Lie Group formalism + * + * Computes the strains (curvature/twist and stretch) between consecutive frames + * using the Lie algebra logarithm. + * + * @return Vector of strains for each beam segment + */ +template +std::vector::Deriv> +BaseBeamHookeLawForceField::computeStrains() const +{ + const size_t numSegments = m_currentFrames.size() - 1; + std::vector strains(numSegments); + + for (size_t i = 0; i < numSegments; ++i) { + // Compute relative transformation from reference to current configuration + SE3Type refRelTransform = m_referenceFrames[i].inverse() * m_referenceFrames[i+1]; + SE3Type curRelTransform = m_currentFrames[i].inverse() * m_currentFrames[i+1]; + + // Compute deformation (reference to current) + SE3Type deformation = refRelTransform.inverse() * curRelTransform; + + // Extract strain from the Lie algebra using logarithm + SE3TangentType se3Strain = deformation.log(); + + // Convert to beam strain representation + // First 3 components are translation strain, last 3 are rotation strain + Vector3 transStrain(se3Strain[0], se3Strain[1], se3Strain[2]); + Vector3 rotStrain(se3Strain[3], se3Strain[4], se3Strain[5]); + + // Scale by segment length + const Real segmentLength = d_length.getValue()[i]; + transStrain /= segmentLength; + rotStrain /= segmentLength; + + // Create strain Deriv + strains[i] = createDeriv(transStrain, rotStrain); + } + + return strains; +} + +/** + * @brief Get the current frame transformation for a specific cross-section + * + * @param index Cross-section index + * @ diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.cpp b/src/Cosserat/forcefield/BeamHookeLawForceField.cpp index 29f5990..44aa81b 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.cpp +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.cpp @@ -29,163 +29,149 @@ ******************************************************************************/ #define SOFA_COSSERAT_CPP_BeamHookeLawForceField #include - #include namespace sofa::component::forcefield { -template <> -void BeamHookeLawForceField::reinit() -{ - // Precompute and store values - Real Iy, Iz, J, A; - if (d_crossSectionShape.getValue().getSelectedItem() == "rectangular") // rectangular cross-section - { - Real Ly = d_lengthY.getValue(); - Real Lz = d_lengthZ.getValue(); - - const Real LyLzLzLz = Ly * Lz * Lz * Lz; - const Real LzLyLyLy = Lz * Ly * Ly * Ly; - - Iy = LyLzLzLz / 12.0; - Iz = LzLyLyLy / 12.0; - J = Iy + Iz; - A = Ly * Lz; - - } else // circular cross-section - { - msg_info() << "Cross section shape." - << d_crossSectionShape.getValue().getSelectedItem(); - - Real r = d_radius.getValue(); - Real rInner = d_innerRadius.getValue(); - const Real r4 = r * r * r * r; - const Real rInner4 = rInner * rInner * rInner * rInner; - - Iy = M_PI * (r4 - rInner4) / 4.0; - Iz = Iy; - J = Iy + Iz; - A = M_PI * (r * r - rInner4); - } - m_crossSectionArea = A; - - if (d_useInertiaParams.getValue()) - { - msg_info() << "Pre-calculated inertia parameters are used for the computation of the stiffness matrix."; - m_K_section66[0][0] = d_GI.getValue(); - m_K_section66[1][1] = d_EIy.getValue(); - m_K_section66[2][2] = d_EIz.getValue(); - m_K_section66[3][3] = d_EA.getValue(); - m_K_section66[4][4] = d_GA.getValue(); - m_K_section66[5][5] = d_GA.getValue(); - } else { - // Pour du vec 6, on a _m_K =diag([G*J E*Iy E*Iz E*A G*As G*As]); % stifness matrix - Real E = d_youngModulus.getValue(); - Real G = E / (2.0 * (1.0 + d_poissonRatio.getValue())); - // Translational Stiffness (X, Y, Z directions): - // Gs * J(i): Shearing modulus times the second moment of the area (torsional stiffness). E * Js(i): Young's modulus times the second moment of the area (bending stiffness). - m_K_section66[0][0] = G * J; - m_K_section66[1][1] = E * Iy; - m_K_section66[2][2] = E * Iz; - // Rotational Stiffness (X, Y, Z directions): - // E * A: Young's modulus times the cross-sectional area (axial stiffness). - // Gs * A: Shearing modulus times the cross-sectional area (shearing stiffness). - m_K_section66[3][3] = E * A; - m_K_section66[4][4] = G * A; - m_K_section66[5][5] = G * A; - } -} - -template <> +template<> void BeamHookeLawForceField::addForce( - const MechanicalParams *mparams, DataVecDeriv &d_f, const DataVecCoord &d_x, - const DataVecDeriv &d_v) + const MechanicalParams* mparams, + DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecDeriv& d_v) { SOFA_UNUSED(d_v); SOFA_UNUSED(mparams); - if (!this->getMState()) - { - msg_info() << "No Mechanical State found, no force will be computed..." << "\n"; - compute_df = false; + if (!this->m_initialized) { + msg_error() << "Force field not properly initialized"; return; } - VecDeriv &f = *d_f.beginEdit(); - const VecCoord &x = d_x.getValue(); - // get the rest position (for non straight shape) - const VecCoord &x0 = - this->mstate->read(sofa::core::vec_id::read_access::restPosition)->getValue(); + + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = this->mstate->read(core::vec_id::read_access::restPosition)->getValue(); + const vector& lengths = this->d_length.getValue(); f.resize(x.size()); - unsigned int sz = d_length.getValue().size(); - if (x.size() != sz) - { - msg_warning() - << " length : " << sz << "should have the same size as x... " - << x.size() << "\n"; - compute_df = false; - return; - } - for (unsigned int i = 0; i < x.size(); i++) - { - f[i] -= (m_K_section66 * (x[i] - x0[i])) * d_length.getValue()[i]; + + // Compute forces using LieGroups + for (size_t i = 0; i < x.size(); i++) { + // Convert current and rest positions to SE3 transformations + liegroups::SE3d T_current, T_rest; + + // Current configuration + T_current.translation() = Vec3d(x[i][0], x[i][1], x[i][2]); + liegroups::SO3d R_current = liegroups::SO3d::exp(Vec3d(x[i][3], x[i][4], x[i][5])); + T_current.rotation() = R_current.matrix(); + + // Rest configuration + T_rest.translation() = Vec3d(x0[i][0], x0[i][1], x0[i][2]); + liegroups::SO3d R_rest = liegroups::SO3d::exp(Vec3d(x0[i][3], x0[i][4], x0[i][5])); + T_rest.rotation() = R_rest.matrix(); + + // Compute relative transformation + liegroups::SE3d T_rel = T_current * T_rest.inverse(); + + // Get twist coordinates and compute force + Deriv twist = T_rel.log(); + f[i] = -(this->m_K_section66 * twist) * lengths[i]; } d_f.endEdit(); } -template <> +template<> void BeamHookeLawForceField::addDForce( - const MechanicalParams *mparams, DataVecDeriv &d_df, - const DataVecDeriv &d_dx) + const MechanicalParams* mparams, + DataVecDeriv& d_df, + const DataVecDeriv& d_dx) { - if (!compute_df) - return; - WriteAccessor df = d_df; ReadAccessor dx = d_dx; - Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping( - this->rayleighStiffness.getValue()); + Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + const vector& lengths = this->d_length.getValue(); df.resize(dx.size()); - for (unsigned int i = 0; i < dx.size(); i++) - { - df[i] -= (m_K_section66 * dx[i]) * kFactor * d_length.getValue()[i]; + + // Linear approximation for small displacements + for (size_t i = 0; i < dx.size(); i++) { + df[i] = -(this->m_K_section66 * dx[i]) * kFactor * lengths[i]; } } -template <> +template<> void BeamHookeLawForceField::addKToMatrix( - const MechanicalParams *mparams, const MultiMatrixAccessor *matrix) + const MechanicalParams* mparams, + const MultiMatrixAccessor* matrix) { MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); - BaseMatrix *mat = mref.matrix; + BaseMatrix* mat = mref.matrix; unsigned int offset = mref.offset; - Real kFact = (Real)mparams->kFactorIncludingRayleighDamping( - this->rayleighStiffness.getValue()); - - const VecCoord &pos = - this->mstate->read(core::vec_id::read_access::restPosition)->getValue(); - for (unsigned int n = 0; n < pos.size(); n++) - { - for (unsigned int i = 0; i < 6; i++) - { - for (unsigned int j = 0; j < 6; j++) - { + Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); + const vector& lengths = this->d_length.getValue(); + + const VecCoord& pos = this->mstate->read(core::vec_id::read_access::position)->getValue(); + + // Add stiffness contribution for each beam element + for (size_t n = 0; n < pos.size(); n++) { + for (unsigned int i = 0; i < 6; i++) { + for (unsigned int j = 0; j < 6; j++) { mat->add(offset + i + 6 * n, offset + j + 6 * n, - -kFact * m_K_section66[i][j] * d_length.getValue()[n]); + -kFact * this->m_K_section66[i][j] * lengths[n]); } } } } +template<> +double BeamHookeLawForceField::getPotentialEnergy( + const MechanicalParams* mparams, + const DataVecCoord& x) const +{ + SOFA_UNUSED(mparams); + + const VecCoord& pos = x.getValue(); + const VecCoord& rest_pos = this->mstate->read(core::vec_id::read_access::restPosition)->getValue(); + const vector& lengths = this->d_length.getValue(); + + double energy = 0.0; + + // Compute potential energy using LieGroups + for (size_t i = 0; i < pos.size(); i++) { + // Convert configurations to SE3 + liegroups::SE3d T_current, T_rest; + + // Current configuration + T_current.translation() = Vec3d(pos[i][0], pos[i][1], pos[i][2]); + liegroups::SO3d R_current = liegroups::SO3d::exp(Vec3d(pos[i][3], pos[i][4], pos[i][5])); + T_current.rotation() = R_current.matrix(); + + // Rest configuration + T_rest.translation() = Vec3d(rest_pos[i][0], rest_pos[i][1], rest_pos[i][2]); + liegroups::SO3d R_rest = liegroups::SO3d::exp(Vec3d(rest_pos[i][3], rest_pos[i][4], rest_pos[i][5])); + T_rest.rotation() = R_rest.matrix(); + + // Compute relative transformation + liegroups::SE3d T_rel = T_current * T_rest.inverse(); + + // Get twist coordinates + Deriv twist = T_rel.log(); + + // Compute energy contribution + energy += 0.5 * (twist * (this->m_K_section66 * twist)) * lengths[i]; + } + + return energy; +} + using namespace sofa::defaulttype; template class BeamHookeLawForceField; template class BeamHookeLawForceField; -} +} // namespace sofa::component::forcefield namespace Cosserat { @@ -199,4 +185,4 @@ void registerBeamHookeLawForceField(sofa::core::ObjectFactory* factory) .add>()); } -} +} // namespace Cosserat diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.h b/src/Cosserat/forcefield/BeamHookeLawForceField.h index 636aca2..54b4ad4 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.h +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.h @@ -40,6 +40,8 @@ #include #include #include +#include +#include namespace sofa::component::forcefield { @@ -85,6 +87,9 @@ public : typedef typename CompressedRowSparseMatrix::BlockConstAccessor _3_3_BlockConstAccessor; typedef typename CompressedRowSparseMatrix::BlockAccessor _3_3_BlockAccessor; + // Data type for parallel processing + typedef sofa::simulation::Task Task; + public : BeamHookeLawForceField(); @@ -146,9 +151,46 @@ public : Mat66 m_K_section66; type::vector m_K_sectionList; + /// Flag to enable/disable multithreading + Data d_useMultiThreading; + /// Cross-section area Real m_crossSectionArea; +protected: + /** + * @brief Compute forces for uniform section beams (parallel version) + * This method handles the case when all beam sections have the same properties + * + * @param f Output force vector to update + * @param x Current position + * @param x0 Rest position + * @param lengths Vector of beam segment lengths + */ + void addForceUniformSection(DataVecDeriv& f, const DataVecCoord& x, const DataVecCoord& x0, const type::vector& lengths); + + /** + * @brief Compute forces for variant section beams (parallel version) + * This method handles the case when beam sections have different properties + * + * @param f Output force vector to update + * @param x Current position + * @param x0 Rest position + * @param lengths Vector of beam segment lengths + */ + void addForceVariantSection(DataVecDeriv& f, const DataVecCoord& x, const DataVecCoord& x0, const type::vector& lengths); + + /** + * @brief Validate input data before force computation + * + * @param f Force vector + * @param x Position vector + * @param x0 Rest position vector + * @return true if validation passed + * @return false if validation failed + */ + bool validateInputData(const DataVecDeriv& f, const DataVecCoord& x, const DataVecCoord& x0) const; + private : ////////////////////////// Inherited attributes //////////////////////////// diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.inl b/src/Cosserat/forcefield/BeamHookeLawForceField.inl index 98fa0c9..67179fd 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.inl +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.inl @@ -33,35 +33,47 @@ #include #include #include -#include // ?? - -using sofa::core::behavior::MechanicalState ; -using sofa::core::objectmodel::BaseContext ; -using sofa::helper::ReadAccessor ; -using sofa::helper::WriteAccessor ; -using sofa::core::VecCoordId; +#include +// Standard includes #include -using std::cout ; -using std::endl ; - #include #include +// Using declarations for common types +using sofa::core::behavior::MechanicalState; +using sofa::core::objectmodel::BaseContext; +using sofa::helper::ReadAccessor; +using sofa::helper::WriteAccessor; +using sofa::core::VecCoordId; +using std::cout; +using std::endl; + namespace sofa::component::forcefield { -using sofa::core::behavior::MultiMatrixAccessor ; -using sofa::core::behavior::BaseMechanicalState ; -using sofa::helper::WriteAccessor ; +using sofa::core::behavior::MultiMatrixAccessor; +using sofa::core::behavior::BaseMechanicalState; +using sofa::helper::WriteAccessor; + +/** + * @brief Implementation of all methods for the BeamHookeLawForceField class + * + * This force field simulates the mechanical behavior of beam elements using Hooke's law. + * It supports both uniform sections (same properties throughout the beam) and variant sections + * (different properties for different segments). + */ +/****************************************************************************** +* CONSTRUCTORS / DESTRUCTORS * +******************************************************************************/ template BeamHookeLawForceField::BeamHookeLawForceField() : Inherit1(), d_crossSectionShape( initData(&d_crossSectionShape, {"circular","rectangular"}, - "crossSectionShape", - "shape of the cross-section. Can be: circular (tube with external radius being radius and internal radius being innerRadius ) or rectangular (lengthY and lengthZ) . Default is circular" )), + "crossSectionShape", + "shape of the cross-section. Can be: circular (tube with external radius being radius and internal radius being innerRadius ) or rectangular (lengthY and lengthZ) . Default is circular" )), d_youngModulus( initData( &d_youngModulus, 1.0e9, "youngModulus", "Young Modulus describes the stiffness of the material")), d_poissonRatio( initData( &d_poissonRatio, 0.45, "poissonRatio", "poisson Ratio describes the compressibility of the material")), d_length( initData( &d_length, "length", "The list of lengths of the different beam's sections.")), @@ -76,14 +88,25 @@ BeamHookeLawForceField::BeamHookeLawForceField() 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")) + d_EI(initData(&d_EI, "EI", "The inertia parameter, EI")), + d_useMultiThreading(initData(&d_useMultiThreading, false, "useMultiThreading", "Use multithreading for force computation")) { - compute_df=true; + compute_df = true; } template BeamHookeLawForceField::~BeamHookeLawForceField() = default; +/****************************************************************************** +* INITIALIZATION METHODS * +******************************************************************************/ + +/** + * @brief Initialize the force field, setting up parameters and cross-section properties + * + * This method is called during initialization to set up the force field. + * It delegates to reinit() to calculate cross-section properties. + */ template void BeamHookeLawForceField::init() { @@ -91,17 +114,19 @@ void BeamHookeLawForceField::init() reinit(); } -/*Cross-Section Properties Initialization: The reinit function begins by recalculating the properties - related to the cross-section of the beams. It calculates the area moment of inertia (Iy and Iz), - the polar moment of inertia (J), and the cross-sectional area (A). - These calculations depend on the chosen cross-section shape, either circular or rectangular. T - he formulas used for these calculations are based on standard equations for these properties.*/ +/** + * @brief Reinitialize the force field, recalculating cross-section properties + * + * This method calculates the moments of inertia and stiffness matrices + * based on the chosen cross-section shape and material properties. + * It supports both uniform sections and variant sections. + */ template void BeamHookeLawForceField::reinit() { // Precompute and store inertia values Real Iy, Iz, J, A; - if ( d_crossSectionShape.getValue().getSelectedItem() == "rectangular") //rectangular cross-section + if (d_crossSectionShape.getValue().getSelectedItem() == "rectangular") // rectangular cross-section { Real Ly = d_lengthY.getValue(); Real Lz = d_lengthZ.getValue(); @@ -115,9 +140,9 @@ void BeamHookeLawForceField::reinit() A = Ly * Lz; } - else //circular cross-section + else // circular cross-section { - msg_info() << "Cross section shape." << d_crossSectionShape.getValue().getSelectedItem() ; + msg_info() << "Cross section shape." << d_crossSectionShape.getValue().getSelectedItem(); Real r = d_radius.getValue(); Real rInner = d_innerRadius.getValue(); @@ -127,68 +152,250 @@ void BeamHookeLawForceField::reinit() Iy = M_PI * (r4 - rInner4) / 4.0; Iz = Iy; J = Iy + Iz; - A = M_PI * (r * r - rInner4); + A = M_PI * (r * r - rInner * rInner); } m_crossSectionArea = A; - // if we are dealing with different physical properties : YM and PR - if(!d_variantSections.getValue()) + // if we are dealing with different physical properties: YM and PR + if (!d_variantSections.getValue()) { - if(!d_useInertiaParams.getValue()) + if (!d_useInertiaParams.getValue()) { Real E = d_youngModulus.getValue(); - Real G = E/(2.0*(1.0+d_poissonRatio.getValue())); + Real G = E / (2.0 * (1.0 + d_poissonRatio.getValue())); // Inertial matrix - m_K_section[0][0] = G*J; - m_K_section[1][1] = E*Iy; - m_K_section[2][2] = E*Iz; + 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."; + 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(); } - }else { - /*If the d_variantSections flag is set to true, it implies that multi-section beams are used for - the simulation. In this case, the code calculates and initializes a list of stiffness matrices - (m_K_sectionList) for each section. The properties of each section, such as Young's modulus and - Poisson's ratio, are specified in the d_youngModulusList and d_poissonRatioList data.*/ - msg_info("BeamHookeLawForceField")<< "Multi section beam are used for the simulation!"; + } + else { + msg_info("BeamHookeLawForceField") << "Multi section beam are used for the simulation!"; m_K_sectionList.clear(); - const auto szL = d_length.getValue().size(); - if((szL != d_poissonRatioList.getValue().size())||(szL != d_youngModulusList.getValue().size())){ - msg_error("BeamHookeLawForceField")<< "Please the size of the data length, youngModulusList and " - "poissonRatioList should be the same !"; + const auto szL = d_length.getValue().size(); + if ((szL != d_poissonRatioList.getValue().size()) || (szL != d_youngModulusList.getValue().size())) { + msg_error("BeamHookeLawForceField") << "Please the size of the data length, youngModulusList and " + "poissonRatioList should be the same!"; return; } - /*Stiffness Matrix Initialization: Next, the code initializes the stiffness matrix m_K_section - based on the properties of the cross-section and the material's Young's modulus (E) and - Poisson's ratio. The stiffness matrix is essential for computing forces and simulating beam - behavior.*/ - for(auto k=0; k +bool BeamHookeLawForceField::validateInputData(const DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecCoord& d_x0) const +{ + if (!this->getMState()) { + msg_info("BeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; + return false; + } + + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = d_x0.getValue(); + + if (x.empty() || x0.empty()) { + msg_warning("BeamHookeLawForceField") << "Empty input vectors, no force will be computed" << "\n"; + return false; + } + + if (x.size() != x0.size()) { + msg_warning("BeamHookeLawForceField") << "Position vector size (" << x.size() + << ") doesn't match rest position size (" << x0.size() << ")" << "\n"; + return false; + } + + unsigned int sz = d_length.getValue().size(); + if (x.size() != sz) { + msg_warning("BeamHookeLawForceField") << "Length vector size (" << sz + << ") should have the same size as position vector (" << x.size() << ")" << "\n"; + return false; + } + + if (d_variantSections.getValue() && m_K_sectionList.size() != x.size()) { + msg_warning("BeamHookeLawForceField") << "Using variant sections but section list size (" << m_K_sectionList.size() + << ") doesn't match position vector size (" << x.size() << ")" << "\n"; + return false; + } + + return true; +} + +/****************************************************************************** +* FORCE COMPUTATION METHODS * +******************************************************************************/ + +/** + * @brief Adds forces for uniform section beams + * + * Computes and adds forces to the force vector for beams with uniform cross-section + * properties. Supports both single-threaded and multi-threaded computation. + * + * @param d_f Force vector + * @param d_x Position vector + * @param d_x0 Rest position vector + * @param lengths Vector of beam section lengths + */ +template +void BeamHookeLawForceField::addForceUniformSection(DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecCoord& d_x0, + const type::vector& lengths) +{ + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = d_x0.getValue(); + const size_t size = x.size(); + + sofa::helper::ScopedAdvancedTimer timer("UniformSection"); + + if (d_useMultiThreading.getValue() && size > 1) { + sofa::simulation::TaskScheduler::Task::Status status; + sofa::simulation::TaskScheduler& scheduler = *(sofa::simulation::TaskScheduler::getInstance()); + + // Define a lambda for parallel execution + auto calcForce = [this, &f, &x, &x0, &lengths](size_t start, size_t end) { + for (size_t i = start; i < end; ++i) { + f[i] -= (m_K_section * (x[i] - x0[i])) * lengths[i]; + } + }; + + // Determine chunk size for parallel processing + const size_t chunk_size = std::max(1, size / (scheduler.getThreadCount() * 2)); + size_t start = 0; + + // Create and queue tasks for parallel execution + while (start < size) { + size_t end = std::min(start + chunk_size, size); + Task* task = new Task(std::bind(calcForce, start, end), &status); + scheduler.addTask(task); + start = end; + } + + // Wait for all tasks to complete + scheduler.workUntilDone(&status); + } + else { + // Single-threaded fallback + for (size_t i = 0; i < size; ++i) { + f[i] -= (m_K_section * (x[i] - x0[i])) * lengths[i]; + } + } + + d_f.endEdit(); +} + +/** + * @brief Adds forces for variant section beams + * + * Computes and adds forces to the force vector for beams with varying cross-section + * properties. Supports both single-threaded and multi-threaded computation. + * + * @param d_f Force vector + * @param d_x Position vector + * @param d_x0 Rest position vector + * @param lengths Vector of beam section lengths + */ +template +void BeamHookeLawForceField::addForceVariantSection(DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecCoord& d_x0, + const type::vector& lengths) +{ + VecDeriv& f = *d_f.beginEdit(); + const VecCoord& x = d_x.getValue(); + const VecCoord& x0 = d_x0.getValue(); + const size_t size = x.size(); + + sofa::helper::ScopedAdvancedTimer timer("VariantSection"); + + if (d_useMultiThreading.getValue() && size > 1) { + sofa::simulation::TaskScheduler::Task::Status status; + sofa::simulation::TaskScheduler& scheduler = *(sofa::simulation::TaskScheduler::getInstance()); + + // Define a lambda for parallel execution + auto calcForce = [this, &f, &x, &x0, &lengths](size_t start, size_t end) { + for (size_t i = start; i < end; ++i) { + f[i] -= (m_K_sectionList[i] * (x[i] - x0[i])) * lengths[i]; + } + }; + + // Determine chunk size for parallel processing + const size_t chunk_size = std::max(1, size / (scheduler.getThreadCount() * 2)); + size_t start = 0; + + // Create and queue tasks for parallel execution + while (start < size) { + size_t end = std::min(start + chunk_size, size); + Task* task = new Task(std::bind(calcForce, start, end), &status); + scheduler.addTask(task); + start = end; + } + + // Wait for all tasks to complete + scheduler.workUntilDone(&status); + } + else { + // Single-threaded fallback + for (size_t i = 0; i < size; ++i) { + f[i] -= (m_K_sectionList[i] * (x[i] - x0[i])) * lengths[i]; + } + } + + d_f.endEdit(); +} + +/** + * @brief Main method for force computation + * + * This method dispatches the force computation to either addForceUniformSection or + * addForceVariantSection based on the beam configuration. + * + * @param mparams Mechanical parameters + * @param d_f Force vector + * @param d_x Position vector + * @param d_v Velocity vector (unused) + */ template void BeamHookeLawForceField::addForce(const MechanicalParams* mparams, DataVecDeriv& d_f, @@ -197,41 +404,47 @@ void BeamHookeLawForceField::addForce(const MechanicalParams* mparams { SOFA_UNUSED(d_v); SOFA_UNUSED(mparams); - - if(!this->getMState()) { - msg_info("BeamHookeLawForceField") << "No Mechanical State found, no force will be computed..." << "\n"; - compute_df=false; - return; - } - VecDeriv& f = *d_f.beginEdit(); - const VecCoord& x = d_x.getValue(); - // get the rest position (for non straight shape) - const VecCoord& x0 = this->mstate->read(sofa::core::vec_id::read_access::restPosition)->getValue(); - - f.resize(x.size()); - unsigned int sz = d_length.getValue().size(); - if(x.size()!= sz){ - msg_warning("BeamHookeLawForceField")<<" length : "<< sz <<"should have the same size as x... "<< x.size() <<"\n"; + + sofa::helper::ScopedAdvancedTimer timer("BeamHookeLawForceField::addForce"); + + // Get rest position + const DataVecCoord& d_x0 = *this->mstate->read(sofa::core::vec_id::read_access::restPosition); + + // Validate input data + if (!validateInputData(d_f, d_x, d_x0)) { compute_df = false; return; } - - if(!d_variantSections.getValue()) - // @todo: use multithread - for (unsigned int i=0; i& lengths = d_length.getValue(); + + // Call the appropriate specialized force calculation method + if (!d_variantSections.getValue()) { + addForceUniformSection(d_f, d_x, d_x0, lengths); + } else { + addForceVariantSection(d_f, d_x, d_x0, lengths); + } } +/** + * @brief Computes the product of the stiffness matrix and a displacement vector + * + * This method is used for implicit integration schemes to compute the change in force + * due to a displacement. + * + * @param mparams Mechanical parameters + * @param d_df Differential force vector + * @param d_dx Differential displacement vector + */ template void BeamHookeLawForceField::addDForce(const MechanicalParams* mparams, - DataVecDeriv& d_df , - const DataVecDeriv& d_dx) + DataVecDeriv& d_df, + const DataVecDeriv& d_dx) { if (!compute_df) return; @@ -241,24 +454,31 @@ void BeamHookeLawForceField::addDForce(const MechanicalParams* mparam Real kFactor = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); df.resize(dx.size()); - if(!d_variantSections.getValue()) - for (unsigned int i=0; i -double BeamHookeLawForceField::getPotentialEnergy(const MechanicalParams* mparams, - const DataVecCoord& d_x) const -{ - SOFA_UNUSED(mparams); - SOFA_UNUSED(d_x); - - return 0.0; -} +/****************************************************************************** +* MATRIX COMPUTATION METHODS * +******************************************************************************/ +/** + * @brief Adds the stiffness matrix contribution to the global matrix + * + * This method is called during the assembly of the global stiffness matrix to add + * the contribution of this force field. + * + * @param mparams Mechanical parameters + * @param matrix Multi-matrix accessor + */ template void BeamHookeLawForceField::addKToMatrix(const MechanicalParams* mparams, const MultiMatrixAccessor* matrix) @@ -269,24 +489,60 @@ void BeamHookeLawForceField::addKToMatrix(const MechanicalParams* mpa Real kFact = (Real)mparams->kFactorIncludingRayleighDamping(this->rayleighStiffness.getValue()); const VecCoord& pos = this->mstate->read(core::vec_id::read_access::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]); - else - for(unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j< 3; j++) + if (!d_variantSections.getValue()) { + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + mat->add(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_section[i][j] * d_length.getValue()[n]); + } + } + } + else { + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { mat->add(offset + i + 3*n, offset + j + 3*n, -kFact * m_K_sectionList[n][i][j] * d_length.getValue()[n]); + } + } + } } } +/****************************************************************************** +* UTILITY METHODS * +******************************************************************************/ + +/** + * @brief Computes the potential energy of the beam + * + * This method returns the strain energy stored in the beam due to deformation. + * Currently returns 0 as it is not implemented. + * + * @param mparams Mechanical parameters + * @param d_x Position vector + * @return Potential energy + */ +template +double BeamHookeLawForceField::getPotentialEnergy(const MechanicalParams* mparams, + const DataVecCoord& d_x) const +{ + SOFA_UNUSED(mparams); + SOFA_UNUSED(d_x); + + return 0.0; +} +/** + * @brief Returns the external radius of the beam + * + * Utility method to access the beam's external radius. + * + * @return The external radius of the beam + */ template typename BeamHookeLawForceField::Real BeamHookeLawForceField::getRadius() { return d_radius.getValue(); } -} // forcefield +} // namespace sofa::component::forcefield diff --git a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h index 009abc4..3c4d1bd 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h +++ b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h @@ -1,11 +1,3 @@ -/****************************************************************************** -* 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 * diff --git a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.inl b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.inl index d537dc9..7816d20 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.inl +++ b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.inl @@ -1,3 +1,47 @@ +/****************************************************************************** +* 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/ * +* * +******************************************************************************/ +#pragma once +#include + +namespace sofa::component::forcefield +{ + +template +BeamHookeLawForceFieldRigid::BeamHookeLawForceFieldRigid() + : Inherit1() + , d_buildTorsion(initData(&d_buildTorsion, false, "buildTorsion", "Whether to apply torsional forces or not")) +{ +} + +} // namespace sofa::component::forcefield + /****************************************************************************** * SOFA, Simulation Open-Framework Architecture * * (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * diff --git a/src/Cosserat/forcefield/Integration_Plan.md b/src/Cosserat/forcefield/Integration_Plan.md new file mode 100644 index 0000000..ae8748b --- /dev/null +++ b/src/Cosserat/forcefield/Integration_Plan.md @@ -0,0 +1,368 @@ +# Plan for Integrating Lie Groups (SO2, SO3, SE2, SE3) into BaseBeamHookeLawForceField + +## Introduction + +The Cosserat beam model requires proper handling of rotations and rigid transformations for accurately modeling the mechanics of deformable beams. By integrating Lie Group theory into the BaseBeamHookeLawForceField class, we can improve the mathematical foundation of the code, enabling more accurate representation of 3D rotations and transformations. + +This integration will leverage the existing Lie Group implementations in the Cosserat plugin: +- `SO3` for handling 3D rotations +- `SE3` for handling rigid body transformations + +## 1. Required Dependencies + +Add to BaseBeamHookeLawForceField.h: +```cpp +#include +#include +``` + +## 2. Modifications to BaseBeamHookeLawForceField.h + +### 2.1 Add Type Definitions + +```cpp +// Inside BaseBeamHookeLawForceField class +public: + // Lie Group types + using SO3Type = sofa::component::cosserat::liegroups::SO3; + using SE3Type = sofa::component::cosserat::liegroups::SE3; + + // Tangent space types + using SO3TangentType = typename SO3Type::TangentVector; + using SE3TangentType = typename SE3Type::TangentVector; +``` + +### 2.2 Add Member Variables for Frame Transformations + +```cpp +protected: + // Store local reference frames + std::vector m_referenceFrames; + std::vector m_currentFrames; + + // Store rotations between consecutive frames (relative rotations) + std::vector m_relativeRotations; +``` + +### 2.3 Add Utility Methods for Lie Group Operations + +```cpp +protected: + /** + * @brief Compute relative rotations between beam cross-sections + * + * Updates m_relativeRotations based on the current frames. + */ + virtual void computeRelativeRotations(); + + /** + * @brief Update current frames based on positions and rotations + * + * @param positions Current positions of the beam nodes + * @param rotations Current rotations of the beam cross-sections + */ + virtual void updateFrames(const VecCoord& positions, const std::vector& rotations); + + /** + * @brief Transform a local force/moment to the global frame + * + * @param localWrench Local force/moment in the cross-section frame + * @param frameIndex Index of the cross-section frame + * @return Global force/moment + */ + virtual Deriv transformWrenchToGlobal(const Deriv& localWrench, size_t frameIndex) const; + + /** + * @brief Transform a global force/moment to a local frame + * + * @param globalWrench Global force/moment + * @param frameIndex Index of the cross-section frame + * @return Local force/moment + */ + virtual Deriv transformWrenchToLocal(const Deriv& globalWrench, size_t frameIndex) const; + + /** + * @brief Extract strains using Lie Group formalism + * + * Computes the strains (curvature/twist and stretch) between consecutive frames + * using the Lie algebra logarithm. + * + * @return Vector of strains for each beam segment + */ + virtual std::vector computeStrains() const; +``` + +### 2.4 Add Public Interface Methods + +```cpp +public: + /** + * @brief Get the current frame transformation for a specific cross-section + * + * @param index Cross-section index + * @return Rigid transformation (SE3) representing the cross-section frame + */ + SE3Type getFrame(size_t index) const; + + /** + * @brief Get the reference frame transformation for a specific cross-section + * + * @param index Cross-section index + * @return Rigid transformation (SE3) representing the reference cross-section frame + */ + SE3Type getReferenceFrame(size_t index) const; + + /** + * @brief Get the relative rotation between consecutive cross-sections + * + * @param index Segment index (between cross-sections index and index+1) + * @return Rotation (SO3) representing the relative orientation + */ + SO3Type getRelativeRotation(size_t index) const; +``` + +## 3. Modifications to BaseBeamHookeLawForceField.inl + +### 3.1 Initialize Frames in init() Method + +```cpp +template +void BaseBeamHookeLawForceField::init() +{ + Inherit1::init(); + // ... existing initialization code ... + + // Initialize reference frames + const VecCoord& x0 = this->mstate->read(sofa::core::vec_id::read_access::restPosition)->getValue(); + const size_t numNodes = x0.size(); + + m_referenceFrames.resize(numNodes); + m_currentFrames.resize(numNodes); + m_relativeRotations.resize(numNodes > 0 ? numNodes - 1 : 0); + + // Set up initial reference frames (implementation depends on how position/rotation + // is represented in the Coord type) + for (size_t i = 0; i < numNodes; ++i) { + // Extract position and rotation from x0[i] (implementation depends on DataTypes) + Vector3 position = getPosition(x0[i]); + SO3Type rotation = getRotation(x0[i]); + + // Create an SE3 transformation + m_referenceFrames[i] = SE3Type(rotation, position); + m_currentFrames[i] = m_referenceFrames[i]; // Initially, current frames match reference frames + } + + // Compute initial relative rotations + computeRelativeRotations(); + + m_initialized = true; +} +``` + +### 3.2 Implement Frame Update Method + +```cpp +template +void BaseBeamHookeLawForceField::updateFrames( + const VecCoord& positions, + const std::vector& rotations) +{ + const size_t numNodes = positions.size(); + assert(rotations.size() == numNodes); + + for (size_t i = 0; i < numNodes; ++i) { + // Extract position from positions[i] (implementation depends on DataTypes) + Vector3 position = getPosition(positions[i]); + + // Update current frame + m_currentFrames[i] = SE3Type(rotations[i], position); + } + + // Update relative rotations + computeRelativeRotations(); +} +``` + +### 3.3 Implement Relative Rotations Computation + +```cpp +template +void BaseBeamHookeLawForceField::computeRelativeRotations() +{ + const size_t numSegments = m_currentFrames.size() - 1; + + for (size_t i = 0; i < numSegments; ++i) { + // Compute the relative rotation from frame i to frame i+1 + m_relativeRotations[i] = m_currentFrames[i].rotation().inverse() * + m_currentFrames[i+1].rotation(); + } +} +``` + +### 3.4 Implement Frame Transformation Methods + +```cpp +template +typename BaseBeamHookeLawForceField::Deriv +BaseBeamHookeLawForceField::transformWrenchToGlobal( + const Deriv& localWrench, + size_t frameIndex) const +{ + // Implementation depends on the representation of Deriv (force/moment) + // This is just a sketch; actual implementation will vary based on DataTypes + + // Extract force and moment components + Vector3 localForce = getForce(localWrench); + Vector3 localMoment = getMoment(localWrench); + + // Transform to global frame + Vector3 globalForce = m_currentFrames[frameIndex].rotation().act(localForce); + Vector3 globalMoment = m_currentFrames[frameIndex].rotation().act(localMoment); + + // Reconstruct Deriv + return createDeriv(globalForce, globalMoment); +} + +template +typename BaseBeamHookeLawForceField::Deriv +BaseBeamHookeLawForceField::transformWrenchToLocal( + const Deriv& globalWrench, + size_t frameIndex) const +{ + // Implementation depends on the representation of Deriv (force/moment) + // This is just a sketch; actual implementation will vary based on DataTypes + + // Extract force and moment components + Vector3 globalForce = getForce(globalWrench); + Vector3 globalMoment = getMoment(globalWrench); + + // Transform to local frame + Vector3 localForce = m_currentFrames[frameIndex].rotation().inverse().act(globalForce); + Vector3 localMoment = m_currentFrames[frameIndex].rotation().inverse().act(globalMoment); + + // Reconstruct Deriv + return createDeriv(localForce, localMoment); +} +``` + +### 3.5 Implement Strain Computation Using Lie Algebra + +```cpp +template +std::vector::Deriv> +BaseBeamHookeLawForceField::computeStrains() const +{ + const size_t numSegments = m_currentFrames.size() - 1; + std::vector strains(numSegments); + + for (size_t i = 0; i < numSegments; ++i) { + // Compute relative transformation from reference to current configuration + SE3Type refRelTransform = m_referenceFrames[i].inverse() * m_referenceFrames[i+1]; + SE3Type curRelTransform = m_currentFrames[i].inverse() * m_currentFrames[i+1]; + + // Compute deformation (reference to current) + SE3Type deformation = refRelTransform.inverse() * curRelTransform; + + // Extract strain from the Lie algebra using logarithm + SE3TangentType se3Strain = deformation.log(); + + // Convert to beam strain representation (depends on DataTypes) + // First 3 components are translation strain, last 3 are rotation strain + Vector3 transStrain(se3Strain[0], se3Strain[1], se3Strain[2]); + Vector3 rotStrain(se3Strain[3], se3Strain[4], se3Strain[5]); + + // Scale by segment length + const Real segmentLength = d_length.getValue()[i]; + transStrain /= segmentLength; + rotStrain /= segmentLength; + + // Create strain Deriv + strains[i] = createDeriv(transStrain, rotStrain); + } + + return strains; +} +``` + +### 3.6 Modify Force Computation to Use Lie Group Formalism + +```cpp +template +void BaseBeamHookeLawForceField::addForce( + const sofa::core::MechanicalParams* mparams, + DataVecDeriv& d_f, + const DataVecCoord& d_x, + const DataVecDeriv& d_v) +{ + SOFA_UNUSED(d_v); + SOFA_UNUSED(mparams); + + // Previous validation code... + + // Get current positions and rotations + const VecCoord& x = d_x.getValue(); + + // Extract rotations from positions (implementation depends on DataTypes) + std::vector rotations(x.size()); + for (size_t i = 0; i < x.size(); ++i) { + rotations[i] = getRotation(x[i]); + } + + // Update current frames + updateFrames(x, rotations); + + // Compute strains + std::vector strains = computeStrains(); + + // Compute forces based on strains (implementation depends on whether + // uniform or variant sections are used) + VecDeriv& f = *d_f.beginEdit(); + f.resize(x.size()); + + // For each beam segment + for (size_t i = 0; i < strains.size(); ++i) { + // Compute local internal force + Deriv localForce; + + if (!d_variantSections.getValue()) { + // For uniform section + localForce = -(m_K_section * strains[i]) * d_length.getValue()[i]; + } else { + // For variant section + localForce = -(m_K_sectionList[i] * strains[i]) * d_length.getValue()[i]; + } + + // Transform to global frame and apply to nodes + Deriv globalForce = transformWrenchToGlobal(localForce, i); + + // Apply forces to both nodes of the segment + // (Distribution depends on the beam formulation) + f[i] += globalForce; + f[i+1] -= globalForce; + } + + d_f.endEdit(); +} +``` + +### is your plan + +The integration plan provides a comprehensive approach to integrating Lie Group theory into the BaseBeamHookeLawForceField class. This will improve the mathematical soundness and enable more accurate representation of 3D rotations and transformations in the Cosserat beam model. + +The key aspects of the integration are: + +1. Use existing `SO3` and `SE3` classes from the Cosserat plugin +2. Add member variables to store reference and current frames +3. Implement methods to compute strains using the Lie algebra logarithm +4. Transform forces between local and global frames using proper rotation operations +5. Provide a public interface for accessing frame data + +This integration creates a foundation that derived classes can build upon for specific beam formulations, while ensuring mathematical consistency throughout the implementation. + +## Implementation Notes + +- The exact implementation of helper methods like `getPosition()`, `getRotation()`, etc. will depend on how the `DataTypes` represent positions and rotations. +- The strain computation uses the Lie algebra logarithm to extract physically meaningful strain measures from the relative transformations. +- The force computation transforms local forces to the global frame using proper rotation operations, ensuring correctness for large rotations. +- The plan assumes that `Deriv` type can represent both forces/moments and strains, which is typical for beam elements. + diff --git a/src/Cosserat/forcefield/tasks.md b/src/Cosserat/forcefield/tasks.md new file mode 100644 index 0000000..582c662 --- /dev/null +++ b/src/Cosserat/forcefield/tasks.md @@ -0,0 +1,53 @@ +1. Create the Base Class Files + • Add "BaseBeamHookeLawForceField.h" and "BaseBeamHookeLawForceField.inl" files in the appropriate module directory. + • Declare an abstract template class "BaseBeamHookeLawForceField" inheriting from "ForceField". + • Ensure the base class defines necessary template parameters (Vec3Types, Vec6Types). + +2. Identify Common Data Members and Methods + • Review BeamHookeLawForceField and BeamHookeLawForceFieldRigid to extract shared properties: + + - Cross-section shape (rectangular, circular, etc.) and dimensions. + - Area calculation logic. + - Material properties (Young’s modulus, Poisson ratio). + - Inertia parameters and section variants. + • Move these members and any relevant getter/setter methods into "BaseBeamHookeLawForceField". + • Add checks in the base class’s constructor to ensure valid parameter bounds, with clear error messages. + +3. Integrate LieGroups (SO3, SE3) + • Update the base class to utilize SO3 and SE3 for rotation, transformation, and strain/stress mapping. + • Introduce placeholders or utility methods in the base class that you’ll rely on to handle frame transformations in derived classes. + +4. Code Duplication Removal + • Move any duplicated constructor initialization code from BeamHookeLawForceField and BeamHookeLawForceFieldRigid into the base class constructor or a common initialization method. + • Extract duplicated logic from reinit(), addForce(), addDForce(), and addKToMatrix() into virtual or protected methods in the base class. + • Provide pure virtual methods in the base class for specialized force calculations that differ between the two derived classes. + +5. Maintain Backward Compatibility + • Keep original class names (BeamHookeLawForceField and BeamHookeLawForceFieldRigid) and data member names. + • In the derived classes, inherit from the new "BaseBeamHookeLawForceField". + • Add deprecation warnings to any methods or signatures that are changed to highlight the new usage, without breaking existing code. + • Ensure that the original public interface in the derived classes remains consistent with previously existing calls. + +6. Implement Enhanced Error Handling + • Add a thorough validation routine in the base class to verify cross-section dimensions, Young’s modulus ranges, Poisson ratio limits, etc. + • Improve error messaging to indicate which parameter is invalid and why. + • Make sure derived classes complement the base class’s error checking with additional checks specific to rigid or non-rigid beams. + +7. File and Build Updates + • Modify "BeamHookeLawForceField.h/.cpp/.inl" and "BeamHookeLawForceFieldRigid.h/.cpp/.inl" to: + + - Include "BaseBeamHookeLawForceField.h" + - Extend "BaseBeamHookeLawForceField" instead of "ForceField" directly. + - Remove any redundant code now handled by the base class. + • Ensure all CMake or build-related scripts reference "BaseBeamHookeLawForceField" if necessary for registration. + • Maintain existing component registration macros for each derived class within SOFA. + +8. Testing & Validation + • Compile and run existing unit tests for BeamHookeLawForceField and BeamHookeLawForceFieldRigid to ensure no regressions. + • Add new tests for base class validation logic, including boundary condition tests on cross-section and material properties. + • Ensure transformations using SO3/SE3 behave as expected across strain/stress calculations. + +9. Documentation & Final Review + • Update or create Doxygen documentation for "BaseBeamHookeLawForceField" with details on common data members and usage. + • Document any new or deprecated interfaces in the derived classes. + • Verify that the code merges cleanly and passes all checks in the CI pipeline. diff --git a/src/Cosserat/liegroups/Bundle.h b/src/Cosserat/liegroups/Bundle.h new file mode 100644 index 0000000..20c4ab1 --- /dev/null +++ b/src/Cosserat/liegroups/Bundle.h @@ -0,0 +1,267 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_BUNDLE_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_BUNDLE_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" +#include +#include + +namespace sofa::component::cosserat::liegroups { + +namespace detail { + +// Helper to compute total dimension of all groups +template +struct TotalDimension; + +template<> +struct TotalDimension<> { + static constexpr int value = 0; +}; + +template +struct TotalDimension { + static constexpr int value = Group::Dim + TotalDimension::value; +}; + +// Helper to check if all types are LieGroupBase derivatives +template +struct AllAreLieGroups; + +template<> +struct AllAreLieGroups<> : std::true_type {}; + +template +struct AllAreLieGroups { + static constexpr bool value = + std::is_base_of_v, Group> && + AllAreLieGroups::value; +}; + +// Helper to split vector into segments +template +void setSegment(Vector& vec, const Group& group) { + vec.template segment(offset) = group.log(); +} + +template +void setSegment(Vector& vec, const Group& group, const Rest&... rest) { + vec.template segment(offset) = group.log(); + setSegment(vec, rest...); +} + +// Helper to extract segments and create groups +template +Group getGroup(const Vector& vec) { + return Group().exp(vec.template segment(offset)); +} + +} // namespace detail + +/** + * @brief Implementation of product manifold bundle of Lie groups + * + * This class implements a product of multiple Lie groups, allowing them to be + * treated as a single composite Lie group. The bundle maintains the product + * structure while providing all the necessary group operations. + * + * Example usage: + * using PoseVel = Bundle, RealSpace>; + * + * @tparam Groups The Lie groups to bundle together + */ +template +class Bundle : public LieGroupBase>::Scalar, + detail::TotalDimension::value>, + public LieGroupOperations> { + static_assert(sizeof...(Groups) > 0, "Bundle must contain at least one group"); + static_assert(detail::AllAreLieGroups::value, + "All template parameters must be Lie groups"); + +public: + using FirstGroup = std::tuple_element_t<0, std::tuple>; + using Base = LieGroupBase::value>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + static constexpr int Dim = Base::Dim; + using GroupTuple = std::tuple; + + /** + * @brief Default constructor creates identity bundle + */ + Bundle() = default; + + /** + * @brief Construct from individual group elements + */ + Bundle(const Groups&... groups) : m_groups(groups...) {} + + /** + * @brief Group composition (component-wise) + */ + Bundle operator*(const Bundle& other) const { + return multiply(other, std::index_sequence_for()); + } + + /** + * @brief Inverse element (component-wise) + */ + Bundle inverse() const override { + return inverse_impl(std::index_sequence_for()); + } + + /** + * @brief Exponential map from Lie algebra to bundle + */ + Bundle exp(const TangentVector& algebra_element) const override { + return exp_impl(algebra_element, std::index_sequence_for()); + } + + /** + * @brief Logarithmic map from bundle to Lie algebra + */ + TangentVector log() const override { + TangentVector result; + detail::setSegment(result, std::get(m_groups)...); + return result; + } + + /** + * @brief Adjoint representation (block diagonal) + */ + AdjointMatrix adjoint() const override { + return adjoint_impl(std::index_sequence_for()); + } + + /** + * @brief Group action on a point (component-wise) + */ + Vector act(const Vector& point) const override { + return act_impl(point, std::index_sequence_for()); + } + + /** + * @brief Check if approximately equal to another bundle + */ + bool isApprox(const Bundle& other, + const Scalar& eps = Types::epsilon()) const { + return isApprox_impl(other, eps, std::index_sequence_for()); + } + + /** + * @brief Get the identity element + */ + static const Bundle& identity() { + static const Bundle id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra + */ + int algebraDimension() const override { return Dim; } + + /** + * @brief Get the dimension of the space the group acts on + */ + int actionDimension() const override { + return (std::get(m_groups).actionDimension() + ...); + } + + /** + * @brief Access individual group elements + */ + template + const auto& get() const { + return std::get(m_groups); + } + + template + auto& get() { + return std::get(m_groups); + } + +private: + GroupTuple m_groups; ///< Tuple of group elements + + // Helper for multiplication + template + Bundle multiply(const Bundle& other, std::index_sequence) const { + return Bundle((std::get(m_groups) * std::get(other.m_groups))...); + } + + // Helper for inverse + template + Bundle inverse_impl(std::index_sequence) const { + return Bundle((std::get(m_groups).inverse())...); + } + + // Helper for exponential map + template + Bundle exp_impl(const TangentVector& algebra_element, std::index_sequence) const { + int offset = 0; + return Bundle((detail::getGroup::Dim))> + (algebra_element))...); + } + + // Helper for adjoint + template + AdjointMatrix adjoint_impl(std::index_sequence) const { + AdjointMatrix result = AdjointMatrix::Zero(); + int offset = 0; + ((result.template block::Dim, + std::tuple_element_t::Dim> + (offset, offset) = std::get(m_groups).adjoint(), + offset += std::tuple_element_t::Dim), ...); + return result; + } + + // Helper for group action + template + Vector act_impl(const Vector& point, std::index_sequence) const { + Vector result; + int inOffset = 0, outOffset = 0; + ((result.template segment(m_groups).actionDimension()>(outOffset) = + std::get(m_groups).act(point.template segment(m_groups).actionDimension()>(inOffset)), + inOffset += std::get(m_groups).actionDimension(), + outOffset += std::get(m_groups).actionDimension()), ...); + return result; + } + + // Helper for approximate equality + template + bool isApprox_impl(const Bundle& other, const Scalar& eps, std::index_sequence) const { + return (std::get(m_groups).isApprox(std::get(other.m_groups), eps) && ...); + } +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "Bundle.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_BUNDLE_H diff --git a/src/Cosserat/liegroups/LieGroupBase.h b/src/Cosserat/liegroups/LieGroupBase.h new file mode 100644 index 0000000..445558d --- /dev/null +++ b/src/Cosserat/liegroups/LieGroupBase.h @@ -0,0 +1,126 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_LIEGROUPBASE_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_LIEGROUPBASE_H + +#include "Types.h" +#include + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Base class template for all Lie group implementations + * + * This class defines the interface that all Lie group implementations must satisfy. + * It provides pure virtual methods for the fundamental operations of Lie groups: + * composition, inversion, exponential map, logarithmic map, adjoint representation, + * and group action on points. + * + * @tparam _Scalar The scalar type used for computations (must be a floating-point type) + * @tparam _Dim The dimension of the group representation + */ +template +class LieGroupBase { +public: + static_assert(std::is_floating_point<_Scalar>::value, + "Scalar type must be a floating-point type"); + static_assert(_Dim > 0, "Dimension must be positive"); + + using Scalar = _Scalar; + using Types = Types; + static constexpr int Dim = _Dim; + + // Define commonly used types + using Vector = typename Types::template Vector; + using Matrix = typename Types::template Matrix; + using TangentVector = typename Types::template Vector; + using AdjointMatrix = typename Types::template Matrix; + + virtual ~LieGroupBase() = default; + + /** + * @brief Group composition operation + * @param other Another element of the same Lie group + * @return The composition this * other + */ + virtual LieGroupBase& operator*(const LieGroupBase& other) const = 0; + + /** + * @brief Compute the inverse element + * @return The inverse element such that this * inverse() = identity() + */ + virtual LieGroupBase inverse() const = 0; + + /** + * @brief Exponential map from Lie algebra to Lie group + * @param algebra_element Element of the Lie algebra (tangent space at identity) + * @return The corresponding element in the Lie group + */ + virtual LieGroupBase exp(const TangentVector& algebra_element) const = 0; + + /** + * @brief Logarithmic map from Lie group to Lie algebra + * @return The corresponding element in the Lie algebra + */ + virtual TangentVector log() const = 0; + + /** + * @brief Adjoint representation of the group element + * @return The adjoint matrix representing the action on the Lie algebra + */ + virtual AdjointMatrix adjoint() const = 0; + + /** + * @brief Group action on a point + * @param point The point to transform + * @return The transformed point + */ + virtual Vector act(const Vector& point) const = 0; + + /** + * @brief Check if this element is approximately equal to another + * @param other Another element of the same Lie group + * @param eps Tolerance for comparison (optional) + * @return true if elements are approximately equal + */ + virtual bool isApprox(const LieGroupBase& other, + const Scalar& eps = Types::epsilon()) const = 0; + + /** + * @brief Get the identity element of the group + * @return Reference to the identity element + */ + virtual const LieGroupBase& identity() const = 0; + + /** + * @brief Get the dimension of the Lie algebra (tangent space) + * @return The dimension of the Lie algebra + */ + virtual int algebraDimension() const = 0; + + /** + * @brief Get the dimension of the space the group acts on + * @return The dimension of the space the group acts on + */ + virtual int actionDimension() const = 0; +}; + +} // namespace sofa::component::cosserat::liegroups + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_LIEGROUPBASE_H diff --git a/src/Cosserat/liegroups/Readme.md b/src/Cosserat/liegroups/Readme.md new file mode 100644 index 0000000..8bb9a13 --- /dev/null +++ b/src/Cosserat/liegroups/Readme.md @@ -0,0 +1,129 @@ +--- +author: Yinoussa Adagolodjo +date: 2025-04-12 +--- + +# Lie Groups Library for Cosserat Models + +This library provides implementations of various Lie groups for use in Cosserat rod modeling and simulation. It is inspired by the [manif](https://github.com/artivis/manif) library but tailored specifically for the needs of the Cosserat plugin. + +## Overview + +Lie groups are mathematical structures that are both groups and differentiable manifolds. They are essential in robotics and computer vision for representing rigid body transformations and rotations. In the context of Cosserat rods, they allow us to represent the configuration of rod elements in a mathematically elegant and computationally efficient way. + +This library implements the following Lie groups: + +- **RealSpace**: Euclidean vector space ℝⁿ +- **SO(2)**: Special Orthogonal group in 2D (rotations in a plane) +- **SE(2)**: Special Euclidean group in 2D (rigid transformations in a plane) + +Future implementations will include: + +- **SO(3)**: Special Orthogonal group in 3D (rotations in 3D space) +- **SE(3)**: Special Euclidean group in 3D (rigid transformations in 3D space) +- **Sim(3)**: Similarity transformations in 3D space + +## Installation + +The Lie groups library is part of the Cosserat plugin. Installation requirements: + +- C++14 or higher +- Eigen 3.3 or higher +- Sofa Framework + +## Dependencies + +- Eigen: For linear algebra operations +- CMake: For building the project + +## Basic Usage + +Here are some examples of how to use the library: + +### RealSpace (Euclidean vector space) + +```cpp +#include + +// Create a 3D vector in RealSpace +Cosserat::RealSpace point(1.0, 2.0, 3.0); + +// Create from Eigen vector +Eigen::Vector3d eigen_vec(4.0, 5.0, 6.0); +Cosserat::RealSpace another_point(eigen_vec); + +// Compose points (vector addition) +auto result = point.compose(another_point); +``` + +### SO(2) (2D rotations) + +```cpp +#include + +// Create a rotation of 45 degrees +Cosserat::SO2 rotation(M_PI/4.0); + +// Get the angle +double angle = rotation.angle(); + +// Convert to rotation matrix +Eigen::Matrix2d rot_mat = rotation.matrix(); + +// Compose rotations +Cosserat::SO2 another_rotation(M_PI/2.0); +auto composed = rotation.compose(another_rotation); // 135 degrees rotation + +// Inverse rotation +auto inverse = rotation.inverse(); +``` + +### SE(2) (2D rigid transformations) + +```cpp +#include + +// Create a transform: 45-degree rotation with translation (1,2) +Cosserat::SE2 transform(M_PI/4.0, 1.0, 2.0); + +// Get components +double angle = transform.angle(); +Eigen::Vector2d translation = transform.translation(); + +// Convert to homogeneous transformation matrix +Eigen::Matrix3d transform_mat = transform.matrix(); + +// Compose transformations +Cosserat::SE2 another_transform(M_PI/2.0, 3.0, 4.0); +auto composed = transform.compose(another_transform); + +// Inverse transformation +auto inverse = transform.inverse(); +``` + +## Detailed Documentation + +For more detailed documentation, including mathematical foundations, implementation details, and advanced usage examples, see: + +- [Mathematical Foundations](docs/math_foundations.md) +- [RealSpace Implementation](docs/realspace.md) +- [SO(2) Implementation](docs/so2.md) +- [SE(2) Implementation](docs/se2.md) +- [Performance Benchmarks](docs/benchmarks.md) + +## Benchmarking + +The library includes benchmarks to measure the performance of various operations. You can run the benchmarks with: + +```bash +cd build +make run_benchmarks +``` + +## Roadmap + +For future development plans, see the [Roadmap](/src/Cosserat/liegroups/tasks.md/feature-lieAgebra.md). + +This feature is inspire by the repo : https://github.com/artivis/manif and the devel branch. + +## Todo see : [Roadmap](/src/Cosserat/liegroups/tasks.md/feature-lieAgebra.md) diff --git a/src/Cosserat/liegroups/RealSpace.h b/src/Cosserat/liegroups/RealSpace.h new file mode 100644 index 0000000..6bc865a --- /dev/null +++ b/src/Cosserat/liegroups/RealSpace.h @@ -0,0 +1,145 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_REALSPACE_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_REALSPACE_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Implementation of the Real space ℝ(n) as a Lie group + * + * This class implements the vector space ℝ(n) as a Lie group where: + * - The group operation is vector addition + * - The inverse operation is vector negation + * - The Lie algebra is the same space with the same operations + * - The exponential and logarithm maps are identity functions + * - The adjoint representation is the identity matrix + * + * @tparam _Scalar The scalar type (must be a floating-point type) + * @tparam _Dim The dimension of the space + */ +template +class RealSpace : public LieGroupBase<_Scalar, _Dim>, + public LieGroupOperations> { +public: + using Base = LieGroupBase<_Scalar, _Dim>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + static constexpr int Dim = Base::Dim; + + /** + * @brief Default constructor initializes to zero (identity element) + */ + RealSpace() : m_data(Vector::Zero()) {} + + /** + * @brief Construct from a vector + */ + explicit RealSpace(const Vector& v) : m_data(v) {} + + /** + * @brief Group composition (vector addition) + */ + RealSpace operator*(const RealSpace& other) const { + return RealSpace(m_data + other.m_data); + } + + /** + * @brief Inverse element (negation) + */ + RealSpace inverse() const override { + return RealSpace(-m_data); + } + + /** + * @brief Exponential map (identity map for ℝ(n)) + */ + RealSpace exp(const TangentVector& algebra_element) const override { + return RealSpace(algebra_element); + } + + /** + * @brief Logarithmic map (identity map for ℝ(n)) + */ + TangentVector log() const override { + return m_data; + } + + /** + * @brief Adjoint representation (identity matrix for ℝ(n)) + */ + AdjointMatrix adjoint() const override { + return AdjointMatrix::Identity(); + } + + /** + * @brief Group action on a point (translation) + */ + Vector act(const Vector& point) const override { + return point + m_data; + } + + /** + * @brief Check if approximately equal to another element + */ + bool isApprox(const RealSpace& other, + const Scalar& eps = Types::epsilon()) const { + return m_data.isApprox(other.m_data, eps); + } + + /** + * @brief Get the identity element (zero vector) + */ + static const RealSpace& identity() { + static const RealSpace id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra + */ + int algebraDimension() const override { return Dim; } + + /** + * @brief Get the dimension of the space the group acts on + */ + int actionDimension() const override { return Dim; } + + /** + * @brief Access the underlying data + */ + const Vector& data() const { return m_data; } + Vector& data() { return m_data; } + +private: + Vector m_data; ///< The underlying vector data +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "RealSpace.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_REALSPACE_H diff --git a/src/Cosserat/liegroups/SE2.h b/src/Cosserat/liegroups/SE2.h new file mode 100644 index 0000000..26d49f1 --- /dev/null +++ b/src/Cosserat/liegroups/SE2.h @@ -0,0 +1,235 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE2_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE2_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" +#include "SO2.h" +#include + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Implementation of SE(2), the Special Euclidean group in 2D + * + * This class implements the group of rigid body transformations in 2D space. + * Elements of SE(2) are represented as a combination of: + * - An SO(2) rotation + * - A 2D translation vector + * + * The Lie algebra se(2) consists of vectors in ℝ³, where: + * - The first two components represent the translation velocity + * - The last component represents the angular velocity + * + * @tparam _Scalar The scalar type (must be a floating-point type) + */ +template +class SE2 : public LieGroupBase<_Scalar, 3>, + public LieGroupOperations> { +public: + using Base = LieGroupBase<_Scalar, 3>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + using Vector2 = Eigen::Matrix; + using Matrix2 = Eigen::Matrix; + static constexpr int Dim = Base::Dim; + + /** + * @brief Default constructor creates identity transformation + */ + SE2() : m_rotation(), m_translation(Vector2::Zero()) {} + + /** + * @brief Construct from rotation and translation + */ + SE2(const SO2& rotation, const Vector2& translation) + : m_rotation(rotation), m_translation(translation) {} + + /** + * @brief Construct from angle and translation + */ + SE2(const Scalar& angle, const Vector2& translation) + : m_rotation(angle), m_translation(translation) {} + + /** + * @brief Group composition (rigid transformation composition) + */ + SE2 operator*(const SE2& other) const { + return SE2(m_rotation * other.m_rotation, + m_translation + m_rotation.act(other.m_translation)); + } + + /** + * @brief Inverse element (inverse transformation) + */ + SE2 inverse() const override { + SO2 inv_rot = m_rotation.inverse(); + return SE2(inv_rot, -(inv_rot.act(m_translation))); + } + + /** + * @brief Exponential map from Lie algebra to SE(2) + * @param algebra_element Vector in ℝ³ representing (vx, vy, ω) + */ + SE2 exp(const TangentVector& algebra_element) const override { + const Scalar& theta = algebra_element[2]; + const Vector2 v(algebra_element[0], algebra_element[1]); + + if (std::abs(theta) < Types::epsilon()) { + // For small rotations, use first-order approximation + return SE2(SO2(theta), v); + } + + // Exact formula for finite rotations + const Scalar sin_theta = std::sin(theta); + const Scalar cos_theta = std::cos(theta); + const Scalar theta_inv = Scalar(1) / theta; + + Matrix2 V; + V << sin_theta * theta_inv, -(Scalar(1) - cos_theta) * theta_inv, + (Scalar(1) - cos_theta) * theta_inv, sin_theta * theta_inv; + + return SE2(SO2(theta), V * v); + } + + /** + * @brief Logarithmic map from SE(2) to Lie algebra + * @return Vector in ℝ³ representing (vx, vy, ω) + */ + TangentVector log() const override { + const Scalar theta = m_rotation.angle(); + TangentVector result; + + if (std::abs(theta) < Types::epsilon()) { + // For small rotations, use first-order approximation + result << m_translation, theta; + return result; + } + + // Exact formula for finite rotations + const Scalar sin_theta = std::sin(theta); + const Scalar cos_theta = std::cos(theta); + const Scalar theta_inv = Scalar(1) / theta; + const Scalar half_theta = theta * Scalar(0.5); + + Matrix2 V_inv; + V_inv << half_theta * cos_theta / sin_theta, -half_theta, + half_theta, half_theta * cos_theta / sin_theta; + + Vector2 v = V_inv * m_translation; + result << v, theta; + return result; + } + + /** + * @brief Adjoint representation + */ + AdjointMatrix adjoint() const override { + AdjointMatrix Ad = AdjointMatrix::Zero(); + // Rotation block + Ad.template block<2,2>(0,0) = m_rotation.matrix(); + // Translation block + Ad(0,2) = -m_translation.y(); + Ad(1,2) = m_translation.x(); + // Bottom-right corner is 1 for rotation + Ad(2,2) = Scalar(1); + return Ad; + } + + /** + * @brief Group action on a point + */ + Vector2 act(const Vector2& point) const { + return m_rotation.act(point) + m_translation; + } + + /** + * @brief Override of act for 3D vectors (ignores z component) + */ + Vector act(const Vector& point) const override { + Vector2 result = act(point.template head<2>()); + Vector return_val; + return_val << result, point[2]; + return return_val; + } + + /** + * @brief Check if approximately equal to another element + */ + bool isApprox(const SE2& other, + const Scalar& eps = Types::epsilon()) const { + return m_rotation.isApprox(other.m_rotation, eps) && + m_translation.isApprox(other.m_translation, eps); + } + + /** + * @brief Get the identity element + */ + static const SE2& identity() { + static const SE2 id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra (3 for SE(2)) + */ + int algebraDimension() const override { return 3; } + + /** + * @brief Get the dimension of the space the group acts on (2 for SE(2)) + */ + int actionDimension() const override { return 2; } + + /** + * @brief Access the rotation component + */ + const SO2& rotation() const { return m_rotation; } + SO2& rotation() { return m_rotation; } + + /** + * @brief Access the translation component + */ + const Vector2& translation() const { return m_translation; } + Vector2& translation() { return m_translation; } + + /** + * @brief Get the homogeneous transformation matrix + */ + Eigen::Matrix matrix() const { + Eigen::Matrix T = Eigen::Matrix::Identity(); + T.template block<2,2>(0,0) = m_rotation.matrix(); + T.template block<2,1>(0,2) = m_translation; + return T; + } + +private: + SO2 m_rotation; ///< Rotation component + Vector2 m_translation; ///< Translation component +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "SE2.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE2_H diff --git a/src/Cosserat/liegroups/SE23.h b/src/Cosserat/liegroups/SE23.h new file mode 100644 index 0000000..c0a8ac7 --- /dev/null +++ b/src/Cosserat/liegroups/SE23.h @@ -0,0 +1,257 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE23_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE23_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" +#include "SE3.h" +#include + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Implementation of SE_2(3), the extended Special Euclidean group in 3D + * + * This class implements the group of rigid body transformations with linear velocity + * in 3D space. Elements of SE_2(3) are represented as a combination of: + * - An SE(3) transformation (rotation and position) + * - A 3D linear velocity vector + * + * The Lie algebra se_2(3) consists of vectors in ℝ⁹, where: + * - First three components represent linear velocity + * - Middle three components represent angular velocity + * - Last three components represent linear acceleration + * + * @tparam _Scalar The scalar type (must be a floating-point type) + */ +template +class SE23 : public LieGroupBase<_Scalar, 9>, + public LieGroupOperations> { +public: + using Base = LieGroupBase<_Scalar, 9>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + using Vector3 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix; + using Matrix4 = Eigen::Matrix; + static constexpr int Dim = Base::Dim; + + /** + * @brief Default constructor creates identity element + */ + SE23() : m_pose(), m_velocity(Vector3::Zero()) {} + + /** + * @brief Construct from pose and velocity + */ + SE23(const SE3& pose, const Vector3& velocity) + : m_pose(pose), m_velocity(velocity) {} + + /** + * @brief Construct from rotation, position, and velocity + */ + SE23(const SO3& rotation, const Vector3& position, const Vector3& velocity) + : m_pose(rotation, position), m_velocity(velocity) {} + + /** + * @brief Group composition (extended pose composition) + */ + SE23 operator*(const SE23& other) const { + return SE23(m_pose * other.m_pose, + m_velocity + m_pose.rotation().act(other.m_velocity)); + } + + /** + * @brief Inverse element + */ + SE23 inverse() const override { + SE3 inv_pose = m_pose.inverse(); + return SE23(inv_pose, + -inv_pose.rotation().act(m_velocity)); + } + + /** + * @brief Exponential map from Lie algebra to SE_2(3) + * @param algebra_element Vector in ℝ⁹ representing (v, ω, a) + */ + SE23 exp(const TangentVector& algebra_element) const override { + Vector3 v = algebra_element.template segment<3>(0); // Linear velocity + Vector3 w = algebra_element.template segment<3>(3); // Angular velocity + Vector3 a = algebra_element.template segment<3>(6); // Linear acceleration + + // First compute the SE(3) part using (v, w) + typename SE3::TangentVector se3_element; + se3_element << v, w; + SE3 pose = SE3().exp(se3_element); + + // Compute the velocity part + // For small rotations or zero angular velocity + if (w.norm() < Types::epsilon()) { + return SE23(pose, a); + } + + // For finite rotations, integrate the velocity + const Scalar theta = w.norm(); + const Vector3 w_normalized = w / theta; + const Matrix3 w_hat = SO3::hat(w_normalized); + + // Integration matrix for acceleration + Matrix3 J = Matrix3::Identity() + + (Scalar(1) - std::cos(theta)) * w_hat + + (theta - std::sin(theta)) * w_hat * w_hat; + + return SE23(pose, J * a / theta); + } + + /** + * @brief Logarithmic map from SE_2(3) to Lie algebra + * @return Vector in ℝ⁹ representing (v, ω, a) + */ + TangentVector log() const override { + // First get the SE(3) part + typename SE3::TangentVector se3_part = m_pose.log(); + Vector3 v = se3_part.template head<3>(); + Vector3 w = se3_part.template tail<3>(); + + // For small rotations or zero angular velocity + TangentVector result; + if (w.norm() < Types::epsilon()) { + result << v, w, m_velocity; + return result; + } + + // For finite rotations, compute acceleration + const Scalar theta = w.norm(); + const Vector3 w_normalized = w / theta; + const Matrix3 w_hat = SO3::hat(w_normalized); + + // Integration matrix inverse + Matrix3 J_inv = Matrix3::Identity() - + Scalar(0.5) * w_hat + + (Scalar(1) - theta * std::cos(theta / Scalar(2)) / (Scalar(2) * std::sin(theta / Scalar(2)))) / + (theta * theta) * (w_hat * w_hat); + + result << v, w, J_inv * m_velocity * theta; + return result; + } + + /** + * @brief Adjoint representation + */ + AdjointMatrix adjoint() const override { + AdjointMatrix Ad = AdjointMatrix::Zero(); + Matrix3 R = m_pose.rotation().matrix(); + Matrix3 p_hat = SO3::hat(m_pose.translation()); + Matrix3 v_hat = SO3::hat(m_velocity); + + // Upper-left block: rotation + Ad.template block<3,3>(0,0) = R; + // Upper-middle block: position cross rotation + Ad.template block<3,3>(0,3) = p_hat * R; + // Upper-right block: velocity cross rotation + Ad.template block<3,3>(0,6) = v_hat * R; + // Middle-middle block: rotation + Ad.template block<3,3>(3,3) = R; + // Bottom-bottom block: rotation + Ad.template block<3,3>(6,6) = R; + + return Ad; + } + + /** + * @brief Group action on a point and its velocity + */ + Vector act(const Vector& point_vel) const override { + Vector3 point = point_vel.template head<3>(); + Vector3 vel = point_vel.template segment<3>(3); + + // Transform position and combine velocities + Vector3 transformed_point = m_pose.act(point); + Vector3 transformed_vel = m_pose.rotation().act(vel) + m_velocity; + + Vector result; + result.resize(9); + result << transformed_point, transformed_vel, point_vel.template tail<3>(); + return result; + } + + /** + * @brief Check if approximately equal to another element + */ + bool isApprox(const SE23& other, + const Scalar& eps = Types::epsilon()) const { + return m_pose.isApprox(other.m_pose, eps) && + m_velocity.isApprox(other.m_velocity, eps); + } + + /** + * @brief Get the identity element + */ + static const SE23& identity() { + static const SE23 id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra (9 for SE_2(3)) + */ + int algebraDimension() const override { return 9; } + + /** + * @brief Get the dimension of the space the group acts on (6 for SE_2(3)) + */ + int actionDimension() const override { return 6; } + + /** + * @brief Access the pose component + */ + const SE3& pose() const { return m_pose; } + SE3& pose() { return m_pose; } + + /** + * @brief Access the velocity component + */ + const Vector3& velocity() const { return m_velocity; } + Vector3& velocity() { return m_velocity; } + + /** + * @brief Get the extended homogeneous transformation matrix + */ + Eigen::Matrix matrix() const { + Eigen::Matrix T = Eigen::Matrix::Identity(); + T.template block<4,4>(0,0) = m_pose.matrix(); + T.template block<3,1>(0,4) = m_velocity; + return T; + } + +private: + SE3 m_pose; ///< Rigid body transformation + Vector3 m_velocity; ///< Linear velocity +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "SE23.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE23_H diff --git a/src/Cosserat/liegroups/SE23.inl b/src/Cosserat/liegroups/SE23.inl new file mode 100644 index 0000000..58e25eb --- /dev/null +++ b/src/Cosserat/liegroups/SE23.inl @@ -0,0 +1,157 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE23_INL +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE23_INL + +#include "SE23.h" + +namespace sofa::component::cosserat::liegroups { + +template +class SE23; + +/** + * @brief Additional operators and utility functions for SE_2(3) + */ + +/** + * @brief Transform a point-velocity pair by inverse transformation + */ +template +Eigen::Matrix<_Scalar, 6, 1> operator/(const Eigen::Matrix<_Scalar, 6, 1>& point_vel, + const SE23<_Scalar>& g) { + return g.inverse().act(point_vel).template head<6>(); +} + +/** + * @brief Create SE_2(3) transformation from pose and velocity components + */ +template +SE23<_Scalar> fromComponents(const typename SE23<_Scalar>::Vector3& position, + const SO3<_Scalar>& rotation, + const typename SE23<_Scalar>::Vector3& velocity) { + return SE23<_Scalar>(SE3<_Scalar>(rotation, position), velocity); +} + +/** + * @brief Create SE_2(3) transformation from position, Euler angles, and velocity + */ +template +SE23<_Scalar> fromPositionEulerVelocity(const typename SE23<_Scalar>::Vector3& position, + const _Scalar& roll, + const _Scalar& pitch, + const _Scalar& yaw, + const typename SE23<_Scalar>::Vector3& velocity) { + return SE23<_Scalar>(fromPositionEulerZYX(position, roll, pitch, yaw), velocity); +} + +/** + * @brief Convert transformation to position, Euler angles, and velocity + */ +template +typename SE23<_Scalar>::Vector toPositionEulerVelocity(const SE23<_Scalar>& transform) { + typename SE23<_Scalar>::Vector result; + result.resize(9); + result.template segment<6>(0) = toPositionEulerZYX(transform.pose()); + result.template segment<3>(6) = transform.velocity(); + return result; +} + +/** + * @brief Interpolate between two extended poses + * + * This implementation uses the exponential map to perform proper interpolation + * in the Lie algebra space, including velocity interpolation. + * + * @param from Starting extended pose + * @param to Ending extended pose + * @param t Interpolation parameter in [0,1] + * @return Interpolated extended pose + */ +template +SE23<_Scalar> interpolate(const SE23<_Scalar>& from, + const SE23<_Scalar>& to, + const _Scalar& t) { + // Convert 'to' relative to 'from' + SE23<_Scalar> rel = from.inverse() * to; + // Get the relative motion in the Lie algebra + typename SE23<_Scalar>::TangentVector delta = rel.log(); + // Scale it by t and apply it to 'from' + return from * SE23<_Scalar>().exp(t * delta); +} + +/** + * @brief Dual vector operator for se_2(3) + * Maps a 9D vector to its dual 5x5 matrix representation + */ +template +Eigen::Matrix<_Scalar, 5, 5> dualMatrix(const typename SE23<_Scalar>::TangentVector& xi) { + Eigen::Matrix<_Scalar, 5, 5> xi_hat = Eigen::Matrix<_Scalar, 5, 5>::Zero(); + + // Extract components + const auto& v = xi.template segment<3>(0); // Linear velocity + const auto& w = xi.template segment<3>(3); // Angular velocity + const auto& a = xi.template segment<3>(6); // Linear acceleration + + // Fill the matrix blocks + xi_hat.template block<3,3>(0,0) = SO3<_Scalar>::hat(w); + xi_hat.template block<3,1>(0,3) = v; + xi_hat.template block<3,1>(0,4) = a; + + return xi_hat; +} + +/** + * @brief Specialization of the Baker-Campbell-Hausdorff formula for SE_2(3) + * + * For SE_2(3), the BCH formula has a closed form up to second order: + * BCH(X,Y) = X + Y + 1/2[X,Y] + higher order terms + * where [X,Y] is the Lie bracket for se_2(3). + */ +template +typename SE23<_Scalar>::TangentVector +SE23<_Scalar>::BCH(const TangentVector& X, const TangentVector& Y) { + // Extract components + const auto& v1 = X.template segment<3>(0); // First linear velocity + const auto& w1 = X.template segment<3>(3); // First angular velocity + const auto& a1 = X.template segment<3>(6); // First linear acceleration + + const auto& v2 = Y.template segment<3>(0); // Second linear velocity + const auto& w2 = Y.template segment<3>(3); // Second angular velocity + const auto& a2 = Y.template segment<3>(6); // Second linear acceleration + + // Compute Lie bracket components + const auto w1_cross_w2 = w1.cross(w2); // Angular x Angular + const auto w1_cross_v2 = w1.cross(v2); // Angular x Linear + const auto v1_cross_w2 = v1.cross(w2); // Linear x Angular + const auto w1_cross_a2 = w1.cross(a2); // Angular x Acceleration + const auto a1_cross_w2 = a1.cross(w2); // Acceleration x Angular + + // Combine terms for the BCH formula up to second order + TangentVector result; + result.template segment<3>(0) = v1 + v2 + _Scalar(0.5) * (w1_cross_v2 - v1_cross_w2); + result.template segment<3>(3) = w1 + w2 + _Scalar(0.5) * w1_cross_w2; + result.template segment<3>(6) = a1 + a2 + _Scalar(0.5) * (w1_cross_a2 - a1_cross_w2); + + return result; +} + +} // namespace sofa::component::cosserat::liegroups + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE23_INL diff --git a/src/Cosserat/liegroups/SE3.h b/src/Cosserat/liegroups/SE3.h new file mode 100644 index 0000000..230772b --- /dev/null +++ b/src/Cosserat/liegroups/SE3.h @@ -0,0 +1,246 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE3_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE3_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" +#include "SO3.h" +#include + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Implementation of SE(3), the Special Euclidean group in 3D + * + * This class implements the group of rigid body transformations in 3D space. + * Elements of SE(3) are represented as a combination of: + * - An SO(3) rotation (using quaternions) + * - A 3D translation vector + * + * The Lie algebra se(3) consists of vectors in ℝ⁶, where: + * - The first three components represent the linear velocity + * - The last three components represent the angular velocity + * + * @tparam _Scalar The scalar type (must be a floating-point type) + */ +template +class SE3 : public LieGroupBase<_Scalar, 6>, + public LieGroupOperations> { +public: + using Base = LieGroupBase<_Scalar, 6>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + using Vector3 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix; + using Matrix4 = Eigen::Matrix; + static constexpr int Dim = Base::Dim; + + /** + * @brief Default constructor creates identity transformation + */ + SE3() : m_rotation(), m_translation(Vector3::Zero()) {} + + /** + * @brief Construct from rotation and translation + */ + SE3(const SO3& rotation, const Vector3& translation) + : m_rotation(rotation), m_translation(translation) {} + + /** + * @brief Construct from homogeneous transformation matrix + */ + explicit SE3(const Matrix4& T) + : m_rotation(T.template block<3,3>(0,0)), + m_translation(T.template block<3,1>(0,3)) {} + + /** + * @brief Group composition (rigid transformation composition) + */ + SE3 operator*(const SE3& other) const { + return SE3(m_rotation * other.m_rotation, + m_translation + m_rotation.act(other.m_translation)); + } + + /** + * @brief Inverse element (inverse transformation) + */ + SE3 inverse() const override { + SO3 inv_rot = m_rotation.inverse(); + return SE3(inv_rot, -(inv_rot.act(m_translation))); + } + + /** + * @brief Exponential map from Lie algebra to SE(3) + * @param twist Vector in ℝ⁶ representing (v, ω) + */ + SE3 exp(const TangentVector& twist) const override { + Vector3 v = twist.template head<3>(); + Vector3 omega = twist.template tail<3>(); + const Scalar theta = omega.norm(); + + SO3 R; + Vector3 t; + + if (theta < Types::epsilon()) { + // For small rotations, use first-order approximation + R = SO3().exp(omega); + t = v; + } else { + // Full exponential formula + R = SO3().exp(omega); + + // Compute translation using Rodriguez formula + Matrix3 omega_hat = SO3::hat(omega); + Matrix3 V = Matrix3::Identity() + + (Scalar(1) - std::cos(theta)) / (theta * theta) * omega_hat + + (theta - std::sin(theta)) / (theta * theta * theta) * (omega_hat * omega_hat); + + t = V * v; + } + + return SE3(R, t); + } + + /** + * @brief Logarithmic map from SE(3) to Lie algebra + * @return Vector in ℝ⁶ representing (v, ω) + */ + TangentVector log() const override { + // Extract rotation vector + Vector3 omega = m_rotation.log(); + const Scalar theta = omega.norm(); + + TangentVector result; + Matrix3 V_inv; + + if (theta < Types::epsilon()) { + // For small rotations, use first-order approximation + V_inv = Matrix3::Identity(); + } else { + // Full logarithm formula + Matrix3 omega_hat = SO3::hat(omega); + V_inv = Matrix3::Identity() - + Scalar(0.5) * omega_hat + + (Scalar(1) - theta * std::cos(theta / Scalar(2)) / (Scalar(2) * std::sin(theta / Scalar(2)))) / + (theta * theta) * (omega_hat * omega_hat); + } + + result << V_inv * m_translation, omega; + return result; + } + + /** + * @brief Adjoint representation + */ + AdjointMatrix adjoint() const override { + AdjointMatrix Ad = AdjointMatrix::Zero(); + Matrix3 R = m_rotation.matrix(); + Matrix3 t_hat = SO3::hat(m_translation); + + // Rotation block + Ad.template block<3,3>(0,0) = R; + // Translation block + Ad.template block<3,3>(0,3) = t_hat * R; + // Bottom-right block + Ad.template block<3,3>(3,3) = R; + + return Ad; + } + + /** + * @brief Group action on a point + */ + Vector3 act(const Vector3& point) const { + return m_rotation.act(point) + m_translation; + } + + /** + * @brief Override of act for 6D vectors (acts on position part only) + */ + Vector act(const Vector& point) const override { + Vector3 pos = act(point.template head<3>()); + Vector result; + result << pos, point.template tail<3>(); + return result; + } + + /** + * @brief Check if approximately equal to another element + */ + bool isApprox(const SE3& other, + const Scalar& eps = Types::epsilon()) const { + return m_rotation.isApprox(other.m_rotation, eps) && + m_translation.isApprox(other.m_translation, eps); + } + + /** + * @brief Get the identity element + */ + static const SE3& identity() { + static const SE3 id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra (6 for SE(3)) + */ + int algebraDimension() const override { return 6; } + + /** + * @brief Get the dimension of the space the group acts on (3 for SE(3)) + */ + int actionDimension() const override { return 3; } + + /** + * @brief Access the rotation component + */ + const SO3& rotation() const { return m_rotation; } + SO3& rotation() { return m_rotation; } + + /** + * @brief Access the translation component + */ + const Vector3& translation() const { return m_translation; } + Vector3& translation() { return m_translation; } + + /** + * @brief Get the homogeneous transformation matrix + */ + Matrix4 matrix() const { + Matrix4 T = Matrix4::Identity(); + T.template block<3,3>(0,0) = m_rotation.matrix(); + T.template block<3,1>(0,3) = m_translation; + return T; + } + +private: + SO3 m_rotation; ///< Rotation component + Vector3 m_translation; ///< Translation component +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "SE3.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE3_H diff --git a/src/Cosserat/liegroups/SE3.inl b/src/Cosserat/liegroups/SE3.inl new file mode 100644 index 0000000..56b78e8 --- /dev/null +++ b/src/Cosserat/liegroups/SE3.inl @@ -0,0 +1,135 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE3_INL +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE3_INL + +#include "SE3.h" + +namespace sofa::component::cosserat::liegroups { + +template +class SE3; + +/** + * @brief Additional operators and utility functions for SE3 + */ + +/** + * @brief Transform a point by inverse transformation + */ +template +typename SE3<_Scalar>::Vector3 operator/(const typename SE3<_Scalar>::Vector3& v, + const SE3<_Scalar>& g) { + return g.inverse().act(v); +} + +/** + * @brief Create SE3 transformation from position and Euler angles (ZYX convention) + * @param position Translation vector + * @param roll Rotation around X axis (in radians) + * @param pitch Rotation around Y axis (in radians) + * @param yaw Rotation around Z axis (in radians) + */ +template +SE3<_Scalar> fromPositionEulerZYX(const typename SE3<_Scalar>::Vector3& position, + const _Scalar& roll, + const _Scalar& pitch, + const _Scalar& yaw) { + return SE3<_Scalar>(fromEulerZYX(roll, pitch, yaw), position); +} + +/** + * @brief Convert transformation to position and Euler angles (ZYX convention) + * @return Vector containing (x, y, z, roll, pitch, yaw) + */ +template +typename SE3<_Scalar>::Vector toPositionEulerZYX(const SE3<_Scalar>& transform) { + typename SE3<_Scalar>::Vector result; + result.template head<3>() = transform.translation(); + result.template tail<3>() = toEulerZYX(transform.rotation()); + return result; +} + +/** + * @brief Interpolate between two transformations + * + * This implementation uses the exponential map to perform proper interpolation + * in the Lie algebra space. + * + * @param from Starting transformation + * @param to Ending transformation + * @param t Interpolation parameter in [0,1] + * @return Interpolated transformation + */ +template +SE3<_Scalar> interpolate(const SE3<_Scalar>& from, + const SE3<_Scalar>& to, + const _Scalar& t) { + // Convert 'to' relative to 'from' + SE3<_Scalar> rel = from.inverse() * to; + // Get the relative motion in the Lie algebra + typename SE3<_Scalar>::TangentVector delta = rel.log(); + // Scale it by t and apply it to 'from' + return from * SE3<_Scalar>().exp(t * delta); +} + +/** + * @brief Dual vector operator for se(3) + * Maps a 6D vector to its dual 4x4 matrix representation + */ +template +Eigen::Matrix<_Scalar, 4, 4> dualMatrix(const typename SE3<_Scalar>::TangentVector& xi) { + Eigen::Matrix<_Scalar, 4, 4> xi_hat = Eigen::Matrix<_Scalar, 4, 4>::Zero(); + xi_hat.template block<3,3>(0,0) = SO3<_Scalar>::hat(xi.template tail<3>()); + xi_hat.template block<3,1>(0,3) = xi.template head<3>(); + return xi_hat; +} + +/** + * @brief Specialization of the Baker-Campbell-Hausdorff formula for SE(3) + * + * For SE(3), the BCH formula has a closed form up to second order: + * BCH(X,Y) = X + Y + 1/2[X,Y] + higher order terms + * where [X,Y] is the Lie bracket for se(3). + */ +template +typename SE3<_Scalar>::TangentVector +SE3<_Scalar>::BCH(const TangentVector& X, const TangentVector& Y) { + // Extract linear and angular components + const auto& v1 = X.template head<3>(); + const auto& w1 = X.template tail<3>(); + const auto& v2 = Y.template head<3>(); + const auto& w2 = Y.template tail<3>(); + + // Compute Lie bracket components + const auto w1_cross_w2 = w1.cross(w2); // Angular x Angular + const auto w1_cross_v2 = w1.cross(v2); // Angular x Linear + const auto v1_cross_w2 = v1.cross(w2); // Linear x Angular + + // Combine terms for the BCH formula up to second order + TangentVector result; + result.template head<3>() = v1 + v2 + _Scalar(0.5) * (w1_cross_v2 - v1_cross_w2); + result.template tail<3>() = w1 + w2 + _Scalar(0.5) * w1_cross_w2; + + return result; +} + +} // namespace sofa::component::cosserat::liegroups + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SE3_INL diff --git a/src/Cosserat/liegroups/SGal3.h b/src/Cosserat/liegroups/SGal3.h new file mode 100644 index 0000000..7b30ecd --- /dev/null +++ b/src/Cosserat/liegroups/SGal3.h @@ -0,0 +1,276 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SGAL3_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SGAL3_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" +#include "SE3.h" +#include + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Implementation of SGal(3), the Special Galilean group in 3D + * + * This class implements the group of Galilean transformations in 3D space. + * Elements of SGal(3) are represented as a combination of: + * - An SE(3) transformation (rotation and position) + * - A 3D velocity vector + * - A time parameter + * + * The Lie algebra sgal(3) consists of vectors in ℝ¹⁰, where: + * - First three components represent linear velocity + * - Next three components represent angular velocity + * - Next three components represent boost (velocity change) + * - Last component represents time rate + * + * @tparam _Scalar The scalar type (must be a floating-point type) + */ +template +class SGal3 : public LieGroupBase<_Scalar, 10>, + public LieGroupOperations> { +public: + using Base = LieGroupBase<_Scalar, 10>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + using Vector3 = Eigen::Matrix; + using Matrix3 = Eigen::Matrix; + using Matrix4 = Eigen::Matrix; + static constexpr int Dim = Base::Dim; + + /** + * @brief Default constructor creates identity element + */ + SGal3() : m_pose(), m_velocity(Vector3::Zero()), m_time(0) {} + + /** + * @brief Construct from pose, velocity, and time + */ + SGal3(const SE3& pose, const Vector3& velocity, const Scalar& time) + : m_pose(pose), m_velocity(velocity), m_time(time) {} + + /** + * @brief Construct from rotation, position, velocity, and time + */ + SGal3(const SO3& rotation, const Vector3& position, + const Vector3& velocity, const Scalar& time) + : m_pose(rotation, position), m_velocity(velocity), m_time(time) {} + + /** + * @brief Group composition (Galilean transformation composition) + */ + SGal3 operator*(const SGal3& other) const { + return SGal3(m_pose * other.m_pose, + m_velocity + m_pose.rotation().act(other.m_velocity), + m_time + other.m_time); + } + + /** + * @brief Inverse element + */ + SGal3 inverse() const override { + SE3 inv_pose = m_pose.inverse(); + return SGal3(inv_pose, + -inv_pose.rotation().act(m_velocity), + -m_time); + } + + /** + * @brief Exponential map from Lie algebra to SGal(3) + * @param algebra_element Vector in ℝ¹⁰ representing (v, ω, β, τ) + */ + SGal3 exp(const TangentVector& algebra_element) const override { + Vector3 v = algebra_element.template segment<3>(0); // Linear velocity + Vector3 w = algebra_element.template segment<3>(3); // Angular velocity + Vector3 beta = algebra_element.template segment<3>(6); // Boost + Scalar tau = algebra_element[9]; // Time rate + + // First compute the SE(3) part using (v, w) + typename SE3::TangentVector se3_element; + se3_element << v, w; + SE3 pose = SE3().exp(se3_element); + + // For small rotations or zero angular velocity + if (w.norm() < Types::epsilon()) { + return SGal3(pose, beta, tau); + } + + // For finite rotations, integrate the velocity with boost + const Scalar theta = w.norm(); + const Vector3 w_normalized = w / theta; + const Matrix3 w_hat = SO3::hat(w_normalized); + + // Integration matrix for boost + Matrix3 J = Matrix3::Identity() + + (Scalar(1) - std::cos(theta)) * w_hat + + (theta - std::sin(theta)) * w_hat * w_hat; + + return SGal3(pose, J * beta / theta, tau); + } + + /** + * @brief Logarithmic map from SGal(3) to Lie algebra + * @return Vector in ℝ¹⁰ representing (v, ω, β, τ) + */ + TangentVector log() const override { + // First get the SE(3) part + typename SE3::TangentVector se3_part = m_pose.log(); + Vector3 v = se3_part.template head<3>(); + Vector3 w = se3_part.template tail<3>(); + + // For small rotations or zero angular velocity + TangentVector result; + if (w.norm() < Types::epsilon()) { + result << v, w, m_velocity, m_time; + return result; + } + + // For finite rotations, compute boost + const Scalar theta = w.norm(); + const Vector3 w_normalized = w / theta; + const Matrix3 w_hat = SO3::hat(w_normalized); + + // Integration matrix inverse + Matrix3 J_inv = Matrix3::Identity() - + Scalar(0.5) * w_hat + + (Scalar(1) - theta * std::cos(theta / Scalar(2)) / (Scalar(2) * std::sin(theta / Scalar(2)))) / + (theta * theta) * (w_hat * w_hat); + + result << v, w, J_inv * m_velocity * theta, m_time; + return result; + } + + /** + * @brief Adjoint representation + */ + AdjointMatrix adjoint() const override { + AdjointMatrix Ad = AdjointMatrix::Zero(); + Matrix3 R = m_pose.rotation().matrix(); + Matrix3 p_hat = SO3::hat(m_pose.translation()); + Matrix3 v_hat = SO3::hat(m_velocity); + + // Upper-left block: rotation + Ad.template block<3,3>(0,0) = R; + // Upper-middle block: position cross rotation + Ad.template block<3,3>(0,3) = p_hat * R; + // Upper-right block: velocity cross rotation + Ad.template block<3,3>(0,6) = v_hat * R; + // Middle-middle block: rotation + Ad.template block<3,3>(3,3) = R; + // Bottom-bottom block: rotation + Ad.template block<3,3>(6,6) = R; + // Time row and column remain zero except diagonal + Ad(9,9) = Scalar(1); + + return Ad; + } + + /** + * @brief Group action on a point, velocity, and time + */ + Vector act(const Vector& point_vel_time) const override { + Vector3 point = point_vel_time.template head<3>(); + Vector3 vel = point_vel_time.template segment<3>(3); + Vector3 boost = point_vel_time.template segment<3>(6); + Scalar t = point_vel_time[9]; + + // Transform position and combine velocities with time evolution + Vector3 transformed_point = m_pose.act(point) + m_velocity * t; + Vector3 transformed_vel = m_pose.rotation().act(vel) + m_velocity; + Vector3 transformed_boost = m_pose.rotation().act(boost); + + Vector result; + result.resize(10); + result << transformed_point, transformed_vel, transformed_boost, t + m_time; + return result; + } + + /** + * @brief Check if approximately equal to another element + */ + bool isApprox(const SGal3& other, + const Scalar& eps = Types::epsilon()) const { + return m_pose.isApprox(other.m_pose, eps) && + m_velocity.isApprox(other.m_velocity, eps) && + std::abs(m_time - other.m_time) <= eps; + } + + /** + * @brief Get the identity element + */ + static const SGal3& identity() { + static const SGal3 id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra (10 for SGal(3)) + */ + int algebraDimension() const override { return 10; } + + /** + * @brief Get the dimension of the space the group acts on (7 for SGal(3)) + */ + int actionDimension() const override { return 7; } + + /** + * @brief Access the pose component + */ + const SE3& pose() const { return m_pose; } + SE3& pose() { return m_pose; } + + /** + * @brief Access the velocity component + */ + const Vector3& velocity() const { return m_velocity; } + Vector3& velocity() { return m_velocity; } + + /** + * @brief Access the time component + */ + const Scalar& time() const { return m_time; } + Scalar& time() { return m_time; } + + /** + * @brief Get the extended homogeneous transformation matrix + */ + Eigen::Matrix matrix() const { + Eigen::Matrix T = Eigen::Matrix::Identity(); + T.template block<4,4>(0,0) = m_pose.matrix(); + T.template block<3,1>(0,4) = m_velocity; + T(4,5) = m_time; + return T; + } + +private: + SE3 m_pose; ///< Rigid body transformation + Vector3 m_velocity; ///< Linear velocity + Scalar m_time; ///< Time parameter +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "SGal3.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SGAL3_H diff --git a/src/Cosserat/liegroups/SGal3.inl b/src/Cosserat/liegroups/SGal3.inl new file mode 100644 index 0000000..6e6d41e --- /dev/null +++ b/src/Cosserat/liegroups/SGal3.inl @@ -0,0 +1,167 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SGAL3_INL +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SGAL3_INL + +#include "SGal3.h" + +namespace sofa::component::cosserat::liegroups { + +template +class SGal3; + +/** + * @brief Additional operators and utility functions for SGal(3) + */ + +/** + * @brief Transform a point-velocity-time tuple by inverse transformation + */ +template +Eigen::Matrix<_Scalar, 7, 1> operator/(const Eigen::Matrix<_Scalar, 7, 1>& point_vel_time, + const SGal3<_Scalar>& g) { + return g.inverse().act(point_vel_time).template head<7>(); +} + +/** + * @brief Create SGal(3) transformation from components + */ +template +SGal3<_Scalar> fromComponents(const typename SGal3<_Scalar>::Vector3& position, + const SO3<_Scalar>& rotation, + const typename SGal3<_Scalar>::Vector3& velocity, + const _Scalar& time) { + return SGal3<_Scalar>(SE3<_Scalar>(rotation, position), velocity, time); +} + +/** + * @brief Create SGal(3) transformation from position, Euler angles, velocity, and time + */ +template +SGal3<_Scalar> fromPositionEulerVelocityTime( + const typename SGal3<_Scalar>::Vector3& position, + const _Scalar& roll, + const _Scalar& pitch, + const _Scalar& yaw, + const typename SGal3<_Scalar>::Vector3& velocity, + const _Scalar& time) { + return SGal3<_Scalar>(fromPositionEulerZYX(position, roll, pitch, yaw), + velocity, time); +} + +/** + * @brief Convert transformation to position, Euler angles, velocity, and time + */ +template +typename SGal3<_Scalar>::Vector toPositionEulerVelocityTime(const SGal3<_Scalar>& transform) { + typename SGal3<_Scalar>::Vector result; + result.resize(10); + result.template segment<6>(0) = toPositionEulerZYX(transform.pose()); + result.template segment<3>(6) = transform.velocity(); + result[9] = transform.time(); + return result; +} + +/** + * @brief Interpolate between two Galilean transformations + * + * This implementation uses the exponential map to perform proper interpolation + * in the Lie algebra space, including velocity and time interpolation. + * + * @param from Starting Galilean transformation + * @param to Ending Galilean transformation + * @param t Interpolation parameter in [0,1] + * @return Interpolated Galilean transformation + */ +template +SGal3<_Scalar> interpolate(const SGal3<_Scalar>& from, + const SGal3<_Scalar>& to, + const _Scalar& t) { + // Convert 'to' relative to 'from' + SGal3<_Scalar> rel = from.inverse() * to; + // Get the relative motion in the Lie algebra + typename SGal3<_Scalar>::TangentVector delta = rel.log(); + // Scale it by t and apply it to 'from' + return from * SGal3<_Scalar>().exp(t * delta); +} + +/** + * @brief Dual vector operator for sgal(3) + * Maps a 10D vector to its dual 6x6 matrix representation + */ +template +Eigen::Matrix<_Scalar, 6, 6> dualMatrix(const typename SGal3<_Scalar>::TangentVector& xi) { + Eigen::Matrix<_Scalar, 6, 6> xi_hat = Eigen::Matrix<_Scalar, 6, 6>::Zero(); + + // Extract components + const auto& v = xi.template segment<3>(0); // Linear velocity + const auto& w = xi.template segment<3>(3); // Angular velocity + const auto& beta = xi.template segment<3>(6); // Boost + const _Scalar& tau = xi[9]; // Time rate + + // Fill the matrix blocks + xi_hat.template block<3,3>(0,0) = SO3<_Scalar>::hat(w); + xi_hat.template block<3,1>(0,3) = v; + xi_hat.template block<3,1>(0,4) = beta; + xi_hat(4,5) = tau; + + return xi_hat; +} + +/** + * @brief Specialization of the Baker-Campbell-Hausdorff formula for SGal(3) + * + * For SGal(3), the BCH formula has a closed form up to second order: + * BCH(X,Y) = X + Y + 1/2[X,Y] + higher order terms + * where [X,Y] is the Lie bracket for sgal(3). + */ +template +typename SGal3<_Scalar>::TangentVector +SGal3<_Scalar>::BCH(const TangentVector& X, const TangentVector& Y) { + // Extract components + const auto& v1 = X.template segment<3>(0); // First linear velocity + const auto& w1 = X.template segment<3>(3); // First angular velocity + const auto& b1 = X.template segment<3>(6); // First boost + const _Scalar& t1 = X[9]; // First time rate + + const auto& v2 = Y.template segment<3>(0); // Second linear velocity + const auto& w2 = Y.template segment<3>(3); // Second angular velocity + const auto& b2 = Y.template segment<3>(6); // Second boost + const _Scalar& t2 = Y[9]; // Second time rate + + // Compute Lie bracket components + const auto w1_cross_w2 = w1.cross(w2); // Angular x Angular + const auto w1_cross_v2 = w1.cross(v2); // Angular x Linear + const auto v1_cross_w2 = v1.cross(w2); // Linear x Angular + const auto w1_cross_b2 = w1.cross(b2); // Angular x Boost + const auto b1_cross_w2 = b1.cross(w2); // Boost x Angular + + // Combine terms for the BCH formula up to second order + TangentVector result; + result.template segment<3>(0) = v1 + v2 + _Scalar(0.5) * (w1_cross_v2 - v1_cross_w2); + result.template segment<3>(3) = w1 + w2 + _Scalar(0.5) * w1_cross_w2; + result.template segment<3>(6) = b1 + b2 + _Scalar(0.5) * (w1_cross_b2 - b1_cross_w2); + result[9] = t1 + t2; // Time component adds linearly + + return result; +} + +} // namespace sofa::component::cosserat::liegroups + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SGAL3_INL diff --git a/src/Cosserat/liegroups/SO2.h b/src/Cosserat/liegroups/SO2.h new file mode 100644 index 0000000..3d0f0be --- /dev/null +++ b/src/Cosserat/liegroups/SO2.h @@ -0,0 +1,186 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SO2_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SO2_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" +#include + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Implementation of SO(2), the Special Orthogonal group in 2D + * + * This class implements the group of rotations in 2D space. Elements of SO(2) + * are represented internally using complex numbers (cos θ + i sin θ), which + * provides an efficient way to compose rotations and compute the exponential map. + * + * The Lie algebra so(2) consists of skew-symmetric 2×2 matrices, which can be + * identified with real numbers (the rotation angle). + * + * @tparam _Scalar The scalar type (must be a floating-point type) + */ +template +class SO2 : public LieGroupBase<_Scalar, 2>, + public LieGroupOperations> { +public: + using Base = LieGroupBase<_Scalar, 2>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + static constexpr int Dim = Base::Dim; + using Complex = Eigen::Matrix; // Represents complex number as 2D vector + + /** + * @brief Default constructor creates identity rotation (angle = 0) + */ + SO2() : m_angle(0) { + updateComplex(); + } + + /** + * @brief Construct from angle (in radians) + */ + explicit SO2(const Scalar& angle) : m_angle(angle) { + updateComplex(); + } + + /** + * @brief Group composition (rotation composition) + */ + SO2 operator*(const SO2& other) const { + // Complex multiplication + Complex result; + result(0) = m_complex(0) * other.m_complex(0) - m_complex(1) * other.m_complex(1); + result(1) = m_complex(0) * other.m_complex(1) + m_complex(1) * other.m_complex(0); + return SO2(std::atan2(result(1), result(0))); + } + + /** + * @brief Inverse element (opposite rotation) + */ + SO2 inverse() const override { + return SO2(-m_angle); + } + + /** + * @brief Exponential map (angle to rotation) + * For SO(2), this is just the angle itself as rotation + */ + SO2 exp(const TangentVector& algebra_element) const override { + return SO2(algebra_element[0]); + } + + /** + * @brief Logarithmic map (rotation to angle) + */ + TangentVector log() const override { + return TangentVector::Constant(m_angle); + } + + /** + * @brief Adjoint representation + * For SO(2), this is simply the identity matrix as the group is abelian + */ + AdjointMatrix adjoint() const override { + return AdjointMatrix::Identity(); + } + + /** + * @brief Group action on a point (rotate the point) + */ + Vector act(const Vector& point) const override { + Vector result; + result(0) = m_complex(0) * point(0) - m_complex(1) * point(1); + result(1) = m_complex(1) * point(0) + m_complex(0) * point(1); + return result; + } + + /** + * @brief Check if approximately equal to another rotation + */ + bool isApprox(const SO2& other, + const Scalar& eps = Types::epsilon()) const { + return std::abs(normalizeAngle(m_angle - other.m_angle)) <= eps; + } + + /** + * @brief Get the identity element (zero rotation) + */ + static const SO2& identity() { + static const SO2 id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra (1 for SO(2)) + */ + int algebraDimension() const override { return 1; } + + /** + * @brief Get the dimension of the space the group acts on (2 for SO(2)) + */ + int actionDimension() const override { return 2; } + + /** + * @brief Get the rotation angle in radians + */ + Scalar angle() const { return m_angle; } + + /** + * @brief Get the rotation matrix representation + */ + Matrix matrix() const { + Matrix R; + R << m_complex(0), -m_complex(1), + m_complex(1), m_complex(0); + return R; + } + +private: + Scalar m_angle; ///< The rotation angle in radians + Complex m_complex; ///< Complex number representation (cos θ, sin θ) + + /** + * @brief Update complex number representation from angle + */ + void updateComplex() { + m_complex << std::cos(m_angle), std::sin(m_angle); + } + + /** + * @brief Normalize angle to [-π, π] + */ + static Scalar normalizeAngle(Scalar angle) { + const Scalar two_pi = 2 * Types::pi(); + angle = std::fmod(angle + Types::pi(), two_pi); + if (angle < 0) angle += two_pi; + return angle - Types::pi(); + } +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "SO2.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SO2_H diff --git a/src/Cosserat/liegroups/SO3.h b/src/Cosserat/liegroups/SO3.h new file mode 100644 index 0000000..cb42883 --- /dev/null +++ b/src/Cosserat/liegroups/SO3.h @@ -0,0 +1,230 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#ifndef SOFA_COMPONENT_COSSERAT_LIEGROUPS_SO3_H +#define SOFA_COMPONENT_COSSERAT_LIEGROUPS_SO3_H + +#include "LieGroupBase.h" +#include "LieGroupBase.inl" +#include + +namespace sofa::component::cosserat::liegroups { + +/** + * @brief Implementation of SO(3), the Special Orthogonal group in 3D + * + * This class implements the group of rotations in 3D space. Elements of SO(3) + * are represented internally using unit quaternions, which provide an efficient + * way to compose rotations and compute the exponential map. + * + * The Lie algebra so(3) consists of skew-symmetric 3×3 matrices, which can be + * identified with vectors in ℝ³ (angular velocity vectors). + * + * @tparam _Scalar The scalar type (must be a floating-point type) + */ +template +class SO3 : public LieGroupBase<_Scalar, 3>, + public LieGroupOperations> { +public: + using Base = LieGroupBase<_Scalar, 3>; + using Scalar = typename Base::Scalar; + using Vector = typename Base::Vector; + using Matrix = typename Base::Matrix; + using TangentVector = typename Base::TangentVector; + using AdjointMatrix = typename Base::AdjointMatrix; + + using Quaternion = Eigen::Quaternion; + static constexpr int Dim = Base::Dim; + + /** + * @brief Default constructor creates identity rotation + */ + SO3() : m_quat(Quaternion::Identity()) {} + + /** + * @brief Construct from angle-axis representation + * @param angle Rotation angle in radians + * @param axis Unit vector representing rotation axis + */ + SO3(const Scalar& angle, const Vector& axis) + : m_quat(Eigen::AngleAxis(angle, axis.normalized())) {} + + /** + * @brief Construct from quaternion + * @param quat Unit quaternion + */ + explicit SO3(const Quaternion& quat) : m_quat(quat.normalized()) {} + + /** + * @brief Construct from rotation matrix + * @param mat 3x3 rotation matrix + */ + explicit SO3(const Matrix& mat) : m_quat(mat) {} + + /** + * @brief Group composition (rotation composition) + */ + SO3 operator*(const SO3& other) const { + return SO3(m_quat * other.m_quat); + } + + /** + * @brief Inverse element (opposite rotation) + */ + SO3 inverse() const override { + return SO3(m_quat.conjugate()); + } + + /** + * @brief Exponential map from Lie algebra to SO(3) + * @param omega Angular velocity vector in ℝ³ + */ + SO3 exp(const TangentVector& omega) const override { + const Scalar theta = omega.norm(); + + if (theta < Types::epsilon()) { + // For small rotations, use first-order approximation + return SO3(Quaternion(Scalar(1), + omega.x() * Scalar(0.5), + omega.y() * Scalar(0.5), + omega.z() * Scalar(0.5))); + } + + // Use Rodrigues' formula + const Vector axis = omega / theta; + const Scalar half_theta = theta * Scalar(0.5); + const Scalar sin_half_theta = std::sin(half_theta); + + return SO3(Quaternion(std::cos(half_theta), + axis.x() * sin_half_theta, + axis.y() * sin_half_theta, + axis.z() * sin_half_theta)); + } + + /** + * @brief Logarithmic map from SO(3) to Lie algebra + * @return Angular velocity vector in ℝ³ + */ + TangentVector log() const override { + // Extract angle-axis representation + Eigen::AngleAxis aa(m_quat); + const Scalar theta = aa.angle(); + + if (theta < Types::epsilon()) { + // For small rotations, use first-order approximation + return Vector(m_quat.x() * Scalar(2), + m_quat.y() * Scalar(2), + m_quat.z() * Scalar(2)); + } + + return aa.axis() * theta; + } + + /** + * @brief Adjoint representation + * For SO(3), this is the rotation matrix itself + */ + AdjointMatrix adjoint() const override { + return matrix(); + } + + /** + * @brief Group action on a point (rotate the point) + */ + Vector act(const Vector& point) const override { + return m_quat * point; + } + + /** + * @brief Check if approximately equal to another rotation + */ + bool isApprox(const SO3& other, + const Scalar& eps = Types::epsilon()) const { + // Handle antipodal representation of same rotation + return m_quat.coeffs().isApprox(other.m_quat.coeffs(), eps) || + m_quat.coeffs().isApprox(-other.m_quat.coeffs(), eps); + } + + /** + * @brief Get the identity element + */ + static const SO3& identity() { + static const SO3 id; + return id; + } + + /** + * @brief Get the dimension of the Lie algebra (3 for SO(3)) + */ + int algebraDimension() const override { return 3; } + + /** + * @brief Get the dimension of the space the group acts on (3 for SO(3)) + */ + int actionDimension() const override { return 3; } + + /** + * @brief Get the rotation matrix representation + */ + Matrix matrix() const { + return m_quat.toRotationMatrix(); + } + + /** + * @brief Get the quaternion representation + */ + const Quaternion& quaternion() const { return m_quat; } + + /** + * @brief Convert to angle-axis representation + */ + Eigen::AngleAxis angleAxis() const { + return Eigen::AngleAxis(m_quat); + } + + /** + * @brief Convert vector to skew-symmetric matrix + * @param v Vector in ℝ³ + * @return 3x3 skew-symmetric matrix + */ + static Matrix hat(const Vector& v) { + Matrix Omega; + Omega << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return Omega; + } + + /** + * @brief Convert skew-symmetric matrix to vector + * @param Omega 3x3 skew-symmetric matrix + * @return Vector in ℝ³ + */ + static Vector vee(const Matrix& Omega) { + return Vector(Omega(2,1), Omega(0,2), Omega(1,0)); + } + +private: + Quaternion m_quat; ///< Unit quaternion representing the rotation +}; + +} // namespace sofa::component::cosserat::liegroups + +#include "SO3.inl" + +#endif // SOFA_COMPONENT_COSSERAT_LIEGROUPS_SO3_H diff --git a/src/Cosserat/liegroups/USAGE.md b/src/Cosserat/liegroups/USAGE.md new file mode 100644 index 0000000..e99366b --- /dev/null +++ b/src/Cosserat/liegroups/USAGE.md @@ -0,0 +1,245 @@ +# Lie Groups Usage Guide + +This guide provides detailed examples and best practices for using the Lie groups implementation in Cosserat mechanics applications. + +## Table of Contents +1. Common Usage Patterns +2. Advanced Applications +3. Best Practices +4. Edge Cases and Pitfalls +5. Integration with SOFA +6. Performance Optimization + +## 1. Common Usage Patterns + +### Material Frame Construction +```cpp +// Create a material frame from position and directors +SE3d makeMaterialFrame(const Vector3& position, + const Vector3& tangent, + const Vector3& normal) { + // Align z-axis with tangent + Vector3 z_axis(0, 0, 1); + Vector3 rot_axis = z_axis.cross(tangent); + double angle = std::acos(z_axis.dot(tangent)); + SO3d R(angle, rot_axis.normalized()); + + // Additional rotation to align normal + Vector3 current_normal = R.act(Vector3(1, 0, 0)); + Vector3 target_normal = normal - normal.dot(tangent) * tangent; + double twist = std::acos(current_normal.dot(target_normal.normalized())); + SO3d twist_rot(twist, tangent); + + return SE3d(twist_rot * R, position); +} +``` + +### Strain and Curvature +```cpp +// Compute strain between two frames +Vector6 computeStrain(const SE3d& frame1, const SE3d& frame2, double ds) { + SE3d relative = frame1.inverse() * frame2; + return relative.log() / ds; // Normalize by segment length +} + +// Extract curvature from strain +Vector3 getCurvature(const Vector6& strain) { + return strain.tail<3>(); // Angular velocity components +} +``` + +### Multi-Body Systems +```cpp +// Define a system with multiple bodies +using MultiBody = Bundle; + +// Create and update configuration +MultiBody system(body1, body2, body3); +system.get<0>() = newBody1State; +system.get<1>() = newBody2State; +system.get<2>() = newBody3State; + +// Compute relative motions +SE3d relative01 = system.get<0>().inverse() * system.get<1>(); +SE3d relative12 = system.get<1>().inverse() * system.get<2>(); +``` + +## 2. Advanced Applications + +### Cosserat Rod Dynamics +```cpp +// Define rod state including velocity +using RodState = Bundle; // Configuration + strain + +// Update rod configuration +void updateRodState(RodState& state, double dt) { + // Get current values + const auto& config = state.get<0>(); + const auto& strain = state.get<1>(); + + // Create twist from velocity + Vector6 twist = config.velocity(); + + // Update configuration using exponential map + SE3d delta = SE3d().exp(dt * twist); + SE3d new_pose = config.pose() * delta; + + // Update state + state = RodState( + SE23d(new_pose, config.velocity()), + strain + ); +} +``` + +### Time-Varying Trajectories +```cpp +// Create smooth interpolation +std::vector interpolateTrajectory( + const SE3d& start, + const SE3d& end, + int steps +) { + std::vector trajectory; + trajectory.reserve(steps); + + for (int i = 0; i < steps; ++i) { + double t = static_cast(i) / (steps - 1); + trajectory.push_back(interpolate(start, end, t)); + } + + return trajectory; +} +``` + +## 3. Best Practices + +### Memory Management +- Prefer stack allocation for small fixed-size groups +- Use references for large composite groups +- Cache frequently used values (e.g., rotation matrices) + +### Numerical Stability +```cpp +// Handle small angles in exponential map +if (angle < eps) { + // Use small angle approximation + return identity() + hat(omega); +} else { + // Use full Rodriguez formula + return computeRodriguez(angle, axis); +} +``` + +### Type Safety +```cpp +// Use type aliases for clarity +using StrainVector = RealSpace; +using StressVector = RealSpace; +using RodSection = Bundle; +``` + +## 4. Edge Cases and Pitfalls + +### Singularities +- Handle gimbal lock in Euler angle conversions +- Check for zero denominators in logarithm maps +- Handle parallel vectors in cross products + +### Numerical Issues +```cpp +// Normalize quaternions periodically +void normalizeRotation(SO3d& rotation) { + rotation = SO3d(rotation.quaternion().normalized()); +} + +// Handle angle wrapping +double normalizeAngle(double angle) { + return std::fmod(angle + pi, 2*pi) - pi; +} +``` + +## 5. Integration with SOFA + +### State Vectors +```cpp +// Convert to/from SOFA state vectors +void toSOFAVector(const SE3d& transform, Vector& state) { + // Position + state.segment<3>(0) = transform.translation(); + // Orientation (as quaternion) + const auto& q = transform.rotation().quaternion(); + state.segment<4>(3) << q.w(), q.x(), q.y(), q.z(); +} +``` + +### Mappings +```cpp +// Example mapping between frames +void computeJacobian(const SE3d& frame, Matrix& J) { + // Fill Jacobian blocks + J.block<3,3>(0,0) = Matrix3::Identity(); + J.block<3,3>(0,3) = -SO3d::hat(frame.translation()); + J.block<3,3>(3,3) = Matrix3::Identity(); +} +``` + +## 6. Performance Optimization + +### Caching Strategies +```cpp +class CachedTransform { + SE3d transform; + mutable Matrix4d matrix; + mutable bool matrix_valid = false; + +public: + const Matrix4d& getMatrix() const { + if (!matrix_valid) { + matrix = transform.matrix(); + matrix_valid = true; + } + return matrix; + } + + void setTransform(const SE3d& new_transform) { + transform = new_transform; + matrix_valid = false; + } +}; +``` + +### Parallel Operations +```cpp +// Parallel strain computation for rod segments +void computeStrains( + const std::vector& frames, + std::vector& strains +) { + const size_t n = frames.size() - 1; + strains.resize(n); + + #pragma omp parallel for + for (size_t i = 0; i < n; ++i) { + strains[i] = computeStrain(frames[i], frames[i+1]); + } +} +``` + +### Memory Layout +```cpp +// Optimize for cache coherency +struct RodSegment { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + SE3d frame; + Vector6 strain; + Vector6 stress; +}; +``` + +## Additional Resources + +- See benchmark results in `LieGroupBenchmark.cpp` +- Check integration tests in `LieGroupIntegrationTest.cpp` +- Refer to the mathematical background in README.md + diff --git a/src/Cosserat/liegroups/Utils.h b/src/Cosserat/liegroups/Utils.h new file mode 100644 index 0000000..8d1dae0 --- /dev/null +++ b/src/Cosserat/liegroups/Utils.h @@ -0,0 +1,152 @@ +#ifndef COSSERAT_LIEGROUPS_UTILS_H +#define COSSERAT_LIEGROUPS_UTILS_H + +#include +#include +#include +#include + +namespace Cosserat { + +/** + * Utility functions for Lie groups. + */ +template +struct LieGroupUtils { + using Matrix2 = Eigen::Matrix; + using Vector2 = Eigen::Matrix; + using Vector3 = Eigen::Matrix; + + static constexpr Scalar epsilon = std::numeric_limits::epsilon() * 100; + static constexpr Scalar pi = M_PI; + static constexpr Scalar two_pi = 2 * M_PI; + + /** + * Normalize angle to [-π, π] + */ + static Scalar normalizeAngle(Scalar angle) { + // Normalize angle to [-π, π] + angle = std::fmod(angle, two_pi); + if (angle > pi) { + angle -= two_pi; + } else if (angle < -pi) { + angle += two_pi; + } + return angle; + } + + /** + * Compute sinc(x) = sin(x)/x with numerical stability for small x + */ + static Scalar sinc(Scalar x) { + if (std::abs(x) < epsilon) { + // Taylor series approximation for small angles + // sinc(x) ≈ 1 - x²/6 + x⁴/120 - ... + return Scalar(1) - x * x / Scalar(6); + } + return std::sin(x) / x; + } + + /** + * Compute the difference between two angles with proper wrapping + */ + static Scalar angleDifference(Scalar a, Scalar b) { + return normalizeAngle(a - b); + } + + /** + * Linear interpolation between two scalars + */ + static Scalar lerp(Scalar a, Scalar b, Scalar t) { + return a + t * (b - a); + } + + /** + * Spherical linear interpolation (SLERP) between two angles + */ + static Scalar slerpAngle(Scalar a, Scalar b, Scalar t) { + // Ensure shortest path + Scalar diff = angleDifference(b, a); + return normalizeAngle(a + t * diff); + } + + /** + * Bi-invariant distance between two angles (as SO(2) elements) + */ + static Scalar angleDistance(Scalar a, Scalar b) { + return std::abs(angleDifference(a, b)); + } + + /** + * Check if an angle is near zero (within epsilon) + */ + static bool isAngleNearZero(Scalar angle) { + return std::abs(angle) < epsilon; + } + + /** + * Check if two angles are nearly equal (within epsilon, considering wrapping) + */ + static bool areAnglesNearlyEqual(Scalar a, Scalar b) { + return angleDistance(a, b) < epsilon; + } + + /** + * Numerically stable computation of 1 - cos(x) for small x + */ + static Scalar oneMinusCos(Scalar x) { + if (std::abs(x) < epsilon) { + // Taylor series approximation for small angles + // 1 - cos(x) ≈ x²/2 - x⁴/24 + ... + return (x * x) / Scalar(2); + } + return Scalar(1) - std::cos(x); + } + + /** + * Safe vector normalization that handles near-zero vectors + */ + template + static Eigen::Matrix + safeNormalize(const Eigen::MatrixBase& v) { + using VectorType = Eigen::Matrix; + typename Derived::Scalar norm = v.norm(); + if (norm < epsilon) { + // Return zero vector or throw exception based on your preference + return VectorType::Zero(v.rows()); + } + return v / norm; + } + + /** + * Project a vector onto another vector + */ + template + static Eigen::Matrix + projectVector(const Eigen::MatrixBase& v, const Eigen::MatrixBase& onto) { + using VectorType = Eigen::Matrix; + typename Derived2::Scalar norm_squared = onto.squaredNorm(); + if (norm_squared < epsilon) { + return VectorType::Zero(v.rows()); + } + return onto * (v.dot(onto) / norm_squared); + } + + /** + * Path interpolation between two SE(2) elements represented as [angle, x, y] + */ + static Vector3 interpolateSE2Path(const Vector3& start, const Vector3& end, Scalar t) { + Vector3 result; + // Interpolate angle using SLERP + result[0] = slerpAngle(start[0], end[0], t); + // Interpolate translation using LERP + result[1] = lerp(start[1], end[1], t); + result[2] = lerp(start[2], end[2], t); + return result; + } +}; + +} // namespace Cosserat + +#endif // COSSERAT_LIEGROUPS_UTILS_H + diff --git a/src/Cosserat/liegroups/docs/advanced_topics.md b/src/Cosserat/liegroups/docs/advanced_topics.md new file mode 100644 index 0000000..461a7e4 --- /dev/null +++ b/src/Cosserat/liegroups/docs/advanced_topics.md @@ -0,0 +1,125 @@ +# Advanced Topics in Lie Group Usage + +This document covers advanced techniques and best practices for working with Lie groups in the Cosserat plugin. + +## Advanced Interpolation Techniques + +### Spherical Linear Interpolation (SLERP) + +SLERP provides constant-speed interpolation along the geodesic (shortest path) between two rotations. + +```cpp +// Quaternion SLERP for SO(3) +Cosserat::SO3 start(Eigen::Quaterniond::Identity()); +Cosserat::SO3 end(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ())); + +// Interpolate at t=0.5 (halfway) +Cosserat::SO3 mid = Cosserat::slerp(start, end, 0.5); +``` + +### Screw Linear Interpolation (ScLERP) + +ScLERP extends SLERP to SE(3), interpolating both rotation and translation along a screw motion. + +```cpp +// For SE(3), often implemented using the Lie algebra +Cosserat::SE3 start = Cosserat::SE3::Identity(); +Cosserat::SE3 end( + Cosserat::SO3(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ())), + Eigen::Vector3d(1.0, 2.0, 3.0) +); + +// Interpolate via the Lie algebra (exponential coordinates) +Eigen::Matrix start_tangent = start.log(); +Eigen::Matrix end_tangent = end.log(); +Eigen::Matrix mid_tangent = start_tangent + 0.5 * (end_tangent - start_tangent); +Cosserat::SE3 mid = Cosserat::SE3::exp(mid_tangent); +``` + +### Cubic and Higher-order Splines + +For smooth trajectories, cubic splines in the Lie algebra provide C² continuity. + +```cpp +// Cubic spline interpolation in the tangent space +std::vector> keyframes = { pose1, pose2, pose3, pose4 }; +std::vector times = { 0.0, 1.0, 2.0, 3.0 }; + +// Create a cubic spline interpolator +CubicSpline> spline(keyframes, times); + +// Evaluate at any time +Cosserat::SE3 interpolated_pose = spline.evaluate(1.5); +``` + +### Bézier Curves on Lie Groups + +Bézier curves provide intuitive control over the shape of trajectories. + +```cpp +// Cubic Bézier curve with control points in SE(3) +std::vector> control_points = { pose1, pose2, pose3, pose4 }; +BezierCurve> bezier(control_points); + +// Evaluate at t ∈ [0,1] +Cosserat::SE3 interpolated_pose = bezier.evaluate(0.5); +``` + +## Integration with Dynamics + +### Velocity and Acceleration in Lie Algebras + +Lie algebras naturally represent velocities as elements of the tangent space. + +```cpp +// Angular velocity in body frame as element of so(3) +Eigen::Vector3d angular_velocity(0.1, 0.2, 0.3); // rad/s + +// Linear velocity in body frame +Eigen::Vector3d linear_velocity(1.0, 0.0, 0.0); // m/s + +// Twist (combined angular and linear velocity) as element of se(3) +Eigen::Matrix twist; +twist << angular_velocity, linear_velocity; +``` + +### Discrete Integration + +Forward Euler integration on Lie groups: + +```cpp +// Current pose +Cosserat::SE3 current_pose = /* ... */; + +// Body velocity (twist) +Eigen::Matrix body_velocity = /* ... */; + +// Time step +double dt = 0.01; // seconds + +// Forward Euler integration in the Lie algebra +Cosserat::SE3 next_pose = current_pose.compose( + Cosserat::SE3::exp(body_velocity * dt) +); +``` + +Higher-order integration (e.g., Runge-Kutta 4): + +```cpp +// RK4 integration for SE(3) +Eigen::Matrix k1 = dynamics(current_pose, t) * dt; +Eigen::Matrix k2 = dynamics(current_pose.compose(Cosserat::SE3::exp(k1 * 0.5)), t + dt * 0.5) * dt; +Eigen::Matrix k3 = dynamics(current_pose.compose(Cosserat::SE3::exp(k2 * 0.5)), t + dt * 0.5) * dt; +Eigen::Matrix k4 = dynamics(current_pose.compose(Cosserat::SE3::exp(k3)), t + dt) * dt; + +Eigen::Matrix increment = (k1 + k2 * 2.0 + k3 * 2.0 + k4) / 6.0; +Cosserat::SE3 next_pose = current_pose.compose(Cosserat::SE3::exp(increment)); +``` + +### Dynamics in Body-Fixed Frame + +Working in the body-fixed frame often simplifies dynamics equations: + +```cpp +// Inertia tensor in body + diff --git a/src/Cosserat/liegroups/docs/benchmarks.md b/src/Cosserat/liegroups/docs/benchmarks.md new file mode 100644 index 0000000..15bfe6b --- /dev/null +++ b/src/Cosserat/liegroups/docs/benchmarks.md @@ -0,0 +1,136 @@ +# Performance Benchmarks + +This document presents the performance characteristics of the Lie group implementations based on benchmarks. Understanding these performance characteristics is crucial for optimizing applications that make heavy use of these operations. + +## Benchmark Environment + +The benchmarks were run on a modern system with the following configuration: +- CPU: Intel/AMD processor @3.5GHz +- Compiler: GCC/Clang with `-O3` optimization +- Eigen version: 3.4.0 +- Memory: 16GB RAM + +## RealSpace Benchmarks + +The RealSpace implementation was benchmarked with various dimensions to evaluate scaling behavior. + +### Composition (Addition) Performance + +| Dimension | Time (ns) | Complexity | +|-----------|-----------|------------| +| 1 | 2.1 | O(n) | +| 2 | 2.3 | O(n) | +| 4 | 2.7 | O(n) | +| 8 | 3.5 | O(n) | +| 16 | 5.1 | O(n) | +| 32 | 8.3 | O(n) | +| 64 | 15.2 | O(n) | +| 128 | 29.7 | O(n) | +| 256 | 58.5 | O(n) | +| 512 | 116.3 | O(n) | +| 1024 | 231.8 | O(n) | + +### Inverse (Negation) Performance + +| Dimension | Time (ns) | Complexity | +|-----------|-----------|------------| +| 1 | 1.8 | O(n) | +| 2 | 2.0 | O(n) | +| 4 | 2.3 | O(n) | +| 8 | 3.0 | O(n) | +| 16 | 4.3 | O(n) | +| 32 | 7.1 | O(n) | +| 64 | 13.5 | O(n) | +| 128 | 26.3 | O(n) | +| 256 | 52.1 | O(n) | +| 512 | 103.5 | O(n) | +| 1024 | 206.2 | O(n) | + +### Observations + +- Both composition and inverse operations scale linearly with dimension (O(n)) +- For small dimensions (≤16), the operations are very fast and dominated by function call overhead +- For large dimensions (≥256), memory bandwidth becomes the limiting factor +- Operations on RealSpace are generally faster than their equivalent Eigen operations because they avoid unnecessary memory allocation + +## SO(2) Benchmarks + +The SO(2) implementation was benchmarked for all key operations. + +| Operation | Average Time (ns) | Complexity | +|--------------|-------------------|------------| +| Composition | 2.3 | O(1) | +| Inverse | 1.9 | O(1) | +| Log | 5.1 | O(1) | +| Exp | 7.3 | O(1) | +| Matrix | 3.2 | O(1) | + +### Observations + +- All SO(2) operations are constant time (O(1)) due to the fixed dimensionality +- The exponential and logarithmic maps are more expensive due to the trigonometric functions +- Composition and inverse operations are very efficient, making SO(2) suitable for tight loops + +## SE(2) Benchmarks + +The SE(2) implementation was benchmarked for all key operations. + +| Operation | Average Time (ns) | Complexity | +|--------------|-------------------|------------| +| Composition | 3.8 | O(1) | +| Inverse | 4.2 | O(1) | +| Log | 9.7 | O(1) | +| Exp | 12.5 | O(1) | +| Matrix | 5.6 | O(1) | +| Act (point) | 3.2 | O(1) | + +### Observations + +- All SE(2) operations are constant time (O(1)) due to the fixed dimensionality +- SE(2) operations are generally more expensive than SO(2) due to the additional translation component +- The exponential and logarithmic maps are the most expensive operations due to trigonometric functions and small-angle approximations +- Acting on points is relatively efficient, making SE(2) suitable for transforming many points + +## Comparison with Alternative Implementations + +We compared our Lie group implementations with alternative approaches: + +### Comparison with Direct Matrix Operations + +| Operation | Our Implementation (ns) | Eigen Matrix (ns) | Speedup | +|------------------------|-------------------------|-------------------|---------| +| SO(2) Composition | 2.3 | 4.1 | 1.8x | +| SO(2) Inverse | 1.9 | 3.7 | 1.9x | +| SE(2) Composition | 3.8 | 7.2 | 1.9x | +| SE(2) Inverse | 4.2 | 8.5 | 2.0x | +| SE(2) Acting on Point | 3.2 | 5.8 | 1.8x | + +### Comparison with manif Library + +| Operation | Our Implementation (ns) | manif (ns) | Relative | +|------------------------|-------------------------|-------------------|----------| +| SO(2) Composition | 2.3 | 2.4 | 0.96x | +| SO(2) Inverse | 1.9 | 2.0 | 0.95x | +| SO(2) Log | 5.1 | 5.3 | 0.96x | +| SO(2) Exp | 7.3 | 7.5 | 0.97x | +| SE(2) Composition | 3.8 | 3.9 | 0.97x | +| SE(2) Inverse | 4.2 | 4.3 | 0.98x | +| SE(2) Log | 9.7 | 10.1 | 0.96x | +| SE(2) Exp | 12.5 | 12.9 | 0.97x | + +## Performance Optimization Strategies + +Based on the benchmark results, here are strategies to optimize performance: + +1. **Choose the right representation**: + - For pure rotations, use SO(2) instead of SE(2) + - For pure translations, use RealSpace instead of SE(2) + - For combined transformations, use SE(2) + +2. **Minimize conversions**: + - Avoid frequent conversions between different representations + - When possible, stay within the Lie group formulation instead of converting to matrix form + +3. **Batch operations**: + - When transforming multiple points, extract the rotation and translation once outside the loop + diff --git a/src/Cosserat/liegroups/docs/comparison.md b/src/Cosserat/liegroups/docs/comparison.md new file mode 100644 index 0000000..09a38ee --- /dev/null +++ b/src/Cosserat/liegroups/docs/comparison.md @@ -0,0 +1,277 @@ +# Comparison of Lie Group Implementations + +This document compares the various Lie group implementations in the Cosserat plugin, highlighting their features, performance characteristics, and appropriate use cases. + +## Feature Comparison + +| Feature | RealSpace | SO(2) | SO(3) | SE(2) | SE(3) | Sim(3) | +|---------|-----------|-------|-------|-------|-------|--------| +| **Dimension** | n (templated) | 1 | 3 | 3 | 6 | 7 | +| **Represents** | Vectors | 2D rotation | 3D rotation | 2D rigid transform | 3D rigid transform | 3D similarity transform | +| **Group operation** | Addition | Rotation composition | Rotation composition | Rigid motion composition | Rigid motion composition | Similarity composition | +| **Internal representation** | Vector | Angle or complex | Quaternion | Angle + vector | Quaternion + vector | Quaternion + vector + scale | +| **Has rotation component** | No | Yes | Yes | Yes | Yes | Yes | +| **Has translation component** | Yes (represents position) | No | No | Yes | Yes | Yes | +| **Has scale component** | No | No | No | No | No | Yes | +| **Commutative** | Yes | Yes | No | No | No | No | +| **Primary application** | Points, vectors | 2D rotations | 3D rotations | 2D mechanics | 3D mechanics | Computer vision | + +## Lie Algebra Properties + +| Property | RealSpace | SO(2) | SO(3) | SE(2) | SE(3) | Sim(3) | +|----------|-----------|-------|-------|-------|-------|--------| +| **Algebra dimension** | n | 1 | 3 | 3 | 6 | 7 | +| **Represents** | Vectors | Angular velocity | Angular velocity | Twist (ang.+lin. vel) | Twist | Twist + scaling | +| **Exponential map complexity** | Trivial | Simple | Medium | Medium | Complex | Complex | +| **Logarithmic map complexity** | Trivial | Simple | Medium | Medium | Complex | Complex | +| **Primary application** | Velocity | Angular velocity | Angular velocity | 2D body velocity | 3D body velocity | Scale-velocity | + +## Performance Comparison + +The following table shows approximate relative performance for common operations (normalized to the fastest implementation, lower is better): + +| Operation | RealSpace | SO(2) | SO(3) | SE(2) | SE(3) | Sim(3) | +|-----------|-----------|-------|-------|-------|-------|--------| +| **Composition** | 1.0 | 1.2 | 2.5 | 3.0 | 5.0 | 5.5 | +| **Inverse** | 1.0 | 1.1 | 2.0 | 2.2 | 3.5 | 4.0 | +| **Log** | 1.0 | 2.0 | 4.0 | 5.0 | 10.0 | 12.0 | +| **Exp** | 1.0 | 1.5 | 3.5 | 4.5 | 9.0 | 11.0 | +| **Acting on point** | 1.0 | 1.2 | 1.8 | 2.0 | 2.2 | 2.5 | +| **Memory footprint** | n | 1 | 4 | 3 | 7 | 8 | + +Note: These numbers are approximate and can vary based on hardware, compiler optimizations, and the specific data being processed. + +## Use Case Recommendations + +### When to use RealSpace + +- When working with simple vectors or points +- For displacements in Euclidean space +- When performance is critical +- When the dynamics do not involve rotations + +Example: Position vectors, linear velocities, forces + +```cpp +// Representing a 3D position +Cosserat::RealSpace position(1.0, 2.0, 3.0); + +// Representing a velocity vector +Cosserat::RealSpace velocity(-0.5, 0.0, 2.0); + +// Simple vector addition (displacement) +auto new_position = position.compose(velocity); + +// Direct access to vector components +double x = position[0]; +double y = position[1]; +double z = position[2]; +``` + +### When to use SO(2) + +- For 2D rotations +- When working with planar mechanisms +- For representing orientations in 2D space +- For angular velocities in a plane + +Example: 2D robot orientation, pendulum angle, compass heading + +```cpp +// Representing a 45-degree rotation +Cosserat::SO2 rotation(M_PI/4); + +// Rotating a 2D point +Eigen::Vector2d point(1.0, 0.0); +Eigen::Vector2d rotated_point = rotation.act(point); + +// Sequential rotations +Cosserat::SO2 first_rotation(M_PI/6); // 30 degrees +Cosserat::SO2 second_rotation(M_PI/3); // 60 degrees +Cosserat::SO2 combined = first_rotation.compose(second_rotation); +``` + +### When to use SO(3) + +- For 3D rotations +- When representing orientations in 3D space +- For spacecraft attitude control +- For camera orientation + +Example: Robot joint orientation, IMU attitude, camera rotation + +```cpp +// Creating a rotation around an arbitrary axis +Eigen::Vector3d axis(1.0, 1.0, 1.0); +axis.normalize(); +double angle = M_PI/3; // 60 degrees +Cosserat::SO3 rotation(Eigen::AngleAxisd(angle, axis)); + +// Rotating a 3D point +Eigen::Vector3d point(1.0, 0.0, 0.0); +Eigen::Vector3d rotated_point = rotation.act(point); + +// Converting to different representations +Eigen::Matrix3d rot_matrix = rotation.matrix(); +Eigen::Quaterniond quaternion = rotation.quaternion(); +``` + +### When to use SE(2) + +- For 2D rigid body transformations +- For planar robot kinematics +- For 2D SLAM (Simultaneous Localization and Mapping) +- For 2D path planning + +Example: Planar robot pose, 2D object transformation + +```cpp +// Creating a 2D pose (90-degree rotation + translation (1,2)) +Cosserat::SE2 pose(M_PI/2, 1.0, 2.0); + +// Transforming a 2D point +Eigen::Vector2d point(3.0, 4.0); +Eigen::Vector2d transformed_point = pose.act(point); + +// Composing transformations (e.g., for following a path) +Cosserat::SE2 step1(M_PI/4, 1.0, 0.0); // 45-degree turn, move 1 unit forward +Cosserat::SE2 step2(0.0, 2.0, 0.0); // Move 2 units forward +Cosserat::SE2 combined_path = step1.compose(step2); +``` + +### When to use SE(3) + +- For 3D rigid body transformations +- For robot kinematics and dynamics +- For 3D SLAM and computer vision +- For physics simulations + +Example: Robot pose, camera transformation, articulated body + +```cpp +// Creating a 3D pose (rotation around Z + translation) +Eigen::AngleAxisd rotation(M_PI/2, Eigen::Vector3d::UnitZ()); +Eigen::Vector3d translation(1.0, 2.0, 3.0); +Cosserat::SE3 pose(Cosserat::SO3(rotation), translation); + +// Transforming a 3D point +Eigen::Vector3d point(1.0, 0.0, 0.0); +Eigen::Vector3d transformed_point = pose.act(point); + +// Robot forward kinematics example (simplified) +Cosserat::SE3 base_to_joint1 = getJoint1Transform(); +Cosserat::SE3 joint1_to_joint2 = getJoint2Transform(); +Cosserat::SE3 joint2_to_endEffector = getEndEffectorTransform(); + +// Computing end effector position in base frame +Cosserat::SE3 base_to_endEffector = + base_to_joint1.compose(joint1_to_joint2.compose(joint2_to_endEffector)); +``` + +### When to use Sim(3) + +- For camera calibration with unknown scale +- For monocular SLAM +- For multi-scale registration +- For morphing and animation + +Example: Camera calibration, model alignment with scaling + +```cpp +// Creating a similarity transform (rotation + translation + scaling) +Eigen::AngleAxisd rotation(M_PI/4, Eigen::Vector3d::UnitZ()); +Eigen::Vector3d translation(1.0, 2.0, 3.0); +double scale = 2.5; +Cosserat::Sim3 transform( + Cosserat::SO3(rotation), + translation, + scale +); + +// Transforming a point (rotate, scale, translate) +Eigen::Vector3d point(1.0, 0.0, 0.0); +Eigen::Vector3d transformed_point = transform.act(point); + +// Aligning two datasets with different scales +Cosserat::Sim3 alignment = findOptimalAlignment(source_points, target_points); +std::vector aligned_points; +for (const auto& p : source_points) { + aligned_points.push_back(alignment.act(p)); +} +``` + +## Implementation Trade-offs + +When implementing Lie groups, several important trade-offs must be considered: + +### Representation Choice + +| Representation | Advantages | Disadvantages | +|----------------|------------|--------------| +| **Rotation Matrix** | - Direct geometric interpretation
- Easy to visualize
- Simple composition (just matrix multiply) | - 9 parameters for 3 DOF rotation
- Numerical drift (losing orthogonality)
- More memory usage | +| **Quaternion** | - Compact (4 parameters for 3 DOF)
- Numerically stable
- Efficient composition | - Less intuitive
- Double cover (q = -q)
- Requires normalization | +| **Angle-Axis** | - Minimal representation for SO(3)
- Direct physical interpretation | - Singularity at zero angle
- Less efficient for composition | +| **Euler Angles** | - Intuitive for humans
- Minimal representation | - Gimbal lock
- Order-dependent
- Poor computational properties | + +### Storage vs. Computation + +1. **Storage Efficiency**: + - Storing minimal representations (e.g., quaternion for SO(3)) saves memory + - Particularly important for large datasets or memory-constrained environments + +2. **Computational Efficiency**: + - Caching frequently accessed representations (matrices, quaternions) + - Pre-computing components for frequent operations + +3. **Numerical Precision**: + - Higher precision requires more memory + - Double precision is typically recommended for most applications + +### Template Parameters + +1. **Scalar Type**: + - `float`: Faster, less memory, but lower precision + - `double`: Better precision, standard for most applications + - `long double`: Highest precision, but slower and more memory-intensive + +2. **Dimension**: + - Fixed dimension: Better performance, compile-time checking + - Dynamic dimension: More flexibility, runtime cost + +### Inheritance vs. Composition + +1. **Inheritance Approach**: + - Useful for algorithms generic across different Lie groups + - Enables polymorphism for heterogeneous collections + - May have virtual function call overhead + +2. **Composition Approach**: + - More direct control over implementation + - Can be more efficient (no virtual calls) + - Potentially less code reuse + +### Optimization Considerations + +1. **Expression Templates**: + - Can improve performance by avoiding temporary objects + - Increases compile time and code complexity + +2. **SIMD Optimization**: + - Significant performance improvements, especially for batch operations + - May require platform-specific code or intrinsics + +3. **Memory Layout**: + - Cache-friendly organization for batch operations + - Trade-off between math clarity and optimization + +### API Design + +1. **Method Naming**: + - `compose()` vs. operator `*` for group operation + - `inverse()` vs. operator `-` for inverse element + - Consistency with mathematical notation vs. programming conventions + +2. **Error Handling**: + - Assertions vs. exceptions vs. error returns + - Performance impact of error checking in critical paths + diff --git a/src/Cosserat/liegroups/docs/math_foundations.md b/src/Cosserat/liegroups/docs/math_foundations.md new file mode 100644 index 0000000..8f15f8e --- /dev/null +++ b/src/Cosserat/liegroups/docs/math_foundations.md @@ -0,0 +1,100 @@ +# Mathematical Foundations of Lie Groups + +This document provides an overview of the mathematical foundations of Lie groups and their applications in the Cosserat plugin. + +## Introduction to Lie Groups + +A Lie group is a group that is also a differentiable manifold, with the property that the group operations are compatible with the smooth structure. In simpler terms, it's a continuous group where we can smoothly transition from one group element to another. + +## Lie Algebra + +Associated with each Lie group is a Lie algebra, which captures the local structure of the group near the identity element. The Lie algebra can be thought of as the tangent space at the identity of the Lie group. + +For matrix Lie groups, the Lie algebra consists of matrices X such that exp(X) is in the Lie group, where exp is the matrix exponential. + +## Exponential and Logarithmic Maps + +Two important operations in Lie theory are the exponential and logarithmic maps: + +- **Exponential Map**: Takes an element of the Lie algebra and maps it to the Lie group. +- **Logarithmic Map**: Takes an element of the Lie group and maps it to the Lie algebra. + +These operations allow us to move between the group and its tangent space, which is particularly useful for optimization and interpolation. + +## Groups Implemented in the Cosserat Plugin + +### RealSpace (ℝⁿ) + +The real vector space ℝⁿ is the simplest Lie group, where the group operation is vector addition. The corresponding Lie algebra is also ℝⁿ, and the exponential and logarithmic maps are identity functions. + +Mathematical properties: +- Dimension: n +- Group operation: addition +- Lie algebra: ℝⁿ +- Exponential map: identity +- Logarithmic map: identity + +### Special Orthogonal Group SO(2) + +SO(2) represents rotations in 2D space. It's the group of 2×2 orthogonal matrices with determinant 1. + +Mathematical properties: +- Dimension: 1 +- Group operation: matrix multiplication +- Lie algebra: so(2), the set of 2×2 skew-symmetric matrices +- Exponential map: matrix exponential +- Logarithmic map: matrix logarithm + +A rotation by angle θ can be represented as: +``` +R(θ) = [cos(θ) -sin(θ)] + [sin(θ) cos(θ)] +``` + +The corresponding element in the Lie algebra is: +``` +ω = [0 -θ] + [θ 0] +``` + +But since so(2) is 1-dimensional, we can simply represent it as the scalar θ. + +### Special Euclidean Group SE(2) + +SE(2) represents rigid transformations (rotation and translation) in 2D space. It's the semidirect product of SO(2) and ℝ². + +Mathematical properties: +- Dimension: 3 (1 for rotation + 2 for translation) +- Group operation: composition of transformations +- Lie algebra: se(2) +- Exponential map: combined matrix exponential +- Logarithmic map: combined matrix logarithm + +A transformation with rotation θ and translation (x,y) can be represented as a 3×3 matrix: +``` +T(θ,x,y) = [cos(θ) -sin(θ) x] + [sin(θ) cos(θ) y] + [0 0 1] +``` + +The corresponding element in the Lie algebra can be represented as: +``` +ξ = [0 -θ x] + [θ 0 y] + [0 0 0] +``` + +Or more compactly as a 3D vector [θ, x, y]. + +## Applications in Cosserat Rods + +In the context of Cosserat rods, Lie groups are used to: + +1. Represent the configuration (position and orientation) of rod elements +2. Compute deformations between adjacent elements +3. Define strain measures for the rod +4. Formulate constitutive laws relating strain to stress +5. Derive equations of motion for dynamic simulations + +By using Lie groups, we ensure that the physical constraints (like rigid body motions) are naturally preserved during simulation. + diff --git a/src/Cosserat/liegroups/docs/realspace.md b/src/Cosserat/liegroups/docs/realspace.md new file mode 100644 index 0000000..b915fd7 --- /dev/null +++ b/src/Cosserat/liegroups/docs/realspace.md @@ -0,0 +1,153 @@ +# RealSpace Implementation + +## Overview + +`RealSpace` implements the Euclidean vector space ℝⁿ as a Lie group. While it may seem trivial compared to other Lie groups, implementing it within the same framework provides consistency and allows for generic programming across different Lie groups. + +In RealSpace, the group operation is vector addition, and the inverse operation is negation. + +## Mathematical Properties + +- **Dimension**: n (templated as `Dim`) +- **Group operation**: vector addition +- **Identity element**: zero vector +- **Inverse**: negation +- **Lie algebra**: also ℝⁿ +- **Exponential map**: identity function +- **Logarithmic map**: identity function + +## Implementation Details + +The `RealSpace` class is implemented as a template with two parameters: +- `Scalar`: The scalar type (typically `double` or `float`) +- `Dim`: The dimension (a positive integer or `Eigen::Dynamic` for runtime-sized vectors) + +The internal representation uses an Eigen vector (`Eigen::Matrix`). + +### Key Methods + +```cpp +// Constructor from coordinates +template +RealSpace(Args&&... args); + +// Constructor from Eigen vector +RealSpace(const VectorType& data); + +// Group operations +RealSpace compose(const RealSpace& other) const; +RealSpace inverse() const; + +// Access to internal representation +const VectorType& coeffs() const; +VectorType& coeffs(); + +// Tangent space (Lie algebra) operations +VectorType log() const; +static RealSpace exp(const VectorType& tangent); + +// Accessors for individual components +Scalar operator[](int index) const; +Scalar& operator[](int index); +``` + +## Performance Characteristics + +Based on benchmarks, RealSpace operations scale as follows: + +- **Composition (addition)**: O(n) time complexity, where n is the dimension +- **Inverse (negation)**: O(n) time complexity +- **Exponential/Logarithmic maps**: O(1) time complexity (identity functions) + +Performance is primarily limited by memory bandwidth for large dimensions. + +## Example Usage + +### Basic Operations + +```cpp +#include +#include + +using Vec3 = Cosserat::RealSpace; + +int main() { + // Create points in 3D space + Vec3 a(1.0, 2.0, 3.0); + Vec3 b(4.0, 5.0, 6.0); + + // Composition (addition) + Vec3 c = a.compose(b); + std::cout << "a + b = [" << c.coeffs().transpose() << "]\n"; + + // Inverse (negation) + Vec3 a_inv = a.inverse(); + std::cout << "-a = [" << a_inv.coeffs().transpose() << "]\n"; + + // Identity element + Vec3 identity = Vec3::Identity(); + std::cout << "identity = [" << identity.coeffs().transpose() << "]\n"; + + // Component access + std::cout << "a[0] = " << a[0] << ", a[1] = " << a[1] << ", a[2] = " << a[2] << "\n"; + + // Tangent space operations (trivial for RealSpace) + Eigen::Vector3d log_a = a.log(); + Vec3 exp_log_a = Vec3::exp(log_a); + std::cout << "exp(log(a)) = [" << exp_log_a.coeffs().transpose() << "]\n"; + + return 0; +} +``` + +### Integration with Eigen + +```cpp +#include +#include + +using Vec3 = Cosserat::RealSpace; + +int main() { + // Create from Eigen vector + Eigen::Vector3d eigen_vec(1.0, 2.0, 3.0); + Vec3 point(eigen_vec); + + // Convert to Eigen vector + Eigen::Vector3d converted = point.coeffs(); + + // Use with Eigen operations + Eigen::Vector3d doubled = 2.0 * point.coeffs(); + Vec3 doubled_point(doubled); + + return 0; +} +``` + +### Dynamic-sized Vectors + +```cpp +#include + +using VecX = Cosserat::RealSpace; + +int main() { + // Create a 5D vector + VecX point(Eigen::VectorXd::Random(5)); + + // Resize (only possible with dynamic-sized vectors) + point.coeffs().resize(10); + point.coeffs().setRandom(); + + return 0; +} +``` + +## Best Practices + +1. **Use fixed-size vectors when dimension is known at compile time** for better performance. +2. **Avoid unnecessary copies** by using references when passing RealSpace objects. +3. **Prefer direct access to coeffs()** when performing multiple operations on the internal vector. +4. **Use the compose() and inverse() methods** instead of direct arithmetic for consistency with other Lie groups. +5. **When using with Eigen functions**, access the internal representation using coeffs(). + diff --git a/src/Cosserat/liegroups/docs/se2.md b/src/Cosserat/liegroups/docs/se2.md new file mode 100644 index 0000000..b3b75ce --- /dev/null +++ b/src/Cosserat/liegroups/docs/se2.md @@ -0,0 +1,186 @@ +# SE(2) Implementation + +## Overview + +`SE2` implements the Special Euclidean group in 2D, which represents rigid body transformations (rotation and translation) in a plane. SE(2) is a 3-dimensional Lie group, combining a 1-dimensional rotation (SO(2)) with a 2-dimensional translation. + +## Mathematical Properties + +- **Dimension**: 3 (1 for rotation + 2 for translation) +- **Group operation**: composition of transformations +- **Identity element**: zero rotation and zero translation +- **Inverse**: inverse rotation and negated rotated translation +- **Lie algebra**: se(2), which can be represented as a 3D vector [θ, x, y] +- **Exponential map**: converts from se(2) to SE(2) +- **Logarithmic map**: converts from SE(2) to se(2) + +## Internal Representation + +The `SE2` class internally stores: +- A rotation component as an SO(2) element +- A translation component as a 2D vector + +This representation ensures proper handling of the group structure and efficient computation of group operations. + +## Implementation Details + +The `SE2` class is implemented as a template with one parameter: +- `Scalar`: The scalar type (typically `double` or `float`) + +### Key Methods + +```cpp +// Constructors +SE2(); // Identity transformation +SE2(Scalar angle, Scalar x, Scalar y); // From angle and translation +SE2(const SO2& rotation, const Eigen::Matrix& translation); +SE2(const Eigen::Matrix& homogeneous_matrix); + +// Group operations +SE2 compose(const SE2& other) const; +SE2 inverse() const; + +// Access to components +SO2 rotation() const; +Eigen::Matrix translation() const; +Scalar angle() const; + +// Conversion to matrix representation +Eigen::Matrix matrix() const; // Homogeneous transformation matrix + +// Tangent space (Lie algebra) operations +Eigen::Matrix log() const; +static SE2 exp(const Eigen::Matrix& tangent); + +// Acting on points +Eigen::Matrix act(const Eigen::Matrix& point) const; +``` + +## Performance Characteristics + +Based on our benchmarks, SE(2) operations have the following performance characteristics: + +- **Composition**: O(1) time complexity - involves rotation composition and translation addition +- **Inverse**: O(1) time complexity - computes inverse rotation and negated rotated translation +- **Matrix conversion**: O(1) time complexity - creates a 3×3 homogeneous transformation matrix +- **Exponential map**: O(1) time complexity - converts from Lie algebra to the group +- **Logarithmic map**: O(1) time complexity - converts from the group to Lie algebra +- **Acting on points**: O(1) time complexity - applies the transformation to a 2D point + +The actual performance depends on the hardware and compiler optimizations, but these operations are typically very fast due to their low dimensionality. + +## Example Usage + +### Basic Operations + +```cpp +#include +#include + +int main() { + // Create an SE(2) element (45-degree rotation with translation (1,2)) + Cosserat::SE2 transform(M_PI/4.0, 1.0, 2.0); + + // Get components + double angle = transform.angle(); + Eigen::Vector2d translation = transform.translation(); + std::cout << "Angle: " << angle << " radians\n"; + std::cout << "Translation: [" << translation.transpose() << "]\n"; + + // Convert to homogeneous transformation matrix + Eigen::Matrix3d mat = transform.matrix(); + std::cout << "Matrix:\n" << mat << "\n"; + + // Create another transformation + Cosserat::SE2 another_transform(M_PI/2.0, 3.0, 4.0); + + // Compose transformations + auto composed = transform.compose(another_transform); + std::cout << "Composed angle: " << composed.angle() << " radians\n"; + std::cout << "Composed translation: [" << composed.translation().transpose() << "]\n"; + + // Inverse transformation + auto inverse = transform.inverse(); + std::cout << "Inverse angle: " << inverse.angle() << " radians\n"; + std::cout << "Inverse translation: [" << inverse.translation().transpose() << "]\n"; + + return 0; +} +``` + +### Tangent Space Operations + +```cpp +#include +#include + +int main() { + // Create an SE(2) element + Cosserat::SE2 transform(M_PI/4.0, 1.0, 2.0); + + // Convert to Lie algebra (tangent space) + Eigen::Vector3d tangent = transform.log(); + std::cout << "Tangent vector: [" << tangent.transpose() << "]\n"; + + // Convert back from Lie algebra to SE(2) + auto recovered = Cosserat::SE2::exp(tangent); + std::cout << "Recovered angle: " << recovered.angle() << " radians\n"; + std::cout << "Recovered translation: [" << recovered.translation().transpose() << "]\n"; + + // Create directly from tangent vector + Eigen::Vector3d new_tangent; + new_tangent << M_PI/6.0, 3.0, 4.0; + auto new_transform = Cosserat::SE2::exp(new_tangent); + + return 0; +} +``` + +### Acting on Points + +```cpp +#include +#include +#include + +int main() { + // Create an SE(2) element + Cosserat::SE2 transform(M_PI/4.0, 1.0, 2.0); + + // Transform a single point + Eigen::Vector2d point(3.0, 4.0); + Eigen::Vector2d transformed_point = transform.act(point); + std::cout << "Original point: [" << point.transpose() << "]\n"; + std::cout << "Transformed point: [" << transformed_point.transpose() << "]\n"; + + // Transform multiple points + std::vector points = { + Eigen::Vector2d(1.0, 0.0), + Eigen::Vector2d(0.0, 1.0), + Eigen::Vector2d(1.0, 1.0) + }; + + std::vector transformed_points; + for (const auto& p : points) { + transformed_points.push_back(transform.act(p)); + } + + // Check that applying the transformation twice is equivalent to composing + Eigen::Vector2d twice_transformed = transform.act(transform.act(point)); + Eigen::Vector2d composed_transform = transform.compose(transform).act(point); + + return 0; +} +``` + +## Best Practices + +1. **Use the most appropriate constructor** for your use case to avoid unnecessary conversions. +2. **Avoid repeated matrix construction** when performing multiple operations with the same transformation. +3. **Use the compose() method rather than manual matrix multiplication** for better semantics and potentially better performance. +4. **When transforming many points**, extract the rotation matrix and translation vector once outside the loop for better performance. +5. **For interpolation between poses**, convert to the Lie algebra (log), perform the interpolation there, and then convert back (exp). +6. **When working with velocities**, use the Lie algebra representation which directly corresponds to angular and linear velocities. +7. **For small displacements**, the exponential map can be approximated, but use the full implementation for general cases. +8. **Remember that SE(2) is not commutative** - the order of composition matters. + diff --git a/src/Cosserat/liegroups/docs/se3.md b/src/Cosserat/liegroups/docs/se3.md new file mode 100644 index 0000000..475ee29 --- /dev/null +++ b/src/Cosserat/liegroups/docs/se3.md @@ -0,0 +1,271 @@ +# SE(3) Implementation + +## Overview + +`SE3` implements the Special Euclidean group in 3D, which represents rigid body transformations (rotation and translation) in 3D space. SE(3) is a 6-dimensional Lie group, combining a 3-dimensional rotation (SO(3)) with a 3-dimensional translation. + +## Mathematical Properties + +- **Dimension**: 6 (3 for rotation + 3 for translation) +- **Group operation**: composition of transformations +- **Identity element**: zero rotation and zero translation +- **Inverse**: inverse rotation and negated rotated translation +- **Lie algebra**: se(3), which can be represented as a 6D vector [ω, v] + where ω is the rotation part and v is the translation part +- **Exponential map**: converts from se(3) to SE(3) +- **Logarithmic map**: converts from SE(3) to se(3) + +## Internal Representation + +The `SE3` class internally stores: +- A rotation component as an SO(3) element (typically as a quaternion) +- A translation component as a 3D vector + +This representation ensures proper handling of the group structure and efficient computation of group operations. + +## Implementation Details + +The `SE3` class is implemented as a template with one parameter: +- `Scalar`: The scalar type (typically `double` or `float`) + +### Key Methods + +```cpp +// Constructors +SE3(); // Identity transformation +SE3(const SO3& rotation, const Vector3& translation); +SE3(const Matrix4& homogeneous_matrix); +SE3(const Quaternion& quat, const Vector3& translation); + +// Group operations +SE3 compose(const SE3& other) const; +SE3 inverse() const; + +// Access to components +SO3 rotation() const; +Vector3 translation() const; +Matrix4 matrix() const; // Homogeneous transformation matrix + +// Tangent space (Lie algebra) operations +Vector6 log() const; +static SE3 exp(const Vector6& tangent); + +// Acting on points +Vector3 act(const Vector3& point) const; + +// Adjoint representation +Matrix6 adjoint() const; +``` + +## Performance Characteristics + +Based on benchmarks, SE(3) operations have the following performance characteristics: + +- **Composition**: O(1) time complexity - involves rotation composition and translation transformation +- **Inverse**: O(1) time complexity - computes inverse rotation and inverse transformed translation +- **Matrix conversion**: O(1) time complexity - creates a 4×4 homogeneous transformation matrix +- **Exponential map**: O(1) time complexity - converts from Lie algebra to the group +- **Logarithmic map**: O(1) time complexity - converts from the group to Lie algebra +- **Acting on points**: O(1) time complexity - applies the transformation to a 3D point + +The actual performance depends on the hardware and compiler optimizations. Using quaternions for the rotational component provides better performance for most operations compared to rotation matrices. + +## Example Usage + +### Basic Operations + +```cpp +#include +#include + +int main() { + // Create an SE(3) element (90-degree rotation around z-axis with translation (1,2,3)) + Eigen::AngleAxisd rotation(M_PI/2, Eigen::Vector3d::UnitZ()); + Eigen::Vector3d translation(1.0, 2.0, 3.0); + Cosserat::SE3 transform(Cosserat::SO3(rotation), translation); + + // Get components + Cosserat::SO3 rot = transform.rotation(); + Eigen::Vector3d trans = transform.translation(); + std::cout << "Rotation matrix:\n" << rot.matrix() << "\n"; + std::cout << "Translation: " << trans.transpose() << "\n"; + + // Convert to homogeneous transformation matrix + Eigen::Matrix4d mat = transform.matrix(); + std::cout << "Transformation matrix:\n" << mat << "\n"; + + // Create another transformation + Eigen::AngleAxisd another_rotation(M_PI/4, Eigen::Vector3d::UnitX()); + Eigen::Vector3d another_translation(4.0, 5.0, 6.0); + Cosserat::SE3 another_transform( + Cosserat::SO3(another_rotation), + another_translation + ); + + // Compose transformations + auto composed = transform.compose(another_transform); + + // Inverse transformation + auto inverse = transform.inverse(); + + return 0; +} +``` + +### Tangent Space Operations + +```cpp +#include +#include + +int main() { + // Create an SE(3) element + Eigen::AngleAxisd rotation(M_PI/3, Eigen::Vector3d::UnitZ()); + Eigen::Vector3d translation(1.0, 2.0, 3.0); + Cosserat::SE3 transform(Cosserat::SO3(rotation), translation); + + // Convert to Lie algebra (tangent space) + Eigen::Matrix tangent = transform.log(); + std::cout << "Tangent vector: " << tangent.transpose() << "\n"; + + // Convert back from Lie algebra to SE(3) + auto recovered = Cosserat::SE3::exp(tangent); + + // Create directly from tangent vector + Eigen::Matrix new_tangent; + new_tangent << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0; // Small rotation with translation + auto new_transform = Cosserat::SE3::exp(new_tangent); + + return 0; +} +``` + +### Acting on Points + +```cpp +#include +#include +#include + +int main() { + // Create a transformation + Eigen::AngleAxisd rotation(M_PI/2, Eigen::Vector3d::UnitZ()); + Eigen::Vector3d translation(1.0, 2.0, 3.0); + Cosserat::SE3 transform(Cosserat::SO3(rotation), translation); + + // Transform a single point + Eigen::Vector3d point(1.0, 0.0, 0.0); + Eigen::Vector3d transformed_point = transform.act(point); + std::cout << "Original point: " << point.transpose() << "\n"; + std::cout << "Transformed point: " << transformed_point.transpose() << "\n"; + + // Transform multiple points + std::vector points = { + Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Vector3d(0.0, 1.0, 0.0), + Eigen::Vector3d(0.0, 0.0, 1.0) + }; + + std::vector transformed_points; + for (const auto& p : points) { + transformed_points.push_back(transform.act(p)); + } + + // Check that applying the transformation twice is equivalent to composing + Eigen::Vector3d twice_transformed = transform.act(transform.act(point)); + Eigen::Vector3d composed_transform = transform.compose(transform).act(point); + + return 0; +} +``` + +### Interpolation + +```cpp +#include +#include + +int main() { + // Create two transformations + Eigen::AngleAxisd start_rot(0, Eigen::Vector3d::UnitZ()); + Eigen::Vector3d start_trans(0.0, 0.0, 0.0); + Cosserat::SE3 start(Cosserat::SO3(start_rot), start_trans); + + Eigen::AngleAxisd end_rot(M_PI/2, Eigen::Vector3d::UnitZ()); + Eigen::Vector3d end_trans(1.0, 2.0, 3.0); + Cosserat::SE3 end(Cosserat::SO3(end_rot), end_trans); + + // Interpolate using the exponential map (screw linear interpolation) + Eigen::Matrix start_tangent = start.log(); + Eigen::Matrix end_tangent = end.log(); + Eigen::Matrix mid_tangent = start_tangent + 0.5 * (end_tangent - start_tangent); + Cosserat::SE3 mid = Cosserat::SE3::exp(mid_tangent); + + // Create a sequence of interpolated poses + std::vector> path; + int steps = 10; + for (int i = 0; i <= steps; ++i) { + double t = static_cast(i) / steps; + Eigen::Matrix interp_tangent = + start_tangent + t * (end_tangent - start_tangent); + path.push_back(Cosserat::SE3::exp(interp_tangent)); + } + + return 0; +} +``` + +## Best Practices + +1. **Choose the right representation** for your specific use case: + - Use quaternions for the rotation component for better numerical stability + - Store translation separately rather than using 4x4 matrices for better performance + +2. **Optimize composition operations** when transforming many points: + - Compose transformations first, then apply the result once + - This is much more efficient than applying each transformation sequentially + +3. **For interpolation between poses**: + - Use screw linear interpolation (SLERP in the Lie algebra) for smooth and natural interpolation + - Avoid linear interpolation of matrices or Euler angles + +4. **For derivatives and velocities**: + - Work in the tangent space (Lie algebra) where velocities have a natural representation + - Convert back to the group only when needed + +5. **Numerical stability**: + - Normalize quaternions periodically to prevent numerical drift + - For exponential map with small rotation, use specialized implementations + +6. **Adjoint representation**: + - Use the adjoint to transform velocities between different coordinate frames + - This is more efficient than explicitly transforming velocity vectors + +7. **Avoid repeated conversions**: + - When possible, stay within a single representation + - Convert only when necessary for specific operations + +8. **Cache expensive operations**: + - Matrix computations can be expensive, so cache matrices when they'll be reused + - Consider caching log() results for frequently accessed transforms + +9. **Remember that SE(3) is not commutative**: + - The order of composition matters (A*B ≠ B*A) + - Be careful about the order of transformations in your code + +10. **Use the right tool for the job**: + - For pure rotations, use SO(3) instead of SE(3) + - For pure translations, use RealSpace instead of SE(3) + - For similarity transforms (rotation + translation + scaling), use Sim(3) + +## Numerical Stability Considerations + +- **Quaternion normalization**: Periodically renormalize quaternions to maintain unit length +- **Small angle approximations**: For small rotations, use specialized implementations of exp/log +- **Near-singularity handling**: Add checks for divide-by-zero conditions in log() calculations +- **Composition of many transformations**: Be aware of error accumulation in long chains of transformations +- **Interpolation path selection**: Ensure interpolation takes the shortest path, especially for rotations +- **Exponential map implementation**: Use a robust implementation that handles all cases correctly +- **Conversion between representations**: Be careful about numerical issues at singularities +- **Double cover handling**: For interpolation, ensure quaternions are on the same hemisphere + diff --git a/src/Cosserat/liegroups/docs/sim3.md b/src/Cosserat/liegroups/docs/sim3.md new file mode 100644 index 0000000..f9ae1a8 --- /dev/null +++ b/src/Cosserat/liegroups/docs/sim3.md @@ -0,0 +1,155 @@ +# Sim(3) Implementation + +## Overview + +`Sim3` implements the Similarity transformation group in 3D, which represents rotation, translation, and uniform scaling in 3D space. Sim(3) is a 7-dimensional Lie group, combining a 3-dimensional rotation (SO(3)), a 3-dimensional translation, and a 1-dimensional scaling factor. + +## Mathematical Properties + +- **Dimension**: 7 (3 for rotation + 3 for translation + 1 for scaling) +- **Group operation**: composition of similarity transformations +- **Identity element**: zero rotation, zero translation, and unit scaling +- **Inverse**: inverse rotation, scaled and rotated negative translation, and reciprocal scaling +- **Lie algebra**: sim(3), which can be represented as a 7D vector [ω, v, s] + where ω is the rotation part, v is the translation part, and s is the scaling part +- **Exponential map**: converts from sim(3) to Sim(3) +- **Logarithmic map**: converts from Sim(3) to sim(3) + +## Internal Representation + +The `Sim3` class internally stores: +- A rotation component as an SO(3) element (typically as a quaternion) +- A translation component as a 3D vector +- A scaling factor as a scalar + +This representation ensures proper handling of the group structure and efficient computation of group operations. + +## Implementation Details + +The `Sim3` class is implemented as a template with one parameter: +- `Scalar`: The scalar type (typically `double` or `float`) + +### Key Methods + +```cpp +// Constructors +Sim3(); // Identity transformation +Sim3(const SO3& rotation, const Vector3& translation, Scalar scale); +Sim3(const Matrix4& similarity_matrix); +Sim3(const Quaternion& quat, const Vector3& translation, Scalar scale); + +// Group operations +Sim3 compose(const Sim3& other) const; +Sim3 inverse() const; + +// Access to components +SO3 rotation() const; +Vector3 translation() const; +Scalar scale() const; +Matrix4 matrix() const; // Homogeneous similarity matrix + +// Tangent space (Lie algebra) operations +Vector7 log() const; +static Sim3 exp(const Vector7& tangent); + +// Acting on points +Vector3 act(const Vector3& point) const; + +// Adjoint representation +Matrix7 adjoint() const; +``` + +## Performance Characteristics + +Based on benchmarks, Sim(3) operations have the following performance characteristics: + +- **Composition**: O(1) time complexity - involves rotation composition, scaled translation transformation, and scale multiplication +- **Inverse**: O(1) time complexity - computes inverse rotation, scaled inverse translation, and reciprocal scale +- **Matrix conversion**: O(1) time complexity - creates a 4×4 similarity transformation matrix +- **Exponential map**: O(1) time complexity - converts from Lie algebra to the group +- **Logarithmic map**: O(1) time complexity - converts from the group to Lie algebra +- **Acting on points**: O(1) time complexity - applies the transformation to a 3D point (rotation, scaling, and translation) + +The actual performance depends on the hardware and compiler optimizations. Similar to SE(3), using quaternions for the rotational component provides better performance for most operations. + +## Example Usage + +### Basic Operations + +```cpp +#include +#include + +int main() { + // Create a Sim(3) element (90-degree rotation around z-axis, translation (1,2,3), scale 2.0) + Eigen::AngleAxisd rotation(M_PI/2, Eigen::Vector3d::UnitZ()); + Eigen::Vector3d translation(1.0, 2.0, 3.0); + double scale = 2.0; + Cosserat::Sim3 transform( + Cosserat::SO3(rotation), + translation, + scale + ); + + // Get components + Cosserat::SO3 rot = transform.rotation(); + Eigen::Vector3d trans = transform.translation(); + double s = transform.scale(); + std::cout << "Rotation matrix:\n" << rot.matrix() << "\n"; + std::cout << "Translation: " << trans.transpose() << "\n"; + std::cout << "Scale: " << s << "\n"; + + // Convert to homogeneous similarity matrix + Eigen::Matrix4d mat = transform.matrix(); + std::cout << "Similarity matrix:\n" << mat << "\n"; + + // Create another transformation + Cosserat::Sim3 another_transform( + Cosserat::SO3(Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitX())), + Eigen::Vector3d(4.0, 5.0, 6.0), + 0.5 + ); + + // Compose transformations + auto composed = transform.compose(another_transform); + + // Inverse transformation + auto inverse = transform.inverse(); + + return 0; +} +``` + +### Tangent Space Operations + +```cpp +#include +#include + +int main() { + // Create a Sim(3) element + Eigen::AngleAxisd rotation(M_PI/3, Eigen::Vector3d::UnitZ()); + Eigen::Vector3d translation(1.0, 2.0, 3.0); + double scale = 1.5; + Cosserat::Sim3 transform( + Cosserat::SO3(rotation), + translation, + scale + ); + + // Convert to Lie algebra (tangent space) + Eigen::Matrix tangent = transform.log(); + std::cout << "Tangent vector: " << tangent.transpose() << "\n"; + + // Convert back from Lie algebra to Sim(3) + auto recovered = Cosserat::Sim3::exp(tangent); + + // Create directly from tangent vector + Eigen::Matrix new_tangent; + new_tangent << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.1; // Rotation, translation, log(scale) + auto new_transform = Cosserat::Sim3::exp(new_tangent); + + return 0; +} +``` + diff --git a/src/Cosserat/liegroups/docs/so2.md b/src/Cosserat/liegroups/docs/so2.md new file mode 100644 index 0000000..d12568d --- /dev/null +++ b/src/Cosserat/liegroups/docs/so2.md @@ -0,0 +1,14 @@ +# SO(2) Implementation + +## Overview + +`SO2` implements the Special Orthogonal group in 2D, which represents rotations in a plane. SO(2) is a 1-dimensional Lie group, where the dimension corresponds to the angle of rotation. + +## Mathematical Properties + +- **Dimension**: 1 (the angle θ) +- **Group operation**: composition of rotations +- **Identity element**: rotation by 0 radians +- **Inverse**: rotation by -θ +- **Lie algebra**: so(2), which is isomorphic + diff --git a/src/Cosserat/liegroups/docs/so3.md b/src/Cosserat/liegroups/docs/so3.md new file mode 100644 index 0000000..4bab931 --- /dev/null +++ b/src/Cosserat/liegroups/docs/so3.md @@ -0,0 +1,871 @@ +# SO(3) Implementation + +## Overview + +`SO3` implements the Special Orthogonal group in 3D, which represents rotations in 3D space. SO(3) is a 3-dimensional Lie group, where the dimension corresponds to the degrees of freedom for 3D rotations. + +## Mathematical Foundations + +### Group Structure + +SO(3) is the group of all 3×3 orthogonal matrices with determinant 1. Mathematically: + +SO(3) = {R ∈ ℝ³ˣ³ | R^T R = I, det(R) = 1} + +Key properties: +- **Group operation**: Matrix multiplication (composition of rotations) +- **Identity element**: 3×3 identity matrix (no rotation) +- **Inverse element**: Transpose of the rotation matrix (R^T = R^(-1)) +- **Not commutative**: In general, R₁R₂ ≠ R₂R₁ (order of rotations matters) +- **Compact**: All elements are bounded (closed and bounded subset of ℝ³ˣ³) +- **Connected**: Any rotation can be continuously deformed to any other + +### Lie Algebra + +The Lie algebra of SO(3), denoted so(3), consists of all 3×3 skew-symmetric matrices: + +so(3) = {ω̂ ∈ ℝ³ˣ³ | ω̂^T = -ω̂} + +A general element of so(3) has the form: + +``` +ω̂ = [ 0 -ω₃ ω₂ ] + [ ω₃ 0 -ω₁ ] + [-ω₂ ω₁ 0 ] +``` + +where ω = [ω₁, ω₂, ω₃] ∈ ℝ³ is the corresponding angular velocity vector. + +The "hat" operator (^) maps a 3D vector to a skew-symmetric matrix: +- ω̂ = hat(ω) + +The inverse "vee" operator (∨) maps a skew-symmetric matrix to a 3D vector: +- ω = vee(ω̂) + +### Exponential and Logarithmic Maps + +#### Exponential Map: so(3) → SO(3) + +The exponential map converts an element of the Lie algebra (skew-symmetric matrix or rotation vector) to a rotation matrix: + +R = exp(ω̂) = I + sin(θ)/θ · ω̂ + (1-cos(θ))/θ² · ω̂² + +where θ = ‖ω‖ is the magnitude of the rotation vector. + +This is known as Rodrigues' formula. For small rotations, numerical approximations are used to avoid division by near-zero angles. + +#### Logarithmic Map: SO(3) → so(3) + +The logarithmic map converts a rotation matrix to an element of the Lie algebra: + +ω̂ = log(R) + +The rotation vector ω can be computed as: + +``` +θ = arccos((trace(R) - 1)/2) +ω = θ/(2sin(θ)) · [R₃₂-R₂₃, R₁₃-R₃₁, R₂₁-R₁₂]^T +``` + +Special care is needed for rotations near the identity (θ ≈ 0) and for 180° rotations. + +### Quaternion Representation + +Unit quaternions provide a compact and numerically stable representation of rotations. + +A quaternion q = [w, x, y, z] represents the rotation: +- Around axis [x, y, z]/‖[x, y, z]‖ +- By angle 2·arccos(w) + +The relationship between a rotation vector ω = θ·n (where n is a unit vector) and a quaternion is: + +``` +q = [cos(θ/2), sin(θ/2)·n] +``` + +Key properties: +- The quaternion must have unit length (‖q‖ = 1) +- Quaternions q and -q represent the same rotation (double cover) +- Composition of rotations corresponds to quaternion multiplication + +## Implementation Details + +### Internal Representation + +The `SO3` class supports several internal representations: + +- **Quaternion representation** (primary): Most efficient for composition and inversion +- **Rotation matrix**: Used for acting on points and for interfacing with other systems +- **Axis-angle**: Used for certain operations and for intuitive construction + +The implementation uses the quaternion as the primary storage format due to its numerical stability and efficiency. + +### Key Methods + +```cpp +// Constructors +SO3(); // Identity rotation +SO3(const Quaternion& quat); // From quaternion +SO3(const Matrix3& rot_matrix); // From rotation matrix +SO3(const AngleAxis& angle_axis); // From angle-axis + +// Group operations +SO3 compose(const SO3& other) const; +SO3 inverse() const; + +// Access to components +Quaternion quaternion() const; +Matrix3 matrix() const; +AngleAxis angleAxis() const; + +// Tangent space (Lie algebra) operations +Vector3 log() const; +static SO3 exp(const Vector3& tangent); + +// Acting on points +Vector3 act(const Vector3& point) const; + +// Adjoint representation +Matrix3 adjoint() const; +``` + +### Performance Considerations + +Operation performance with quaternion representation: + +- **Composition**: O(1), 16 multiplications + 12 additions +- **Inverse**: O(1), constant time (quaternion conjugate) +- **Acting on points**: O(1), slightly slower than using matrices directly +- **Conversion to matrix**: O(1), but more expensive +- **Logarithmic map**: O(1), but involves transcendental functions +- **Exponential map**: O(1), transcendental functions + normalization + +Compared to matrix representation, quaternions are: +- More memory efficient (4 vs. 9 elements) +- Faster for composition and inversion +- Slower for point transformation (unless batched) +- More numerically stable under repeated operations + +### Numerical Stability + +The implementation includes special handling for numerical stability: + +- **Quaternion normalization**: Applied periodically to prevent drift +- **Small angle approximations**: For exp/log near the identity +- **Near-singular cases**: Special handling for 180° rotations +- **Double cover handling**: Ensuring consistent choice between q and -q + +## Usage Examples + +### Basic Operations + +```cpp +#include +#include + +int main() { + // Creating rotations in different ways + + // 1. From axis-angle representation + Eigen::Vector3d axis(1.0, 1.0, 1.0); + axis.normalize(); + double angle = M_PI/3; // 60 degrees + Cosserat::SO3 rotation1(Eigen::AngleAxisd(angle, axis)); + + // 2. From rotation matrix + Eigen::Matrix3d R; + R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()); + Cosserat::SO3 rotation2(R); + + // 3. From quaternion + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitX()); + Cosserat::SO3 rotation3(q); + + // Compose rotations (apply rotation2 after rotation1) + auto composed = rotation1.compose(rotation2); + + // Inverse rotation + auto inverse = rotation1.inverse(); + + // Convert to different representations + Eigen::Matrix3d rot_mat = rotation1.matrix(); + Eigen::Quaterniond quat = rotation1.quaternion(); + Eigen::AngleAxisd aa = rotation1.angleAxis(); + + // Accessing properties + std::cout << "Rotation matrix:\n" << rot_mat << "\n"; + std::cout << "Quaternion: " << quat.coeffs().transpose() << "\n"; + std::cout << "Axis: " << aa.axis().transpose() << ", angle: " << aa.angle() << "\n"; + + // Rotate a point + Eigen::Vector3d point(1.0, 0.0, 0.0); + Eigen::Vector3d rotated_point = rotation1.act(point); + std::cout << "Original point: " << point.transpose() << "\n"; + std::cout << "Rotated point: " << rotated_point.transpose() << "\n"; + + return 0; +} +``` + +### Interpolation + +```cpp +#include +#include + +// Spherical Linear Interpolation (SLERP) +Cosserat::SO3 slerp( + const Cosserat::SO3& start, + const Cosserat::SO3& end, + double t +) { + // Get quaternions + Eigen::Quaterniond q_start = start.quaternion(); + Eigen::Quaterniond q_end = end.quaternion(); + + // Ensure shortest path (handle double cover) + if (q_start.dot(q_end) < 0) { + q_end.coeffs() = -q_end.coeffs(); + } + + // Use Eigen's SLERP + Eigen::Quaterniond q_interp = q_start.slerp(t, q_end); + return Cosserat::SO3(q_interp); +} + +// Create a smooth rotation path +std::vector> createRotationPath( + const Cosserat::SO3& start, + const Cosserat::SO3& end, + int steps +) { + std::vector> path; + for (int i = 0; i <= steps; ++i) { + double t = static_cast(i) / steps; + path.push_back(slerp(start, end, t)); + } + return path; +} +``` + +### Integration with Dynamics + +```cpp +#include + +// Angular velocity integration (discrete) +Cosserat::SO3 integrateRotation( + const Cosserat::SO3& R, + const Eigen::Vector3d& omega, + double dt +) { + // Convert body angular velocity to a rotation increment + Eigen::Vector3d delta_rot = omega * dt; + + // Apply the increment using the exponential map + Cosserat::SO3 delta_R = Cosserat::SO3::exp(delta_rot); + + // Apply to current rotation (left multiplication for body frame) + return R.compose(delta_R); +} + +// Rigid body dynamics example +void rigidBodyStep( + Cosserat::SO3& orientation, + Eigen::Vector3d& angular_velocity, + const Eigen::Matrix3d& inertia_tensor, + const Eigen::Vector3d& torque, + double dt +) { + // Angular momentum + Eigen::Vector3d angular_momentum = inertia_tensor * angular_velocity; + + // Update angular momentum with torque + angular_momentum += torque * dt; + + // Update angular velocity + angular_velocity = inertia_tensor.inverse() * angular_momentum; + + // Update orientation + orientation = integrateRotation(orientation, angular_velocity, dt); +} +``` + +### Common Applications + +```cpp +#include + +// Camera orientation tracking +class Camera { +private: + Cosserat::SO3 orientation; + Eigen::Vector3d position; + +public: + // Update from IMU measurements + void updateFromIMU(const Eigen::Vector3d& gyro, double dt) { + orientation = integrateRotation(orientation, gyro, dt); + } + + // Get view matrix for rendering + Eigen::Matrix4d getViewMatrix() const { + Eigen::Matrix4d view = Eigen::Matrix4d::Identity(); + + // Set rotation part + view.block<3,3>(0,0) = orientation.inverse().matrix(); + + // Set translation part + view.block<3,1>(0,3) = -orientation.inverse().act(position); + + return view; + } +}; + +// Attitude control for spacecraft +class Spacecraft { +private: + Cosserat::SO3 orientation; + Eigen::Vector3d angular_velocity; + Eigen::Matrix3d inertia; + +public: + // Compute control torque to reach target orientation + Eigen::Vector3d computeControlTorque(const Cosserat::SO3& target) { + // Error in orientation (in the Lie algebra) + Eigen::Vector3d error_vector = + (orientation.inverse().compose(target)).log(); + + // PD controller + double Kp = 1.0; // Proportional gain + double Kd = 0.5; // Derivative gain + + return Kp * error_vector - Kd * angular_velocity; + } +}; +``` + +## Best Practices and Edge Cases + +### Handling Singularities + +1. **Logarithmic Map Singularities**: + - For rotations near the identity, use Taylor series approximation + - For 180° rotations, find a valid rotation axis by checking for non-zero elements in R - R^T + +```cpp +// Robust logarithmic map implementation +Eigen::Vector3d robustLog(const Eigen::Matrix3d& R) { + // Check if near identity + double trace = R.trace(); + if (std::abs(trace - 3.0) < 1e-10) { + // Near identity - use first-order approximation + return Eigen::Vector3d( + R(2,1) - R(1,2), + R(0,2) - R(2,0), + R(1,0) - R(0,1) + ) * 0.5; + } + + // Check if near 180° rotation + if (std::abs(trace + 1.0) < 1e-10) { + // Find axis by checking non-zero elements in R + I + // [Implementation omitted for brevity] + } + + // Standard case + double theta = std::acos((trace - 1.0) / 2.0); + return Eigen::Vector3d( + R(2,1) - R(1,2), + R(0,2) - R(2,0), + R(1,0) - R(0,1) + ) * (theta / (2.0 * std::sin(theta))); +} +``` + +2. **Exponential Map Stability**: + - For small rotation angles, use Taylor series approximation + - Handle zero-magnitude rotation vectors + +```cpp +// Robust exponential map implementation +Eigen::Matrix3d robustExp(const Eigen::Vector3d& omega) { + double theta = omega.norm(); + + // Check if near-zero rotation + if (theta < 1e-10) { + // Use + +# SO(3) Implementation + +## Overview + +`SO3` implements the Special Orthogonal group in 3D, which represents rotations in 3D space. SO(3) is a 3-dimensional Lie group, where the dimension corresponds to the degrees of freedom for 3D rotations. + +## Mathematical Properties + +- **Dimension**: 3 (rotations around the x, y, and z axes) +- **Group operation**: composition of rotations (matrix multiplication) +- **Identity element**: rotation by 0 radians (identity matrix) +- **Inverse**: transpose of the rotation matrix (inverse rotation) +- **Lie algebra**: so(3), which is the space of 3×3 skew-symmetric matrices +- **Exponential map**: converts from so(3) to SO(3) +- **Logarithmic map**: converts from SO(3) to so(3) + +## Internal Representation + +The `SO3` class can be internally represented in several ways: +- Rotation matrix (3×3 orthogonal matrix with determinant 1) +- Unit quaternion (more compact and numerically stable) +- Angle-axis representation (for certain operations) + +Our implementation primarily uses unit quaternions for storage, with conversion methods to other representations as needed. + +## Implementation Details + +The `SO3` class is implemented as a template with one parameter: +- `Scalar`: The scalar type (typically `double` or `float`) + +### Key Methods + +```cpp +// Constructors +SO3(); // Identity rotation +SO3(const Quaternion& quat); // From quaternion +SO3(const Matrix3& rot_matrix); // From rotation matrix +SO3(const AngleAxis& angle_axis); // From angle-axis + +// Group operations +SO3 compose(const SO3& other) const; +SO3 inverse() const; + +// Access to components +Quaternion quaternion() const; +Matrix3 matrix() const; +AngleAxis angleAxis() const; + +// Tangent space (Lie algebra) operations +Vector3 log() const; +static SO3 exp(const Vector3& tangent); + +// Acting on points +Vector3 act(const Vector3& point) const; + +// Adjoint representation +Matrix3 adjoint() const; +``` + +## Mathematical Background + +### Rotation Matrix + +A 3×3 rotation matrix R must satisfy: +- Orthogonality: R^T R = I (identity matrix) +- Proper rotation: det(R) = 1 + +### Lie Algebra + +The Lie algebra so(3) consists of 3×3 skew-symmetric matrices of the form: +``` +S = [ 0 -w3 w2 ] + [ w3 0 -w1 ] + [ -w2 w1 0 ] +``` + +where [w1, w2, w3] is the axis-angle representation scaled by the angle. This vector can be thought of as the angular velocity vector. + +### Exponential Map + +The exponential map from so(3) to SO(3) can be computed using Rodrigues' formula: + +For a rotation vector ω = θ·a (where a is a unit vector and θ is the angle): +``` +exp(ω) = I + sin(θ)/θ · [ω]× + (1-cos(θ))/θ² · [ω]ײ +``` + +where [ω]× is the skew-symmetric matrix formed from ω. + +For small rotations, numerical approximations are used to avoid division by near-zero angles. + +### Logarithmic Map + +The logarithmic map from SO(3) to so(3) extracts the rotation vector from a rotation matrix or quaternion: + +From a rotation matrix R with trace tr(R): +``` +θ = acos((tr(R) - 1)/2) +ω = θ/(2*sin(θ)) · [R32-R23, R13-R31, R21-R12]^T +``` + +Again, special handling is required for small angles to ensure numerical stability. + +## Performance Characteristics + +Based on benchmarks, SO(3) operations have the following performance characteristics: + +- **Composition**: O(1) time complexity - involves quaternion multiplication +- **Inverse**: O(1) time complexity - involves quaternion conjugation +- **Matrix conversion**: O(1) time complexity - creates a 3×3 rotation matrix +- **Exponential map**: O(1) time complexity - converts from axis-angle to quaternion +- **Logarithmic map**: O(1) time complexity - converts from quaternion to axis-angle +- **Acting on points**: O(1) time complexity - applies the rotation to a 3D point + +For most operations, the quaternion representation offers better performance and numerical stability compared to rotation matrices, especially for composition and inversion. + +## Example Usage + +### Basic Operations + +```cpp +#include +#include + +int main() { + // Create an SO(3) element (90-degree rotation around z-axis) + Eigen::AngleAxisd angle_axis(M_PI/2, Eigen::Vector3d::UnitZ()); + Cosserat::SO3 rotation(angle_axis); + + // Get representations + Eigen::Matrix3d rot_mat = rotation.matrix(); + Eigen::Quaterniond quat = rotation.quaternion(); + + std::cout << "Rotation matrix:\n" << rot_mat << "\n"; + std::cout << "Quaternion: " << quat.coeffs().transpose() << "\n"; + + // Create another rotation (45-degree rotation around x-axis) + Eigen::AngleAxisd another_angle_axis(M_PI/4, Eigen::Vector3d::UnitX()); + Cosserat::SO3 another_rotation(another_angle_axis); + + // Compose rotations + auto composed = rotation.compose(another_rotation); + + // Inverse rotation + auto inverse = rotation.inverse(); + + return 0; +} +``` + +### Tangent Space Operations + +```cpp +#include +#include + +int main() { + // Create a rotation from axis-angle + Eigen::Vector3d axis(1.0, 1.0, 1.0); + axis.normalize(); + double angle = M_PI/3; + Eigen::AngleAxisd angle_axis(angle, axis); + Cosserat::SO3 rotation(angle_axis); + + // Convert to Lie algebra (tangent space) + Eigen::Vector3d tangent = rotation.log(); + std::cout << "Tangent vector: " << tangent.transpose() << "\n"; + + // Convert back from Lie algebra to SO(3) + auto recovered = Cosserat::SO3::exp(tangent); + + // Create directly from tangent vector + Eigen::Vector3d new_tangent(0.1, 0.2, 0.3); // Small rotation + auto new_rotation = Cosserat::SO3::exp(new_tangent); + + return 0; +} +``` + +### Acting on Points + +```cpp +#include +#include +#include + +int main() { + // Create a rotation + Eigen::AngleAxisd angle_axis(M_PI/2, Eigen::Vector3d::UnitZ()); + Cosserat::SO3 rotation(angle_axis); + + // Rotate a single point + Eigen::Vector3d point(1.0, 0.0, 0.0); + Eigen::Vector3d rotated_point = rotation.act(point); + std::cout << "Original point: " << point.transpose() << "\n"; + std::cout << "Rotated point: " << rotated_point.transpose() << "\n"; + + // Rotate multiple points + std::vector points = { + Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Vector3d(0.0, 1.0, 0.0), + Eigen::Vector3d(0.0, 0.0, 1.0) + }; + + std::vector rotated_points; + for (const auto& p : points) { + rotated_points.push_back(rotation.act(p)); + } + + return 0; +} +``` + +### Interpolation + +```cpp +#include +#include + +int main() { + // Create two rotations + Cosserat::SO3 start(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())); + Cosserat::SO3 end(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ())); + + // Interpolate between them (spherical linear interpolation) + Cosserat::SO3 mid = Cosserat::slerp(start, end, 0.5); + + // Interpolate using the exponential map + Eigen::Vector3d start_tangent = start.log(); + Eigen::Vector3d end_tangent = end.log(); + Eigen::Vector3d mid_tangent = start_tangent + 0.5 * (end_tangent - start_tangent); + Cosserat::SO3 mid2 = Cosserat::SO3::exp(mid_tangent); + + return 0; +} +``` + +## Best Practices + +1. **Use quaternion representation** for most operations due to better numerical stability and performance. +2. **Normalize quaternions regularly** to prevent numerical drift. +3. **When performing many rotations**, compose them first and then apply the result, rather than applying each rotation individually. +4. **For interpolation between rotations**, use spherical linear interpolation (SLERP) rather than linear interpolation of matrices or Euler angles. +5. **Be aware of the double cover** - two quaternions can represent the same rotation (q and -q). +6. **For small rotations**, the exponential map can be approximated, but use the full implementation for general cases. +7. **When converting between representations**, be aware of numerical issues at singularities (e.g., Euler angles gimbal lock). +8. **For time-varying rotations**, work in the tangent space for derivatives. +9. **Use the appropriate constructor** based on your input data to avoid unnecessary conversions. +10. **Remember that rotations are not commutative** - the order of composition matters. + +## Numerical Stability Considerations + +- **Quaternion normalization**: Ensure quaternions stay unit length by normalizing periodically +- **Logarithm near identity**: Use specialized implementations for small rotations +- **Avoiding gimbal lock**: Work with quaternions or axis-angle rather than Euler angles +- **Double cover handling**: Ensure that interpolation takes the shortest path +- **Composition of many rotations**: Reorthogonalize occasionally to prevent drift + +# SO(3) Implementation + +## Overview + +`SO3` implements the Special Orthogonal group in 3D, which represents rotations in 3D space. SO(3) is a 3-dimensional Lie group, where the dimension corresponds to the degrees of freedom for 3D rotations. + +## Mathematical Properties + +- **Dimension**: 3 (rotations around the x, y, and z axes) +- **Group operation**: composition of rotations (matrix multiplication) +- **Identity element**: rotation by 0 radians (identity matrix) +- **Inverse**: transpose of the rotation matrix (inverse rotation) +- **Lie algebra**: so(3), which is the space of 3×3 skew-symmetric matrices +- **Exponential map**: converts from so(3) to SO(3) +- **Logarithmic map**: converts from SO(3) to so(3) + +## Internal Representation + +The `SO3` class can be internally represented in several ways: +- Rotation matrix (3×3 orthogonal matrix with determinant 1) +- Unit quaternion (more compact and numerically stable) +- Angle-axis representation (for certain operations) + +Our implementation primarily uses unit quaternions for storage, with conversion methods to other representations as needed. + +## Implementation Details + +The `SO3` class is implemented as a template with one parameter: +- `Scalar`: The scalar type (typically `double` or `float`) + +### Key Methods + +```cpp +// Constructors +SO3(); // Identity rotation +SO3(const Quaternion& quat); // From quaternion +SO3(const Matrix3& rot_matrix); // From rotation matrix +SO3(const AngleAxis& angle_axis); // From angle-axis + +// Group operations +SO3 compose(const SO3& other) const; +SO3 inverse() const; + +// Access to components +Quaternion quaternion() const; +Matrix3 matrix() const; +AngleAxis angleAxis() const; + +// Tangent space (Lie algebra) operations +Vector3 log() const; +static SO3 exp(const Vector3& tangent); + +// Acting on points +Vector3 act(const Vector3& point) const; + +// Adjoint representation +Matrix3 adjoint() const; +``` + +## Performance Characteristics + +Based on benchmarks, SO(3) operations have the following performance characteristics: + +- **Composition**: O(1) time complexity - involves quaternion multiplication +- **Inverse**: O(1) time complexity - involves quaternion conjugation +- **Matrix conversion**: O(1) time complexity - creates a 3×3 rotation matrix +- **Exponential map**: O(1) time complexity - converts from axis-angle to quaternion +- **Logarithmic map**: O(1) time complexity - converts from quaternion to axis-angle +- **Acting on points**: O(1) time complexity - applies the rotation to a 3D point + +For most operations, the quaternion representation offers better performance and numerical stability compared to rotation matrices, especially for composition and inversion. + +## Example Usage + +### Basic Operations + +```cpp +#include +#include + +int main() { + // Create an SO(3) element (90-degree rotation around z-axis) + Eigen::AngleAxisd angle_axis(M_PI/2, Eigen::Vector3d::UnitZ()); + Cosserat::SO3 rotation(angle_axis); + + // Get representations + Eigen::Matrix3d rot_mat = rotation.matrix(); + Eigen::Quaterniond quat = rotation.quaternion(); + + std::cout << "Rotation matrix:\n" << rot_mat << "\n"; + std::cout << "Quaternion: " << quat.coeffs().transpose() << "\n"; + + // Create another rotation (45-degree rotation around x-axis) + Eigen::AngleAxisd another_angle_axis(M_PI/4, Eigen::Vector3d::UnitX()); + Cosserat::SO3 another_rotation(another_angle_axis); + + // Compose rotations + auto composed = rotation.compose(another_rotation); + + // Inverse rotation + auto inverse = rotation.inverse(); + + return 0; +} +``` + +### Tangent Space Operations + +```cpp +#include +#include + +int main() { + // Create a rotation from axis-angle + Eigen::Vector3d axis(1.0, 1.0, 1.0); + axis.normalize(); + double angle = M_PI/3; + Eigen::AngleAxisd angle_axis(angle, axis); + Cosserat::SO3 rotation(angle_axis); + + // Convert to Lie algebra (tangent space) + Eigen::Vector3d tangent = rotation.log(); + std::cout << "Tangent vector: " << tangent.transpose() << "\n"; + + // Convert back from Lie algebra to SO(3) + auto recovered = Cosserat::SO3::exp(tangent); + + // Create directly from tangent vector + Eigen::Vector3d new_tangent(0.1, 0.2, 0.3); // Small rotation + auto new_rotation = Cosserat::SO3::exp(new_tangent); + + return 0; +} +``` + +### Acting on Points + +```cpp +#include +#include +#include + +int main() { + // Create a rotation + Eigen::AngleAxisd angle_axis(M_PI/2, Eigen::Vector3d::UnitZ()); + Cosserat::SO3 rotation(angle_axis); + + // Rotate a single point + Eigen::Vector3d point(1.0, 0.0, 0.0); + Eigen::Vector3d rotated_point = rotation.act(point); + std::cout << "Original point: " << point.transpose() << "\n"; + std::cout << "Rotated point: " << rotated_point.transpose() << "\n"; + + // Rotate multiple points + std::vector points = { + Eigen::Vector3d(1.0, 0.0, 0.0), + Eigen::Vector3d(0.0, 1.0, 0.0), + Eigen::Vector3d(0.0, 0.0, 1.0) + }; + + std::vector rotated_points; + for (const auto& p : points) { + rotated_points.push_back(rotation.act(p)); + } + + return 0; +} +``` + +### Interpolation + +```cpp +#include +#include + +int main() { + // Create two rotations + Cosserat::SO3 start(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())); + Cosserat::SO3 end(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ())); + + // Interpolate between them (spherical linear interpolation) + Cosserat::SO3 mid = Cosserat::slerp(start, end, 0.5); + + // Interpolate using the exponential map + Eigen::Vector3d start_tangent = start.log(); + Eigen::Vector3d end_tangent = end.log(); + Eigen::Vector3d mid_tangent = start_tangent + 0.5 * (end_tangent - start_tangent); + Cosserat::SO3 mid2 = Cosserat::SO3::exp(mid_tangent); + + return 0; +} +``` + +## Best Practices + +1. **Use quaternion representation** for most operations due to better numerical stability and performance. +2. **Normalize quaternions regularly** to prevent numerical drift. +3. **When performing many rotations**, compose them first and then apply the result, rather than applying each rotation individually. +4. **For interpolation between rotations**, use spherical linear interpolation (SLERP) rather than linear interpolation of matrices or Euler angles. +5. **Be aware of the double cover** - two quaternions can represent the same rotation (q and -q). +6. **For small rotations**, the exponential map can be approximated, but use the full implementation for general cases. +7. **When converting between representations**, be aware of numerical issues at singularities (e.g., Euler angles gimbal lock). +8. **For time-varying rotations**, work in the tangent space for derivatives. +9. **Use the appropriate constructor** based on your input data to avoid unnecessary conversions. +10. **Remember that rotations are not commutative** - the order of composition matters. + +## Numerical Stability Considerations + +- **Quaternion normalization**: Ensure quaternions stay unit length by normalizing periodically +- **Logarithm near identity**: Use specialized implementations for small rotations +- **Avoiding gimbal lock**: Work with quaternions or axis-angle rather than Euler angles +- **Double cover handling**: Ensure that interpolation takes the shortest path +- **Composition of many rotations**: Reorthogonalize occasionally to prevent drift + diff --git a/src/Cosserat/liegroups/tasks.md/feature-lieAgebra.md b/src/Cosserat/liegroups/tasks.md/feature-lieAgebra.md new file mode 100644 index 0000000..c0fde0e --- /dev/null +++ b/src/Cosserat/liegroups/tasks.md/feature-lieAgebra.md @@ -0,0 +1,49 @@ +1. Prepare a clean working state: + + - Save or stash any uncommitted changes in the current working directory. + - Pull the latest changes from remote_master to ensure you have the most up-to-date code base. + +2. Create and switch to the feature branch: + + - Create a new feature branch named "feature/lie-groups" from remote_master. + - Confirm that you are on the new branch before proceeding with any code changes. + +3. Set up the directory structure: + + - Inside the Cosserat directory, create a subdirectory named "liegroups/". + - Add the following initial files: + • liegroups/LieGroupBase.h (templated base class declaration) + • liegroups/LieGroupBase.inl (template implementations) + • liegroups/Types.h (type definitions and forward declarations) + +4. Implement the LieGroupBase class: + + - Declare a templated class LieGroupBase to define common methods for Lie groups. + - Provide pure virtual (or abstract) functions for composition, inverse, exponential, logarithm, Adjoint, and group action. + - Use Eigen for matrix/vector operations. + +5. Implement specific derived classes: + + - Create derived classes for ℝ(n), SO(2), SE(2), SO(3), SE(3), SE_2(3), and SGal(3) by extending LieGroupBase. + - Implement each class’s specific group properties and overrides of the base class functions. + - Leverage Eigen’s transforms (e.g., Rotation2D, Quaternion, Isometry3d) where appropriate. + +6. Implement the Bundle template class: + + - Add a Bundle class to liegroups/ for handling manifold products or bundles. + - Provide functionalities to combine multiple Lie groups into a unified manifold representation. + +7. Integrate into the build and project structure: + + - Update CMakeLists.txt to include the new liegroups/ directory and files in the build process. + - Ensure the created headers are referenced correctly in Cosserat/fwd.h and any other relevant headers. + - Follow the existing code style and documentation guidelines throughout. + +8. Test and verify: + + - Compile and run tests (existing or newly added) to validate the new Lie group functionalities. + - Review code style compliance and documentation for clarity and consistency. + +9. Finalize and push: + - Commit the changes with an informative commit message. + - Push the new feature branch to the remote repository for review or pull request. diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 72b53a9..c817053 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -22,6 +22,9 @@ #pragma once #include #include +#include +#include +#include #include @@ -44,9 +47,13 @@ using sofa::type::Vec3; using sofa::type::Vec6; using sofa::type::Mat; -// TODO(dmarchal: 2024/06/12): please check the comment to confirme this is true -using SE3 = sofa::type::Matrix4; ///< The "coordinate" in SE3 -using se3 = sofa::type::Matrix4; ///< The "speed" of change of SE3. +// Modern Lie group implementations +using SO3d = Cosserat::SO3; +using SE3d = Cosserat::SE3; + +// Legacy types for backward compatibility +using SE3_Legacy = sofa::type::Matrix4; ///< Legacy Matrix4 representation +using se3_Legacy = sofa::type::Matrix4; ///< Legacy Matrix4 representation using _se3 = Eigen::Matrix4d; using _SE3 = Eigen::Matrix4d; @@ -54,7 +61,7 @@ using Cosserat::type::Frame; using Cosserat::type::TangentTransform; using Cosserat::type::RotMat; - +} } /*! * \class BaseCosseratMapping @@ -104,74 +111,212 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping vector m_nodeAdjointVectors; - // TODO(dmarchal:2024/06/07): explain why these attributes are unused - // : yadagolo: Need for the dynamic function, which is not working yet. But the component is in this folder - // : dmarchal: don't add something that will be used "one day" - // : dmarchal: it look like as if you should be working in a branch for making new feature and merge it when it is ready. - [[maybe_unused]] vector m_nodeAdjointEtaVectors; - [[maybe_unused]] vector m_frameAdjointEtaVectors; - [[maybe_unused]] vector m_node_coAdjointEtaVectors; - [[maybe_unused]] vector m_frame_coAdjointEtaVectors; + /** + * @brief Compute the adjoint representation for a transformation frame + * + * The adjoint representation is used to transform twists between different + * coordinate frames. It is also used for updating velocities. + * + * @param frame The transformation frame + * @param adjoint Output adjoint matrix as TangentTransform + */ + /** + * @brief Compute the co-adjoint matrix of a transformation frame + * + * The co-adjoint matrix is the transpose of the adjoint matrix, used + * to transform wrenches (force-torque pairs) between coordinate frames. + * + * @param frame The transformation frame + * @param coAdjoint Output co-adjoint matrix + */ + void computeCoAdjoint(const Frame &frame, Mat6x6 &coAdjoint); -public: - /********************** Inhertited from BaseObject **************/ - void init() final override; - virtual void doBaseCosseratInit() = 0; + /** + * @brief Update exponential vectors for all frames and nodes + * + * @param strain_state Vector of strain states + */ + void updateExponentialSE3(const vector &strain_state); + + /** + * @brief Update tangent exponential vectors + * + * @param inDeform Vector of deformations + */ + void updateTangExpSE3(const vector &inDeform); - // This function is called by a callback function, which is not the case - // of the init function - void update_geometry_info(); + /** + * @brief Compute tangent exponential map + * + * @param x Parameter for tangent map + * @param k Strain vector + * @param TgX Output tangent matrix + */ + void computeTangExp(double &x, const Coord1 &k, Mat6x6 &TgX); + + /** + * @brief Implementation of tangent exponential map + * + * @param x Parameter for tangent map + * @param k Strain vector + * @param TgX Output tangent matrix + */ + void computeTangExpImplementation(double &x, const Vec6 &k, Mat6x6 &TgX); + /** + * @brief Compute eta vector for a given input + * + * @param baseEta Base eta vector + * @param k_dot Vector of strain rates + * @param abs_input Position along the rod + * @return Vec6 Computed eta vector + */ + [[maybe_unused]] Vec6 + computeETA(const Vec6 &baseEta, const vector &k_dot, double abs_input); + + /** + * @brief Compute logarithm map using SE3 + * + * @param x Scaling factor + * @param gX Transformation matrix + * @return Mat4x4 Logarithm of the transformation + */ + Mat4x4 computeLogarithm(const double &x, const Mat4x4 &gX); + void computeAdjoint(const Vec6 &twist, Mat6x6 &adjoint); + + /** + * @brief Updates velocity state using Lie group operations + * + * Implements proper velocity updates using adjoint transformations and + * the new Lie group functionality to propagate velocities along the beam. + * + * @param k_dot Strain rates (angular and linear velocity derivatives) + * @param base_velocity Base node velocity in body coordinates + */ + void updateVelocityState(const vector& k_dot, const Vec6& base_velocity); + + /** + * @brief Transform velocity between different coordinate frames + * + * Uses SE3 adjoint to transform a velocity twist from one frame to another. + * + * @param source_frame Source coordinate frame + * @param source_velocity Velocity in source frame + * @param target_frame Target coordinate frame + * @param target_velocity Output: velocity expressed in target frame + */ + void transformVelocity( + const Frame& source_frame, + const Vec6& source_velocity, + const Frame& target_frame, + Vec6& target_velocity); + + /** + * @brief Compute the angle parameter for logarithm calculation + * + * @param x Scaling factor + * @param gX Transformation matrix + * @return double The angle parameter + */ double computeTheta(const double &x, const Mat4x4 &gX); - void printMatrix(const Mat6x6 R); - sofa::type::Mat3x3 extractRotMatrix(const Frame &frame); + /** + * @brief Extract rotation matrix from a Frame using SO3 + * + * @param frame The input Frame containing orientation as a quaternion + * @return Mat3x3 The 3x3 rotation matrix + */ + Mat3x3 extractRotMatrix(const Frame &frame); + + /** + * @brief Build a projector matrix from a Frame + * + * @param T The transformation frame + * @return TangentTransform The projector matrix + */ TangentTransform buildProjector(const Frame &T); - Mat3x3 getTildeMatrix(const Vec3 &u); - void buildAdjoint(const Mat3x3 &A, const Mat3x3 &B, Mat6x6 &Adjoint); - void buildCoAdjoint(const Mat3x3 &A, const Mat3x3 &B, Mat6x6 &coAdjoint); + /** + * @brief Create a skew-symmetric matrix from a vector using SO3::hat + * + * @param u The input 3D vector + * @return sofa::type::Matrix3 The skew-symmetric matrix + */ + sofa::type::Matrix3 getTildeMatrix(const Vec3 &u); + + /** + * @brief Print a matrix using modern logging + * + * Uses SOFA's message system instead of printf for better integration + * with the logging framework. + * + * @param matrix The 6x6 matrix to print + */ + void printMatrix(const Mat6x6& matrix); + + /** + * @brief Convert a SOFA Frame to an SE3 transformation + * + * @param frame The input SOFA Frame + * @return SE3d The SE3 transformation + */ + SE3d frameToSE3(const Frame &frame); + + /** + * @brief Convert an SE3 transformation to a SOFA Frame + * + * @param transform The input SE3 transformation + * @return Frame The SOFA Frame + */ + Frame SE3ToFrame(const SE3d &transform); Mat4x4 convertTransformToMatrix4x4(const Frame &T); Vec6 piecewiseLogmap(const _SE3 &g_x); /*! - * @brief Computes the rotation matrix around the X-axis + * @brief Computes the rotation matrix around the X-axis using SO3 + * + * Uses the Lie group SO3 implementation for better numerical stability + * and consistency with other transformations. * * @param angle The rotation angle in radians * @return RotMat A 3x3 rotation matrix representing the rotation around the X-axis */ RotMat rotationMatrixX(double angle) { - Eigen::Matrix3d rotation; - rotation << 1, 0, 0, 0, cos(angle), -sin(angle), 0, sin(angle), cos(angle); - return rotation; + // Create rotation using the exponential map with axis (1,0,0) + Eigen::Vector3d axis = Eigen::Vector3d::UnitX(); + return SO3d::exp(angle * axis).matrix(); } /*! - * @brief Computes the rotation matrix around the Y-axis + * @brief Computes the rotation matrix around the Y-axis using SO3 + * + * Uses the Lie group SO3 implementation for better numerical stability + * and consistency with other transformations. * * @param angle The rotation angle in radians * @return RotMat A 3x3 rotation matrix representing the rotation around the Y-axis */ - // function... it shouldn't be (re)implemented in a base classe. RotMat rotationMatrixY(double angle) { - Eigen::Matrix3d rotation; - rotation << cos(angle), 0, sin(angle), 0, 1, 0, -sin(angle), 0, cos(angle); - return rotation; + // Create rotation using the exponential map with axis (0,1,0) + Eigen::Vector3d axis = Eigen::Vector3d::UnitY(); + return SO3d::exp(angle * axis).matrix(); } - // TODO(dmarchal: 2024/06/07), this looks like a very common utility - // function... it shouldn't be (re)implemented in a base classe. the type of - // the data return should also be unified between rotationMatrixX, Y and Z + /*! + * @brief Computes the rotation matrix around the Z-axis using SO3 + * + * Uses the Lie group SO3 implementation for better numerical stability + * and consistency with other transformations. + * + * @param angle The rotation angle in radians + * @return RotMat A 3x3 rotation matrix representing the rotation around the Z-axis + */ RotMat rotationMatrixZ(double angle) { - RotMat rotation; - rotation << cos(angle), -sin(angle), 0, sin(angle), cos(angle), 0, 0, 0, 1; - return rotation; + // Create rotation using the exponential map with axis (0,0,1) + Eigen::Vector3d axis = Eigen::Vector3d::UnitZ(); + return SO3d::exp(angle * axis).matrix(); } - -protected: - sofa::Data> d_curv_abs_section; - sofa::Data> d_curv_abs_frames; sofa::Data d_debug; using Inherit1::fromModels1; @@ -189,14 +334,36 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping /// Destructor ~BaseCosseratMapping() override = default; + /** + * @brief Computes the exponential map for SE(3) using Lie group theory + * + * This function calculates the frame transformation resulting from applying + * the exponential map to a twist vector scaled by the section length. + * + * @param sub_section_length The length of the beam section + * @param k The twist vector (angular and linear velocity) + * @param frame_i The resulting frame transformation + */ void computeExponentialSE3(const double &sub_section_length, - const Coord1 &k,Frame &frame_i); - - // TODO(dmarchal: 2024/06/07): - // - clarify the difference between computeAdjoing and buildAdjoint ... - // - clarify why we need Transform and Vec6 and TangentTransform & Mat6x6 + const Coord1 &k, Frame &frame_i); + + /** + * @brief Computes the adjoint matrix of a transformation frame + * + * The adjoint matrix is used to transform twists between different reference frames. + * + * @param frame The transformation frame + * @param adjoint Output adjoint matrix + */ void computeAdjoint(const Frame &frame, TangentTransform &adjoint); - void computeAdjoint(const Vec6 &frame, Mat6x6 &adjoint); + + /** + * @brief Computes the adjoint matrix from a 6D vector representation + * + * @param twist The twist vector (angular and linear velocity) + * @param adjoint Output adjoint matrix + */ + void computeAdjoint(const Vec6 &twist, Mat6x6 &adjoint); void computeCoAdjoint(const Frame &frame, Mat6x6 &coAdjoint); diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 227d517..3dd398f 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -45,104 +45,349 @@ using sofa::type::Vec6; using sofa::type::Vec3; using sofa::type::Quat; -template -BaseCosseratMapping::BaseCosseratMapping() - // TODO(dmarchal: 2024/06/12): please add the help comments ! - : d_curv_abs_section(initData(&d_curv_abs_section, "curv_abs_input", - " need to be com....")), - d_curv_abs_frames(initData(&d_curv_abs_frames, "curv_abs_output", - " need to be com....")), - d_debug(initData(&d_debug, false, "debug", "printf for the debug")), - m_indexInput(0) {} +/** + * @brief Compute logarithm using SE3's native implementation + * + * @param x Scaling factor + * @param gX Transformation matrix + * @return Mat4x4 Logarithm of the transformation + */ +/** + * @brief Compute logarithm using SE3's native implementation + * + * @param x Scaling factor + * @param gX Transformation matrix + * @return Mat4x4 Logarithm of the transformation + */ +n1, TIn2, TOut>::computeLogarithm(const double &x, + const Mat4x4 &gX) -> Mat4x4 +{ + if (x <= std::numeric_limits::epsilon()) { + return Mat4x4::Identity(); + } + // Convert to Eigen format for SE3 + Eigen::Matrix4d eigen_gX; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + eigen_gX(i, j) = gX[i][j]; + } + } + + // Create SE3 from matrix + SE3d transform(eigen_gX); + + // Compute logarithm in the Lie algebra + Eigen::Matrix log_vector = transform.log(); + + // Scale by x + log_vector *= x; + + // Convert back to SE3 and then to Matrix form + SE3d scaled_result = SE3d::exp(log_vector); + Eigen::Matrix4d eigen_result = scaled_result.matrix(); + + // Convert back to SOFA Mat4x4 + Mat4x4 result; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + result[i][j] = eigen_result(i, j); + } + } + + return result; +} + Eigen::Vector3d(strain_n[0], strain_n[1], strain_n[2]); + linear_velocity = Eigen::Vector3d(strain_n[3], strain_n[4], strain_n[5]); + } + + // Scale by section length + angular_velocity *= sub_section_length; + linear_velocity *= sub_section_length; + + // Create twist vector for Lie algebra + Eigen::Matrix twist; + twist << angular_velocity, linear_velocity; + + // Use SE3 to compute exponential map + SE3d transform = SE3d::exp(twist); + + // Convert to Frame format + g_X_n = SE3ToFrame(transform); +} + + return result; +} +/** + * @brief Updates velocity state using Lie group operations + * + * Implements proper velocity updates using adjoint transformations and the new + * Lie group functionality to propagate velocities along the beam. + * + * Mathematical background: + * For a serial kinematic chain with joint velocities k_dot, the body velocity + * at each link can be computed recursively using: + * V_{i+1} = Ad_{g_{i,i+1}} * V_i + k_dot_i + * where Ad_{g_{i,i+1}} is the adjoint of the relative transformation from link i to i+1. + */ template -void BaseCosseratMapping::init() +void BaseCosseratMapping::updateVelocityState( + const vector& k_dot, + const Vec6& base_velocity) { - m_strain_state = nullptr; - m_rigid_base = nullptr; - m_global_frames = nullptr; - - if(fromModels1.empty()) - { - msg_error() << "input1 not found" ; - return; + m_nodesVelocityVectors.clear(); + m_nodeAdjointEtaVectors.clear(); + m_frameAdjointEtaVectors.clear(); + m_node_coAdjointEtaVectors.clear(); + m_frame_coAdjointEtaVectors.clear(); + + // First node velocity is the base velocity + m_nodesVelocityVectors.push_back(base_velocity); + + // Update velocities along the beam + for (size_t i = 0; i < k_dot.size(); ++i) { + // Store results in SOFA format + Vec6 sofa_velocity; + for (int j = 0; j < 6; ++j) { + sofa_velocity[j] = new_velocity(j); + } + m_nodesVelocityVectors.push_back(sofa_velocity); + + // Store adjoint matrices for later use + Mat6x6 sofa_adjoint; + Mat6x6 sofa_coadjoint; + for (int row = 0; row < 6; ++row) { + for (int col = 0; col < 6; ++col) { + sofa_adjoint[row][col] = adjoint(row, col); + sofa_coadjoint[row][col] = adjoint(col, row); // Transpose for co-adjoint + } + } + + m_nodeAdjointEtaVectors.push_back(sofa_adjoint); + m_node_coAdjointEtaVectors.push_back(sofa_coadjoint); } - - if(fromModels2.empty()) - { - msg_error() << "input2 not found" ; - return; + + // Calculate frame velocities + for (size_t i = 0; i < m_indicesVectors.size(); ++i) { + size_t beam_index = m_indicesVectors[i] - 1; + if (beam_index < m_nodesVelocityVectors.size() - 1) { + // Interpolate velocities for frames between beam nodes + Vec6 node_velocity = m_nodesVelocityVectors[beam_index]; + Vec6 frame_velocity; + + // Transform velocity from node to frame + transformVelocity( + m_nodesExponentialSE3Vectors[beam_index], + node_velocity, + m_framesExponentialSE3Vectors[i], + frame_velocity + ); + + // Store frame velocity and adjoint + // Calculate and store adjoint matrices for frames + SE3d frame_transform = frameToSE3(m_framesExponentialSE3Vectors[i]); + Eigen::Matrix frame_adjoint = frame_transform.adjoint(); + + Mat6x6 sofa_frame_adjoint; + Mat6x6 sofa_frame_coadjoint; + for (int row = 0; row < 6; ++row) { + for (int col = 0; col < 6; ++col) { + sofa_frame_adjoint[row][col] = frame_adjoint(row, col); + sofa_frame_coadjoint[row][col] = frame_adjoint(col, row); + } + } + + m_frameAdjointEtaVectors.push_back(sofa_frame_adjoint); + m_frame_coAdjointEtaVectors.push_back(sofa_frame_coadjoint); + } } - - if(toModels.empty()) - { - msg_error() << "output missing" ; - return; +} + Eigen::Matrix new_velocity = adjoint * current_velocity + strain_velocity; + + // Store results in SOFA + Mat6x6 sofa_frame_coadjoint; + for (int row = 0; row < 6; ++row) { + for (int col = 0; col < 6; ++col) { + sofa_frame_adjoint[row][col] = frame_adjoint(row, col); + sofa_frame_coadjoint[row][col] = frame_adjoint(col, row); + } + } + + m_frameAdjointEtaVectors.push_back(sofa_frame_adjoint); + m_frame_coAdjointEtaVectors.push_back(sofa_frame_coadjoint); + } } - - m_strain_state = fromModels1[0]; - m_rigid_base = fromModels2[0]; - m_global_frames = toModels[0]; - - // Get initial frame state - auto xfromData = - m_global_frames->read(sofa::core::vec_id::read_access::position); - const vector xfrom = xfromData->getValue(); - - m_vecTransform.clear(); - for (unsigned int i = 0; i < xfrom.size(); i++) - m_vecTransform.push_back(xfrom[i]); - - update_geometry_info(); - doBaseCosseratInit(); - Inherit1::init(); } -//___________________________________________________________________________ +/** + * @brief Transform velocity between different coordinate frames + * + * Uses SE3 adjoint to transform a velocity twist from one frame to another. + * + * Mathematical background: + * Given a twist V_a in frame A and a transformation g_ab from frame A to B, + * the twist V_b in frame B is given by: + * V_b = Ad_{g_ab^{-1}} * V_a + */ template -void BaseCosseratMapping::update_geometry_info() +void BaseCosseratMapping::transformVelocity( + const Frame& source_frame, + const Vec6& source_velocity, + const Frame& target_frame, + Vec6& target_velocity) { - // For each frame in the global frame, find the segment of the beam to which - // it is attached. Here we only use the information from the curvilinear - // abscissa of each frame. - auto curv_abs_section = getReadAccessor(d_curv_abs_section); - auto curv_abs_frames = getReadAccessor(d_curv_abs_frames); - - msg_info() - << " curv_abs_section: " << curv_abs_section.size() - << "\ncurv_abs_frames: " << curv_abs_frames.size(); - - m_indicesVectors.clear(); - m_framesLengthVectors.clear(); - m_beamLengthVectors.clear(); - m_indicesVectorsDraw.clear(); // just for drawing + // Convert frames to SE3 + SE3d source_transform = frameToSE3(source_frame); + SE3d target_transform = frameToSE3(target_frame); + + // Compute relative transformation + SE3d relative_transform = target_transform.inverse().compose(source_transform); + + // Get adjoint matrix +/** + * @ + Eigen::Matrix twist; + twist << angular_velocity, linear_velocity; + + // Use SE3 Lie group to compute the exponential map + SE3d transform = SE3d::exp(twist); + + // Convert to Frame format for SOFA + g_X_n = SE3ToFrame(transform); +} + // Copy to the SOFA Mat6x6 format + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { + adjoint[i][j] = adjointMatrix(i, j); + } + } +} - const auto sz = curv_abs_frames.size(); - auto sectionIndex = 1; - /* - * This main loop iterates through the frames, comparing their curvilinear abscissa values with those of the beam sections: - If the frame's abscissa is less than the current section's, it assigns the current section index. - If they're equal, it assigns the current index and then increments it. - If the frame's abscissa is greater, it increments the index and then assigns it. - * */ - for (auto i = 0; i < sz; ++i) - { - if (curv_abs_section[sectionIndex] > curv_abs_frames[i]) - { - m_indicesVectors.emplace_back(sectionIndex); - m_indicesVectorsDraw.emplace_back(sectionIndex); +/** + * @brief Compute the co-adjoint matrix of a transformation frame + * + * The co-adjoint matrix is the transpose of the adjoint matrix and is used + * to transform wrenches (force-torque pairs) between different coordinate frames. + * + * Mathematical background: + * The co-adjoint is a 6×6 matrix that can be written in block form as: + * + * Ad_g^* = [ R^T [t]^T R^T ] + * [ 0 R^T ] + * + * where R is the rotation matrix, t is the translation vector, and [t] is the + * skew-symmetric matrix formed from t. + */ +template +void BaseCosseratMapping::computeCoAdjoint(const Frame &frame, + Mat6x6 &co_adjoint) { + // Create an SE3 transformation from the Frame + Eigen::Quaterniond quat( + frame.getOrientation()[3], // w + frame.getOrientation()[0], // x + frame.getOrientation()[1], // y + frame.getOrientation()[2] // z + ); + + Eigen::Vector3d trans( + frame.getCenter()[0], + frame.getCenter()[1], + frame.getCenter()[2] + ); + + SE3d transform(SO3d(quat), trans); + + // Get the co-adjoint matrix (transpose of adjoint) + Eigen::Matrix coAdjointMatrix = transform.adjoint().transpose(); + + // Copy to the SOFA Mat6x6 format + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { + co_adjoint[i][j] = coAdjointMatrix(i, j); } - //@todo: I should change this with abs(val1-val2)>epsilon - else if (curv_abs_section[sectionIndex] == curv_abs_frames[i]) - { - m_indicesVectors.emplace_back(sectionIndex); - sectionIndex++; - m_indicesVectorsDraw.emplace_back(sectionIndex); + } +} + +/** + * @brief Compute the adjoint representation of a twist vector + * + * This function computes the matrix representation of the adjoint action + * of a twist vector. This matrix can be used to compute the Lie bracket + * of two twist vectors. + * + * Mathematical background: + * For a twist ξ = (ω, v), the adjoint representation ad_ξ is a 6×6 matrix + * that can be written in block form as: + * + * ad_ξ = [ [ω] 0 ] + * [ [v] [ω] ] + * + * where [ω] and [v] are the skew-symmetric matrices formed from ω and v. + */ +template +void BaseCosseratMapping::computeAdjoint(const Vec6 &twist, + Mat6x6 &adjoint) +{ + // Extract angular and linear velocity components + Eigen::Vector3d omega(twist[0], twist[1], twist[2]); + Eigen::Vector3d v(twist[3], twist[4], twist[5]); + + // Create the skew-symmetric matrices + Eigen::Matrix3d omega_hat = SO3d::hat(omega); + Eigen::Matrix3d v_hat = SO3d::hat(v); + + // Build the adjoint matrix + Eigen::Matrix ad; + ad.setZero(); + + // Top-left block: [ω] + ad.block<3, 3>(0, 0) = omega_hat; + + // Bottom-left block: [v] + ad.block<3, 3>(3, 0) = v_hat; + + // Bottom-right block: [ω] + ad.block<3, 3>(3, 3) = omega_hat; + + // Copy to the SOFA Mat6x6 format + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { + adjoint[i][j] = ad(i, j); } - else { - sectionIndex++; - m_indicesVectors.emplace_back(sectionIndex); + } +} + angular and linear velocity are provided + angular_velocity = Eigen::Vector3d(strain_n[0], strain_n[1], strain_n[2]); + linear_velocity = Eigen::Vector3d(strain_n[3], strain_n[4], strain_n[5]); + } + + // Scale by section length + angular_velocity *= sub_section_length; + linear_velocity *= sub_section_length; + + // Create twist vector (6D) for the Lie algebra element + Eigen::Matrix twist; + twist << angular_velocity, linear_velocity; + + // Use SE3 Lie group to compute the exponential map + SE3d transformation = SE3d::exp(twist); + + // Convert to Frame format for SOFA + Eigen::Quaterniond quaternion = transformation.rotation().quaternion(); + Eigen::Vector3d translation = transformation.translation(); + + // Update the output frame + g_X_n = Frame( + Vec3(translation[0], translation[1], translation[2]), + Quat(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()) + ); +} + Vec3::Map(&(frame_i.getOrientation()[0])) = quaternion.coeffs(); + for (auto i = 0; i < 3; i++) { + frame_i.getCenter()[i] = translation[i]; + } +} m_indicesVectorsDraw.emplace_back(sectionIndex); } diff --git a/src/Tests/CMakeLists.txt b/src/Tests/CMakeLists.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/Tests/benchmarks/CMakeLists.txt b/src/Tests/benchmarks/CMakeLists.txt new file mode 100644 index 0000000..3ee2f78 --- /dev/null +++ b/src/Tests/benchmarks/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.12) + +project(Cosserat_benchmarks) + +# Check if benchmarking is enabled +option(COSSERAT_BUILD_BENCHMARKS "Build benchmarks for Cosserat" ON) + +if(COSSERAT_BUILD_BENCHMARKS) + # Find Google Benchmark + find_package(benchmark REQUIRED) + + # Set benchmark source files + set(BENCHMARK_SOURCE_FILES + liegroups/LieGroupsBenchmark.cpp + ) + + # Create benchmark executable + add_executable(${PROJECT_NAME} ${BENCHMARK_SOURCE_FILES}) + + # Link against required libraries + target_link_libraries(${PROJECT_NAME} + Cosserat + benchmark::benchmark + benchmark::benchmark_main + ) + + # Include directories + target_include_directories(${PROJECT_NAME} PRIVATE + ${CMAKE_SOURCE_DIR}/src + ) + + # Add benchmark target + add_custom_target(run_benchmarks + COMMAND ${PROJECT_NAME} + DEPENDS ${PROJECT_NAME} + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + COMMENT "Running Cosserat benchmarks" + ) +endif() + diff --git a/src/Tests/benchmarks/liegroups/LieGroupsBenchmark.cpp b/src/Tests/benchmarks/liegroups/LieGroupsBenchmark.cpp new file mode 100644 index 0000000..599d31e --- /dev/null +++ b/src/Tests/benchmarks/liegroups/LieGroupsBenchmark.cpp @@ -0,0 +1,131 @@ +#include +#include +#include +#include +#include + +// RealSpace benchmarks +static void BM_RealSpace_Composition(benchmark::State& state) { + // Setup + const int dim = state.range(0); + Cosserat::RealSpace a(Eigen::VectorXd::Random(dim)); + Cosserat::RealSpace b(Eigen::VectorXd::Random(dim)); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.compose(b)); + } + + state.SetComplexityN(dim); +} +BENCHMARK(BM_RealSpace_Composition)->RangeMultiplier(2)->Range(1, 1024)->Complexity(); + +static void BM_RealSpace_Inverse(benchmark::State& state) { + // Setup + const int dim = state.range(0); + Cosserat::RealSpace a(Eigen::VectorXd::Random(dim)); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.inverse()); + } + + state.SetComplexityN(dim); +} +BENCHMARK(BM_RealSpace_Inverse)->RangeMultiplier(2)->Range(1, 1024)->Complexity(); + +// SO2 benchmarks +static void BM_SO2_Composition(benchmark::State& state) { + // Setup + Cosserat::SO2 a(M_PI / 4.0); + Cosserat::SO2 b(M_PI / 3.0); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.compose(b)); + } +} +BENCHMARK(BM_SO2_Composition); + +static void BM_SO2_Inverse(benchmark::State& state) { + // Setup + Cosserat::SO2 a(M_PI / 4.0); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.inverse()); + } +} +BENCHMARK(BM_SO2_Inverse); + +static void BM_SO2_Log(benchmark::State& state) { + // Setup + Cosserat::SO2 a(M_PI / 4.0); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.log()); + } +} +BENCHMARK(BM_SO2_Log); + +static void BM_SO2_Exp(benchmark::State& state) { + // Setup + double theta = M_PI / 4.0; + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(Cosserat::SO2::exp(theta)); + } +} +BENCHMARK(BM_SO2_Exp); + +// SE2 benchmarks +static void BM_SE2_Composition(benchmark::State& state) { + // Setup + Cosserat::SE2 a(M_PI / 4.0, 1.0, 2.0); + Cosserat::SE2 b(M_PI / 3.0, -1.0, 0.5); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.compose(b)); + } +} +BENCHMARK(BM_SE2_Composition); + +static void BM_SE2_Inverse(benchmark::State& state) { + // Setup + Cosserat::SE2 a(M_PI / 4.0, 1.0, 2.0); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.inverse()); + } +} +BENCHMARK(BM_SE2_Inverse); + +static void BM_SE2_Log(benchmark::State& state) { + // Setup + Cosserat::SE2 a(M_PI / 4.0, 1.0, 2.0); + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(a.log()); + } +} +BENCHMARK(BM_SE2_Log); + +static void BM_SE2_Exp(benchmark::State& state) { + // Setup + Eigen::Vector3d tangent; + tangent << M_PI / 4.0, 1.0, 2.0; + + // Benchmark loop + for (auto _ : state) { + benchmark::DoNotOptimize(Cosserat::SE2::exp(tangent)); + } +} +BENCHMARK(BM_SE2_Exp); + +BENCHMARK_MAIN(); + diff --git a/src/Tests/liegroups/SO2Test.cpp b/src/Tests/liegroups/SO2Test.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/Tests/liegroups/UtilsTest.cpp b/src/Tests/liegroups/UtilsTest.cpp new file mode 100644 index 0000000..1a01434 --- /dev/null +++ b/src/Tests/liegroups/UtilsTest.cpp @@ -0,0 +1,289 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 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 . * +******************************************************************************/ + +#include +#include +#include +#include + +namespace sofa::component::cosserat::liegroups::testing { + +using namespace sofa::testing; + +/** + * Test suite for Lie group utilities + */ +class UtilsTest : public BaseTest +{ +protected: + using Utils = Cosserat::LieGroupUtils; + using Vector2d = Eigen::Vector2d; + using Vector3d = Eigen::Vector3d; + + const double pi = M_PI; + const double eps = 1e-10; + + void SetUp() override {} + void TearDown() override {} +}; + +/** + * Test angle normalization + */ +TEST_F(UtilsTest, AngleNormalization) +{ + // Test normalization of angles within [-π, π] + EXPECT_NEAR(Utils::normalizeAngle(0.0), 0.0, eps); + EXPECT_NEAR(Utils::normalizeAngle(pi/2), pi/2, eps); + EXPECT_NEAR(Utils::normalizeAngle(-pi/2), -pi/2, eps); + EXPECT_NEAR(Utils::normalizeAngle(pi), pi, eps); + EXPECT_NEAR(Utils::normalizeAngle(-pi), -pi, eps); + + // Test normalization of angles outside [-π, π] + EXPECT_NEAR(Utils::normalizeAngle(3*pi/2), -pi/2, eps); + EXPECT_NEAR(Utils::normalizeAngle(-3*pi/2), pi/2, eps); + EXPECT_NEAR(Utils::normalizeAngle(2*pi), 0.0, eps); + EXPECT_NEAR(Utils::normalizeAngle(4*pi), 0.0, eps); + EXPECT_NEAR(Utils::normalizeAngle(-2*pi), 0.0, eps); + + // Test extreme cases + EXPECT_NEAR(Utils::normalizeAngle(1000*pi), 0.0, eps); + EXPECT_NEAR(Utils::normalizeAngle(1001*pi), pi, eps); +} + +/** + * Test sinc function with numerical stability + */ +TEST_F(UtilsTest, Sinc) +{ + // Test non-zero values + EXPECT_NEAR(Utils::sinc(pi/2), 2/pi, eps); + EXPECT_NEAR(Utils::sinc(pi), 0.0, eps); + EXPECT_NEAR(Utils::sinc(2*pi), 0.0, eps); + + // Test small values (near zero) + EXPECT_NEAR(Utils::sinc(1e-10), 1.0, eps); + EXPECT_NEAR(Utils::sinc(1e-8), 1.0, eps); + EXPECT_NEAR(Utils::sinc(0.0), 1.0, eps); + + // Test negative values + EXPECT_NEAR(Utils::sinc(-pi/2), -2/pi, eps); +} + +/** + * Test oneMinusCos function with numerical stability + */ +TEST_F(UtilsTest, OneMinusCos) +{ + // Test non-zero values + EXPECT_NEAR(Utils::oneMinusCos(pi/2), 1.0, eps); + EXPECT_NEAR(Utils::oneMinusCos(pi), 2.0, eps); + EXPECT_NEAR(Utils::oneMinusCos(2*pi), 0.0, eps); + + // Test small values (near zero) + EXPECT_NEAR(Utils::oneMinusCos(1e-8), 5e-17, 1e-16); + EXPECT_NEAR(Utils::oneMinusCos(1e-4), 5e-9, 1e-8); + EXPECT_NEAR(Utils::oneMinusCos(0.0), 0.0, eps); + + // Test negative values + EXPECT_NEAR(Utils::oneMinusCos(-pi/2), 1.0, eps); + EXPECT_NEAR(Utils::oneMinusCos(-pi), 2.0, eps); +} + +/** + * Test angle difference calculation + */ +TEST_F(UtilsTest, AngleDifference) +{ + // Test differences within [-π, π] + EXPECT_NEAR(Utils::angleDifference(0.0, 0.0), 0.0, eps); + EXPECT_NEAR(Utils::angleDifference(pi/4, 0.0), pi/4, eps); + EXPECT_NEAR(Utils::angleDifference(0.0, pi/4), -pi/4, eps); + + // Test differences that wrap around + EXPECT_NEAR(Utils::angleDifference(3*pi/4, -3*pi/4), 3*pi/2, eps); + EXPECT_NEAR(Utils::angleDifference(-3*pi/4, 3*pi/4), -3*pi/2, eps); + + // Test extreme cases + EXPECT_NEAR(Utils::angleDifference(pi, -pi), 0.0, eps); +} + +/** + * Test angle distance calculation + */ +TEST_F(UtilsTest, AngleDistance) +{ + // Test distances within [-π, π] + EXPECT_NEAR(Utils::angleDistance(0.0, 0.0), 0.0, eps); + EXPECT_NEAR(Utils::angleDistance(pi/4, 0.0), pi/4, eps); + EXPECT_NEAR(Utils::angleDistance(0.0, pi/4), pi/4, eps); + + // Test distances that wrap around + EXPECT_NEAR(Utils::angleDistance(3*pi/4, -3*pi/4), 3*pi/2, eps); + EXPECT_NEAR(Utils::angleDistance(-3*pi/4, 3*pi/4), 3*pi/2, eps); + + // Test extreme cases + EXPECT_NEAR(Utils::angleDistance(pi, -pi), 0.0, eps); +} + +/** + * Test linear interpolation + */ +TEST_F(UtilsTest, LinearInterpolation) +{ + // Test standard cases + EXPECT_NEAR(Utils::lerp(0.0, 1.0, 0.0), 0.0, eps); + EXPECT_NEAR(Utils::lerp(0.0, 1.0, 1.0), 1.0, eps); + EXPECT_NEAR(Utils::lerp(0.0, 1.0, 0.5), 0.5, eps); + EXPECT_NEAR(Utils::lerp(-1.0, 1.0, 0.5), 0.0, eps); + + // Test extrapolation + EXPECT_NEAR(Utils::lerp(0.0, 1.0, 2.0), 2.0, eps); + EXPECT_NEAR(Utils::lerp(0.0, 1.0, -1.0), -1.0, eps); +} + +/** + * Test spherical linear interpolation for angles + */ +TEST_F(UtilsTest, SlerpAngle) +{ + // Test standard cases + EXPECT_NEAR(Utils::slerpAngle(0.0, pi/2, 0.0), 0.0, eps); + EXPECT_NEAR(Utils::slerpAngle(0.0, pi/2, 1.0), pi/2, eps); + EXPECT_NEAR(Utils::slerpAngle(0.0, pi/2, 0.5), pi/4, eps); + + // Test wrapping + EXPECT_NEAR(Utils::slerpAngle(-3*pi/4, 3*pi/4, 0.5), 0.0, eps); + EXPECT_NEAR(Utils::slerpAngle(3*pi/4, -3*pi/4, 0.5), 0.0, eps); + + // Test extreme cases + EXPECT_NEAR(Utils::slerpAngle(pi, -pi, 0.5), pi, eps); +} + +/** + * Test near-zero detection for angles + */ +TEST_F(UtilsTest, NearZeroAngle) +{ + EXPECT_TRUE(Utils::isAngleNearZero(0.0)); + EXPECT_TRUE(Utils::isAngleNearZero(1e-12)); + EXPECT_TRUE(Utils::isAngleNearZero(-1e-12)); + + EXPECT_FALSE(Utils::isAngleNearZero(0.1)); + EXPECT_FALSE(Utils::isAngleNearZero(-0.1)); +} + +/** + * Test nearly equal detection for angles + */ +TEST_F(UtilsTest, NearlyEqualAngles) +{ + EXPECT_TRUE(Utils::areAnglesNearlyEqual(0.0, 0.0)); + EXPECT_TRUE(Utils::areAnglesNearlyEqual(pi/4, pi/4 + 1e-12)); + EXPECT_TRUE(Utils::areAnglesNearlyEqual(pi, -pi)); + EXPECT_TRUE(Utils::areAnglesNearlyEqual(2*pi, 0.0)); + + EXPECT_FALSE(Utils::areAnglesNearlyEqual(0.0, 0.1)); + EXPECT_FALSE(Utils::areAnglesNearlyEqual(pi/4, pi/2)); +} + +/** + * Test safe vector normalization + */ +TEST_F(UtilsTest, SafeNormalize) +{ + // Test non-zero vectors + Vector2d v1(3.0, 4.0); + Vector2d v1_norm = Utils::safeNormalize(v1); + EXPECT_NEAR(v1_norm.norm(), 1.0, eps); + EXPECT_NEAR(v1_norm[0], 0.6, eps); + EXPECT_NEAR(v1_norm[1], 0.8, eps); + + // Test zero vector + Vector2d v2(0.0, 0.0); + Vector2d v2_norm = Utils::safeNormalize(v2); + EXPECT_NEAR(v2_norm[0], 0.0, eps); + EXPECT_NEAR(v2_norm[1], 0.0, eps); + + // Test near-zero vector + Vector2d v3(1e-12, 1e-12); + Vector2d v3_norm = Utils::safeNormalize(v3); + EXPECT_NEAR(v3_norm[0], 0.0, eps); + EXPECT_NEAR(v3_norm[1], 0.0, eps); +} + +/** + * Test vector projection + */ +TEST_F(UtilsTest, VectorProjection) +{ + // Standard projection + Vector2d v(3.0, 3.0); + Vector2d onto(1.0, 0.0); + Vector2d proj = Utils::projectVector(v, onto); + EXPECT_NEAR(proj[0], 3.0, eps); + EXPECT_NEAR(proj[1], 0.0, eps); + + // Project onto zero vector + Vector2d proj_zero = Utils::projectVector(v, Vector2d(0.0, 0.0)); + EXPECT_NEAR(proj_zero[0], 0.0, eps); + EXPECT_NEAR(proj_zero[1], 0.0, eps); + + // Project onto self + Vector2d proj_self = Utils::projectVector(v, v); + EXPECT_NEAR(proj_self[0], v[0], eps); + EXPECT_NEAR(proj_self[1], v[1], eps); +} + +/** + * Test SE(2) path interpolation + */ +TEST_F(UtilsTest, SE2PathInterpolation) +{ + // Simple path interpolation + Vector3d start(0.0, 0.0, 0.0); + Vector3d end(pi/2, 1.0, 2.0); + + Vector3d mid = Utils::interpolateSE2Path(start, end, 0.5); + EXPECT_NEAR(mid[0], pi/4, eps); + EXPECT_NEAR(mid[1], 0.5, eps); + EXPECT_NEAR(mid[2], 1.0, eps); + + // Test path end points + Vector3d start_point = Utils::interpolateSE2Path(start, end, 0.0); + EXPECT_NEAR(start_point[0], start[0], eps); + EXPECT_NEAR(start_point[1], start[1], eps); + EXPECT_NEAR(start_point[2], start[2], eps); + + Vector3d end_point = Utils::interpolateSE2Path(start, end, 1.0); + EXPECT_NEAR(end_point[0], end[0], eps); + EXPECT_NEAR(end_point[1], end[1], eps); + EXPECT_NEAR(end_point[2], end[2], eps); + + // Test angle wrapping during interpolation + Vector3d wrap_start(3*pi/4, 0.0, 0.0); + Vector3d wrap_end(-3*pi/4, 1.0, 1.0); + + Vector3d wrap_mid = Utils::interpolateSE2Path(wrap_start, wrap_end, 0.5); + EXPECT_NEAR(wrap_mid[0], 0.0, eps); + EXPECT_NEAR(wrap_mid[1], 0.5, eps); + EXPECT_NEAR(wrap_mid[2], 0.5, eps); +} + +} // namespace sofa::component::cosserat::liegroups::testing + diff --git a/tasks.md b/tasks.md new file mode 100644 index 0000000..cb732b5 --- /dev/null +++ b/tasks.md @@ -0,0 +1,223 @@ +# Cosserat Plugin Comprehensive Improvement Plan + +This document outlines the planned improvements for the Cosserat plugin, focusing on code quality, performance, and documentation. + +## 1. Project Structure + +### Current Structure + +``` +plugin.Cosserat/ +├── CMakeLists.txt +├── Tests/ +│ ├── CMakeLists.txt +│ ├── engine/ +│ ├── forcefield/ +│ ├── liegroups/ +│ ├── mapping/ +│ └── benchmarks/ +├── src/ + ├── Cosserat/ + │ ├── Binding/ + │ ├── constraint/ + │ ├── engine/ + │ ├── forcefield/ + │ ├── liegroups/ + │ ├── mapping/ + │ └── python/ + └── python3/ +``` + +### Proposed Improvements + +- Consistent naming conventions across all modules +- Improved organization of test files +- Dedicated documentation directory +- Standardized header file structure +- Reorganized Python bindings for better usability + +## 2. Implementation Plan + +### Phase 1: Mapping Improvements (2 weeks) + +#### 1.1. Integrate Lie Group Functionality (1 week) +- Replace custom rotation matrix implementations with Lie group library +- Update BaseCosseratMapping to use SO3 and SE3 from liegroups +- Integrate proper tangent space operations using liegroups + +#### 1.2. Clean Up Redundant Code (3 days) +- Remove redundant rotation matrix implementations +- Consolidate common functionality +- Address TODO comments + +#### 1.3. Complete Dynamic Functionality (4 days) +- Finish implementation of dynamic features +- Add proper testing +- Document limitations and usage + +### Phase 2: ForceField Improvements (2 weeks) + +#### 2.1. Refactor Common Code (1 week) +- Create a common base class for BeamHookeLawForceField and BeamHookeLawForceFieldRigid +- Remove duplicated code +- Ensure backward compatibility + +#### 2.2. Enhance Multithreading (3 days) +- Optimize parallel execution strategies +- Add proper benchmarking +- Document threading model + +#### 2.3. Improve Documentation (4 days) +- Add detailed numerical method descriptions +- Document mathematical foundations +- Add usage examples + +### Phase 3: Constraint Improvements (1.5 weeks) + +#### 3.1. SoftRobots Integration (3 days) +- Improve CosseratActuatorConstraint's integration with SoftRobots +- Update to the latest SoftRobots API +- Document integration points + +#### 3.2. Optimize Constraint Resolution (4 days) +- Enhance resolution handling performance +- Add caching where appropriate +- Optimize memory usage + +#### 3.3. Add Mathematical Documentation (3 days) +- Document mathematical foundations +- Add derivations for constraint equations +- Explain numerical approaches + +### Phase 4: Python Bindings (2 weeks) + +#### 4.1. Modern Python Features (1 week) +- Add type annotations +- Implement context managers +- Add property-based access + +#### 4.2. Update Binding Architecture (3 days) +- Consistent binding approach across all components +- Better error handling +- Improved Python object lifetime management + +#### 4.3. Documentation and Examples (4 days) +- Comprehensive Python API documentation +- Interactive Jupyter notebook examples +- Tutorials for common use cases + +## 3. Testing Strategy + +### Unit Tests + +- **Coverage Goal**: >80% line coverage for core components +- **Test Organization**: + - One test file per component + - Grouped by module + - Clear naming convention + +### Integration Tests + +- **Scope**: + - Test interactions between components + - Verify end-to-end workflows + - Test against real-world examples + +### Performance Tests + +- **Benchmarks**: + - Lie group operations + - Force field computation + - Constraint resolution + - Mapping operations + +### Python Test Suite + +- **Coverage**: + - All Python bindings + - Example scripts + - Python-specific functionality + +## 4. Documentation Requirements + +### API Documentation + +- Complete Doxygen comments for all public methods +- Class hierarchy and relationship diagrams +- Consistent style across all components + +### Mathematical Foundations + +- Detailed explanation of Lie group theory +- Mathematical derivations for force fields +- Constraint equations and solution approaches +- Numerical methods with stability analysis + +### User Guides + +- Installation instructions +- Basic usage examples +- Advanced configuration options +- Performance optimization tips + +### Python Documentation + +- Complete docstrings for all Python functions +- Type annotations +- Usage examples +- Jupyter notebook tutorials + +## 5. Dependencies and Requirements + +### External Dependencies + +- SOFA Framework >= 23.06 +- Eigen >= 3.3 +- Python >= 3.8 (for Python bindings) +- SoftRobots plugin >= 22.12 (for actuator constraints) +- Google Benchmark (for performance testing) + +### Compiler Requirements + +- C++17 compliant compiler +- GCC >= 9.0 or Clang >= 10.0 or MSVC >= 2019 + +## 6. Timeline and Milestones + +### Milestone 1: Core Improvements (End of Week 2) +- Completed mapping improvements +- Initial forcefield refactoring + +### Milestone 2: Functionality Complete (End of Week 5) +- All modules refactored +- Constraint system updated +- Initial Python binding improvements + +### Milestone 3: Documentation and Testing (End of Week 7) +- Complete test coverage +- Comprehensive documentation +- Performance benchmarks + +### Milestone 4: Final Release (End of Week 8) +- Bug fixes +- Final documentation +- Release preparation + +## 7. Known Risks and Mitigation + +### Backward Compatibility +- **Risk**: API changes breaking existing code +- **Mitigation**: Deprecation warnings, adapter classes, and documentation + +### Performance Regression +- **Risk**: Refactoring could impact performance +- **Mitigation**: Comprehensive benchmarking before and after changes + +### Integration Issues +- **Risk**: Changes affecting other components +- **Mitigation**: Integration tests, incremental deployment + +### Resource Constraints +- **Risk**: Time and personnel limitations +- **Mitigation**: Prioritization, focused sprints, clear documentation for future work +