ign-physics-ignition-physics5_5.1.0/0000775000175000017500000000000014143524174017350 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/0000775000175000017500000000000014143524174021013 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/include/0000775000175000017500000000000014143524174022436 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/include/ignition/0000775000175000017500000000000014143524174024256 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/include/ignition/physics/0000775000175000017500000000000014143524174025740 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/include/ignition/physics/dartsim/0000775000175000017500000000000014143524174027403 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/include/ignition/physics/dartsim/World.hh0000664000175000017500000000331114143524174031011 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_WORLD_HH_ #define IGNITION_PHYSICS_DARTSIM_WORLD_HH_ #include #include namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// class RetrieveWorld : public virtual Feature { public: template class World : public virtual Feature::World { /// \brief Get the underlying dartsim world for this World object. public: dart::simulation::WorldPtr GetDartsimWorld(); }; public: template class Implementation : public virtual Feature::Implementation { public: virtual dart::simulation::WorldPtr GetDartsimWorld( const Identity &_worldID) = 0; }; }; ///////////////////////////////////////////////// template dart::simulation::WorldPtr RetrieveWorld::World ::GetDartsimWorld() { return this->template Interface() ->GetDartsimWorld(this->identity); } } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/CMakeLists.txt0000664000175000017500000001040014143524174023546 0ustar jriverojrivero# This component expresses custom features of the dartsim plugin, which can # expose native dartsim data types. ign_add_component(dartsim INTERFACE DEPENDS_ON_COMPONENTS sdf heightmap mesh GET_TARGET_NAME features) target_link_libraries(${features} INTERFACE ${DART_LIBRARIES}) target_include_directories(${features} SYSTEM INTERFACE ${DART_INCLUDE_DIRS}) if (MSVC) # needed by DART, see https://github.com/dartsim/dart/issues/753 target_compile_options(${features} INTERFACE "/permissive-") endif() install( DIRECTORY include/ DESTINATION "${IGN_INCLUDE_INSTALL_DIR_FULL}") ign_get_libsources_and_unittests(sources test_sources) # TODO(MXG): Think about an ign_add_plugin(~) macro for ign-cmake set(engine_name dartsim-plugin) ign_add_component(${engine_name} SOURCES ${sources} DEPENDS_ON_COMPONENTS dartsim GET_TARGET_NAME dartsim_plugin) target_link_libraries(${dartsim_plugin} PUBLIC ${features} ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-heightmap ${PROJECT_LIBRARY_TARGET_NAME}-mesh ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-math${IGN_MATH_VER}::eigen3 PRIVATE # We need to link this, even when the profiler isn't used to get headers. ignition-common${IGN_COMMON_VER}::profiler ) # The ignition fork of DART contains additional code that allows customizing # contact constraints. We check for the presence of "ContactSurface.hpp", which # was added to enable these customizations, to detect if the feature is # available. include(CheckIncludeFileCXX) if (MSVC) set(CMAKE_REQUIRED_FLAGS "/std:c++14") else() set(CMAKE_REQUIRED_FLAGS "-std=c++14") endif() set(CMAKE_REQUIRED_INCLUDES "${DART_INCLUDE_DIRS};${EIGEN3_INCLUDE_DIRS}") set(CMAKE_REQUIRED_QUIET false) CHECK_INCLUDE_FILE_CXX(dart/constraint/ContactSurface.hpp DART_HAS_CONTACT_SURFACE_HEADER) if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${dartsim_plugin} PRIVATE DART_HAS_CONTACT_SURFACE) else() message(STATUS "The version of DART does not support Contact constraint customizations.") endif() # Note that plugins are currently being installed in 2 places: /lib and the engine-plugins dir install(TARGETS ${dartsim_plugin} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}) # The library created by `ign_add_component` includes the ign-physics version # (i.e. libignition-physics1-name-plugin.so), but for portability, # we also install an unversioned symlink into the same versioned folder. set(versioned ${CMAKE_SHARED_LIBRARY_PREFIX}${dartsim_plugin}${CMAKE_SHARED_LIBRARY_SUFFIX}) set(unversioned ${CMAKE_SHARED_LIBRARY_PREFIX}${PROJECT_NAME_NO_VERSION_LOWER}-${engine_name}${CMAKE_SHARED_LIBRARY_SUFFIX}) if (WIN32) # disable MSVC inherit via dominance warning target_compile_options(${dartsim_plugin} PUBLIC "/wd4250") INSTALL(CODE "EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E copy ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${versioned} ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}\/${unversioned})") else() EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned}) INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${IGNITION_PHYSICS_ENGINE_INSTALL_DIR}) endif() # Testing ign_build_tests( TYPE UNIT SOURCES ${test_sources} LIB_DEPS ${features} ignition-plugin${IGN_PLUGIN_VER}::loader ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ${PROJECT_LIBRARY_TARGET_NAME}-sdf ${PROJECT_LIBRARY_TARGET_NAME}-heightmap ${PROJECT_LIBRARY_TARGET_NAME}-mesh TEST_LIST tests) foreach(test ${tests}) target_compile_definitions(${test} PRIVATE "dartsim_plugin_LIB=\"$\"" "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" "IGNITION_PHYSICS_RESOURCE_DIR=\"${IGNITION_PHYSICS_RESOURCE_DIR}\"") if (DART_HAS_CONTACT_SURFACE_HEADER) target_compile_definitions(${test} PRIVATE DART_HAS_CONTACT_SURFACE) endif() # Helps when we want to build a single test after making changes to dartsim_plugin add_dependencies(${test} ${dartsim_plugin}) endforeach() foreach(test UNIT_FindFeatures_TEST UNIT_RequestFeatures_TEST) if(TARGET ${test}) target_compile_definitions(${test} PRIVATE "dartsim_plugin_LIB=\"$\"") endif() endforeach() ign-physics-ignition-physics5_5.1.0/dartsim/worlds/0000775000175000017500000000000014143524174022325 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/worlds/world_with_nested_model.sdf0000664000175000017500000000400614143524174027727 0ustar jriverojrivero 1 2 2 0 0 0 3 1 1 0 0 1.5707 2 0 1 0 1.5707 0 0 2 nested_link1 nested_link2 1 0 0 1 2 3 1 2 3 link1 nested_model::nested_link1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/test.world0000664000175000017500000002422614143524174024363 0ustar jriverojrivero true 0 0 1 100 100 100 50 false 0 0 1 100 100 0.9 0.9 0.9 1 1 0 0 0 0 0 100 0 0 0.01 0 0 0 0.8 0.02 1 1 0 1 -0.275 0 1.1 0 0 0 0.2 0.2 2.2 1 1 0 1 0 0 0.01 0 0 0 0.8 0.02 -0.275 0 1.1 0 0 0 0.2 0.2 2.2 1.1 0 0 2.1 -1.5708 0 0 0 0 0 0.5 0 0 0 -0.05 0 0 0 1.5708 0 0.1 0.3 1 1 0 1 0 0 1.0 0 1.5708 0 0.1 0.2 1 1 0 1 0 0 0.5 0 0 0 0.1 0.9 1 1 0 1 -0.05 0 0 0 1.5708 0 0.1 0.3 0.1 0 0 1.0 0 1.5708 0 0.1 0.2 0 0 0.5 0 0 0 0.1 0.9 0.25 1.0 2.1 -2 0 0 0 0 0 0.5 0 0 0 0 0 0 0 1.5708 0 0.08 0.3 1 1 0 1 0 0 0.5 0 0 0 0.1 0.9 1 1 0 1 0 0 0 0 1.5708 0 0.08 0.3 0 0 0.5 0 0 0 0.1 0.9 base upper_link 1.0 0 0 3.0 upper_link lower_link 1.0 0 0 3.0 0.0 10.0 10.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 0.8 10 0 2 0 0 0 100 0 0 0 -1.57 0 0 0.1 0.2 1 0 0 0 0 0 1 0.1 0.1 0.1 world base -1 0 0 0 0 0 base bar 0 1 0 -0.5 0.5 100 10 10 2 0 0 0 100 0 0 0 -1.57 0 0 0.1 0.2 0 0 -1 0 0 0 1 0.1 0.1 0.1 world base 0 0 1 0 0 0 base bar 0 1 0 link0 link1 2 link0 link1 link2 link3 link4 link5 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/falling.world0000664000175000017500000000354114143524174025015 0ustar jriverojrivero 0 0 2 0 0.78539816339 0 0.0 0.0 0.0 0 0 0 0.4 0 0 0.4 0 0.4 1.0 0.0 0.0 0.0 0 0 0 1 0.0 0.0 0.0 0 0 0 1 true 0 0 -0.5 0 0 0.78539816339 1 0 0 1 0 1 1.0 0.0 0.0 0.0 0 0 0 100 100 1 100 100 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/shapes_bitmask.sdf0000664000175000017500000000733714143524174026032 0ustar jriverojrivero true 0 0 1 0 0 1 100 100 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0 0 10.0 0 0 0 1 0 0 1 0 1 1.0 1 1 1 0x01 1 1 1 1 0 0 1 1 0 0 1 1 0 0 1 0 0.75 10.0 0 0 0 1 0 0 1 0 1 1.0 1 1 1 0x02 1 1 1 0 1 0 1 0 1 0 1 0 1 0 1 0 -0.75 10.0 0 0 0 1 0 0 1 0 1 1.0 1 1 1 0x03 1 1 1 1 1 0 1 1 1 0 1 1 1 0 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/contact.sdf0000664000175000017500000000464714143524174024471 0ustar jriverojrivero true 0 0 1 100 100 false 0 0 1 100 100 0 0 0.5 0 0 0 1 1 0.1 0 1 0.5 0 0 0 1 1 1 1 0 0.5 0 0 0 1 1 2 1 1 0.5 0 0 0 1 1 3 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/model_frames.sdf0000664000175000017500000000303014143524174025454 0ustar jriverojrivero 0.0 0 2 0 0 0 0.5 0 0 0 0 3.14159265358979 0.5 0 1 0 0 0 1.0 0 0 0 0 0 0.0 0 1 0 0 0 0.0 0 2 0 0 0 1 1.0 0 0 0 0 0 0.0 0 1 0 0 0 0.0 0 0 0 0 0 1.0 0 0 0 0 0 0 0 1 0 0 0 0 0 2 0 0 0 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/empty.sdf0000664000175000017500000000012314143524174024155 0ustar jriverojrivero ign-physics-ignition-physics5_5.1.0/dartsim/worlds/world_frames.sdf0000664000175000017500000000053614143524174025513 0ustar jriverojrivero 1 0 0 0 0 0 0 1 0 0 0 0 0.0 0 2 0 0 0 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/string_pendulum.sdf0000664000175000017500000000350314143524174026243 0ustar jriverojrivero 0 0 1 0 0 0 world support 0.001 0 0 0 0.7 0 0 support bob 1 0 0 0 0 -1 0 0 0 6 0.01 0.01 0.01 0.1 0.0 0.8 0.0 1 0.0 0.8 0.0 1 0 0 0.5 0 0 0 0.01 1.0 0.8 0.8 0.0 1 0.8 0.8 0.0 1 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/slip_compliance.sdf0000664000175000017500000000367714143524174026201 0ustar jriverojrivero true 0 0 1 100 100 100 50 false 0 0 1 100 100 0.9 0.9 0.9 1 0 0 0.5 0 0 0 1 0 0 1 0 1 1.0 0.0 0.0 0.0 0 0 0 1 1 1 1.0 1.0 1 0 0 0 0 1 1 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/joint_across_nested_models.sdf0000664000175000017500000000657714143524174030444 0ustar jriverojrivero true 0 0 1 0 0 1 100 100 0 0 0.25 0 0.0 0 1 1 0.5 1 0 0 1 1 0 0 1 1 0 0 1 link1 nested_model::link2 1 0 0.0 0 0.0 0 1 1 0.5 1 1 0.5 1 0 0 1 1 0 0 1 1 0 0 1 0 5 0.25 0 0.0 0 1 1 0.5 1 0 0 1 1 0 0 1 1 0 0 1 nested_model::link2 link1 1.0 0 0.0 0 0.0 0 1 1 0.5 1 1 0.5 1 0 0 1 1 0 0 1 1 0 0 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/joint_across_models.sdf0000664000175000017500000000647614143524174027100 0ustar jriverojrivero true 0 0 1 0 0 1 100 100 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0 0 0.25 0 0.0 0 1 0 0 1 0 1 1.0 1 1 0.5 1 1 0.5 1 0 0 1 1 0 0 1 1 0 0 1 0 0 3.0 0 0.0 0 1 0 0 1 0 1 1.0 1 1 0.5 1 1 0.5 1 0 0 1 1 0 0 1 1 0 0 1 10 0 3.0 0 0.0 0 1 0 0 1 0 1 1.0 1 1 0.5 1 1 0.5 1 0 0 1 1 0 0 1 1 0 0 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/shapes.sdf0000664000175000017500000001135514143524174024313 0ustar jriverojrivero 0 0 0.5 0 0 0 0.16666 0 0 0.16666 0 0.16666 1.0 1 1 1 1 1 1 1 0 0 1 1 0 0 1 1 0 0 1 0 -1.5 0.5 0 0 0 0.1458 0 0 0.1458 0 0.125 1.0 0.5 1.0 0.5 1.0 0 1 0 1 0 1 0 1 0 1 0 1 0 1.5 0.5 0 0 0 0.1 0 0 0.1 0 0.1 1.0 0.5 0.5 0 0 1 1 0 0 1 1 0 0 1 1 0 -3.0 0.5 0 0 0 0.074154 0 0 0.074154 0 0.018769 1.0 0.2 0.6 0.2 0.6 1 1 0 1 1 1 0 1 1 1 0 1 0 3.0 0.5 0 0 0 0.068 0 0 0.058 0 0.026 1.0 0.2 0.3 0.5 0.2 0.3 0.5 1 0 1 1 1 0 1 1 1 0 1 1 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/pendulum_joint_wrench.sdf0000664000175000017500000001621314143524174027430 0ustar jriverojrivero true 0 0 10 0 0 0 1 1 1 1 0.5 0.5 0.5 1 1000 0.9 0.01 0.001 -0.5 0.1 -0.9 true 0 0 1 100 100 0 0 1 100 100 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0.8 0.8 1 true 0.0 1 11.48 0 0 0 1 1 1 0.0 0.0 1 1 1 0.8 0.2 0.2 1 0.8 0.2 0.2 1 0.8 0.2 0.2 1 1000 0 0 0.01 0 0 0 0.8 0.02 0.8 0.8 0.8 1 0.8 0.8 0.8 1 -0.1 0 1.115 0 0 0 0.2 0.2 2.2 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.8 0 0.05 0 0 0 0.05 -0.8 0 0.05 0 0 0 0.05 0 0.8 0.05 0 0 0 0.05 0 -0.8 0.05 0 0 0 0.05 base sensor_link 0 0 1 0.125 0.0 2.1 0 1.5707963267948966 0 0.4 0.01333 0.01333 0.02 0.08 0.25 0.0 0.8 0.0 1 0.0 0.8 0.0 1 sensor_link arm 0.125 0.0 1.6 0 0 0 6.0 .515 .515 .03 0 0 0.5 0 1.5707963267948966 0 0.1 0.2 0.0 0.0 0.8 1 0.0 0.0 0.8 1 0 0 0.5 0 1.5707963267948966 0 0.1 0.3 0 0 -0.5 0 0 0 0.11 0.8 0.8 0.8 1 0.5 0.5 0.0 1 0 0 -0.5 0 0 0 0.11 0.1 1.0 0.8 0.8 0.8 1 0.8 0.8 0.8 1 0.1 1.0 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/ground.sdf0000664000175000017500000000145114143524174024322 0ustar jriverojrivero true 0 0 1 100 100 100 50 0 0 1 100 100 ign-physics-ignition-physics5_5.1.0/dartsim/worlds/world_with_nested_model_joint_to_world.sdf0000664000175000017500000000352614143524174033051 0ustar jriverojrivero 1 2 3 1 2 3 world link1 1 0 0 1 2 2 0 0 0 3 1 1 0 0 1.5707 2 0 1 0 1.5707 0 0 2 world nested_link1 1 0 0 nested_link1 nested_link2 1 0 0 ign-physics-ignition-physics5_5.1.0/dartsim/src/0000775000175000017500000000000014143524174021602 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/dartsim/src/CustomFeatures.cc0000664000175000017500000000160514143524174025064 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include "CustomFeatures.hh" namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// dart::simulation::WorldPtr CustomFeatures::GetDartsimWorld( const Identity &_worldID) { return this->worlds.at(_worldID); } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/LinkFeatures.hh0000664000175000017500000000251514143524174024522 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_LINKFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_LINKFEATURES_HH_ #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { struct LinkFeatureList : FeatureList< AddLinkExternalForceTorque > { }; class LinkFeatures : public virtual Base, public virtual Implements3d { // ----- Add Link Force/Torque ----- public: void AddLinkExternalForceInWorld( const Identity &_id, const LinearVectorType &_force, const LinearVectorType &_position) override; public: void AddLinkExternalTorqueInWorld( const Identity &_id, const AngularVectorType &_torque) override; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/KinematicsFeatures.hh0000664000175000017500000000247314143524174025717 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_KINEMATICSFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_KINEMATICSFEATURES_HH_ #include #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { struct KinematicsFeatureList : FeatureList< LinkFrameSemantics, ShapeFrameSemantics, JointFrameSemantics, FreeGroupFrameSemantics > { }; class KinematicsFeatures : public virtual Base, public virtual Implements3d { public: FrameData3d FrameDataRelativeToWorld(const FrameID &_id) const; public: const dart::dynamics::Frame *SelectFrame(const FrameID &_id) const; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/LinkFeatures_TEST.cc0000664000175000017500000003073514143524174025354 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 3.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include // Features #include #include #include #include #include #include #include #include #include #include #include "test/Utils.hh" struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::AddLinkExternalForceTorque, ignition::physics::ForwardStep, ignition::physics::Gravity, ignition::physics::sdf::ConstructSdfWorld, ignition::physics::sdf::ConstructSdfModel, ignition::physics::sdf::ConstructSdfLink, ignition::physics::GetEntities, ignition::physics::GetLinkBoundingBox, ignition::physics::GetModelBoundingBox > { }; using namespace ignition; using TestEnginePtr = physics::Engine3dPtr; using TestWorldPtr = physics::World3dPtr; class LinkFeaturesFixture : public ::testing::Test { protected: void SetUp() override { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); this->engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, this->engine); } protected: TestEnginePtr engine; }; // A predicate-formatter for asserting that two vectors are approximately equal. class AssertVectorApprox { public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) { } public: ::testing::AssertionResult operator()( const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, Eigen::Vector3d _n) { if (ignition::physics::test::Equal(_m, _n, this->tol)) return ::testing::AssertionSuccess(); return ::testing::AssertionFailure() << _mExpr << " and " << _nExpr << " ([" << _m.transpose() << "] and [" << _n.transpose() << "]" << ") are not equal"; } private: double tol; }; TestWorldPtr LoadWorld( const TestEnginePtr &_engine, const std::string &_sdfFile, const Eigen::Vector3d &_gravity = Eigen::Vector3d{0, 0, -9.8}) { sdf::Root root; const sdf::Errors errors = root.Load(_sdfFile); EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); auto graphErrors = sdfWorld->ValidateGraphs(); EXPECT_EQ(0u, graphErrors.size()) << graphErrors; TestWorldPtr world = _engine->ConstructWorld(*sdfWorld); EXPECT_NE(nullptr, world); world->SetGravity(_gravity); AssertVectorApprox vectorPredicate(1e-10); EXPECT_PRED_FORMAT2(vectorPredicate, _gravity, world->GetGravity()); return world; } // Test setting force and torque. TEST_F(LinkFeaturesFixture, LinkForceTorque) { auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf", Eigen::Vector3d::Zero()); { AssertVectorApprox vectorPredicate(1e-10); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), world->GetGravity()); } // Add a sphere sdf::Model modelSDF; modelSDF.SetName("sphere"); modelSDF.SetRawPose(math::Pose3d(0, 0, 2, 0, 0, IGN_PI)); auto model = world->ConstructModel(modelSDF); const double mass = 1.0; math::MassMatrix3d massMatrix{mass, math::Vector3d{0.4, 0.4, 0.4}, math::Vector3d::Zero}; sdf::Link linkSDF; linkSDF.SetName("sphere_link"); linkSDF.SetInertial({massMatrix, math::Pose3d::Zero}); auto link = model->ConstructLink(linkSDF); ignition::physics::ForwardStep::Input input; ignition::physics::ForwardStep::State state; ignition::physics::ForwardStep::Output output; AssertVectorApprox vectorPredicate(1e-4); // Check that link is at rest { const auto frameData = link->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.linearVelocity); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.angularVelocity); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.linearAcceleration); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.angularAcceleration); } // The moment of inertia of the sphere is a multiple of the identity matrix. // This means that the moi is invariant to rotation so we can use this matrix // without converting it to the world frame. Eigen::Matrix3d moi = math::eigen3::convert(massMatrix.Moi()); // Apply forces in the world frame at zero offset // API: AddExternalForce(relForce, relPosition) // API: AddExternalTorque(relTorque) const Eigen::Vector3d cmdForce{1, -1, 0}; link->AddExternalForce( physics::RelativeForce3d(physics::FrameID::World(), cmdForce), physics::RelativePosition3d(*link, Eigen::Vector3d::Zero())); const Eigen::Vector3d cmdTorque{0, 0, 0.1 * IGN_PI}; link->AddExternalTorque( physics::RelativeTorque3d(physics::FrameID::World(), cmdTorque)); world->Step(output, state, input); { const auto frameData = link->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, cmdForce, mass * (frameData.linearAcceleration)); // The moment of inertia of the sphere is a multiple of the identity matrix. // Hence the gyroscopic coupling terms are zero EXPECT_PRED_FORMAT2(vectorPredicate, cmdTorque, moi * frameData.angularAcceleration); } world->Step(output, state, input); // Check that the forces and torques are reset { const auto frameData = link->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.linearAcceleration); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.angularAcceleration); } // Apply forces in the local frame // The sphere is rotated by pi in the +z so the local x and y axes are in // the -x and -y of the world frame respectively const Eigen::Vector3d cmdLocalForce{1, -1, 0}; link->AddExternalForce( physics::RelativeForce3d(*link, cmdLocalForce), physics::RelativePosition3d(*link, Eigen::Vector3d::Zero())); const Eigen::Vector3d cmdLocalTorque{0.1 * IGN_PI, 0, 0}; link->AddExternalTorque(physics::RelativeTorque3d(*link, cmdLocalTorque)); world->Step(output, state, input); { const Eigen::Vector3d expectedForce = Eigen::AngleAxisd(IGN_PI, Eigen::Vector3d::UnitZ()) * cmdLocalForce; const Eigen::Vector3d expectedTorque = Eigen::AngleAxisd(IGN_PI, Eigen::Vector3d::UnitZ()) * cmdLocalTorque; const auto frameData = link->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, expectedForce, mass * (frameData.linearAcceleration)); // The moment of inertia of the sphere is a multiple of the identity matrix. // Hence the gyroscopic coupling terms are zero EXPECT_PRED_FORMAT2(vectorPredicate, expectedTorque, moi * frameData.angularAcceleration); } // Test the other AddExternalForce and AddExternalTorque APIs // API: AddExternalForce(force) // API: AddExternalTorque(torque) link->AddExternalForce(cmdForce); link->AddExternalTorque(cmdTorque); world->Step(output, state, input); { const auto frameData = link->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, cmdForce, mass * (frameData.linearAcceleration)); // The moment of inertia of the sphere is a multiple of the identity matrix. // Hence the gyroscopic coupling terms are zero EXPECT_PRED_FORMAT2(vectorPredicate, cmdTorque, moi * frameData.angularAcceleration); } // Apply the force at an offset // API: AddExternalForce(relForce, relPosition) Eigen::Vector3d offset{0.1, 0.2, 0.3}; link->AddExternalForce(physics::RelativeForce3d(*link, cmdLocalForce), physics::RelativePosition3d(*link, offset)); world->Step(output, state, input); { const auto frameData = link->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, frameData.pose.linear() * cmdLocalForce, mass * (frameData.linearAcceleration)); // The moment of inertia of the sphere is a multiple of the identity matrix. // Hence the gyroscopic coupling terms are zero EXPECT_PRED_FORMAT2(vectorPredicate, frameData.pose.linear() * offset.cross(cmdLocalForce), moi * frameData.angularAcceleration); } // Apply force at an offset using the more convenient API // API: AddExternalForce(force, frame, position) link->AddExternalForce(cmdLocalForce, *link, offset); world->Step(output, state, input); { const auto frameData = link->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, frameData.pose.linear() * cmdLocalForce, mass * (frameData.linearAcceleration)); // The moment of inertia of the sphere is a multiple of the identity matrix. // Hence the gyroscopic coupling terms are zero EXPECT_PRED_FORMAT2(vectorPredicate, frameData.pose.linear() * offset.cross(cmdLocalForce), moi * frameData.angularAcceleration); } } TEST_F(LinkFeaturesFixture, AxisAlignedBoundingBox) { auto world = LoadWorld(this->engine, TEST_WORLD_DIR "test.world"); auto model = world->GetModel("double_pendulum_with_base"); auto baseLink = model->GetLink("base"); auto bbox = baseLink->GetAxisAlignedBoundingBox(); AssertVectorApprox vectorPredicate(1e-4); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(0.2, -0.8, 0), bbox.min()); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(1.8, 0.8, 2.2), bbox.max()); // test with non-world frame auto bboxModelFrame = baseLink->GetAxisAlignedBoundingBox( model->GetFrameID()); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(-0.8, -0.8, 0), bboxModelFrame.min()); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(0.8, 0.8, 2.2), bboxModelFrame.max()); // test with non-world rotated frame auto upperLink = model->GetLink("upper_link"); auto bboxUpperLinkFrame = baseLink->GetAxisAlignedBoundingBox( upperLink->GetFrameID()); EXPECT_PRED_FORMAT2(vectorPredicate, physics::Vector3d(-0.8, -0.1, -0.8), bboxUpperLinkFrame.min()); EXPECT_PRED_FORMAT2(vectorPredicate, physics::Vector3d(0.8, 2.1, 0.8), bboxUpperLinkFrame.max()); } TEST_F(LinkFeaturesFixture, ModelAxisAlignedBoundingBox) { auto world = LoadWorld(this->engine, TEST_WORLD_DIR "contact.sdf"); auto model = world->GetModel("sphere"); auto bbox = model->GetAxisAlignedBoundingBox(); AssertVectorApprox vectorPredicate(1e-4); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(-1, -1, -0.5), bbox.min()); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(2, 2, 1.5), bbox.max()); // test with non-world frame auto link = model->GetLink("link0"); auto bboxLinkFrame = model->GetAxisAlignedBoundingBox( link->GetFrameID()); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(-1, -1, -1.0), bboxLinkFrame.min()); EXPECT_PRED_FORMAT2( vectorPredicate, physics::Vector3d(2, 2, 1.0), bboxLinkFrame.max()); } ///////////////////////////////////////////////// int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/CustomMeshShape.cc0000664000175000017500000001550514143524174025167 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include "CustomMeshShape.hh" #include #include #include #include namespace ignition { namespace physics { namespace dartsim { namespace { ///////////////////////////////////////////////// unsigned int CheckNumVerticesPerFaces( const ignition::common::SubMesh &_inputSubmesh, const unsigned int _submeshIndex, const std::string &_path) { using namespace ignition::common; const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); const auto printWarning = [&](const std::string type_str) { ignwarn << "[dartsim::CustomMeshShape] The dartsim plugin does not support " << type_str << " meshes, requested by submesh [" << _submeshIndex << ":" << _inputSubmesh.Name() << "] in the input mesh [" << _path << "]. This submesh will be ignored.\n"; return 0u; }; if (SubMesh::POINTS == type) return 1u; if (SubMesh::LINES == type) return 2u; if (SubMesh::LINESTRIPS == type) return printWarning("linestrip"); if (SubMesh::TRIANGLES == type) return 3u; if (SubMesh::TRIFANS == type) return printWarning("trifan"); if (SubMesh::TRISTRIPS == type) return printWarning("tristrip"); ignwarn << "[dartsim::CustomMeshShape] One of the submeshes [" << _submeshIndex << ":" << _inputSubmesh.Name() << "] in the input " << "mesh [" << _path << "] has an unknown primitive type value [" << type << "]. This submesh will be ignored.\n"; return 0; } ///////////////////////////////////////////////// unsigned int GetPrimitiveType( const ignition::common::SubMesh &_inputSubmesh) { using namespace ignition::common; const SubMesh::PrimitiveType type = _inputSubmesh.SubMeshPrimitiveType(); if (SubMesh::POINTS == type) return aiPrimitiveType_POINT; if (SubMesh::LINES == type) return aiPrimitiveType_LINE; if (SubMesh::TRIANGLES == type) return aiPrimitiveType_TRIANGLE; return 0; } } ///////////////////////////////////////////////// CustomMeshShape::CustomMeshShape( const ignition::common::Mesh &_input, const Eigen::Vector3d &_scale) : dart::dynamics::MeshShape(_scale, nullptr) { // Create the root aiNode* node = new aiNode; // Allocate space for pointer arrays const unsigned int numSubMeshes = _input.SubMeshCount(); node->mNumMeshes = numSubMeshes; node->mMeshes = new unsigned int[numSubMeshes]; for (unsigned int i = 0; i < numSubMeshes; ++i) node->mMeshes[i] = i; aiScene *scene = new aiScene; scene->mNumMeshes = numSubMeshes; scene->mMeshes = new aiMesh*[numSubMeshes]; scene->mRootNode = node; // NOTE(MXG): We are ignoring materials and colors from the mesh, because // gazebo will only use dartsim for physics, not for rendering. scene->mMaterials = nullptr; // Fill in submesh contents for (unsigned int i = 0; i < numSubMeshes; ++i) { const ignition::common::SubMeshPtr &inputSubmesh = _input.SubMeshByIndex(i).lock(); scene->mMeshes[i] = nullptr; if (!inputSubmesh) { ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i << "] in the input mesh [" << _input.Path() << "] has expired!\n"; continue; } std::unique_ptr mesh = std::make_unique(); mesh->mMaterialIndex = static_cast(-1); const unsigned int numVertices = inputSubmesh->VertexCount(); if (inputSubmesh->NormalCount() != numVertices) { ignerr << "[dartsim::CustomMeshShape] One of the submeshes [" << i << ":" << inputSubmesh->Name() << "] in the input mesh [" << _input.Path() << "] does not have a normal count [" << inputSubmesh->NormalCount() << "] that matches its vertex " << "count [" << numVertices << "]. This submesh will be " << "ignored!\n"; continue; } mesh->mNumVertices = numVertices; mesh->mVertices = new aiVector3D[numVertices]; mesh->mNormals = new aiVector3D[numVertices]; const unsigned int numVerticesPerFace = CheckNumVerticesPerFaces(*inputSubmesh, i, _input.Path()); if (0 == numVerticesPerFace) continue; mesh->mPrimitiveTypes = GetPrimitiveType(*inputSubmesh); if (0 == mesh->mPrimitiveTypes) continue; const unsigned int numFaces = static_cast( static_cast(inputSubmesh->IndexCount()) /static_cast(numVerticesPerFace)); // Fill in the contents of each submesh mesh->mNumFaces = numFaces; mesh->mFaces = new aiFace[numFaces]; unsigned int currentPrimitiveIndex = 0; bool primitiveIndexOverflow = false; for (unsigned int j = 0; j < numFaces; ++j) { mesh->mFaces[j].mNumIndices = numVerticesPerFace; mesh->mFaces[j].mIndices = new unsigned int[numVerticesPerFace]; for (unsigned int k = 0; k < numVerticesPerFace; ++k) { int vertexIndex = inputSubmesh->Index(currentPrimitiveIndex); if (vertexIndex == -1) { ignwarn << "[dartsim::CustomMeshShape] The submesh [" << i << ":" << inputSubmesh->Name() << "] of mesh [" << _input.Path() << "] overflowed at primitive index [" << currentPrimitiveIndex << "]. Its expected number of " << "primitive indices is [" << _input.IndexCount() << "]. This submesh will be ignored.\n"; primitiveIndexOverflow = true; break; } mesh->mFaces[j].mIndices[k] = static_cast(vertexIndex); ++currentPrimitiveIndex; } if (primitiveIndexOverflow) break; } if (primitiveIndexOverflow) continue; for (unsigned int j = 0; j < numVertices; ++j) { const ignition::math::Vector3d &v = inputSubmesh->Vertex(j); for (unsigned int k = 0; k < 3; ++k) mesh->mVertices[j][k] = static_cast(v[k]); const ignition::math::Vector3d &n = inputSubmesh->Normal(j); for (unsigned int k = 0; k < 3; ++k) mesh->mNormals[j][k] = static_cast(n[k]); } scene->mMeshes[i] = mesh.release(); } this->mMesh = scene; this->mIsBoundingBoxDirty = true; this->mIsVolumeDirty = true; } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/WorldFeatures.hh0000664000175000017500000000345014143524174024713 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_WORLDFEATURES_HH_ #include #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { struct WorldFeatureList : FeatureList< CollisionDetector, Gravity, Solver > { }; class WorldFeatures : public virtual Base, public virtual Implements3d { // Documentation inherited public: void SetWorldCollisionDetector( const Identity &_id, const std::string &_collisionDetector) override; // Documentation inherited public: const std::string &GetWorldCollisionDetector(const Identity &_id) const override; // Documentation inherited public: void SetWorldGravity( const Identity &_id, const LinearVectorType &_gravity) override; // Documentation inherited public: LinearVectorType GetWorldGravity(const Identity &_id) const override; // Documentation inherited public: void SetWorldSolver(const Identity &_id, const std::string &_solver) override; // Documentation inherited public: const std::string &GetWorldSolver(const Identity &_id) const override; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/SDFFeatures.hh0000664000175000017500000001100414143524174024232 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_SDFFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_SDFFEATURES_HH_ #include #include #include #include #include #include #include #include #include #include "Base.hh" #include "EntityManagementFeatures.hh" namespace ignition { namespace physics { namespace dartsim { struct SDFFeatureList : FeatureList< sdf::ConstructSdfWorld, sdf::ConstructSdfModel, sdf::ConstructSdfNestedModel, sdf::ConstructSdfLink, sdf::ConstructSdfJoint, sdf::ConstructSdfCollision, sdf::ConstructSdfVisual > { }; class SDFFeatures : public virtual EntityManagementFeatures, public virtual Implements3d { public: Identity ConstructSdfWorld( const Identity &/*_engine*/, const ::sdf::World &_sdfWorld) override; public: Identity ConstructSdfModel( const Identity &_parentID, const ::sdf::Model &_sdfModel) override; public: Identity ConstructSdfNestedModel( const Identity &_parentID, const ::sdf::Model &_sdfModel) override; public: Identity ConstructSdfLink( const Identity &_modelID, const ::sdf::Link &_sdfLink) override; public: Identity ConstructSdfJoint( const Identity &_modelID, const ::sdf::Joint &_sdfJoint) override; public: Identity ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision) override; public: Identity ConstructSdfVisual( const Identity &_linkID, const ::sdf::Visual &_visual) override; private: dart::dynamics::BodyNode *FindOrConstructLink( const dart::dynamics::SkeletonPtr &_model, const Identity &_modelID, const ::sdf::Model &_sdfModel, const std::string &_linkName); /// \brief Construct a joint between two input links. /// \param[in] _modelInfo Contains the joint's parent model /// \param[in] _sdfJoint Contains joint parameters /// \param[in] _parent Pointer to parent link. If nullptr, the parent is /// assumed to be world /// \param[in] _child Pointer to child link. If nullptr, the child is assumed /// to be world private: Identity ConstructSdfJoint(const ModelInfo &_modelInfo, const ::sdf::Joint &_sdfJoint, dart::dynamics::BodyNode * const _parent, dart::dynamics::BodyNode * const _child); private: Eigen::Isometry3d ResolveSdfLinkReferenceFrame( const std::string &_frame, const ModelInfo &_model) const; private: Eigen::Isometry3d ResolveSdfJointReferenceFrame( const std::string &_frame, const dart::dynamics::BodyNode *_child) const; /// \brief Construct a dartsim entity from a sdf::model /// \param[in] _parentID Id of parent /// \param[in] _sdfModel sdf::Model to construct entity from /// \return The entity identity if constructed otherwise an invalid identity private: Identity ConstructSdfModelImpl(std::size_t _parentID, const ::sdf::Model &_sdfModel); /// \brief Find the dartsim BodyNode associated with the link name /// \param[in] _worldName Name of world that contains the link /// \param[in] _jointModelName The name of the model associated with the joint /// \param[in] _linkRelativeName The relative name of the link as specified in /// the sdformat description in the scope of the model identified by /// _jointModelName /// \returns The matched body node if exactly one match is found, otherwise /// a nullptr private: dart::dynamics::BodyNode *FindBodyNode( const std::string &_worldName, const std::string &_jointModelName, const std::string &_linkRelativeName); }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/SimulationFeatures.hh0000664000175000017500000000762314143524174025756 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_SIMULATIONFEATURES_HH_ #include #include #include #include #include #ifdef DART_HAS_CONTACT_SURFACE #include #endif #include #include #include #include #include #include #include "Base.hh" namespace dart { namespace collision { class Contact; } } namespace ignition { namespace physics { namespace dartsim { struct SimulationFeatureList : FeatureList< ForwardStep, #ifdef DART_HAS_CONTACT_SURFACE SetContactPropertiesCallbackFeature, #endif GetContactsFromLastStepFeature > { }; #ifdef DART_HAS_CONTACT_SURFACE class IgnContactSurfaceHandler : public dart::constraint::ContactSurfaceHandler { public: dart::constraint::ContactSurfaceParams createParams( const dart::collision::Contact& _contact, size_t _numContactsOnCollisionObject) const override; public: dart::constraint::ContactConstraintPtr createConstraint( dart::collision::Contact& _contact, size_t _numContactsOnCollisionObject, double _timeStep) const override; public: typedef SetContactPropertiesCallbackFeature Feature; public: typedef Feature::Implementation Impl; public: Impl::SurfaceParamsCallback surfaceParamsCallback; public: std::function< std::optional(const dart::collision::Contact&) > convertContact; public: mutable typename Feature::ContactSurfaceParams lastIgnParams; }; using IgnContactSurfaceHandlerPtr = std::shared_ptr; #endif class SimulationFeatures : public CanWriteExpectedData>, public virtual Base, public virtual Implements3d { public: using GetContactsFromLastStepFeature::Implementation ::ContactInternal; public: SimulationFeatures() = default; public: ~SimulationFeatures() override = default; public: void WorldForwardStep( const Identity &_worldID, ForwardStep::Output &_h, ForwardStep::State &_x, const ForwardStep::Input &_u) override; public: void Write(ChangedWorldPoses &_changedPoses) const; public: std::vector GetContactsFromLastStep( const Identity &_worldID) const override; /// \brief link poses from the most recent pose change/update. /// The key is the link's ID, and the value is the link's pose private: mutable std::unordered_map prevLinkPoses; private: std::optional convertContact( const dart::collision::Contact& _contact) const; #ifdef DART_HAS_CONTACT_SURFACE public: void AddContactPropertiesCallback( const Identity &_worldID, const std::string &_callbackID, SurfaceParamsCallback _callback) override; public: bool RemoveContactPropertiesCallback( const Identity &_worldID, const std::string &_callbackID) override; private: std::unordered_map< std::string, IgnContactSurfaceHandlerPtr> contactSurfaceHandlers; #endif }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/SimulationFeatures_TEST.cc0000664000175000017500000004372514143524174026606 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include // Features #include #include #include #include #include #include #include #include #include #include #include struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::LinkFrameSemantics, ignition::physics::ForwardStep, ignition::physics::GetContactsFromLastStepFeature, ignition::physics::GetEntities, ignition::physics::GetShapeBoundingBox, ignition::physics::CollisionFilterMaskFeature, #ifdef DART_HAS_CONTACT_SURFACE ignition::physics::SetContactPropertiesCallbackFeature, #endif ignition::physics::sdf::ConstructSdfWorld > { }; using TestWorldPtr = ignition::physics::World3dPtr; using TestShapePtr = ignition::physics::Shape3dPtr; using TestWorld = ignition::physics::World3d; using TestContactPoint = TestWorld::ContactPoint; using TestExtraContactData = TestWorld::ExtraContactData; using TestContact = TestWorld::Contact; #ifdef DART_HAS_CONTACT_SURFACE using ContactSurfaceParams = ignition::physics::SetContactPropertiesCallbackFeature:: ContactSurfaceParams; #endif std::unordered_set LoadWorlds( const std::string &_library, const std::string &_world) { ignition::plugin::Loader loader; loader.LoadLib(_library); const std::set pluginNames = ignition::physics::FindFeatures3d::From(loader); EXPECT_LT(0u, pluginNames.size()); std::unordered_set worlds; for (const std::string &name : pluginNames) { ignition::plugin::PluginPtr plugin = loader.Instantiate(name); std::cout << " -- Plugin name: " << name << std::endl; auto engine = ignition::physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); sdf::Root root; const sdf::Errors &errors = root.Load(_world); EXPECT_EQ(0u, errors.size()); const sdf::World *sdfWorld = root.WorldByIndex(0); auto world = engine->ConstructWorld(*sdfWorld); worlds.insert(world); } return worlds; } /// \brief Step forward in a world /// \param[in] _world The world to step in /// \param[in] _firstTime Whether this is the very first time this world is /// being stepped in (true) or not (false) /// \param[in] _numSteps The number of steps to take in _world /// \return true if the forward step output was checked, false otherwise bool StepWorld(const TestWorldPtr &_world, bool _firstTime, const std::size_t _numSteps = 1) { ignition::physics::ForwardStep::Input input; ignition::physics::ForwardStep::State state; ignition::physics::ForwardStep::Output output; bool checkedOutput = false; for (size_t i = 0; i < _numSteps; ++i) { _world->Step(output, state, input); // If link poses have changed, this should have been written to output. // Link poses are considered "changed" if they are new, so if this is the // very first step in a world, all of the link data is new and output // should not be empty if (_firstTime && (i == 0)) { EXPECT_FALSE( output.Get().entries.empty()); checkedOutput = true; } } return checkedOutput; } class SimulationFeatures_TEST : public ::testing::Test, public ::testing::WithParamInterface {}; // Test that the dartsim plugin loaded all the relevant information correctly. TEST_P(SimulationFeatures_TEST, Falling) { const std::string library = GetParam(); if (library.empty()) return; std::cout << "Testing library " << library << std::endl; auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/falling.world"); for (const auto &world : worlds) { auto checkedOutput = StepWorld(world, true, 1000); EXPECT_TRUE(checkedOutput); auto link = world->GetModel(0)->GetLink(0); auto pos = link->FrameDataRelativeToWorld().pose.translation(); EXPECT_NEAR(pos.z(), 1.0, 5e-2); } } TEST_P(SimulationFeatures_TEST, ShapeBoundingBox) { const std::string library = GetParam(); if (library.empty()) return; auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/falling.world"); for (const auto &world : worlds) { auto sphere = world->GetModel("sphere"); auto sphereCollision = sphere->GetLink(0)->GetShape(0); auto ground = world->GetModel("box"); auto groundCollision = ground->GetLink(0)->GetShape(0); // Test the bounding boxes in the local frames auto sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(*sphereCollision); auto groundAABB = groundCollision->GetAxisAlignedBoundingBox(*groundCollision); EXPECT_EQ(ignition::math::Vector3d(-1, -1, -1), ignition::math::eigen3::convert(sphereAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), ignition::math::eigen3::convert(sphereAABB).Max()); EXPECT_EQ(ignition::math::Vector3d(-50, -50, -0.5), ignition::math::eigen3::convert(groundAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(50, 50, 0.5), ignition::math::eigen3::convert(groundAABB).Max()); // Test the bounding boxes in the world frames sphereAABB = sphereCollision->GetAxisAlignedBoundingBox(); groundAABB = groundCollision->GetAxisAlignedBoundingBox(); // The sphere shape has a radius of 1.0, so its bounding box will have // dimensions of 1.0 x 1.0 x 1.0. When that bounding box is transformed by // a 45-degree rotation, the dimensions that are orthogonal to the axis of // rotation will dilate from 1.0 to sqrt(2). const double d = std::sqrt(2); EXPECT_EQ(ignition::math::Vector3d(-d, -1, 2.0 - d), ignition::math::eigen3::convert(sphereAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(d, 1, 2 + d), ignition::math::eigen3::convert(sphereAABB).Max()); EXPECT_EQ(ignition::math::Vector3d(-50*d, -50*d, -1), ignition::math::eigen3::convert(groundAABB).Min()); EXPECT_EQ(ignition::math::Vector3d(50*d, 50*d, 0), ignition::math::eigen3::convert(groundAABB).Max()); } } // Tests collision filtering based on bitmasks TEST_P(SimulationFeatures_TEST, CollideBitmasks) { const std::string library = GetParam(); if (library.empty()) return; auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/shapes_bitmask.sdf"); for (const auto &world : worlds) { auto baseBox = world->GetModel("box_base"); auto filteredBox = world->GetModel("box_filtered"); auto collidingBox = world->GetModel("box_colliding"); auto checkedOutput = StepWorld(world, true); EXPECT_TRUE(checkedOutput); auto contacts = world->GetContactsFromLastStep(); // Only one box (box_colliding) should collide EXPECT_EQ(4u, contacts.size()); // Now disable collisions for the colliding box as well auto collidingShape = collidingBox->GetLink(0)->GetShape(0); auto filteredShape = filteredBox->GetLink(0)->GetShape(0); collidingShape->SetCollisionFilterMask(0xF0); // Also test the getter EXPECT_EQ(0xF0, collidingShape->GetCollisionFilterMask()); // Step and make sure there is no collisions checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); contacts = world->GetContactsFromLastStep(); EXPECT_EQ(0u, contacts.size()); // Now remove both filter masks (no collision will be filtered) // Equivalent to set to 0xFF collidingShape->RemoveCollisionFilterMask(); filteredShape->RemoveCollisionFilterMask(); checkedOutput = StepWorld(world, false); EXPECT_FALSE(checkedOutput); // Expect both objects to collide contacts = world->GetContactsFromLastStep(); EXPECT_EQ(8u, contacts.size()); } } TEST_P(SimulationFeatures_TEST, RetrieveContacts) { const std::string library = GetParam(); if (library.empty()) return; auto worlds = LoadWorlds(library, TEST_WORLD_DIR "/contact.sdf"); for (const auto &world : worlds) { auto sphere = world->GetModel("sphere"); auto groundPlane = world->GetModel("ground_plane"); auto groundPlaneCollision = groundPlane->GetLink(0)->GetShape(0); // Use a set because the order of collisions is not determined. std::set possibleCollisions = { groundPlaneCollision, sphere->GetLink(0)->GetShape(0), sphere->GetLink(1)->GetShape(0), sphere->GetLink(2)->GetShape(0), sphere->GetLink(3)->GetShape(0), }; std::map expectations { {sphere->GetLink(0)->GetShape(0), {0.0, 0.0, 0.0}}, {sphere->GetLink(1)->GetShape(0), {0.0, 1.0, 0.0}}, {sphere->GetLink(2)->GetShape(0), {1.0, 0.0, 0.0}}, {sphere->GetLink(3)->GetShape(0), {1.0, 1.0, 0.0}}, }; const double gravity = 9.8; std::map forceExpectations { // Contact force expectations are: link mass * gravity. {sphere->GetLink(0)->GetShape(0), 0.1 * gravity}, {sphere->GetLink(1)->GetShape(0), 1.0 * gravity}, {sphere->GetLink(2)->GetShape(0), 2.0 * gravity}, {sphere->GetLink(3)->GetShape(0), 3.0 * gravity}, }; // This procedure checks the validity of a generated contact point. It is // used both when checking the contacts after the step is finished and for // checking them inside the contact joint properties callback. The callback // is called after the contacts are generated but before they affect the // physics. That is why contact force is zero during the callback. auto checkContact = [&](const TestContact& _contact, const bool zeroForce) { const auto &contactPoint = _contact.Get(); ASSERT_TRUE(contactPoint.collision1); ASSERT_TRUE(contactPoint.collision2); EXPECT_TRUE(possibleCollisions.find(contactPoint.collision1) != possibleCollisions.end()); EXPECT_TRUE(possibleCollisions.find(contactPoint.collision2) != possibleCollisions.end()); EXPECT_NE(contactPoint.collision1, contactPoint.collision2); Eigen::Vector3d expectedContactPos = Eigen::Vector3d::Zero(); // The test expectations are all on the collision that is not the ground // plane. auto testCollision = contactPoint.collision1; if (testCollision == groundPlaneCollision) { testCollision = contactPoint.collision2; } expectedContactPos = expectations.at(testCollision); EXPECT_TRUE(ignition::physics::test::Equal(expectedContactPos, contactPoint.point, 1e-6)); // Check if the engine populated the extra contact data struct const auto* extraContactData = _contact.Query(); ASSERT_NE(nullptr, extraContactData); // The normal of the contact force is a vector pointing up (z positive) EXPECT_NEAR(extraContactData->normal[0], 0.0, 1e-3); EXPECT_NEAR(extraContactData->normal[1], 0.0, 1e-3); EXPECT_NEAR(extraContactData->normal[2], 1.0, 1e-3); // The contact force has only a z component and its value is // the the weight of the sphere times the gravitational acceleration EXPECT_NEAR(extraContactData->force[0], 0.0, 1e-3); EXPECT_NEAR(extraContactData->force[1], 0.0, 1e-3); EXPECT_NEAR(extraContactData->force[2], zeroForce ? 0 : forceExpectations.at(testCollision), 1e-3); }; #ifdef DART_HAS_CONTACT_SURFACE size_t numContactCallbackCalls = 0u; auto contactCallback = [&]( const TestContact& _contact, size_t _numContactsOnCollision, ContactSurfaceParams& _surfaceParams) { numContactCallbackCalls++; checkContact(_contact, true); EXPECT_EQ(1u, _numContactsOnCollision); // the values in _surfaceParams are implemented as std::optional to allow // physics engines fill only those parameters that are actually // implemented ASSERT_TRUE(_surfaceParams.frictionCoeff.has_value()); ASSERT_TRUE(_surfaceParams.secondaryFrictionCoeff.has_value()); // not implemented in DART yet EXPECT_FALSE(_surfaceParams.rollingFrictionCoeff.has_value()); // not implemented in DART yet EXPECT_FALSE(_surfaceParams.secondaryRollingFrictionCoeff.has_value()); // not implemented in DART yet EXPECT_FALSE(_surfaceParams.torsionalFrictionCoeff.has_value()); ASSERT_TRUE(_surfaceParams.slipCompliance.has_value()); ASSERT_TRUE(_surfaceParams.secondarySlipCompliance.has_value()); ASSERT_TRUE(_surfaceParams.restitutionCoeff.has_value()); ASSERT_TRUE(_surfaceParams.firstFrictionalDirection.has_value()); ASSERT_TRUE(_surfaceParams.contactSurfaceMotionVelocity.has_value()); // these constraint parameters are implemented in DART but are not filled // when the callback is called; they are only read after the callback ends EXPECT_FALSE(_surfaceParams.errorReductionParameter.has_value()); EXPECT_FALSE(_surfaceParams.maxErrorReductionVelocity.has_value()); EXPECT_FALSE(_surfaceParams.maxErrorAllowance.has_value()); EXPECT_FALSE(_surfaceParams.constraintForceMixing.has_value()); EXPECT_NEAR(_surfaceParams.frictionCoeff.value(), 1.0, 1e-6); EXPECT_NEAR(_surfaceParams.secondaryFrictionCoeff.value(), 1.0, 1e-6); EXPECT_NEAR(_surfaceParams.slipCompliance.value(), 0.0, 1e-6); EXPECT_NEAR(_surfaceParams.secondarySlipCompliance.value(), 0.0, 1e-6); EXPECT_NEAR(_surfaceParams.restitutionCoeff.value(), 0.0, 1e-6); EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 1), _surfaceParams.firstFrictionalDirection.value(), 1e-6)); EXPECT_TRUE(ignition::physics::test::Equal(Eigen::Vector3d(0, 0, 0), _surfaceParams.contactSurfaceMotionVelocity.value(), 1e-6)); }; world->AddContactPropertiesCallback("test", contactCallback); #endif // The first step already has contacts, but the contact force due to the // impact does not match the steady-state force generated by the // body's weight. StepWorld(world, true); #ifdef DART_HAS_CONTACT_SURFACE // There are 4 collision bodies in the world all colliding at the same time EXPECT_EQ(4u, numContactCallbackCalls); #endif // After a second step, the contact force reaches steady-state StepWorld(world, false); #ifdef DART_HAS_CONTACT_SURFACE // There are 4 collision bodies in the world all colliding at the same time EXPECT_EQ(8u, numContactCallbackCalls); #endif auto contacts = world->GetContactsFromLastStep(); EXPECT_EQ(4u, contacts.size()); for (auto &contact : contacts) { checkContact(contact, false); } #ifdef DART_HAS_CONTACT_SURFACE // removing a non-existing callback yields no error but returns false EXPECT_FALSE(world->RemoveContactPropertiesCallback("foo")); // removing an existing callback works and the callback is no longer called EXPECT_TRUE(world->RemoveContactPropertiesCallback("test")); // Third step StepWorld(world, false); // Number of callback calls is the same as after the 2nd call EXPECT_EQ(8u, numContactCallbackCalls); // Now we check that changing _surfaceParams inside the contact properties // callback affects the result of the simulation; we set // contactSurfaceMotionVelocity to [1,0,0] which accelerates the contact // points from 0 m/s to 1 m/s in a single simulation step. auto contactCallback2 = [&]( const TestContact& /*_contact*/, size_t /*_numContactsOnCollision*/, ContactSurfaceParams& _surfaceParams) { numContactCallbackCalls++; // friction direction is [0,0,1] and contact surface motion velocity uses // the X value to denote the desired velocity along the friction direction _surfaceParams.contactSurfaceMotionVelocity->x() = 1.0; }; world->AddContactPropertiesCallback("test2", contactCallback2); numContactCallbackCalls = 0u; // Fourth step StepWorld(world, false); EXPECT_EQ(4u, numContactCallbackCalls); // Adjust the expected forces to account for the added acceleration along Z forceExpectations = { // Contact force expectations are: // link mass * (gravity + acceleration to 1 m.s^-1 in 1 ms) {sphere->GetLink(0)->GetShape(0), 0.1 * gravity + 100}, {sphere->GetLink(1)->GetShape(0), 1.0 * gravity + 999.99}, {sphere->GetLink(2)->GetShape(0), 2.0 * gravity + 1999.98}, {sphere->GetLink(3)->GetShape(0), 3.0 * gravity + 2999.97}, }; // Verify that the detected contacts correspond to the adjusted expectations contacts = world->GetContactsFromLastStep(); EXPECT_EQ(4u, contacts.size()); for (auto &contact : contacts) { checkContact(contact, false); } EXPECT_TRUE(world->RemoveContactPropertiesCallback("test2")); #endif } } INSTANTIATE_TEST_CASE_P(PhysicsPlugins, SimulationFeatures_TEST, ::testing::ValuesIn(ignition::physics::test::g_PhysicsPluginLibraries),); // NOLINT int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/plugin.cc0000664000175000017500000000340414143524174023410 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include "Base.hh" #include "CustomFeatures.hh" #include "JointFeatures.hh" #include "KinematicsFeatures.hh" #include "LinkFeatures.hh" #include "SDFFeatures.hh" #include "ShapeFeatures.hh" #include "SimulationFeatures.hh" #include "EntityManagementFeatures.hh" #include "FreeGroupFeatures.hh" #include "WorldFeatures.hh" namespace ignition { namespace physics { namespace dartsim { struct DartsimFeatures : FeatureList< CustomFeatureList, EntityManagementFeatureList, FreeGroupFeatureList, JointFeatureList, KinematicsFeatureList, LinkFeatureList, SDFFeatureList, ShapeFeatureList, SimulationFeatureList, WorldFeatureList > { }; class Plugin : public virtual Base, public virtual CustomFeatures, public virtual EntityManagementFeatures, public virtual FreeGroupFeatures, public virtual JointFeatures, public virtual KinematicsFeatures, public virtual LinkFeatures, public virtual SDFFeatures, public virtual ShapeFeatures, public virtual SimulationFeatures, public virtual WorldFeatures { }; IGN_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, DartsimFeatures) } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/FreeGroupFeatures.hh0000664000175000017500000000411014143524174025514 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_FREEGROUPFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_FREEGROUPFEATURES_HH_ #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { struct FreeGroupFeatureList : FeatureList< FindFreeGroupFeature, SetFreeGroupWorldPose, SetFreeGroupWorldVelocity // Note: FreeGroupFrameSemantics is covered in KinematicsFeatures.hh > { }; class FreeGroupFeatures : public virtual Base, public virtual Implements3d { // ----- FindFreeGroupFeature ----- Identity FindFreeGroupForModel(const Identity &_modelID) const override; Identity FindFreeGroupForLink(const Identity &_linkID) const override; Identity GetFreeGroupRootLink(const Identity &_groupID) const override; struct FreeGroupInfo { /// \brief Pointer to the root link DartBodyNode *link; /// \brief If the FreeGroup is wrapping an entire model, then this will /// contain a pointer to that model DartSkeleton *model; }; FreeGroupInfo GetCanonicalInfo(const Identity &_groupID) const; void SetFreeGroupWorldPose( const Identity &_groupID, const PoseType &_pose) override; void SetFreeGroupWorldLinearVelocity( const Identity &_groupID, const LinearVelocity &_linearVelocity) override; void SetFreeGroupWorldAngularVelocity( const Identity &_groupID, const AngularVelocity &_angularVelocity) override; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/WorldFeatures.cc0000664000175000017500000001151014143524174024675 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include #include #include #include #include "WorldFeatures.hh" namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// void WorldFeatures::SetWorldCollisionDetector( const Identity &_id, const std::string &_collisionDetector) { auto world = this->ReferenceInterface(_id); auto collisionDetector = world->getConstraintSolver()->getCollisionDetector(); if (_collisionDetector == "bullet") { collisionDetector = dart::collision::BulletCollisionDetector::create(); } else if (_collisionDetector == "fcl") { collisionDetector = dart::collision::FCLCollisionDetector::create(); } else if (_collisionDetector == "ode") { collisionDetector = dart::collision::OdeCollisionDetector::create(); } else if (_collisionDetector == "dart") { collisionDetector = dart::collision::DARTCollisionDetector::create(); } else { ignerr << "Collision detector [" << _collisionDetector << "] is not supported, defaulting to [" << collisionDetector->getType() << "]." << std::endl; } world->getConstraintSolver()->setCollisionDetector(collisionDetector); ignmsg << "Using [" << world->getConstraintSolver()->getCollisionDetector() ->getType() << "] collision detector" << std::endl; } ///////////////////////////////////////////////// const std::string &WorldFeatures::GetWorldCollisionDetector(const Identity &_id) const { auto world = this->ReferenceInterface(_id); return world->getConstraintSolver()->getCollisionDetector()->getType(); } ///////////////////////////////////////////////// void WorldFeatures::SetWorldGravity( const Identity &_id, const LinearVectorType &_gravity) { auto world = this->ReferenceInterface(_id); world->setGravity(_gravity); } ///////////////////////////////////////////////// WorldFeatures::LinearVectorType WorldFeatures::GetWorldGravity( const Identity &_id) const { auto world = this->ReferenceInterface(_id); return world->getGravity(); } ///////////////////////////////////////////////// void WorldFeatures::SetWorldSolver(const Identity &_id, const std::string &_solver) { auto world = this->ReferenceInterface(_id); auto solver = dynamic_cast( world->getConstraintSolver()); if (!solver) { ignwarn << "Failed to cast constraint solver to [BoxedLcpConstraintSolver]" << std::endl; return; } std::shared_ptr boxedSolver; if (_solver == "dantzig" || _solver == "DantzigBoxedLcpSolver") { boxedSolver = std::make_shared(); } else if (_solver == "pgs" || _solver == "PgsBoxedLcpSolver") { boxedSolver = std::make_shared(); } else { ignerr << "Solver [" << _solver << "] is not supported, defaulting to [" << solver->getBoxedLcpSolver()->getType() << "]." << std::endl; } if (boxedSolver != nullptr) solver->setBoxedLcpSolver(boxedSolver); ignmsg << "Using [" << solver->getBoxedLcpSolver()->getType() << "] solver." << std::endl; } ///////////////////////////////////////////////// const std::string &WorldFeatures::GetWorldSolver(const Identity &_id) const { auto world = this->ReferenceInterface(_id); auto solver = dynamic_cast( world->getConstraintSolver()); if (!solver) { static const std::string empty{""}; return empty; } return solver->getBoxedLcpSolver()->getType(); } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/ShapeFeatures_TEST.cc0000664000175000017500000002020114143524174025502 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 3.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include #include // Features #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "test/Utils.hh" #include "ShapeFeatures.hh" using namespace ignition; using TestFeatureList = ignition::physics::FeatureList< physics::dartsim::RetrieveWorld, physics::AttachFixedJointFeature, physics::AddLinkExternalForceTorque, physics::LinkFrameSemantics, physics::DetachJointFeature, physics::SetJointTransformFromParentFeature, physics::ForwardStep, physics::FreeJointCast, physics::GetBasicJointState, physics::GetEntities, physics::RevoluteJointCast, physics::SetJointVelocityCommandFeature, #if DART_VERSION_AT_LEAST(6, 10, 0) physics::GetShapeFrictionPyramidSlipCompliance, physics::SetShapeFrictionPyramidSlipCompliance, #endif physics::sdf::ConstructSdfModel, physics::sdf::ConstructSdfWorld, physics::sdf::ConstructSdfLink >; using TestEnginePtr = physics::Engine3dPtr; using TestWorldPtr = physics::World3dPtr; class ShapeFeaturesFixture : public ::testing::Test { protected: void SetUp() override { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); this->engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, this->engine); } protected: TestEnginePtr engine; }; // A predicate-formatter for asserting that two vectors are approximately equal. class AssertVectorApprox { public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) { } public: ::testing::AssertionResult operator()( const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, Eigen::Vector3d _n) { if (ignition::physics::test::Equal(_m, _n, this->tol)) return ::testing::AssertionSuccess(); return ::testing::AssertionFailure() << _mExpr << " and " << _nExpr << " ([" << _m.transpose() << "] and [" << _n.transpose() << "]" << ") are not equal"; } private: double tol; }; #if DART_VERSION_AT_LEAST(6, 10, 0) ///////////////////////////////////////////////// TEST_F(ShapeFeaturesFixture, PrimarySlipCompliance) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "slip_compliance.sdf"); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"box"}; const std::string linkName{"box_link"}; const std::string shapeName{"box_collision"}; auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel(modelName); auto boxLink = model->GetLink(linkName); auto boxShape = boxLink->GetShape(shapeName); AssertVectorApprox vectorPredicate(1e-4); ignition::physics::ForwardStep::Input input; ignition::physics::ForwardStep::State state; ignition::physics::ForwardStep::Output output; // Check that link is at rest { const auto frameData = boxLink->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.linearVelocity); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.angularVelocity); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.linearAcceleration); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.angularAcceleration); } const Eigen::Vector3d cmdForce{1, 0, 0}; const double primarySlip = 0.5; // expect 0.0 initial slip EXPECT_DOUBLE_EQ(0.0, boxShape->GetPrimarySlipCompliance()); boxShape->SetPrimarySlipCompliance(primarySlip); EXPECT_DOUBLE_EQ(primarySlip, boxShape->GetPrimarySlipCompliance()); const std::size_t numSteps = 10000; for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); boxLink->AddExternalForce( physics::RelativeForce3d(physics::FrameID::World(), cmdForce), physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero())); } { // velocity = slip * applied force const auto frameData = boxLink->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, primarySlip * cmdForce, (frameData.linearVelocity)); } } ///////////////////////////////////////////////// TEST_F(ShapeFeaturesFixture, SecondarySlipCompliance) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "slip_compliance.sdf"); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"box"}; const std::string linkName{"box_link"}; const std::string shapeName{"box_collision"}; auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel(modelName); auto boxLink = model->GetLink(linkName); auto boxShape = boxLink->GetShape(shapeName); AssertVectorApprox vectorPredicate(1e-4); ignition::physics::ForwardStep::Input input; ignition::physics::ForwardStep::State state; ignition::physics::ForwardStep::Output output; // Check that link is at rest { const auto frameData = boxLink->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.linearVelocity); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.angularVelocity); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.linearAcceleration); EXPECT_PRED_FORMAT2(vectorPredicate, Eigen::Vector3d::Zero(), frameData.angularAcceleration); } const Eigen::Vector3d cmdForce{0, 1, 0}; const double secondarySlip = 0.25; // expect 0.0 initial slip EXPECT_DOUBLE_EQ(0.0, boxShape->GetSecondarySlipCompliance()); boxShape->SetSecondarySlipCompliance(secondarySlip); EXPECT_DOUBLE_EQ(secondarySlip, boxShape->GetSecondarySlipCompliance()); const std::size_t numSteps = 10000; for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); boxLink->AddExternalForce( physics::RelativeForce3d(physics::FrameID::World(), cmdForce), physics::RelativePosition3d(*boxLink, Eigen::Vector3d::Zero())); } { // velocity = slip * applied force const auto frameData = boxLink->FrameDataRelativeToWorld(); EXPECT_PRED_FORMAT2(vectorPredicate, secondarySlip * cmdForce, (frameData.linearVelocity)); } } #endif ///////////////////////////////////////////////// int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/LinkFeatures.cc0000664000175000017500000000252514143524174024511 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include "LinkFeatures.hh" namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// void LinkFeatures::AddLinkExternalForceInWorld( const Identity &_id, const LinearVectorType &_force, const LinearVectorType &_position) { auto bn = this->ReferenceInterface(_id)->link; bn->addExtForce(_force, _position, false, false); } ///////////////////////////////////////////////// void LinkFeatures::AddLinkExternalTorqueInWorld( const Identity &_id, const AngularVectorType &_torque) { auto bn = this->ReferenceInterface(_id)->link; bn->addExtTorque(_torque, false); } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/CustomHeightmapShape.hh0000664000175000017500000000324214143524174026206 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_ #include #include namespace ignition { namespace physics { namespace dartsim { /// \brief This class creates a custom derivative of dartsim's HeightmapShape /// class which allows an ignition::common::Heightmap to be converted into a /// HeightmapShape that can be used by dartsim. /// Using float precision because Bullet's collision detector doesn't support /// double. common::HeightmapData also holds floats. class CustomHeightmapShape : public dart::dynamics::HeightmapShape { /// \brief Constructor /// \param[in] _input Holds heightmap data. /// \param[in] _size Heightmap size in meters. /// \param[in] _subSampling How much to subsample. public: CustomHeightmapShape( const common::HeightmapData &_input, const Eigen::Vector3d &_size, const int _subSampling); }; } } } #endif // IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMHEIGHTMAPSHAPE_HH_ ign-physics-ignition-physics5_5.1.0/dartsim/src/Base_TEST.cc0000664000175000017500000001463614143524174023634 0ustar jriverojrivero/* * Copyright (C) 2020 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include "dart/dynamics/BoxShape.hpp" #include "dart/dynamics/FreeJoint.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/simulation/World.hpp" #include "Base.hh" #include "EntityManagementFeatures.hh" #include "SDFFeatures.hh" using namespace ignition::physics; TEST(BaseClass, RemoveModel) { dartsim::Base base; base.InitiateEngine(0); dart::simulation::WorldPtr world = dart::simulation::World::create("default"); auto worldID = base.AddWorld(world, world->getName()); EXPECT_TRUE(base.worlds.HasEntity(worldID)); EXPECT_EQ(worldID, base.worlds.IdentityOf(world->getName())); std::map modelIDs; auto addDummyModel = [&](int ind) { std::string name = std::string("skel") + std::to_string(ind); auto skel = dart::dynamics::Skeleton::create(name); auto frame = dart::dynamics::SimpleFrame::createShared( dart::dynamics::Frame::World(), name + "_frame"); auto boxShape = std::make_shared( Eigen::Vector3d::Constant(1.0)); auto pair = skel->createJointAndBodyNodePair(); auto sn = pair.second->createShapeNodeWith( boxShape); auto res = base.AddModel({skel, name, frame, ""}, worldID); ASSERT_TRUE(base.models.HasEntity(std::get<0>(res))); const auto &modelInfo = base.models.at(std::get<0>(res)); EXPECT_EQ(skel, modelInfo->model); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(skel->getName(), pair.second->getName())); auto linkID = base.AddLink(pair.second, fullName, std::get<0>(res)); ASSERT_TRUE(base.links.HasEntity(linkID)); const auto &linkInfo = base.links.at(linkID); EXPECT_EQ(pair.second->getName(), linkInfo->name); EXPECT_EQ(pair.second, linkInfo->link); base.AddJoint(pair.first); base.AddShape({sn, name + "_shape"}); modelIDs[name] = std::get<0>(res); }; for (int i = 0; i < 5; ++i) { addDummyModel(i); } // Result is skel0, skel1, skel2, skel3, skel4 EXPECT_EQ(5u, base.models.size()); EXPECT_EQ(5u, base.links.size()); EXPECT_EQ(5u, base.linksByName.size()); EXPECT_EQ(5u, base.joints.size()); EXPECT_EQ(5u, base.shapes.size()); std::size_t testModelID = modelIDs["skel2"]; EXPECT_EQ(2u, base.models.idToIndexInContainer[testModelID]); // Remove skel2 base.RemoveModelImpl(worldID, testModelID); modelIDs.erase("skel2"); // Check that other resouces (links, shapes, etc) are also removed EXPECT_EQ(4u, base.models.size()); EXPECT_EQ(4u, base.links.size()); EXPECT_EQ(4u, base.linksByName.size()); EXPECT_EQ(4u, base.joints.size()); EXPECT_EQ(4u, base.shapes.size()); // Check that the index of each model matches up with its index in the world auto checkModelIndices = [&] { for (const auto &[name, modelID] : modelIDs) { auto modelIndex = base.models.idToIndexInContainer[modelID]; EXPECT_EQ(name, world->getSkeleton(modelIndex)->getName()); } }; // Check model indices after removing skel2 checkModelIndices(); // Collect the model names in a vector so we can remove them from the modelIDs // map in a for loop. std::vector modelNames; for (auto item : modelIDs) { modelNames.push_back(item.first); } // Remove all of the models while checking sizes and indices; std::size_t curSize = 4; for (auto name : modelNames) { auto modelID = modelIDs[name]; base.RemoveModelImpl(worldID, modelID); modelIDs.erase(name); --curSize; EXPECT_EQ(curSize, base.models.size()); EXPECT_EQ(curSize, base.links.size()); EXPECT_EQ(curSize, base.linksByName.size()); EXPECT_EQ(curSize, base.joints.size()); EXPECT_EQ(curSize, base.shapes.size()); checkModelIndices(); } EXPECT_EQ(0u, curSize); } TEST(BaseClass, AddNestedModel) { dartsim::Base base; base.InitiateEngine(0); dart::simulation::WorldPtr world = dart::simulation::World::create("default"); auto worldID = base.AddWorld(world, world->getName()); EXPECT_TRUE(base.worlds.HasEntity(worldID)); EXPECT_EQ(worldID, base.worlds.IdentityOf(world->getName())); auto createSkel = [](const std::string &_skelName) { auto skel = dart::dynamics::Skeleton::create(_skelName); auto frame = dart::dynamics::SimpleFrame::createShared( dart::dynamics::Frame::World(), _skelName + "_frame"); return dartsim::ModelInfo{skel, _skelName, frame, ""}; }; const auto &[parentModelID, parentModelInfo] = base.AddModel(createSkel("parent_model"), worldID); EXPECT_EQ(0u, parentModelInfo.nestedModels.size()); const auto &[nestedModel1ID, nestedModel1Info] = base.AddNestedModel( createSkel("parent_model::nested_model1"), parentModelID, worldID); ASSERT_TRUE(base.models.HasEntity(nestedModel1ID)); EXPECT_EQ(nestedModel1Info.model, base.models.at(nestedModel1ID)->model); ASSERT_EQ(1u, parentModelInfo.nestedModels.size()); EXPECT_EQ(nestedModel1ID, parentModelInfo.nestedModels[0]); const auto &[nestedModel2ID, nestedModel2Info] = base.AddNestedModel( createSkel("parent_model::nested_model2"), parentModelID, worldID); ASSERT_TRUE(base.models.HasEntity(nestedModel2ID)); EXPECT_EQ(nestedModel2Info.model, base.models.at(nestedModel2ID)->model); ASSERT_EQ(2u, parentModelInfo.nestedModels.size()); EXPECT_EQ(nestedModel2ID, parentModelInfo.nestedModels[1]); } int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/EntityManagementFeatures.hh0000664000175000017500000001356714143524174027107 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_GETENTITIESFEATURE_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_GETENTITIESFEATURE_HH_ #include #include #include #include #include #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { struct EntityManagementFeatureList : FeatureList< GetEntities, RemoveEntities, ConstructEmptyWorldFeature, ConstructEmptyModelFeature, ConstructEmptyNestedModelFeature, ConstructEmptyLinkFeature, CollisionFilterMaskFeature > { }; class EntityManagementFeatures : public virtual Base, public virtual Implements3d { // ----- Get entities ----- public: const std::string &GetEngineName(const Identity &) const override; public: std::size_t GetEngineIndex(const Identity &) const override; public: std::size_t GetWorldCount(const Identity &) const override; public: Identity GetWorld( const Identity &, std::size_t _worldIndex) const override; public: Identity GetWorld( const Identity &, const std::string &_worldName) const override; public: const std::string &GetWorldName( const Identity &_worldID) const override; public: std::size_t GetWorldIndex(const Identity &_worldID) const override; public: Identity GetEngineOfWorld(const Identity &_worldID) const override; public: std::size_t GetModelCount( const Identity &_worldID) const override; public: Identity GetModel( const Identity &_worldID, std::size_t _modelIndex) const override; public: Identity GetModel( const Identity &_worldID, const std::string &_modelName) const override; public: const std::string &GetModelName( const Identity &_modelID) const override; public: std::size_t GetModelIndex(const Identity &_modelID) const override; public: Identity GetWorldOfModel(const Identity &_modelID) const override; public: std::size_t GetNestedModelCount( const Identity &_modelID) const override; public: Identity GetNestedModel( const Identity &_modelID, std::size_t _modelIndex) const override; public: Identity GetNestedModel( const Identity &_modelID, const std::string &_modelName) const override; public: std::size_t GetLinkCount(const Identity &_modelID) const override; public: Identity GetLink( const Identity &_modelID, std::size_t _linkIndex) const override; public: Identity GetLink( const Identity &_modelID, const std::string &_linkName) const override; public: std::size_t GetJointCount(const Identity &_modelID) const override; public: Identity GetJoint( const Identity &_modelID, std::size_t _jointIndex) const override; public: Identity GetJoint( const Identity &_modelID, const std::string &_jointName) const override; public: const std::string &GetLinkName( const Identity &_linkID) const override; public: std::size_t GetLinkIndex(const Identity &_linkID) const override; public: Identity GetModelOfLink(const Identity &_linkID) const override; public: std::size_t GetShapeCount(const Identity &_linkID) const override; public: Identity GetShape( const Identity &_linkID, std::size_t _shapeIndex) const override; public: Identity GetShape( const Identity &_linkID, const std::string &_shapeName) const override; public: const std::string &GetJointName( const Identity &_jointID) const override; public: std::size_t GetJointIndex(const Identity &_jointID) const override; public: Identity GetModelOfJoint(const Identity &_jointID) const override; public: const std::string &GetShapeName( const Identity &_shapeID) const override; public: std::size_t GetShapeIndex(const Identity &_shapeID) const override; public: Identity GetLinkOfShape(const Identity &_shapeID) const override; // ----- Remove entities ----- public: bool RemoveModelByIndex( const Identity &_worldID, std::size_t _modelIndex) override; public: bool RemoveModelByName( const Identity &_worldID, const std::string &_modelName) override; public: bool RemoveModel(const Identity &_modelID) override; public: bool ModelRemoved(const Identity &_modelID) const override; public: bool RemoveNestedModelByIndex( const Identity &_modelID, std::size_t _modelIndex) override; public: bool RemoveNestedModelByName( const Identity &_modelID, const std::string &_modelName) override; // ----- Construct empty entities ----- public: Identity ConstructEmptyWorld( const Identity &_engineID, const std::string &_name) override; public: Identity ConstructEmptyModel( const Identity &_worldID, const std::string &_name) override; public: Identity ConstructEmptyNestedModel( const Identity &_modelID, const std::string &_name) override; public: Identity ConstructEmptyLink( const Identity &_modelID, const std::string &_name) override; // ----- Manage collision filter masks ----- public: void SetCollisionFilterMask( const Identity &_shapeID, const uint16_t _mask) override; public: uint16_t GetCollisionFilterMask( const Identity &_shapeID) const override; public: void RemoveCollisionFilterMask(const Identity &_shapeID) override; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/FreeGroupFeatures_TEST.cc0000664000175000017500000001461414143524174026353 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include "ignition/physics/FrameSemantics.hh" #include "ignition/physics/FreeGroup.hh" #include "ignition/physics/GetEntities.hh" #include "ignition/physics/RequestEngine.hh" #include "ignition/physics/sdf/ConstructModel.hh" #include "ignition/physics/sdf/ConstructNestedModel.hh" #include "ignition/physics/sdf/ConstructWorld.hh" #include "test/Utils.hh" struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::sdf::ConstructSdfWorld, ignition::physics::sdf::ConstructSdfModel, ignition::physics::sdf::ConstructSdfNestedModel, ignition::physics::GetWorldFromEngine, ignition::physics::GetModelFromWorld, ignition::physics::GetNestedModelFromModel, ignition::physics::GetLinkFromModel, ignition::physics::LinkFrameSemantics, ignition::physics::FindFreeGroupFeature, ignition::physics::SetFreeGroupWorldPose > { }; using World = ignition::physics::World3d; using WorldPtr = ignition::physics::World3dPtr; using ModelPtr = ignition::physics::Model3dPtr; using LinkPtr = ignition::physics::Link3dPtr; ///////////////////////////////////////////////// auto LoadEngine() { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = ignition::physics::RequestEngine3d::From(dartsim); return engine; } ///////////////////////////////////////////////// WorldPtr LoadWorld(const std::string &_world) { auto engine = LoadEngine(); EXPECT_NE(nullptr, engine); if (nullptr == engine) { return nullptr; } sdf::Root root; const sdf::Errors &errors = root.Load(_world); EXPECT_EQ(0u, errors.size()) << errors; EXPECT_EQ(1u, root.WorldCount()); const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); auto world = engine->ConstructWorld(*sdfWorld); EXPECT_NE(nullptr, world); return world; } ModelPtr GetModelFromAbsoluteName(const WorldPtr &_world, const std::string &_absoluteName) { std::vector names = ignition::common::split(_absoluteName, sdf::kSdfScopeDelimiter); if (names.empty()) { return nullptr; } auto currentModel = _world->GetModel(names.front()); for (std::size_t i = 1; i < names.size(); ++i) { if (nullptr == currentModel) return nullptr; currentModel = currentModel->GetNestedModel(names[i]); } return currentModel; } TEST(FreeGroupFeatures, NestedFreeGroup) { WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); ASSERT_NE(nullptr, world); auto checkFreeGroupForModel = [&](const std::string &_modelName) { auto model = GetModelFromAbsoluteName(world, _modelName); if (nullptr == model) return testing::AssertionFailure() << "Model could not be found"; auto freeGroup = model->FindFreeGroup(); if (nullptr == freeGroup) return testing::AssertionFailure() << "Freegroup could not be found"; if (nullptr == freeGroup->RootLink()) return testing::AssertionFailure() << "RootLink could not be found"; return testing::AssertionSuccess(); }; EXPECT_TRUE(checkFreeGroupForModel("parent_model")); // Expect false because the link in nested_model is referenced by a joint. EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model")); EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); } TEST(FreeGroupFeatures, NestedFreeGroupSetWorldPose) { WorldPtr world = LoadWorld(TEST_WORLD_DIR "world_with_nested_model.sdf"); ASSERT_NE(nullptr, world); ignition::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); { auto parentModel = world->GetModel("parent_model"); ASSERT_NE(nullptr, parentModel); auto freeGroup = parentModel->FindFreeGroup(); ASSERT_NE(nullptr, freeGroup); freeGroup->SetWorldPose( ignition::math::eigen3::convert(parentModelNewPose)); auto link1 = parentModel->GetLink("link1"); ASSERT_NE(nullptr, link1); auto frameData = link1->FrameDataRelativeToWorld(); EXPECT_EQ(parentModelNewPose, ignition::math::eigen3::convert(frameData.pose)); } { auto nestedModel = GetModelFromAbsoluteName(world, "parent_model::nested_model"); ASSERT_NE(nullptr, nestedModel); auto nestedLink1 = nestedModel->GetLink("nested_link1"); ASSERT_NE(nullptr, nestedLink1); auto frameData = nestedLink1->FrameDataRelativeToWorld(); // Poses from SDF ignition::math::Pose3d nestedModelPose(1, 2, 2, 0, 0, 0); ignition::math::Pose3d nestedLinkPose(3, 1, 1, 0, 0, IGN_PI_2); ignition::math::Pose3d nestedLinkExpectedPose = parentModelNewPose * nestedModelPose * nestedLinkPose; EXPECT_EQ(nestedLinkExpectedPose, ignition::math::eigen3::convert(frameData.pose)); } { auto parentModel = world->GetModel("parent_model2"); ASSERT_NE(nullptr, parentModel); auto freeGroup = parentModel->FindFreeGroup(); ASSERT_NE(nullptr, freeGroup); freeGroup->SetWorldPose( ignition::math::eigen3::convert(parentModelNewPose)); auto grandChild = GetModelFromAbsoluteName( world, "parent_model2::child_model::grand_child_model"); ASSERT_NE(nullptr, grandChild); auto link1 = grandChild->GetLink("link1"); ASSERT_NE(nullptr, link1); auto frameData = link1->FrameDataRelativeToWorld(); EXPECT_EQ(parentModelNewPose, ignition::math::eigen3::convert(frameData.pose)); } } ign-physics-ignition-physics5_5.1.0/dartsim/src/WorldFeatures_TEST.cc0000664000175000017500000001515714143524174025547 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 3.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include #include #include #include #include "test/Utils.hh" struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::CollisionDetector, ignition::physics::Gravity, ignition::physics::LinkFrameSemantics, ignition::physics::Solver, ignition::physics::ForwardStep, ignition::physics::sdf::ConstructSdfWorld, ignition::physics::GetEntities > { }; using namespace ignition; using TestEnginePtr = physics::Engine3dPtr; using TestWorldPtr = physics::World3dPtr; // A predicate-formatter for asserting that two vectors are approximately equal. class AssertVectorApprox { public: explicit AssertVectorApprox(double _tol = 1e-6) : tol(_tol) { } public: ::testing::AssertionResult operator()( const char *_mExpr, const char *_nExpr, Eigen::Vector3d _m, Eigen::Vector3d _n) { if (ignition::physics::test::Equal(_m, _n, this->tol)) return ::testing::AssertionSuccess(); return ::testing::AssertionFailure() << _mExpr << " and " << _nExpr << " ([" << _m.transpose() << "] and [" << _n.transpose() << "]" << ") are not equal"; } private: double tol; }; ////////////////////////////////////////////////// class WorldFeaturesFixture : public ::testing::Test { protected: void SetUp() override { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); this->engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, this->engine); } protected: TestEnginePtr engine; }; ////////////////////////////////////////////////// TestWorldPtr LoadWorld( const TestEnginePtr &_engine, const std::string &_sdfFile) { sdf::Root root; const sdf::Errors errors = root.Load(_sdfFile); EXPECT_TRUE(errors.empty()); const sdf::World *sdfWorld = root.WorldByIndex(0); return _engine->ConstructWorld(*sdfWorld); } ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, CollisionDetector) { auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf"); EXPECT_EQ("ode", world->GetCollisionDetector()); world->SetCollisionDetector("banana"); EXPECT_EQ("ode", world->GetCollisionDetector()); world->SetCollisionDetector("bullet"); EXPECT_EQ("bullet", world->GetCollisionDetector()); world->SetCollisionDetector("fcl"); EXPECT_EQ("fcl", world->GetCollisionDetector()); world->SetCollisionDetector("ode"); EXPECT_EQ("ode", world->GetCollisionDetector()); world->SetCollisionDetector("dart"); EXPECT_EQ("dart", world->GetCollisionDetector()); } ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, Gravity) { auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/falling.world"); ASSERT_NE(nullptr, world); // Check default gravity value AssertVectorApprox vectorPredicate10(1e-10); EXPECT_PRED_FORMAT2(vectorPredicate10, Eigen::Vector3d(0, 0, -9.8), world->GetGravity()); auto model = world->GetModel("sphere"); ASSERT_NE(nullptr, model); auto link = model->GetLink(0); ASSERT_NE(nullptr, link); // initial link pose const Eigen::Vector3d initialLinkPosition(0, 0, 2); { Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); EXPECT_PRED_FORMAT2(vectorPredicate10, initialLinkPosition, pos); } auto linkFrameID = link->GetFrameID(); // Get default gravity in link frame, which is pitched by pi/4 EXPECT_PRED_FORMAT2(vectorPredicate10, Eigen::Vector3d(6.92964645563, 0, -6.92964645563), world->GetGravity(linkFrameID)); // set gravity along X axis of linked frame, which is pitched by pi/4 world->SetGravity(Eigen::Vector3d(1.4142135624, 0, 0), linkFrameID); EXPECT_PRED_FORMAT2(vectorPredicate10, Eigen::Vector3d(1, 0, -1), world->GetGravity()); // test other SetGravity API // set gravity along Z axis of linked frame, which is pitched by pi/4 physics::RelativeForce3d relativeGravity( linkFrameID, Eigen::Vector3d(0, 0, 1.4142135624)); world->SetGravity(relativeGravity); EXPECT_PRED_FORMAT2(vectorPredicate10, Eigen::Vector3d(1, 0, 1), world->GetGravity()); // Confirm that changed gravity direction affects pose of link ignition::physics::ForwardStep::Input input; ignition::physics::ForwardStep::State state; ignition::physics::ForwardStep::Output output; const size_t numSteps = 1000; for (size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); } AssertVectorApprox vectorPredicate3(1e-3); { Eigen::Vector3d pos = link->FrameDataRelativeToWorld().pose.translation(); EXPECT_PRED_FORMAT2(vectorPredicate3, Eigen::Vector3d(0.5, 0, 2.5), pos); } } ////////////////////////////////////////////////// TEST_F(WorldFeaturesFixture, Solver) { auto world = LoadWorld(this->engine, TEST_WORLD_DIR "/empty.sdf"); EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); world->SetSolver("banana"); EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); world->SetSolver("dantzig"); EXPECT_EQ("DantzigBoxedLcpSolver", world->GetSolver()); world->SetSolver("pgs"); EXPECT_EQ("PgsBoxedLcpSolver", world->GetSolver()); } ///////////////////////////////////////////////// int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/CustomFeatures.hh0000664000175000017500000000221214143524174025071 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_SRC_CUSTOMFEATURES_HH #define IGNITION_PHYSICS_SRC_CUSTOMFEATURES_HH #include #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { using CustomFeatureList = FeatureList< RetrieveWorld >; class CustomFeatures : public virtual Base, public virtual Implements3d { public: dart::simulation::WorldPtr GetDartsimWorld( const Identity &_worldID) override; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/SDFFeatures.cc0000664000175000017500000012101414143524174024223 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include "SDFFeatures.hh" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "CustomMeshShape.hh" namespace ignition { namespace physics { namespace dartsim { namespace { ///////////////////////////////////////////////// /// \brief Resolve the pose of an SDF DOM object with respect to its relative_to /// frame. If that fails, return the raw pose static Eigen::Isometry3d ResolveSdfPose(const ::sdf::SemanticPose &_semPose) { math::Pose3d pose; ::sdf::Errors errors = _semPose.Resolve(pose); if (!errors.empty()) { if (!_semPose.RelativeTo().empty()) { ignerr << "There was an error in SemanticPose::Resolve\n"; for (const auto &err : errors) { ignerr << err.Message() << std::endl; } ignerr << "There is no optimal fallback since the relative_to attribute[" << _semPose.RelativeTo() << "] of the pose is not empty. " << "Falling back to using the raw Pose.\n"; } pose = _semPose.RawPose(); } return math::eigen3::convert(pose); } ///////////////////////////////////////////////// double infIfNeg(const double _value) { if (_value < 0.0) return std::numeric_limits::infinity(); return _value; } /// \brief Invert thread pitch to match the different definitions of /// thread pitch in Gazebo and DART. /// /// [Definitions of thread pitch] /// Gazebo: NEGATIVE angular motion per linear motion. /// DART : linear motion per single rotation. static double InvertThreadPitch(double _pitch) { if (math::equal(std::abs(_pitch), 0.0)) { ignerr << "Zero thread pitch is not allowed.\n"; assert(false); } return -2.0 * IGN_PI / _pitch; } ///////////////////////////////////////////////// template static void CopyStandardJointAxisProperties( const int _index, Properties &_properties, const ::sdf::JointAxis *_sdfAxis) { _properties.mDampingCoefficients[_index] = _sdfAxis->Damping(); _properties.mFrictions[_index] = _sdfAxis->Friction(); _properties.mRestPositions[_index] = _sdfAxis->SpringReference(); _properties.mSpringStiffnesses[_index] = _sdfAxis->SpringStiffness(); _properties.mPositionLowerLimits[_index] = _sdfAxis->Lower(); _properties.mPositionUpperLimits[_index] = _sdfAxis->Upper(); _properties.mIsPositionLimitEnforced = true; _properties.mForceLowerLimits[_index] = -infIfNeg(_sdfAxis->Effort()); _properties.mForceUpperLimits[_index] = infIfNeg(_sdfAxis->Effort()); _properties.mVelocityLowerLimits[_index] = -infIfNeg(_sdfAxis->MaxVelocity()); _properties.mVelocityUpperLimits[_index] = infIfNeg(_sdfAxis->MaxVelocity()); // TODO(MXG): Can dartsim support "Stiffness" and "Dissipation"? } ///////////////////////////////////////////////// static Eigen::Isometry3d GetParentModelFrame( const ModelInfo &_modelInfo) { return _modelInfo.frame->getWorldTransform(); } ///////////////////////////////////////////////// static Eigen::Vector3d ConvertJointAxis( const ::sdf::JointAxis *_sdfAxis, const ModelInfo &_modelInfo, const Eigen::Isometry3d &_T_joint) { math::Vector3d resolvedAxis; ::sdf::Errors errors = _sdfAxis->ResolveXyz(resolvedAxis); if (errors.empty()) return math::eigen3::convert(resolvedAxis); // Error while Resolving xyz. Fallback sdformat 1.6 behavior but treat // xyz_expressed_in = "__model__" as the old use_parent_model_frame const Eigen::Vector3d axis = ignition::math::eigen3::convert(_sdfAxis->Xyz()); if (_sdfAxis->XyzExpressedIn().empty()) return axis; if (_sdfAxis->XyzExpressedIn() == "__model__") { const Eigen::Quaterniond O_R_J{_T_joint.rotation()}; const Eigen::Quaterniond O_R_M{GetParentModelFrame(_modelInfo).rotation()}; const Eigen::Quaterniond J_R_M = O_R_J.inverse() * O_R_M; return J_R_M * axis; } // xyz expressed in a frame other than the joint frame or the parent model // frame is not supported ignerr << "There was an error in JointAxis::ResolveXyz\n"; for (const auto &err : errors) { ignerr << err.Message() << std::endl; } ignerr << "There is no optimal fallback since the expressed_in attribute[" << _sdfAxis->XyzExpressedIn() << "] of the axis's xyz is neither empty" << "nor '__model__'. Falling back to using the raw xyz vector " << "expressed in the joint frame.\n"; return axis; } ///////////////////////////////////////////////// template static JointType *ConstructSingleAxisJoint( const ModelInfo &_modelInfo, const ::sdf::Joint &_sdfJoint, dart::dynamics::BodyNode * const _parent, dart::dynamics::BodyNode * const _child, const Eigen::Isometry3d &_T_joint) { typename JointType::Properties properties; const ::sdf::JointAxis * const sdfAxis = _sdfJoint.Axis(0); // use the default properties if sdfAxis is not set if (sdfAxis) { properties.mAxis = ConvertJointAxis(sdfAxis, _modelInfo, _T_joint); CopyStandardJointAxisProperties(0, properties, sdfAxis); } return _child->moveTo(_parent, properties); } ///////////////////////////////////////////////// static dart::dynamics::UniversalJoint *ConstructUniversalJoint( const ModelInfo &_modelInfo, const ::sdf::Joint &_sdfJoint, dart::dynamics::BodyNode * const _parent, dart::dynamics::BodyNode * const _child, const Eigen::Isometry3d &_T_joint) { dart::dynamics::UniversalJoint::Properties properties; for (const std::size_t index : {0u, 1u}) { const ::sdf::JointAxis * const sdfAxis = _sdfJoint.Axis(index); // use the default properties if sdfAxis is not set if (sdfAxis) { properties.mAxis[index] = ConvertJointAxis(sdfAxis, _modelInfo, _T_joint); CopyStandardJointAxisProperties(index, properties, sdfAxis); } } return _child->moveTo(_parent, properties); } ///////////////////////////////////////////////// struct ShapeAndTransform { dart::dynamics::ShapePtr shape; Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); }; ///////////////////////////////////////////////// static ShapeAndTransform ConstructBox( const ::sdf::Box &_box) { return {std::make_shared( math::eigen3::convert(_box.Size()))}; } ///////////////////////////////////////////////// static ShapeAndTransform ConstructCylinder( const ::sdf::Cylinder &_cylinder) { return {std::make_shared( _cylinder.Radius(), _cylinder.Length())}; } ///////////////////////////////////////////////// static ShapeAndTransform ConstructSphere( const ::sdf::Sphere &_sphere) { return {std::make_shared(_sphere.Radius())}; } ///////////////////////////////////////////////// static ShapeAndTransform ConstructCapsule( const ::sdf::Capsule &_capsule) { return {std::make_shared( _capsule.Radius(), _capsule.Length())}; } ///////////////////////////////////////////////// static ShapeAndTransform ConstructPlane( const ::sdf::Plane &_plane) { // TODO(anyone): We use BoxShape until PlaneShape is completely supported in // DART. Please see: https://github.com/dartsim/dart/issues/114 const Eigen::Vector3d z = Eigen::Vector3d::UnitZ(); const Eigen::Vector3d axis = z.cross(math::eigen3::convert(_plane.Normal())); const double norm = axis.norm(); const double angle = std::asin(norm/(_plane.Normal().Length())); Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); // We check that the angle isn't too close to zero, because otherwise // axis/norm would be undefined. if (angle > 1e-12) tf.rotate(Eigen::AngleAxisd(angle, axis/norm)); // This number was taken from osrf/gazebo. Seems arbitrary. const double planeDim = 2100; tf.translate(Eigen::Vector3d(0.0, 0.0, -planeDim*0.5)); return {std::make_shared( Eigen::Vector3d(planeDim, planeDim, planeDim)), tf}; } ///////////////////////////////////////////////// static ShapeAndTransform ConstructHeightmap( const ::sdf::Heightmap & /*_heightmap*/) { ignerr << "Heightmap construction from an SDF has not been implemented yet " << "for dartsim.\n"; return {nullptr}; } ///////////////////////////////////////////////// static ShapeAndTransform ConstructMesh( const ::sdf::Mesh & /*_mesh*/) { // TODO(MXG): Look into what kind of mesh URI we get here. Will it just be // a local file name, or do we need to resolve the URI? ignerr << "Mesh construction from an SDF has not been implemented yet for " << "dartsim.\n"; return {nullptr}; } ///////////////////////////////////////////////// static ShapeAndTransform ConstructGeometry( const ::sdf::Geometry &_geometry) { if (_geometry.BoxShape()) return ConstructBox(*_geometry.BoxShape()); else if (_geometry.CapsuleShape()) { return ConstructCapsule(*_geometry.CapsuleShape()); } else if (_geometry.CylinderShape()) return ConstructCylinder(*_geometry.CylinderShape()); else if (_geometry.EllipsoidShape()) { // TODO(anyone): Replace this code when Ellipsoid is supported by DART common::MeshManager *meshMgr = common::MeshManager::Instance(); std::string ellipsoidMeshName = std::string("ellipsoid_mesh") + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().X()) + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().Y()) + "_" + std::to_string(_geometry.EllipsoidShape()->Radii().Z()); meshMgr->CreateEllipsoid( ellipsoidMeshName, _geometry.EllipsoidShape()->Radii(), 6, 12); const ignition::common::Mesh * _mesh = meshMgr->MeshByName(ellipsoidMeshName); auto mesh = std::make_shared(*_mesh, Vector3d(1, 1, 1)); auto mesh2 = std::dynamic_pointer_cast(mesh); return {mesh2}; } else if (_geometry.SphereShape()) return ConstructSphere(*_geometry.SphereShape()); else if (_geometry.PlaneShape()) return ConstructPlane(*_geometry.PlaneShape()); else if (_geometry.MeshShape()) return ConstructMesh(*_geometry.MeshShape()); else if (_geometry.HeightmapShape()) return ConstructHeightmap(*_geometry.HeightmapShape()); return {nullptr}; } } // namespace dart::dynamics::BodyNode *SDFFeatures::FindBodyNode( const std::string &_worldName, const std::string &_jointModelName, const std::string &_linkRelativeName) { if (_linkRelativeName == "world") return nullptr; const auto fullName = ::sdf::JoinName( _worldName, ::sdf::JoinName(_jointModelName, _linkRelativeName)); auto it = this->linksByName.find(fullName); if (it != this->linksByName.end()) { return it->second; } ignerr << "Could not find link " << _linkRelativeName << " in model " << _jointModelName << std::endl; return nullptr; } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfWorld( const Identity &_engine, const ::sdf::World &_sdfWorld) { const Identity worldID = this->ConstructEmptyWorld(_engine, _sdfWorld.Name()); const dart::simulation::WorldPtr &world = this->worlds.at(worldID); world->setGravity(ignition::math::eigen3::convert(_sdfWorld.Gravity())); // TODO(MXG): Add a Physics class to the SDFormat DOM and then parse that // information here. For now, we'll just use dartsim's default physics // parameters. for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i) { const ::sdf::Model *model = _sdfWorld.ModelByIndex(i); if (!model) continue; this->ConstructSdfNestedModel(worldID, *model); } return worldID; } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfModel( const Identity &_parentID, const ::sdf::Model &_sdfModel) { return this->ConstructSdfModelImpl(_parentID, _sdfModel); } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfNestedModel(const Identity &_parentID, const ::sdf::Model &_sdfModel) { return this->ConstructSdfModelImpl(_parentID, _sdfModel); } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfModelImpl( std::size_t _parentID, const ::sdf::Model &_sdfModel) { auto worldID = _parentID; std::string modelName = _sdfModel.Name(); const bool isNested = this->models.HasEntity(_parentID); // If this is a nested model, find the world assocated with the model if (isNested) { worldID = this->GetWorldOfModelImpl(_parentID); const auto &skel = this->models.at(_parentID)->model; modelName = ::sdf::JoinName(skel->getName(), _sdfModel.Name()); } dart::dynamics::Frame *parentFrame = this->frames.at(_parentID); dart::dynamics::SkeletonPtr model = dart::dynamics::Skeleton::create(modelName); dart::dynamics::SimpleFramePtr modelFrame = dart::dynamics::SimpleFrame::createShared( parentFrame, modelName + "::__model__", ResolveSdfPose(_sdfModel.SemanticPose())); // Set canonical link name // TODO(anyone) This may not work correctly with nested models and will need // to be updated once multiple canonical links can exist in a nested model // https://github.com/ignitionrobotics/ign-physics/issues/209 auto [modelID, modelInfo] = [&] { if (isNested) { return this->AddNestedModel( // NOLINT {model, _sdfModel.Name(), modelFrame, _sdfModel.CanonicalLinkName()}, _parentID, worldID); } else { return this->AddModel( // NOLINT {model, _sdfModel.Name(), modelFrame, _sdfModel.CanonicalLinkName()}, worldID); } }(); model->setMobile(!_sdfModel.Static()); model->setSelfCollisionCheck(_sdfModel.SelfCollide()); auto modelIdentity = this->GenerateIdentity(modelID, this->models.at(modelID)); // First, recursively construct nested models for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i) { this->ConstructSdfModelImpl(modelID, *_sdfModel.ModelByIndex(i)); } // then, construct all links for (std::size_t i=0; i < _sdfModel.LinkCount(); ++i) { this->FindOrConstructLink( model, modelIdentity, _sdfModel, _sdfModel.LinkByIndex(i)->Name()); } // Next, join all links that have joints for (std::size_t i=0; i < _sdfModel.JointCount(); ++i) { const ::sdf::Joint *sdfJoint = _sdfModel.JointByIndex(i); if (!sdfJoint) { ignerr << "The joint with index [" << i << "] in model [" << _sdfModel.Name() << "] is a nullptr. It will be skipped.\n"; continue; } // Resolve parent and child frames to links std::string parentLinkName; ::sdf::Errors errors = sdfJoint->ResolveParentLink(parentLinkName); if (!errors.empty()) { ignerr << "The link of the parent frame [" << sdfJoint->ParentLinkName() << "] of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] could not be resolved. The joint will not be constructed\n"; for (const auto &error : errors) { ignerr << error << std::endl; } continue; } std::string childLinkName; errors = sdfJoint->ResolveChildLink(childLinkName); if (!errors.empty()) { ignerr << "The link of the child frame [" << sdfJoint->ChildLinkName() << "] of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] could not be resolved. The joint will not be constructed\n"; for (const auto &error : errors) { ignerr << error << std::endl; } continue; } const ::sdf::Link *parentSdfLink = _sdfModel.LinkByName(parentLinkName); if (nullptr == parentSdfLink && parentLinkName != "world") { ignerr << "The link [" << parentLinkName << "] of the parent frame [" << sdfJoint->ParentLinkName() << "] of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] could not be resolved. The joint will not be constructed\n"; continue; } const ::sdf::Link *childSdfLink = _sdfModel.LinkByName(childLinkName); if (nullptr == childSdfLink) { ignerr << "The link [" << childLinkName << "] of the child frame [" << sdfJoint->ChildLinkName() << "] of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] could not be resolved. The joint will not be constructed\n"; continue; } auto * const parent = FindBodyNode(this->worlds[worldID]->getName(), modelName, parentLinkName); if (nullptr == parent && parentLinkName != "world") { ignerr << "The parent [" << sdfJoint->ParentLinkName() << "] of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] was not found. The joint will not be constructed\n"; continue; } auto * const child = FindBodyNode(worlds[worldID]->getName(), modelName, childLinkName); if (nullptr == child) { ignerr << "The child of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] was not found. The joint will not be constructed\n"; continue; } this->ConstructSdfJoint(modelInfo, *sdfJoint, parent, child); } return modelIdentity; } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfLink( const Identity &_modelID, const ::sdf::Link &_sdfLink) { const auto &modelInfo = *this->ReferenceInterface(_modelID); dart::dynamics::BodyNode::Properties bodyProperties; bodyProperties.mName = _sdfLink.Name(); const ignition::math::Inertiald &sdfInertia = _sdfLink.Inertial(); bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass()); // TODO(addisu) Resolve the pose of inertials when frame information is // availabile for ignition::math::Inertial const Eigen::Matrix3d R_inertial{ math::eigen3::convert(sdfInertia.Pose().Rot())}; const Eigen::Matrix3d I_link = R_inertial * math::eigen3::convert(sdfInertia.Moi()) * R_inertial.inverse(); bodyProperties.mInertia.setMoment(I_link); const Eigen::Vector3d localCom = math::eigen3::convert(sdfInertia.Pose().Pos()); bodyProperties.mInertia.setLocalCOM(localCom); dart::dynamics::FreeJoint::Properties jointProperties; jointProperties.mName = bodyProperties.mName + "_FreeJoint"; // TODO(MXG): Consider adding a UUID to this joint name in order to avoid any // potential (albeit unlikely) name collisions. // Note: When constructing a link from this function, we always instantiate // it as a standalone free body within the model. If it should have any joint // constraints, those will be added later. const auto result = modelInfo.model->createJointAndBodyNodePair< dart::dynamics::FreeJoint>(nullptr, jointProperties, bodyProperties); dart::dynamics::FreeJoint * const joint = result.first; const Eigen::Isometry3d tf = GetParentModelFrame(modelInfo) * ResolveSdfPose(_sdfLink.SemanticPose()); joint->setTransform(tf); dart::dynamics::BodyNode * const bn = result.second; auto worldID = this->GetWorldOfModelImpl(_modelID); if (worldID == INVALID_ENTITY_ID) { ignerr << "World of model [" << modelInfo.model->getName() << "] could not be found when creating link [" << _sdfLink.Name() << "]\n"; return this->GenerateInvalidId(); } auto world = this->worlds.at(worldID); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); const std::size_t linkID = this->AddLink(bn, fullName, _modelID); this->AddJoint(joint); auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID)); if (_sdfLink.Name() == modelInfo.canonicalLinkName || (modelInfo.canonicalLinkName.empty() && modelInfo.model->getNumBodyNodes() == 1)) { // We just added the first link, so this is now the canonical link. We // should therefore move the "model frame" from the world onto this new // link, while preserving its location in the world frame. const dart::dynamics::SimpleFramePtr &modelFrame = modelInfo.frame; const Eigen::Isometry3d tf_frame = modelFrame->getWorldTransform(); modelFrame->setParentFrame(bn); modelFrame->setTransform(tf_frame); } for (std::size_t i = 0; i < _sdfLink.CollisionCount(); ++i) { const auto collision = _sdfLink.CollisionByIndex(i); if (collision) this->ConstructSdfCollision(linkIdentity, *collision); } // ign-physics is currently ignoring visuals, so we won't parse them from the // SDF // for (std::size_t i = 0; i < _sdfLink.VisualCount(); ++i) // { // const auto visual = _sdfLink.VisualByIndex(i); // if (visual) // this->ConstructSdfVisual(linkID, *visual); // } return linkIdentity; } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfJoint( const Identity &_modelID, const ::sdf::Joint &_sdfJoint) { const auto &modelInfo = *this->ReferenceInterface(_modelID); if (_sdfJoint.ChildLinkName() == "world") { ignerr << "Asked to create a joint with the world as the child in model " << "[" << modelInfo.model->getName() << "]. This is currently not " << "supported\n"; return this->GenerateInvalidId(); } // If we get an error here, one of two things has occured: // 1. The Model _sdfJoint came from is invalid, i.e, had errors during // sdf::Root::Load and ConstructSdfJoint was called regardless of the // errors. Resolving for the parent link may fail for any number of reasons // and should be reported. // 2. ConstructSdfJoint is being called with an sdf::Joint that was // constructed by the user instead of one obtained from an sdf::Model // object. In this case, the joint does not have a valid frame graph and it // cannot be used to resolve for the parent link name. However, this can // still be a valid use case for ign-gazebo that sends DOM objects // piecemeal with all links and poses resolved. // Since (1) is an error that should be reported and (2) might be a valid use // case, the solution is, when an error is encountered, we first assume (2) // and set the parent and child link names to whatever returned from // sdf::Joint::ParentLinkName() and sdf::Joint::ChildLinkName respectively. // Then we check if a body node with the same relative name exists in DART. If // the link is nested inside a child model, it will be necessary to split the // name to identify the correct parent skeleton. If the corresponding body // node is found, an error will not be printed. // const std::size_t worldID = this->GetWorldOfModelImpl(_modelID); auto & world = this->worlds.at(worldID); std::string parentLinkName; const auto resolveParentErrors = _sdfJoint.ResolveParentLink(parentLinkName); if (!resolveParentErrors.empty()) { // It's possible this wasn't created from an sdf::Model object, like // SDFFeatures_TEST.WorldIsParentOrChild. Try using raw ParentLinkName() in // that case parentLinkName = _sdfJoint.ParentLinkName(); } dart::dynamics::BodyNode * const parent = FindBodyNode(world->getName(), modelInfo.model->getName(), parentLinkName); std::string childLinkName; const auto childResolveErrors = _sdfJoint.ResolveChildLink(childLinkName); if (!childResolveErrors.empty()) { childLinkName = _sdfJoint.ChildLinkName(); } dart::dynamics::BodyNode * const child = FindBodyNode(world->getName(), modelInfo.model->getName(), childLinkName); if (nullptr == parent && parentLinkName != "world") { ignerr << "The link of the parent frame [" << _sdfJoint.ParentLinkName() << "] with resolved link name [" << parentLinkName << "] of joint [" << _sdfJoint.Name() << "] could not be resolved. The joint will not be constructed\n"; return this->GenerateInvalidId(); } if (nullptr == child) { ignerr << "The link of the child frame [" << _sdfJoint.ChildLinkName() << "] with resolved link name [" << childLinkName << "] of joint [" << _sdfJoint.Name() << "] in model [" << modelInfo.model->getName() << "] could not be resolved. The joint will not be constructed\n"; return this->GenerateInvalidId(); } return ConstructSdfJoint(modelInfo, _sdfJoint, parent, child); } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision) { if (!_collision.Geom()) { ignerr << "The geometry element of collision [" << _collision.Name() << "] " << "was a nullptr\n"; return this->GenerateInvalidId(); } const ShapeAndTransform st = ConstructGeometry(*_collision.Geom()); const dart::dynamics::ShapePtr shape = st.shape; const Eigen::Isometry3d tf_shape = st.tf; if (!shape) { // The geometry element was empty, or the shape type is not supported ignerr << "The geometry element of collision [" << _collision.Name() << "] " << "couldn't be created\n"; return this->GenerateInvalidId(); } dart::dynamics::BodyNode *const bn = this->ReferenceInterface(_linkID)->link.get(); // NOTE(MXG): Gazebo requires unique collision shape names per Link, but // dartsim requires unique ShapeNode names per Skeleton, so we decorate the // Collision name for uniqueness sake. const std::string internalName = bn->getName() + ":" + _collision.Name(); dart::dynamics::ShapeNode * const node = bn->createShapeNodeWith< dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>( shape, internalName); // Calling GetElement creates a new element with default values if the element // doesn't exist, so the following is okay. // TODO(addisu) We are using the coefficient specified in the tag. // Either add parameters specific to DART or generic to all physics engines. uint16_t collideBitmask = 0xFF; if (_collision.Element()) { const auto &odeFriction = _collision.Element() ->GetElement("surface") ->GetElement("friction") ->GetElement("ode"); #if DART_VERSION_AT_LEAST(6, 10, 0) auto aspect = node->getDynamicsAspect(); aspect->setFrictionCoeff(odeFriction->Get("mu")); if (odeFriction->HasElement("mu2")) { aspect->setSecondaryFrictionCoeff(odeFriction->Get("mu2")); } if (odeFriction->HasElement("slip1")) { aspect->setPrimarySlipCompliance(odeFriction->Get("slip1")); } if (odeFriction->HasElement("slip2")) { aspect->setSecondarySlipCompliance(odeFriction->Get("slip2")); } if (odeFriction->HasElement("fdir1")) { auto frictionDirectionElem = odeFriction->GetElement("fdir1"); math::Vector3d fdir1 = frictionDirectionElem->Get(); aspect->setFirstFrictionDirection(math::eigen3::convert(fdir1)); const std::string kExpressedIn = "ignition:expressed_in"; if (frictionDirectionElem->HasAttribute(kExpressedIn)) { auto skeleton = bn->getSkeleton(); auto directionFrameBodyNode = skeleton->getBodyNode( frictionDirectionElem->Get(kExpressedIn)); if (nullptr != directionFrameBodyNode) { aspect->setFirstFrictionDirectionFrame(directionFrameBodyNode); } else { ignwarn << "Failed to get body node for [" << _collision.Name() << "], not setting friction direction frame." << std::endl; } } } const auto &surfaceBounce = _collision.Element() ->GetElement("surface") ->GetElement("bounce"); if (surfaceBounce->HasElement("restitution_coefficient")) { aspect->setRestitutionCoeff( surfaceBounce->Get("restitution_coefficient")); } #else // We are setting the friction coefficient of a collision element // to be the coefficient for the whole link. If there are multiple collision // elements, the value of the last one will be the coefficient for the link. // TODO(addisu) Assign the coefficient to the shape node when support is // added in DART. bn->setFrictionCoeff(odeFriction->Get("mu")); const auto &surfaceBounce = _collision.Element() ->GetElement("surface") ->GetElement("bounce"); if (surfaceBounce->HasElement("restitution_coefficient")) { bn->setRestitutionCoeff( surfaceBounce->Get("restitution_coefficient")); } #endif // TODO(anyone) add category_bitmask as well const auto bitmaskElement = _collision.Element() ->GetElement("surface") ->GetElement("contact"); if (bitmaskElement->HasElement("collide_bitmask")) collideBitmask = bitmaskElement->Get("collide_bitmask"); } node->setRelativeTransform(ResolveSdfPose(_collision.SemanticPose()) * tf_shape); const std::size_t shapeID = this->AddShape({node, _collision.Name(), tf_shape}); auto identity = this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); this->SetCollisionFilterMask(identity, collideBitmask); return identity; } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfVisual( const Identity &_linkID, const ::sdf::Visual &_visual) { if (!_visual.Geom()) { ignerr << "The geometry element of visual [" << _visual.Name() << "] was a " << "nullptr\n"; return this->GenerateInvalidId(); } const ShapeAndTransform st = ConstructGeometry(*_visual.Geom()); const dart::dynamics::ShapePtr shape = st.shape; const Eigen::Isometry3d tf_shape = st.tf; if (!shape) { // The geometry element was empty, or the shape type is not supported ignerr << "The geometry element of visual [" << _visual.Name() << "] " << "couldn't be created\n"; return this->GenerateInvalidId(); } dart::dynamics::BodyNode *const bn = this->ReferenceInterface(_linkID)->link.get(); // NOTE(MXG): Gazebo requires unique collision shape names per Link, but // dartsim requires unique ShapeNode names per Skeleton, so we decorate the // Collision name for uniqueness sake. const std::string internalName = bn->getName() + ":visual:" + _visual.Name(); dart::dynamics::ShapeNode * const node = bn->createShapeNodeWith( shape, internalName); node->setRelativeTransform(ResolveSdfPose(_visual.SemanticPose()) * tf_shape); // TODO(MXG): Are there any other visual parameters that we can do anything // with? Do these visual parameters even matter, since dartsim is only // intended for the physics? if (_visual.Material()) { const ignition::math::Color &color = _visual.Material()->Ambient(); node->getVisualAspect()->setColor( Eigen::Vector4d(color.R(), color.G(), color.B(), color.A())); } const std::size_t shapeID = this->AddShape({node, _visual.Name(), tf_shape}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// dart::dynamics::BodyNode *SDFFeatures::FindOrConstructLink( const dart::dynamics::SkeletonPtr &_model, const Identity &_modelID, const ::sdf::Model &_sdfModel, const std::string &_linkName) { dart::dynamics::BodyNode * link = _model->getBodyNode(_linkName); if (link) return link; const ::sdf::Link * const sdfLink = _sdfModel.LinkByName(_linkName); if (!sdfLink) { if (_linkName != "world") { ignerr << "Model [" << _sdfModel.Name() << "] does not contain a Link " << "with the name [" << _linkName << "].\n"; } return nullptr; } return this->links.at(this->ConstructSdfLink(_modelID, *sdfLink))->link.get(); } ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfJoint( const ModelInfo &_modelInfo, const ::sdf::Joint &_sdfJoint, dart::dynamics::BodyNode * const _parent, dart::dynamics::BodyNode * const _child) { // if a specified link is named "world" but cannot be found, we'll assume the // joint is connected to the world bool worldParent = (!_parent && _sdfJoint.ParentLinkName() == "world"); bool worldChild = (!_child && _sdfJoint.ChildLinkName() == "world"); if (worldChild) { ignerr << "Asked to create a joint with the world as the child in model " << "[" << _modelInfo.model->getName() << "]. This is currently not " << "supported\n"; return this->GenerateInvalidId(); } // If either parent or child is null, it's only an error if the link is not // "world" if ((!_parent && !worldParent) || !_child) { { std::stringstream msg; msg << "Asked to create a joint from link [" << _sdfJoint.ParentLinkName() << "] to link [" << _sdfJoint.ChildLinkName() << "] in the model " << "[" << _modelInfo.model->getName() << "], but "; if (!_parent && !worldParent) { msg << "the parent link "; if (!_child) msg << " and "; } if (!_child) msg << "the child link "; msg << "could not be found in that model!\n"; ignerr << msg.str(); return this->GenerateInvalidId(); } } { auto childsParentJoint = _child->getParentJoint(); if (childsParentJoint->getType() != "FreeJoint") { ignerr << "Asked to create a joint between links " << "[" << _parent->getName() << "] as parent and [" << _child->getName() << "] as child, but the child link already " << "has a parent joint of type [" << childsParentJoint->getType() << "].\n"; return this->GenerateInvalidId(); } else if (_parent && _parent->descendsFrom(_child)) { // TODO(MXG): Add support for non-tree graph structures ignerr << "Asked to create a closed kinematic chain between links " << "[" << _parent->getName() << "] and [" << _child->getName() << "], but that is not supported by the dartsim wrapper yet.\n"; return this->GenerateInvalidId(); } } // Save the current transforms of the links so we remember it later const Eigen::Isometry3d T_parent = _parent ? _parent->getWorldTransform() : Eigen::Isometry3d::Identity(); const Eigen::Isometry3d T_child = _child->getWorldTransform(); const Eigen::Isometry3d T_joint = _child->getWorldTransform() * ResolveSdfPose(_sdfJoint.SemanticPose()); const ::sdf::JointType type = _sdfJoint.Type(); dart::dynamics::Joint *joint = nullptr; if (::sdf::JointType::BALL == type) { // SDF does not support any of the properties for ball joint, besides the // name and relative transforms to its parent and child, which will be taken // care of below. All other properties like joint limits, stiffness, etc, // will be the default values of +/- infinity or 0.0. joint = _child->moveTo(_parent); } // TODO(MXG): Consider adding dartsim support for a CONTINUOUS joint type. // Alternatively, support the CONTINUOUS joint type by wrapping the // RevoluteJoint joint type. // TODO(MXG): Consider adding dartsim support for a GEARBOX joint type. It's // unclear to me whether it would be possible to get the same effect by // wrapping a RevoluteJoint type. else if (::sdf::JointType::PRISMATIC == type) { joint = ConstructSingleAxisJoint( _modelInfo, _sdfJoint, _parent, _child, T_joint); } else if (::sdf::JointType::REVOLUTE == type) { joint = ConstructSingleAxisJoint( _modelInfo, _sdfJoint, _parent, _child, T_joint); } // TODO(MXG): Consider adding dartsim support for a REVOLUTE2 joint type. // Alternatively, support the REVOLUTE2 joint type by wrapping two // RevoluteJoint objects into one. else if (::sdf::JointType::SCREW == type) { auto *screw = ConstructSingleAxisJoint( _modelInfo, _sdfJoint, _parent, _child, T_joint); screw->setPitch(InvertThreadPitch(_sdfJoint.ThreadPitch())); joint = screw; } else if (::sdf::JointType::UNIVERSAL == type) { joint = ConstructUniversalJoint( _modelInfo, _sdfJoint, _parent, _child, T_joint); } else { // The joint type is either fixed or unsupported. If it's unsupported, print // out an error message and fall back to a fixed joint if (::sdf::JointType::FIXED != type) { ignerr << "Asked to construct a joint of sdf::JointType [" << static_cast(type) << "], but that is not supported yet. " << "Creating a FIXED joint instead\n"; } // A fixed joint does not have any properties besides the name and relative // transforms to its parent and child, which will be taken care of below. joint = _child->moveTo(_parent); } joint->setName(_sdfJoint.Name()); const Eigen::Isometry3d child_T_postjoint = T_child.inverse() * T_joint; const Eigen::Isometry3d parent_T_prejoint_init = T_parent.inverse() * T_joint; joint->setTransformFromParentBodyNode(parent_T_prejoint_init); joint->setTransformFromChildBodyNode(child_T_postjoint); const std::size_t jointID = this->AddJoint(joint); // Increment BodyNode version since the child could be moved to a new skeleton // when a joint is created. // TODO(azeey) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after Joint::moveTo. _child->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } ///////////////////////////////////////////////// Eigen::Isometry3d SDFFeatures::ResolveSdfLinkReferenceFrame( const std::string &_frame, const ModelInfo &_modelInfo) const { if (_frame.empty()) return GetParentModelFrame(_modelInfo); ignerr << "Requested a reference frame of [" << _frame << "] but currently " << "only the model frame is supported as a reference frame for link " << "poses.\n"; // TODO(MXG): Implement this when frame specifications are nailed down return Eigen::Isometry3d::Identity(); } ///////////////////////////////////////////////// Eigen::Isometry3d SDFFeatures::ResolveSdfJointReferenceFrame( const std::string &_frame, const dart::dynamics::BodyNode *_child) const { if (_frame.empty()) { // This means the joint pose is expressed relative to the child link pose return _child->getWorldTransform(); } ignerr << "Requested a reference frame of [" << _frame << "] but currently " << "only the child link frame is supported as a reference frame for " << "joint poses.\n"; // TODO(MXG): Implement this when frame specifications are nailed down return Eigen::Isometry3d::Identity(); } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/ShapeFeatures.cc0000664000175000017500000004566614143524174024671 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include "ShapeFeatures.hh" #include #include #include #include #include #include #include #include #include #include #include #include #include "CustomHeightmapShape.hh" #include "CustomMeshShape.hh" namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// Pose3d ShapeFeatures::GetShapeRelativeTransform( const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); return shapeInfo->node->getRelativeTransform() * shapeInfo->tf_offset.inverse(); } ///////////////////////////////////////////////// void ShapeFeatures::SetShapeRelativeTransform( const Identity &_shapeID, const Pose3d &_pose) { const auto *shapeInfo = this->ReferenceInterface(_shapeID); shapeInfo->node->setRelativeTransform(_pose * shapeInfo->tf_offset); } ///////////////////////////////////////////////// Identity ShapeFeatures::CastToBoxShape(const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// LinearVector3d ShapeFeatures::GetBoxShapeSize( const Identity &_boxID) const { const auto *boxInfo = this->ReferenceInterface(_boxID); dart::dynamics::BoxShape *box = static_cast( boxInfo->node->getShape().get()); return box->getSize(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachBoxShape( const Identity &_linkID, const std::string &_name, const LinearVector3d &_size, const Pose3d &_pose) { auto box = std::make_shared(_size); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( box, bn->getName() + ":" + _name); sn->setRelativeTransform(_pose); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// Identity ShapeFeatures::CastToCapsuleShape(const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// double ShapeFeatures::GetCapsuleShapeRadius( const Identity &_capsuleID) const { const auto *shapeInfo = this->ReferenceInterface(_capsuleID); dart::dynamics::CapsuleShape *capsule = static_cast( shapeInfo->node->getShape().get()); return capsule->getRadius(); } ///////////////////////////////////////////////// double ShapeFeatures::GetCapsuleShapeLength( const Identity &_capsuleID) const { const auto *shapeInfo = this->ReferenceInterface(_capsuleID); dart::dynamics::CapsuleShape *capsule = static_cast( shapeInfo->node->getShape().get()); return capsule->getHeight(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachCapsuleShape( const Identity &_linkID, const std::string &_name, const double _radius, const double _length, const Pose3d &_pose) { auto capsule = std::make_shared( _radius, _length); auto bn = this->ReferenceInterface(_linkID)->link; dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( capsule, bn->getName() + ":" + _name); sn->setRelativeTransform(_pose); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// Identity ShapeFeatures::CastToCylinderShape(const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// double ShapeFeatures::GetCylinderShapeRadius( const Identity &_cylinderID) const { const auto *shapeInfo = this->ReferenceInterface(_cylinderID); dart::dynamics::CylinderShape *cylinder = static_cast( shapeInfo->node->getShape().get()); return cylinder->getRadius(); } ///////////////////////////////////////////////// double ShapeFeatures::GetCylinderShapeHeight( const Identity &_cylinderID) const { const auto *shapeInfo = this->ReferenceInterface(_cylinderID); dart::dynamics::CylinderShape *cylinder = static_cast( shapeInfo->node->getShape().get()); return cylinder->getHeight(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachCylinderShape( const Identity &_linkID, const std::string &_name, const double _radius, const double _height, const Pose3d &_pose) { auto cylinder = std::make_shared( _radius, _height); auto bn = this->ReferenceInterface(_linkID)->link; dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( cylinder, bn->getName() + ":" + _name); sn->setRelativeTransform(_pose); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// Identity ShapeFeatures::CastToEllipsoidShape(const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// Vector3d ShapeFeatures::GetEllipsoidShapeRadii( const Identity &_ellipsoidID) const { const auto *shapeInfo = this->ReferenceInterface(_ellipsoidID); dart::dynamics::EllipsoidShape *ellipsoid = static_cast( shapeInfo->node->getShape().get()); return ellipsoid->getRadii(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, const Vector3d _radii, const Pose3d &_pose) { common::MeshManager *meshMgr = common::MeshManager::Instance(); std::string ellipsoidMeshName = _name + "_ellipsoid_mesh" + "_" + std::to_string(_radii[0]) + "_" + std::to_string(_radii[1]) + "_" + std::to_string(_radii[2]); meshMgr->CreateEllipsoid(ellipsoidMeshName, ignition::math::Vector3d(_radii[0], _radii[1], _radii[2]), 16, 16); const ignition::common::Mesh * _mesh = meshMgr->MeshByName(ellipsoidMeshName); auto mesh = std::make_shared(*_mesh, Vector3d(1, 1, 1)); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( mesh, bn->getName() + ":" + _name); sn->setRelativeTransform(_pose); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// Identity ShapeFeatures::CastToSphereShape( const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// double ShapeFeatures::GetSphereShapeRadius(const Identity &_sphereID) const { const auto *shapeInfo = this->ReferenceInterface(_sphereID); dart::dynamics::SphereShape *sphere = static_cast( shapeInfo->node->getShape().get()); return sphere->getRadius(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachSphereShape( const Identity &_linkID, const std::string &_name, const double _radius, const Pose3d &_pose) { auto sphere = std::make_shared(_radius); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( sphere, bn->getName() + ":" + _name); sn->setRelativeTransform(_pose); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ////////////////////////////////////////////////// Identity ShapeFeatures::CastToHeightmapShape( const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast *>(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// LinearVector3d ShapeFeatures::GetHeightmapShapeSize( const Identity &_heightmapID) const { const auto *shapeInfo = this->ReferenceInterface(_heightmapID); const auto *heightmap = static_cast *>( shapeInfo->node->getShape().get()); return heightmap->getBoundingBox().getMax() - heightmap->getBoundingBox().getMin(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachHeightmapShape( const Identity &_linkID, const std::string &_name, const common::HeightmapData &_heightmapData, const Pose3d &_pose, const LinearVector3d &_size, int _subSampling) { auto heightmap = std::make_shared(_heightmapData, _size, _subSampling); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( heightmap, bn->getName() + ":" + _name); sn->setRelativeTransform(_pose); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// Identity ShapeFeatures::CastToMeshShape( const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// LinearVector3d ShapeFeatures::GetMeshShapeSize( const Identity &_meshID) const { const auto *shapeInfo = this->ReferenceInterface(_meshID); const dart::dynamics::MeshShape *mesh = static_cast( shapeInfo->node->getShape().get()); return mesh->getBoundingBox().getMax() - mesh->getBoundingBox().getMin(); } ///////////////////////////////////////////////// LinearVector3d ShapeFeatures::GetMeshShapeScale( const Identity &_meshID) const { const auto *shapeInfo = this->ReferenceInterface(_meshID); const dart::dynamics::MeshShape *mesh = static_cast( shapeInfo->node->getShape().get()); return mesh->getScale(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachMeshShape( const Identity &_linkID, const std::string &_name, const ignition::common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) { auto mesh = std::make_shared(_mesh, _scale); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( mesh, bn->getName() + ":" + _name); sn->setRelativeTransform(_pose); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// Identity ShapeFeatures::CastToPlaneShape(const Identity &_shapeID) const { const auto *shapeInfo = this->ReferenceInterface(_shapeID); const dart::dynamics::ShapePtr &shape = shapeInfo->node->getShape(); if (dynamic_cast(shape.get())) return this->GenerateIdentity(_shapeID, this->Reference(_shapeID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// LinearVector3d ShapeFeatures::GetPlaneShapeNormal( const Identity &_planeID) const { const auto *shapeInfo = this->ReferenceInterface(_planeID); const dart::dynamics::PlaneShape *plane = static_cast( shapeInfo->node->getShape().get()); return plane->getNormal(); } ///////////////////////////////////////////////// LinearVector3d ShapeFeatures::GetPlaneShapePoint( const Identity &_planeID) const { const auto *shapeInfo = this->ReferenceInterface(_planeID); const dart::dynamics::PlaneShape *plane = static_cast( shapeInfo->node->getShape().get()); return plane->getOffset() * plane->getNormal(); } ///////////////////////////////////////////////// Identity ShapeFeatures::AttachPlaneShape( const Identity &_linkID, const std::string &_name, const AngularVector3d &_normal, const LinearVector3d &_point) { auto plane = std::make_shared(_normal, _point); DartBodyNode *bn = this->ReferenceInterface(_linkID)->link.get(); dart::dynamics::ShapeNode *sn = bn->createShapeNodeWith( plane, bn->getName() + ":" + _name); const std::size_t shapeID = this->AddShape({sn, _name}); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } ///////////////////////////////////////////////// AlignedBox3d ShapeFeatures::GetShapeAxisAlignedBoundingBox( const Identity &_shapeID) const { const auto &node = this->ReferenceInterface(_shapeID)->node; const dart::math::BoundingBox &box = node->getShape()->getBoundingBox(); return AlignedBox3d(box.getMin(), box.getMax()); } #if DART_VERSION_AT_LEAST(6, 10, 0) ///////////////////////////////////////////////// double ShapeFeatures::GetShapeFrictionPyramidPrimarySlipCompliance( const Identity &_shapeID) const { auto &node = this->ReferenceInterface(_shapeID)->node; auto aspect = node->getDynamicsAspect(); if (nullptr == aspect) { ignerr << "Attempt to get FrictionPyramidPrimarySlipCompliance for a " << "ShapeNode that doesn't have a DynamicAspect. " << "Returning default value of 0.0." << std::endl; return 0.0; } return aspect->getPrimarySlipCompliance(); } ///////////////////////////////////////////////// double ShapeFeatures::GetShapeFrictionPyramidSecondarySlipCompliance( const Identity &_shapeID) const { auto &node = this->ReferenceInterface(_shapeID)->node; auto aspect = node->getDynamicsAspect(); if (nullptr == aspect) { ignerr << "Attempt to get FrictionPyramidSecondarySlipCompliance for a " << "ShapeNode that doesn't have a DynamicAspect. " << "Returning default value of 0.0." << std::endl; return 0.0; } return aspect->getSecondarySlipCompliance(); } ///////////////////////////////////////////////// bool ShapeFeatures::SetShapeFrictionPyramidPrimarySlipCompliance( const Identity &_shapeID, double _value) { auto &node = this->ReferenceInterface(_shapeID)->node; auto aspect = node->getDynamicsAspect(); if (nullptr == aspect) { ignerr << "Attempt to set FrictionPyramidPrimarySlipCompliance for a " << "ShapeNode that doesn't have a DynamicAspect. " << "The parameter has not been set." << std::endl; return false; } aspect->setPrimarySlipCompliance(_value); return true; } ///////////////////////////////////////////////// bool ShapeFeatures::SetShapeFrictionPyramidSecondarySlipCompliance( const Identity &_shapeID, double _value) { auto &node = this->ReferenceInterface(_shapeID)->node; auto aspect = node->getDynamicsAspect(); if (nullptr == aspect) { ignerr << "Attempt to set FrictionPyramidSecondarySlipCompliance for a " << "ShapeNode that doesn't have a DynamicAspect. " << "The parameter has not been set." << std::endl; return false; } aspect->setSecondarySlipCompliance(_value); return true; } #endif } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/EntityManagement_TEST.cc0000664000175000017500000003160314143524174026224 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include #include "EntityManagementFeatures.hh" #include "JointFeatures.hh" #include "KinematicsFeatures.hh" #include "ShapeFeatures.hh" struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::dartsim::EntityManagementFeatureList, ignition::physics::dartsim::JointFeatureList, ignition::physics::dartsim::KinematicsFeatureList, ignition::physics::dartsim::ShapeFeatureList > { }; TEST(EntityManagement_TEST, ConstructEmptyWorld) { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); ASSERT_NE(nullptr, world); EXPECT_EQ("empty world", world->GetName()); EXPECT_EQ(engine, world->GetEngine()); auto model = world->ConstructEmptyModel("empty model"); ASSERT_NE(nullptr, model); EXPECT_EQ("empty model", model->GetName()); EXPECT_EQ(world, model->GetWorld()); EXPECT_NE(model, world->ConstructEmptyModel("dummy")); auto nestedModel = model->ConstructEmptyNestedModel("empty nested model"); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("empty nested model", nestedModel->GetName()); EXPECT_EQ(1u, model->GetNestedModelCount()); EXPECT_EQ(world, nestedModel->GetWorld()); EXPECT_EQ(0u, model->GetIndex()); EXPECT_EQ(nestedModel, model->GetNestedModel(0)); EXPECT_EQ(nestedModel, model->GetNestedModel("empty nested model")); EXPECT_NE(nestedModel, nestedModel->ConstructEmptyNestedModel("dummy")); // This should remain 1 since we're adding a nested model in `nestedModel` not // in `model`. EXPECT_EQ(1u, model->GetNestedModelCount()); EXPECT_EQ(1u, nestedModel->GetNestedModelCount()); auto link = model->ConstructEmptyLink("empty link"); ASSERT_NE(nullptr, link); EXPECT_EQ("empty link", link->GetName()); EXPECT_EQ(model, link->GetModel()); EXPECT_NE(link, model->ConstructEmptyLink("dummy")); EXPECT_EQ(0u, link->GetIndex()); EXPECT_EQ(model, link->GetModel()); auto joint = link->AttachRevoluteJoint(nullptr); EXPECT_NEAR((Eigen::Vector3d::UnitX() - joint->GetAxis()).norm(), 0.0, 1e-6); EXPECT_DOUBLE_EQ(0.0, joint->GetPosition(0)); joint->SetAxis(Eigen::Vector3d::UnitZ()); EXPECT_NEAR((Eigen::Vector3d::UnitZ() - joint->GetAxis()).norm(), 0.0, 1e-6); auto child = model->ConstructEmptyLink("child link"); EXPECT_EQ(2u, child->GetIndex()); EXPECT_EQ(model, child->GetModel()); const std::string boxName = "box"; const Eigen::Vector3d boxSize(0.1, 0.2, 0.3); auto box = link->AttachBoxShape(boxName, boxSize); EXPECT_EQ(boxName, box->GetName()); EXPECT_NEAR((boxSize - box->GetSize()).norm(), 0.0, 1e-6); EXPECT_EQ(1u, link->GetShapeCount()); auto boxCopy = link->GetShape(0u); EXPECT_EQ(box, boxCopy); auto prismatic = child->AttachPrismaticJoint( link, "prismatic", Eigen::Vector3d::UnitZ()); const double zPos = 2.5; const double zVel = 9.1; const double zAcc = 10.2; prismatic->SetPosition(0, zPos); prismatic->SetVelocity(0, zVel); prismatic->SetAcceleration(0, zAcc); const ignition::physics::FrameData3d childData = child->FrameDataRelativeToWorld(); const Eigen::Vector3d childPosition = childData.pose.translation(); EXPECT_DOUBLE_EQ(0.0, childPosition.x()); EXPECT_DOUBLE_EQ(0.0, childPosition.y()); EXPECT_DOUBLE_EQ(zPos, childPosition.z()); const Eigen::Vector3d childVelocity = childData.linearVelocity; EXPECT_DOUBLE_EQ(0.0, childVelocity.x()); EXPECT_DOUBLE_EQ(0.0, childVelocity.y()); EXPECT_DOUBLE_EQ(zVel, childVelocity.z()); const Eigen::Vector3d childAcceleration = childData.linearAcceleration; EXPECT_DOUBLE_EQ(0.0, childAcceleration.x()); EXPECT_DOUBLE_EQ(0.0, childAcceleration.y()); EXPECT_DOUBLE_EQ(zAcc, childAcceleration.z()); const double yPos = 11.5; Eigen::Isometry3d childSpherePose = Eigen::Isometry3d::Identity(); childSpherePose.translate(Eigen::Vector3d(0.0, yPos, 0.0)); auto sphere = child->AttachSphereShape("child sphere", 1.0, childSpherePose); const ignition::physics::FrameData3d sphereData = sphere->FrameDataRelativeToWorld(); const Eigen::Vector3d spherePosition = sphereData.pose.translation(); EXPECT_DOUBLE_EQ(0.0, spherePosition.x()); EXPECT_DOUBLE_EQ(yPos, spherePosition.y()); EXPECT_DOUBLE_EQ(zPos, spherePosition.z()); const Eigen::Vector3d sphereVelocity = sphereData.linearVelocity; EXPECT_DOUBLE_EQ(0.0, sphereVelocity.x()); EXPECT_DOUBLE_EQ(0.0, sphereVelocity.y()); EXPECT_DOUBLE_EQ(zVel, sphereVelocity.z()); const Eigen::Vector3d sphereAcceleration = sphereData.linearAcceleration; EXPECT_DOUBLE_EQ(0.0, sphereAcceleration.x()); EXPECT_DOUBLE_EQ(0.0, sphereAcceleration.y()); EXPECT_DOUBLE_EQ(zAcc, sphereAcceleration.z()); const ignition::physics::FrameData3d relativeSphereData = sphere->FrameDataRelativeTo(*child); const Eigen::Vector3d relativeSpherePosition = relativeSphereData.pose.translation(); EXPECT_DOUBLE_EQ(0.0, relativeSpherePosition.x()); EXPECT_DOUBLE_EQ(yPos, relativeSpherePosition.y()); EXPECT_DOUBLE_EQ(0.0, relativeSpherePosition.z()); auto meshLink = model->ConstructEmptyLink("mesh_link"); meshLink->AttachFixedJoint(child, "fixed"); const std::string meshFilename = ignition::common::joinPaths( IGNITION_PHYSICS_RESOURCE_DIR, "chassis.dae"); auto &meshManager = *ignition::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); auto meshShape = meshLink->AttachMeshShape("chassis", *mesh); const auto originalMeshSize = mesh->Max() - mesh->Min(); const auto meshShapeSize = meshShape->GetSize(); // Note: dartsim uses assimp for storing mesh data, and assimp by default uses // single floating point precision (instead of double precision), so we can't // expect these values to be exact. for (std::size_t i = 0; i < 3; ++i) EXPECT_NEAR(originalMeshSize[i], meshShapeSize[i], 1e-6); EXPECT_NEAR(meshShapeSize[0], 0.5106, 1e-4); EXPECT_NEAR(meshShapeSize[1], 0.3831, 1e-4); EXPECT_NEAR(meshShapeSize[2], 0.1956, 1e-4); const ignition::math::Pose3d pose(0, 0, 0.2, 0, 0, 0); const ignition::math::Vector3d scale(0.5, 1.0, 0.25); auto meshShapeScaled = meshLink->AttachMeshShape("small_chassis", *mesh, ignition::math::eigen3::convert(pose), ignition::math::eigen3::convert(scale)); const auto meshShapeScaledSize = meshShapeScaled->GetSize(); // Note: dartsim uses assimp for storing mesh data, and assimp by default uses // single floating point precision (instead of double precision), so we can't // expect these values to be exact. for (std::size_t i = 0; i < 3; ++i) EXPECT_NEAR(originalMeshSize[i] * scale[i], meshShapeScaledSize[i], 1e-6); EXPECT_NEAR(meshShapeScaledSize[0], 0.2553, 1e-4); EXPECT_NEAR(meshShapeScaledSize[1], 0.3831, 1e-4); EXPECT_NEAR(meshShapeScaledSize[2], 0.0489, 1e-4); auto heightmapLink = model->ConstructEmptyLink("heightmap_link"); heightmapLink->AttachFixedJoint(child, "heightmap_joint"); auto heightmapFilename = ignition::common::joinPaths( IGNITION_PHYSICS_RESOURCE_DIR, "heightmap_bowl.png"); ignition::common::ImageHeightmap data; EXPECT_EQ(0, data.Load(heightmapFilename)); const ignition::math::Vector3d size({129, 129, 10}); auto heightmapShape = heightmapLink->AttachHeightmapShape("heightmap", data, ignition::math::eigen3::convert(pose), ignition::math::eigen3::convert(size)); EXPECT_NEAR(size.X(), heightmapShape->GetSize()[0], 1e-6); EXPECT_NEAR(size.Y(), heightmapShape->GetSize()[1], 1e-6); EXPECT_NEAR(size.Z(), heightmapShape->GetSize()[2], 1e-6); auto heightmapShapeGeneric = heightmapLink->GetShape("heightmap"); ASSERT_NE(nullptr, heightmapShapeGeneric); EXPECT_EQ(nullptr, heightmapShapeGeneric->CastToBoxShape()); auto heightmapShapeRecast = heightmapShapeGeneric->CastToHeightmapShape(); ASSERT_NE(nullptr, heightmapShapeRecast); EXPECT_NEAR(size.X(), heightmapShapeRecast->GetSize()[0], 1e-6); EXPECT_NEAR(size.Y(), heightmapShapeRecast->GetSize()[1], 1e-6); EXPECT_NEAR(size.Z(), heightmapShapeRecast->GetSize()[2], 1e-6); } TEST(EntityManagement_TEST, RemoveEntities) { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); ASSERT_NE(nullptr, world); auto model = world->ConstructEmptyModel("empty model"); ASSERT_NE(nullptr, model); auto modelAlias = world->GetModel(0); model->Remove(); EXPECT_TRUE(model->Removed()); EXPECT_TRUE(modelAlias->Removed()); EXPECT_EQ(nullptr, world->GetModel(0)); EXPECT_EQ(nullptr, world->GetModel("empty model")); EXPECT_EQ(0ul, world->GetModelCount()); // Calling GetName shouldn't throw EXPECT_EQ("empty model", model->GetName()); auto model2 = world->ConstructEmptyModel("model2"); ASSERT_NE(nullptr, model2); EXPECT_EQ(0ul, model2->GetIndex()); world->RemoveModel(0); EXPECT_EQ(0ul, world->GetModelCount()); auto parentModel = world->ConstructEmptyModel("parent model"); ASSERT_NE(nullptr, parentModel); EXPECT_EQ(0u, parentModel->GetNestedModelCount()); auto nestedModel1 = parentModel->ConstructEmptyNestedModel("empty nested model1"); ASSERT_NE(nullptr, nestedModel1); EXPECT_EQ(1u, parentModel->GetNestedModelCount()); EXPECT_TRUE(parentModel->RemoveNestedModel(0)); EXPECT_EQ(0u, parentModel->GetNestedModelCount()); EXPECT_TRUE(nestedModel1->Removed()); auto nestedModel2 = parentModel->ConstructEmptyNestedModel("empty nested model2"); ASSERT_NE(nullptr, nestedModel2); EXPECT_EQ(nestedModel2, parentModel->GetNestedModel(0)); EXPECT_TRUE(parentModel->RemoveNestedModel("empty nested model2")); EXPECT_EQ(0u, parentModel->GetNestedModelCount()); EXPECT_TRUE(nestedModel2->Removed()); auto nestedModel3 = parentModel->ConstructEmptyNestedModel("empty nested model3"); ASSERT_NE(nullptr, nestedModel3); EXPECT_EQ(nestedModel3, parentModel->GetNestedModel(0)); EXPECT_TRUE(nestedModel3->Remove()); EXPECT_EQ(0u, parentModel->GetNestedModelCount()); EXPECT_TRUE(nestedModel3->Removed()); auto nestedModel4 = parentModel->ConstructEmptyNestedModel("empty nested model4"); ASSERT_NE(nullptr, nestedModel4); EXPECT_EQ(nestedModel4, parentModel->GetNestedModel(0)); // Remove the parent model and check that the nested model is removed as well EXPECT_TRUE(parentModel->Remove()); EXPECT_TRUE(nestedModel4->Removed()); } TEST(EntityManagement_TEST, ModelByIndexWithNestedModels) { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, engine); auto world = engine->ConstructEmptyWorld("empty world"); ASSERT_NE(nullptr, world); auto model1 = world->ConstructEmptyModel("model1"); ASSERT_NE(nullptr, model1); EXPECT_EQ(0ul, model1->GetIndex()); auto parentModel = world->ConstructEmptyModel("parent model"); ASSERT_NE(nullptr, parentModel); EXPECT_EQ(1ul, parentModel->GetIndex()); auto nestedModel1 = parentModel->ConstructEmptyNestedModel("empty nested model1"); ASSERT_NE(nullptr, nestedModel1); EXPECT_EQ(0ul, nestedModel1->GetIndex()); auto model2 = world->ConstructEmptyModel("model2"); ASSERT_NE(nullptr, model2); EXPECT_EQ(2ul, model2->GetIndex()); EXPECT_TRUE(model2->Remove()); auto model2Again = world->ConstructEmptyModel("model2_again"); ASSERT_NE(nullptr, model2Again); EXPECT_EQ(2ul, model2Again->GetIndex()); } int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/SDFFeatures_TEST.cc0000664000175000017500000007337014143524174025075 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::GetEntities, ignition::physics::GetBasicJointState, ignition::physics::SetBasicJointState, ignition::physics::LinkFrameSemantics, ignition::physics::dartsim::RetrieveWorld, ignition::physics::sdf::ConstructSdfCollision, ignition::physics::sdf::ConstructSdfJoint, ignition::physics::sdf::ConstructSdfLink, ignition::physics::sdf::ConstructSdfModel, ignition::physics::sdf::ConstructSdfNestedModel, ignition::physics::sdf::ConstructSdfWorld > { }; using World = ignition::physics::World3d; using WorldPtr = ignition::physics::World3dPtr; using ModelPtr = ignition::physics::Model3dPtr; using LinkPtr = ignition::physics::Link3dPtr; ///////////////////////////////////////////////// auto LoadEngine() { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); auto engine = ignition::physics::RequestEngine3d::From(dartsim); return engine; } enum class LoaderType { Whole, Piecemeal }; ///////////////////////////////////////////////// WorldPtr LoadWorldWhole(const std::string &_world) { auto engine = LoadEngine(); EXPECT_NE(nullptr, engine); sdf::Root root; const sdf::Errors &errors = root.Load(_world); EXPECT_EQ(0u, errors.size()); for (const auto error : errors) { std::cout << error << std::endl; } EXPECT_EQ(1u, root.WorldCount()); const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); auto world = engine->ConstructWorld(*sdfWorld); EXPECT_NE(nullptr, world); return world; } ///////////////////////////////////////////////// static ignition::math::Pose3d ResolveSdfPose( const ::sdf::SemanticPose &_semPose) { ignition::math::Pose3d pose; ::sdf::Errors errors = _semPose.Resolve(pose); EXPECT_TRUE(errors.empty()) << errors; return pose; } static sdf::JointAxis ResolveJointAxis(const sdf::JointAxis &_unresolvedAxis) { ignition::math::Vector3d axisXyz; const sdf::Errors resolveAxisErrors = _unresolvedAxis.ResolveXyz(axisXyz); EXPECT_TRUE(resolveAxisErrors.empty()) << resolveAxisErrors; sdf::JointAxis resolvedAxis = _unresolvedAxis; const sdf::Errors setXyzErrors = resolvedAxis.SetXyz(axisXyz); EXPECT_TRUE(setXyzErrors.empty()) << setXyzErrors; resolvedAxis.SetXyzExpressedIn(""); return resolvedAxis; } ///////////////////////////////////////////////// /// Downstream applications, like ign-gazebo, use this way of world construction WorldPtr LoadWorldPiecemeal(const std::string &_world) { auto engine = LoadEngine(); EXPECT_NE(nullptr, engine); if (nullptr == engine) return nullptr; sdf::Root root; const sdf::Errors &errors = root.Load(_world); EXPECT_EQ(0u, errors.size()) << errors; EXPECT_EQ(1u, root.WorldCount()); const sdf::World *sdfWorld = root.WorldByIndex(0); EXPECT_NE(nullptr, sdfWorld); if (nullptr == sdfWorld) return nullptr; sdf::World newWorld; newWorld.SetName(sdfWorld->Name()); newWorld.SetGravity(sdfWorld->Gravity()); auto world = engine->ConstructWorld(newWorld); if (nullptr == world) return nullptr; std::unordered_map modelMap; std::unordered_map linkMap; auto createModel = [&](const sdf::Model *_model, const sdf::Model *_parentModel = nullptr) { ASSERT_NE(nullptr, _model); sdf::Model newSdfModel; newSdfModel.SetName(_model->Name()); newSdfModel.SetRawPose(ResolveSdfPose(_model->SemanticPose())); newSdfModel.SetStatic(_model->Static()); newSdfModel.SetSelfCollide(_model->SelfCollide()); ModelPtr newModel; if (nullptr != _parentModel) { auto it = modelMap.find(_parentModel); ASSERT_TRUE(it != modelMap.end()); newModel = it->second->ConstructNestedModel(newSdfModel); } else { newModel = world->ConstructModel(newSdfModel); } EXPECT_NE(nullptr, newModel); if (nullptr != newModel) { modelMap[_model] = newModel; } }; for (uint64_t i = 0; i < sdfWorld->ModelCount(); ++i) { const auto *model = sdfWorld->ModelByIndex(i); createModel(model); for (uint64_t nestedInd = 0; nestedInd < model->ModelCount(); ++nestedInd) { createModel(model->ModelByIndex(nestedInd), model); } } for (auto [sdfModel, physModel] : modelMap) { for (uint64_t li = 0; li < sdfModel->LinkCount(); ++li) { const auto link = sdfModel->LinkByIndex(li); EXPECT_NE(nullptr, link); if (nullptr == link) return nullptr; sdf::Link newSdfLink; newSdfLink.SetName(link->Name()); newSdfLink.SetRawPose(ResolveSdfPose(link->SemanticPose())); newSdfLink.SetInertial(link->Inertial()); auto newLink = physModel->ConstructLink(newSdfLink); EXPECT_NE(nullptr, newLink); if (nullptr == newLink) return nullptr; linkMap[link] = newLink; } } for (auto [sdfLink, physLink] : linkMap) { for (uint64_t ci = 0; ci < sdfLink->CollisionCount(); ++ci) { physLink->ConstructCollision(*sdfLink->CollisionByIndex(ci)); } } for (auto [sdfModel, physModel] : modelMap) { for (uint64_t ji = 0; ji < sdfModel->JointCount(); ++ji) { const auto sdfJoint = sdfModel->JointByIndex(ji); EXPECT_NE(nullptr, sdfJoint); if (nullptr == sdfJoint) return nullptr; std::string resolvedParentLinkName; const auto resolveParentErrors = sdfJoint->ResolveParentLink(resolvedParentLinkName); EXPECT_TRUE(resolveParentErrors.empty()) << resolveParentErrors; std::string resolvedChildLinkName; const auto resolveChildErrors = sdfJoint->ResolveChildLink(resolvedChildLinkName); EXPECT_TRUE (resolveChildErrors.empty()) << resolveChildErrors; sdf::Joint newSdfJoint; newSdfJoint.SetName(sdfJoint->Name()); if (sdfJoint->Axis(0)) { newSdfJoint.SetAxis(0, ResolveJointAxis(*sdfJoint->Axis(0))); } if (sdfJoint->Axis(1)) { newSdfJoint.SetAxis(1, ResolveJointAxis(*sdfJoint->Axis(1))); } newSdfJoint.SetType(sdfJoint->Type()); newSdfJoint.SetRawPose(ResolveSdfPose(sdfJoint->SemanticPose())); newSdfJoint.SetThreadPitch(sdfJoint->ThreadPitch()); newSdfJoint.SetParentLinkName(resolvedParentLinkName); newSdfJoint.SetChildLinkName(resolvedChildLinkName); physModel->ConstructJoint(newSdfJoint); } } return world; } ///////////////////////////////////////////////// class SDFFeatures_TEST : public ::testing::TestWithParam { public: WorldPtr LoadWorld(const std::string &_world) { switch(this->GetParam()) { case LoaderType::Whole: return LoadWorldWhole(_world); case LoaderType::Piecemeal: return LoadWorldPiecemeal(_world); default: std::cout << "Unknown LoaderType " << std::underlying_type_t(this->GetParam()) << " Using LoadWorldWhole" << std::endl; return LoadWorldWhole(_world); } } }; // Run with different load world functions INSTANTIATE_TEST_CASE_P(LoadWorld, SDFFeatures_TEST, ::testing::Values(LoaderType::Whole, LoaderType::Piecemeal), ); ///////////////////////////////////////////////// // Test that the dartsim plugin loaded all the relevant information correctly. TEST_P(SDFFeatures_TEST, CheckDartsimData) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); ASSERT_EQ(7u, dartWorld->getNumSkeletons()); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(1); ASSERT_NE(nullptr, skeleton); EXPECT_EQ("double_pendulum_with_base", skeleton->getName()); ASSERT_EQ(3u, skeleton->getNumBodyNodes()); auto verify = [](const dart::dynamics::DegreeOfFreedom * dof, double damping, double friction, double springRest, double stiffness, double lower, double upper, double maxForce, double maxVelocity) { EXPECT_DOUBLE_EQ(damping, dof->getDampingCoefficient()); EXPECT_DOUBLE_EQ(friction, dof->getCoulombFriction()); EXPECT_DOUBLE_EQ(springRest, dof->getRestPosition()); EXPECT_DOUBLE_EQ(stiffness, dof->getSpringStiffness()); EXPECT_DOUBLE_EQ(lower, dof->getPositionLowerLimit()); EXPECT_DOUBLE_EQ(upper, dof->getPositionUpperLimit()); EXPECT_DOUBLE_EQ(-maxForce, dof->getForceLowerLimit()); EXPECT_DOUBLE_EQ(maxForce, dof->getForceUpperLimit()); EXPECT_DOUBLE_EQ(-maxVelocity, dof->getVelocityLowerLimit()); EXPECT_DOUBLE_EQ(maxVelocity, dof->getVelocityUpperLimit()); }; // Test that things were parsed correctly. These values are either stated or // implied in the test.world SDF file. verify(skeleton->getJoint(1)->getDof(0), 3.0, 0.0, 0.0, 0.0, -1e16, 1e16, std::numeric_limits::infinity(), std::numeric_limits::infinity()); verify(skeleton->getJoint(2)->getDof(0), 3.0, 0.0, 0.0, 0.0, -1e16, 1e16, std::numeric_limits::infinity(), std::numeric_limits::infinity()); /// \todo (anyone) getBodyNode("blah")->getFrictionCoeff is deprecated, /// disabling these tests. /* EXPECT_DOUBLE_EQ(1.1, skeleton->getBodyNode("base")->getFrictionCoeff()); // The last collision element overwrites the value set by previous collision // elements. We expect mu=1, the default value, instead of 0.1. EXPECT_DOUBLE_EQ(1, skeleton->getBodyNode("upper_link")->getFrictionCoeff()); // Gets the default value when the tag is missing EXPECT_DOUBLE_EQ(1, skeleton->getBodyNode("lower_link")->getFrictionCoeff()); */ for (const auto * joint : {skeleton->getJoint(1), skeleton->getJoint(2)}) { const auto * revolute = dynamic_cast(joint); ASSERT_NE(nullptr, revolute); const Eigen::Vector3d &axis = revolute->getAxis(); EXPECT_DOUBLE_EQ(1.0, axis[0]); EXPECT_DOUBLE_EQ(0.0, axis[1]); EXPECT_DOUBLE_EQ(0.0, axis[2]); } { const dart::dynamics::SkeletonPtr freeBody = dartWorld->getSkeleton("free_body"); ASSERT_NE(nullptr, freeBody); ASSERT_EQ(1u, freeBody->getNumBodyNodes()); const dart::dynamics::BodyNode *bn = freeBody->getBodyNode(0); ASSERT_NE(nullptr, bn); EXPECT_TRUE(dynamic_cast( bn->getParentJoint())); const Eigen::Vector3d translation = bn->getTransform().translation(); EXPECT_DOUBLE_EQ(0.0, translation[0]); EXPECT_DOUBLE_EQ(10.0, translation[1]); EXPECT_DOUBLE_EQ(10.0, translation[2]); #if DART_VERSION_AT_LEAST(6, 10, 0) const dart::dynamics::ShapeNode *collision1 = bn->getShapeNode(0); auto aspect = collision1->getDynamicsAspect(); EXPECT_DOUBLE_EQ(0.8, aspect->getRestitutionCoeff()); #else EXPECT_DOUBLE_EQ(0.8, bn->getRestitutionCoeff()); #endif } const dart::dynamics::SkeletonPtr screwJointTest = dartWorld->getSkeleton("screw_joint_test"); ASSERT_NE(nullptr, screwJointTest); ASSERT_EQ(2u, screwJointTest->getNumBodyNodes()); const auto *screwJoint = dynamic_cast( screwJointTest->getJoint(1)); ASSERT_NE(nullptr, screwJoint); EXPECT_DOUBLE_EQ(-IGN_PI, screwJoint->getPitch()); } ///////////////////////////////////////////////// // Test that joint limits are working by running the simulation TEST_P(SDFFeatures_TEST, CheckJointLimitEnforcement) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const auto model = world->GetModel("joint_limit_test"); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton("joint_limit_test"); ASSERT_NE(nullptr, skeleton); auto * const joint = dynamic_cast( skeleton->getJoint(1)); auto jointPhys = model->GetJoint(1); ASSERT_NE(nullptr, joint); // the joint starts at 0. Apply force in either direction and check the limits // are enforced auto verify = [&](std::size_t index, const double force, const double tol) { dartWorld->reset(); dart::dynamics::DegreeOfFreedom * const dof = joint->getDof(index); jointPhys->SetForce(index, force); for (std::size_t i = 0; i < 1000; ++i) { dartWorld->step(); } jointPhys->SetForce(index, force); EXPECT_LE(dof->getPositionLowerLimit() - tol, dof->getPosition()); EXPECT_LE(dof->getForceLowerLimit() - tol, dof->getForce()); EXPECT_LE(dof->getVelocityLowerLimit() - tol, dof->getVelocity()); EXPECT_GE(dof->getPositionUpperLimit() + tol, dof->getPosition()); EXPECT_GE(dof->getForceUpperLimit() + tol, dof->getForce()); EXPECT_GE(dof->getVelocityUpperLimit() + tol, dof->getVelocity()); }; verify(0, -1000, 2e-3); verify(0, 1000, 2e-3); } // Create Model with parent and child links. If a link is not set, the joint // will use the world as that link. auto CreateTestModel(WorldPtr _world, const std::string &_model, const std::optional &_parentLink, const std::optional &_childLink) { sdf::Model sdfModel; sdfModel.SetName(_model); auto model = _world->ConstructModel(sdfModel); EXPECT_NE(nullptr, model); sdf::Joint sdfJoint; sdfJoint.SetName("joint0"); sdfJoint.SetType(sdf::JointType::REVOLUTE); if (_parentLink) { auto parent = model->ConstructLink(*_parentLink); EXPECT_NE(nullptr, parent); sdfJoint.SetParentLinkName(_parentLink->Name()); } else { sdfJoint.SetParentLinkName("world"); } if (_childLink) { auto child = model->ConstructLink(*_childLink); EXPECT_NE(nullptr, child); sdfJoint.SetChildLinkName(_childLink->Name()); } else { sdfJoint.SetChildLinkName("world"); } auto joint0 = model->ConstructJoint(sdfJoint); return std::make_tuple(model, joint0); } ///////////////////////////////////////////////// // Test joints with world as parent or child TEST_P(SDFFeatures_TEST, WorldIsParentOrChild) { auto engine = LoadEngine(); ASSERT_NE(nullptr, engine); sdf::World sdfWorld; sdfWorld.SetName("default"); auto world = engine->ConstructWorld(sdfWorld); EXPECT_NE(nullptr, world); std::optional parent = sdf::Link(); parent->SetName("parent"); std::optional child = sdf::Link(); child->SetName("child"); { const auto &[model, joint] = CreateTestModel(world, "test0", std::nullopt, std::nullopt); EXPECT_EQ(nullptr, joint); } { const auto &[model, joint] = CreateTestModel(world, "test1", parent, child); EXPECT_NE(nullptr, joint); } { const auto &[model, joint] = CreateTestModel(world, "test2", std::nullopt, child); EXPECT_NE(nullptr, joint); } { const auto &[model, joint] = CreateTestModel(world, "test3", parent, std::nullopt); EXPECT_EQ(nullptr, joint); } } ///////////////////////////////////////////////// TEST_P(SDFFeatures_TEST, WorldWithNestedModel) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); ASSERT_NE(nullptr, world); EXPECT_EQ(2u, world->GetModelCount()); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); // check top level model EXPECT_EQ("parent_model", world->GetModel(0)->GetName()); auto parentModel = world->GetModel("parent_model"); ASSERT_NE(nullptr, parentModel); auto joint1 = parentModel->GetJoint("joint1"); EXPECT_NE(nullptr, joint1); EXPECT_EQ(3u, parentModel->GetNestedModelCount()); auto nestedModel = parentModel->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); auto nestedJoint = parentModel->GetJoint("nested_joint"); EXPECT_NE(nullptr, nestedJoint); EXPECT_EQ(1u, parentModel->GetLinkCount()); EXPECT_NE(nullptr, parentModel->GetLink("link1")); EXPECT_EQ(nullptr, parentModel->GetLink("nested_link1")); EXPECT_EQ(nullptr, parentModel->GetLink("nested_link2")); ASSERT_EQ(2u, nestedModel->GetLinkCount()); auto nestedLink1 = nestedModel->GetLink("nested_link1"); ASSERT_NE(nullptr, nestedLink1); EXPECT_EQ(0u, nestedLink1->GetIndex()); EXPECT_EQ(nestedLink1, nestedModel->GetLink(0)); auto nestedLink2 = nestedModel->GetLink("nested_link2"); ASSERT_NE(nullptr, nestedLink2); EXPECT_EQ(1u, nestedLink2->GetIndex()); EXPECT_EQ(nestedLink2, nestedModel->GetLink(1)); auto nestedModelSkel = dartWorld->getSkeleton("parent_model::nested_model"); ASSERT_NE(nullptr, nestedModelSkel); // nested_model::nested_link1 would have moved to the parent_model skeleton so // we expect to not find it in the nested_model skeleton EXPECT_EQ(nullptr, nestedModelSkel->getBodyNode("nested_link1")); auto nestedModel2 = world->GetModel("parent_model::nested_model2"); ASSERT_NE(nullptr, nestedModel2); EXPECT_EQ(1u, nestedModel2->GetLinkCount()); EXPECT_NE(nullptr, nestedModel2->GetLink("nested_link1")); auto nestedModel3 = world->GetModel("parent_model::nested_model3"); ASSERT_NE(nullptr, nestedModel3); EXPECT_EQ(1u, nestedModel3->GetLinkCount()); EXPECT_NE(nullptr, nestedModel3->GetLink("link1")); } ///////////////////////////////////////////////// TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) { WorldPtr world = this->LoadWorld( TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf"); ASSERT_NE(nullptr, world); EXPECT_EQ(1u, world->GetModelCount()); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); // check top level model auto parentModel = world->GetModel("parent_model"); ASSERT_NE(nullptr, parentModel); EXPECT_EQ("parent_model", parentModel->GetName()); EXPECT_EQ(1u, parentModel->GetJointCount()); EXPECT_EQ(1u, parentModel->GetLinkCount()); auto joint1 = parentModel->GetJoint(0); ASSERT_NE(nullptr, joint1); EXPECT_EQ(joint1->GetName(), "joint1"); auto link1 = parentModel->GetLink("link1"); EXPECT_NE(nullptr, link1); auto nestedModel = parentModel->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("nested_model", nestedModel->GetName()); EXPECT_EQ(2u, nestedModel->GetLinkCount()); EXPECT_EQ(2u, nestedModel->GetJointCount()); auto nestedJoint1 = nestedModel->GetJoint("nested_joint1"); EXPECT_NE(nullptr, nestedJoint1); auto nestedJoint2 = nestedModel->GetJoint("nested_joint2"); EXPECT_NE(nullptr, nestedJoint2); auto nestedLink1 = nestedModel->GetLink("nested_link1"); EXPECT_NE(nullptr, nestedLink1); auto nestedLink2 = nestedModel->GetLink("nested_link2"); EXPECT_NE(nullptr, nestedLink2); } ///////////////////////////////////////////////// // Test that joint type falls back to fixed if the type is not supported TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/test.world"); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton("unsupported_joint_test"); ASSERT_NE(nullptr, skeleton); ASSERT_EQ(6u, skeleton->getNumBodyNodes()); for (const auto &jointName : {"j0", "j1", "j2"}) { const auto *joint = skeleton->getJoint(jointName); ASSERT_NE(nullptr, joint) << " joint '" << jointName << "'doesn't exist in this skeleton"; const auto *fixedJoint = dynamic_cast(joint); EXPECT_NE(nullptr, fixedJoint) << " joint type is: " << joint->getType(); } } ///////////////////////////////////////////////// // Check that joints between links in different models work as expected TEST_P(SDFFeatures_TEST, JointsAcrossNestedModels) { WorldPtr world = this->LoadWorld( TEST_WORLD_DIR "/joint_across_nested_models.sdf"); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); auto checkModel = [&world](const std::string &_modelName){ SCOPED_TRACE("checkModel " + _modelName); // check top level model auto parentModel = world->GetModel(_modelName); ASSERT_NE(nullptr, parentModel); auto link1 = parentModel->GetLink("link1"); ASSERT_NE(nullptr, link1); auto nestedModel = parentModel->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); auto link2 = nestedModel->GetLink("link2"); ASSERT_NE(nullptr, link2); Eigen::Vector3d link1Pos = link1->FrameDataRelativeToWorld().pose.translation(); Eigen::Vector3d link2Pos = link2->FrameDataRelativeToWorld().pose.translation(); EXPECT_NEAR(0.25, link1Pos.z(), 1e-6); EXPECT_NEAR(0.25, link2Pos.z(), 1e-6); }; { SCOPED_TRACE("Before step"); checkModel("M1"); checkModel("M2"); } for (int i = 0; i < 1000; ++i) { dartWorld->step(); } { SCOPED_TRACE("After step"); checkModel("M1"); checkModel("M2"); } } ///////////////////////////////////////////////// class SDFFeatures_FrameSemantics: public SDFFeatures_TEST { }; // Run with different load world functions INSTANTIATE_TEST_CASE_P(LoadWorld, SDFFeatures_FrameSemantics, ::testing::Values(LoaderType::Whole, LoaderType::Piecemeal), ); ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, LinkRelativeTo) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); ASSERT_NE(nullptr, world); const std::string modelName = "link_relative_to"; dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); ASSERT_EQ(2u, skeleton->getNumBodyNodes()); const dart::dynamics::BodyNode *link2 = skeleton->getBodyNode("L2"); ASSERT_NE(nullptr, link2); // Expect the world pose of L2 to be 0 0 3 0 0 pi Eigen::Isometry3d expWorldPose = Eigen::Translation3d(0, 0, 3) * Eigen::AngleAxisd(IGN_PI, Eigen::Vector3d::UnitZ()); dartWorld->step(); // Step once and check EXPECT_TRUE(ignition::physics::test::Equal( expWorldPose, link2->getWorldTransform(), 1e-3)); } ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, CollisionRelativeTo) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); ASSERT_NE(nullptr, world); const std::string modelName = "collision_relative_to"; dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); ASSERT_EQ(2u, skeleton->getNumBodyNodes()); const dart::dynamics::BodyNode *link2 = skeleton->getBodyNode("L2"); ASSERT_NE(nullptr, link2); const auto collision = link2->getShapeNode(0); ASSERT_TRUE(collision); // Expect the pose of c1 relative to L2 (the parent link) to be the same // as the pose of L1 relative to L2 Eigen::Isometry3d expPose; expPose = Eigen::Translation3d(0, 0, -1); EXPECT_TRUE(ignition::physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); // Step once and check, the relative pose should still be the same dartWorld->step(); EXPECT_TRUE(ignition::physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); } ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_links"; dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); ASSERT_EQ(2u, skeleton->getNumBodyNodes()); const dart::dynamics::BodyNode *link1 = skeleton->getBodyNode("L1"); ASSERT_NE(nullptr, link1); const dart::dynamics::BodyNode *link2 = skeleton->getBodyNode("L2"); ASSERT_NE(nullptr, link2); // Expect the world pose of L1 to be the same as the world pose of F1 Eigen::Isometry3d link1ExpPose; link1ExpPose = Eigen::Translation3d(1, 0, 1); EXPECT_TRUE(ignition::physics::test::Equal( link1ExpPose, link1->getWorldTransform(), 1e-5)); // Expect the world pose of L2 to be the same as the world pose of F2, which // is at the origin of the model Eigen::Isometry3d link2ExpPose; link2ExpPose = Eigen::Translation3d(1, 0, 0); EXPECT_TRUE(ignition::physics::test::Equal( link2ExpPose, link2->getWorldTransform(), 1e-5)); // Step once and check dartWorld->step(); EXPECT_TRUE(ignition::physics::test::Equal( link1ExpPose, link1->getWorldTransform(), 1e-5)); EXPECT_TRUE(ignition::physics::test::Equal( link2ExpPose, link2->getWorldTransform(), 1e-5)); } ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_collisions"; dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); ASSERT_EQ(1u, skeleton->getNumBodyNodes()); const dart::dynamics::BodyNode *link1 = skeleton->getBodyNode("L1"); ASSERT_NE(nullptr, link1); const auto collision = link1->getShapeNode(0); ASSERT_TRUE(collision); // Expect the pose of c1 relative to L1 (the parent link) to be the same // as the pose of F1 relative to L1 Eigen::Isometry3d expPose; expPose = Eigen::Translation3d(0, 0, 1); EXPECT_TRUE(ignition::physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); // Step once and check dartWorld->step(); EXPECT_TRUE(ignition::physics::test::Equal( expPose, collision->getRelativeTransform(), 1e-5)); } ///////////////////////////////////////////////// TEST_P(SDFFeatures_FrameSemantics, ExplicitWorldFrames) { WorldPtr world = this->LoadWorld(TEST_WORLD_DIR"/world_frames.sdf"); ASSERT_NE(nullptr, world); const std::string modelName = "M"; dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); ASSERT_EQ(1u, skeleton->getNumBodyNodes()); const dart::dynamics::BodyNode *link1 = skeleton->getBodyNode("L1"); ASSERT_NE(nullptr, link1); // Expect the world pose of M to be (1 1 2 0 0 0) taking into acount the chain // of explicit frames relative to which its pose is expressed Eigen::Isometry3d expPose; expPose = Eigen::Translation3d(1, 1, 2); // Since we can't get the skeleton's world transform, we use the world // transform of L1 which is at the origin of the model frame. EXPECT_TRUE(ignition::physics::test::Equal( expPose, link1->getWorldTransform(), 1e-5)); // Step once and check dartWorld->step(); EXPECT_TRUE(ignition::physics::test::Equal( expPose, link1->getWorldTransform(), 1e-5)); } ///////////////////////////////////////////////// TEST_P(SDFFeatures_TEST, Shapes) { auto world = this->LoadWorld(TEST_WORLD_DIR"/shapes.sdf"); ASSERT_NE(nullptr, world); auto dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); ASSERT_EQ(5u, dartWorld->getNumSkeletons()); int count{0}; for (auto name : {"box", "cylinder", "sphere", "capsule", "ellipsoid"}) { const auto skeleton = dartWorld->getSkeleton(count++); ASSERT_NE(nullptr, skeleton); EXPECT_EQ(name, skeleton->getName()); ASSERT_EQ(1u, skeleton->getNumBodyNodes()); } } int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/Collisions_TEST.cc0000664000175000017500000001262714143524174025076 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include // Features #include #include #include #include #include #include #include #include #include using TestFeatureList = ignition::physics::FeatureList< ignition::physics::LinkFrameSemantics, ignition::physics::ForwardStep, ignition::physics::GetEntities, ignition::physics::ConstructEmptyWorldFeature, ignition::physics::ConstructEmptyModelFeature, ignition::physics::ConstructEmptyLinkFeature, ignition::physics::mesh::AttachMeshShapeFeature, ignition::physics::AttachPlaneShapeFeature, ignition::physics::SetFreeJointRelativeTransformFeature, ignition::physics::AttachFixedJointFeature >; using TestWorldPtr = ignition::physics::World3dPtr; using TestEnginePtr = ignition::physics::Engine3dPtr; using WorldConstructor = std::function; std::unordered_set LoadWorlds( const std::string &_library, const WorldConstructor &_constructor) { ignition::plugin::Loader loader; loader.LoadLib(_library); const std::set pluginNames = ignition::physics::FindFeatures3d::From(loader); std::unordered_set worlds; for (const std::string &name : pluginNames) { ignition::plugin::PluginPtr plugin = loader.Instantiate(name); std::cout << " -- Plugin name: " << name << std::endl; auto engine = ignition::physics::RequestEngine3d::From(plugin); EXPECT_NE(nullptr, engine); worlds.insert(_constructor(engine)); } return worlds; } class Collisions_TEST : public ::testing::Test, public ::testing::WithParamInterface {}; INSTANTIATE_TEST_CASE_P(PhysicsPlugins, Collisions_TEST, ::testing::ValuesIn(ignition::physics::test::g_PhysicsPluginLibraries),); // NOLINT TestWorldPtr ConstructMeshPlaneWorld( const ignition::physics::Engine3dPtr &_engine, const ignition::common::Mesh &_mesh) { auto world = _engine->ConstructEmptyWorld("world"); Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); tf.translation()[2] = 2.0; auto model = world->ConstructEmptyModel("mesh"); auto link = model->ConstructEmptyLink("link"); // TODO(anyone): This test is somewhat awkward because we lift up the mesh // from the center of the link instead of lifting up the link or the model. // We're doing this because we don't currently have an API for moving models // or links around. See the conversation here for more: // https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-physics/pull-requests/46/page/1#comment-87592809 link->AttachMeshShape("mesh", _mesh, tf); model = world->ConstructEmptyModel("plane"); link = model->ConstructEmptyLink("link"); link->AttachPlaneShape("plane", ignition::physics::LinearVector3d::UnitZ()); link->AttachFixedJoint(nullptr); return world; } TEST_P(Collisions_TEST, MeshAndPlane) { const std::string library = GetParam(); if (library.empty()) return; const std::string meshFilename = IGNITION_PHYSICS_RESOURCE_DIR "/chassis.dae"; auto &meshManager = *ignition::common::MeshManager::Instance(); auto *mesh = meshManager.Load(meshFilename); std::cout << "Testing library " << library << std::endl; auto worlds = LoadWorlds(library, [&](const TestEnginePtr &_engine) { return ConstructMeshPlaneWorld(_engine, *mesh); }); for (const auto &world : worlds) { const auto link = world->GetModel(0)->GetLink(0); EXPECT_NEAR( 0.0, link->FrameDataRelativeToWorld().pose.translation()[2], 1e-6); ignition::physics::ForwardStep::Output output; ignition::physics::ForwardStep::State state; ignition::physics::ForwardStep::Input input; for (std::size_t i = 0; i < 1000; ++i) { world->Step(output, state, input); } // Make sure the mesh was stopped by the plane. In 2000 time steps at the // default step size of 0.001, a free falling body should drop approximately // 19.6 meters. As long as the body is somewhere near 1.91, then it has been // stopped by the plane (the exact value might vary because the body might // be rocking side-to-side after falling). EXPECT_NEAR( -1.91, link->FrameDataRelativeToWorld().pose.translation()[2], 0.05); } } int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/JointFeatures.cc0000664000175000017500000005452414143524174024705 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include "JointFeatures.hh" namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// double JointFeatures::GetJointPosition( const Identity &_id, const std::size_t _dof) const { return this->ReferenceInterface(_id)->joint->getPosition(_dof); } ///////////////////////////////////////////////// double JointFeatures::GetJointVelocity( const Identity &_id, const std::size_t _dof) const { return this->ReferenceInterface(_id)->joint->getVelocity(_dof); } ///////////////////////////////////////////////// double JointFeatures::GetJointAcceleration( const Identity &_id, const std::size_t _dof) const { return this->ReferenceInterface(_id)->joint->getAcceleration(_dof); } ///////////////////////////////////////////////// double JointFeatures::GetJointForce( const Identity &_id, const std::size_t _dof) const { return this->ReferenceInterface(_id)->joint->getForce(_dof); } ///////////////////////////////////////////////// Pose3d JointFeatures::GetJointTransform(const Identity &_id) const { return this->ReferenceInterface(_id) ->joint->getRelativeTransform(); } ///////////////////////////////////////////////// void JointFeatures::SetJointPosition( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is finite. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (!std::isfinite(_value)) { ignerr << "Invalid joint position value [" << _value << "] set on joint [" << joint->getName() << " DOF " << _dof << "]. The value will be ignored\n"; return; } joint->setPosition(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointVelocity( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is finite. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (!std::isfinite(_value)) { ignerr << "Invalid joint velocity value [" << _value << "] set on joint [" << joint->getName() << " DOF " << _dof << "]. The value will be ignored\n"; return; } joint->setVelocity(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointAcceleration( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is finite. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (!std::isfinite(_value)) { ignerr << "Invalid joint acceleration value [" << _value << "] set on joint [" << joint->getName() << " DOF " << _dof << "]. The value will be ignored\n"; return; } joint->setAcceleration(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointForce( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is finite. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (!std::isfinite(_value)) { ignerr << "Invalid joint force value [" << _value << "] set on joint [" << joint->getName() << " DOF " << _dof << "]. The value will be ignored\n"; return; } if (joint->getActuatorType() != dart::dynamics::Joint::FORCE) { joint->setActuatorType(dart::dynamics::Joint::FORCE); } this->ReferenceInterface(_id)->joint->setCommand(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointVelocityCommand( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is finite. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (!std::isfinite(_value)) { ignerr << "Invalid joint velocity value [" << _value << "] commanded on joint [" << joint->getName() << " DOF " << _dof << "]. The command will be ignored\n"; return; } if (joint->getActuatorType() != dart::dynamics::Joint::SERVO) { joint->setActuatorType(dart::dynamics::Joint::SERVO); } // warn about bug https://github.com/dartsim/dart/issues/1583 if ((joint->getPositionLowerLimit(_dof) > -1e16 || joint->getPositionUpperLimit(_dof) < 1e16 ) && (!std::isfinite(joint->getForceUpperLimit(_dof)) || !std::isfinite(joint->getForceLowerLimit(_dof)))) { static bool informed = false; if (!informed) { ignerr << "Velocity control does not respect positional limits of " << "joints if these joints do not have an effort limit. Please, " << "set min and max effort for joint [" << joint->getName() << "] to values about -1e6 and 1e6 (or higher if working with " << "heavy links)." << std::endl; informed = true; } } joint->setCommand(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointMinPosition( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (std::isnan(_value)) { ignerr << "Invalid minimum joint position value [" << _value << "] commanded on joint [" << joint->getName() << " DOF " << _dof << "]. The command will be ignored\n"; return; } #if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); #else joint->setPositionLimitEnforced(true); #endif // We do not check min/max mismatch, we leave that to DART. joint->setPositionLowerLimit(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointMaxPosition( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (std::isnan(_value)) { ignerr << "Invalid maximum joint position value [" << _value << "] commanded on joint [" << joint->getName() << " DOF " << _dof << "]. The command will be ignored\n"; return; } #if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); #else joint->setPositionLimitEnforced(true); #endif // We do not check min/max mismatch, we leave that to DART. joint->setPositionUpperLimit(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointMinVelocity( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (std::isnan(_value)) { ignerr << "Invalid minimum joint velocity value [" << _value << "] commanded on joint [" << joint->getName() << " DOF " << _dof << "]. The command will be ignored\n"; return; } #if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); #else joint->setPositionLimitEnforced(true); #endif // We do not check min/max mismatch, we leave that to DART. joint->setVelocityLowerLimit(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointMaxVelocity( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (std::isnan(_value)) { ignerr << "Invalid maximum joint velocity value [" << _value << "] commanded on joint [" << joint->getName() << " DOF " << _dof << "]. The command will be ignored\n"; return; } #if DART_VERSION_AT_LEAST(6, 10, 0) joint->setLimitEnforcement(true); #else joint->setPositionLimitEnforced(true); #endif // We do not check min/max mismatch, we leave that to DART. joint->setVelocityUpperLimit(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointMinEffort( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (std::isnan(_value)) { ignerr << "Invalid minimum joint effort value [" << _value << "] commanded on joint [" << joint->getName() << " DOF " << _dof << "]. The command will be ignored\n"; return; } // We do not check min/max mismatch, we leave that to DART. joint->setForceLowerLimit(_dof, _value); } ///////////////////////////////////////////////// void JointFeatures::SetJointMaxEffort( const Identity &_id, const std::size_t _dof, const double _value) { auto joint = this->ReferenceInterface(_id)->joint; // Take extra care that the value is valid. A nan can cause the DART // constraint solver to fail, which will in turn either cause a crash or // collisions to fail if (std::isnan(_value)) { ignerr << "Invalid maximum joint effort value [" << _value << "] commanded on joint [" << joint->getName() << " DOF " << _dof << "]. The command will be ignored\n"; return; } // We do not check min/max mismatch, we leave that to DART. joint->setForceUpperLimit(_dof, _value); } ///////////////////////////////////////////////// std::size_t JointFeatures::GetJointDegreesOfFreedom(const Identity &_id) const { return this->ReferenceInterface(_id)->joint->getNumDofs(); } ///////////////////////////////////////////////// Pose3d JointFeatures::GetJointTransformFromParent(const Identity &_id) const { return this->ReferenceInterface(_id) ->joint->getTransformFromParentBodyNode(); } ///////////////////////////////////////////////// Pose3d JointFeatures::GetJointTransformToChild(const Identity &_id) const { return this->ReferenceInterface(_id) ->joint->getTransformFromChildBodyNode().inverse(); } ///////////////////////////////////////////////// void JointFeatures::SetJointTransformFromParent( const Identity &_id, const Pose3d &_pose) { this->ReferenceInterface(_id) ->joint->setTransformFromParentBodyNode(_pose); } ///////////////////////////////////////////////// void JointFeatures::SetJointTransformToChild( const Identity &_id, const Pose3d &_pose) { this->ReferenceInterface(_id) ->joint->setTransformFromChildBodyNode(_pose.inverse()); } ///////////////////////////////////////////////// void JointFeatures::DetachJoint(const Identity &_jointId) { auto joint = this->ReferenceInterface(_jointId)->joint; if (joint->getType() == "FreeJoint") { // don't need to do anything, joint is already a FreeJoint return; } auto child = joint->getChildBodyNode(); auto transform = child->getWorldTransform(); auto spatialVelocity = child->getSpatialVelocity( dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); if (!this->links.HasEntity(child)) { return; } auto childLinkInfo = this->links.at(child); dart::dynamics::SkeletonPtr skeleton; { // Find the original skeleton the child BodyNode belonged to std::string oldName = child->getName(); if (oldName != childLinkInfo->name) { std::size_t originalNameIndex = oldName.rfind(childLinkInfo->name); if (originalNameIndex > 1 && originalNameIndex != std::string::npos && this->models.HasEntity(joint->getSkeleton())) { // Assume that the original and the current skeletons are in the same // world. auto worldId = this->GetWorldOfModelImpl( this->models.IdentityOf(joint->getSkeleton())); auto dartWorld = this->worlds.at(worldId); std::string modelName = oldName.substr(0, originalNameIndex - 1); skeleton = dartWorld->getSkeleton(modelName); if (skeleton) { child->setName(childLinkInfo->name); } } if (nullptr == skeleton) { ignerr << "Could not find the original skeleton of BodyNode " << "[" << oldName << "] when detaching joint " << "[" << joint->getName() << "]. Detached links may not work " << "as expected.\n"; } } } dart::dynamics::FreeJoint *freeJoint; if (skeleton) { freeJoint = child->moveTo(skeleton, nullptr); } else { freeJoint = child->moveTo(nullptr); } freeJoint->setTransform(transform); freeJoint->setSpatialVelocity(spatialVelocity, dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); // TODO(addisu) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after moveTo. child->incrementVersion(); } ///////////////////////////////////////////////// Identity JointFeatures::CastToFixedJoint( const Identity &_jointID) const { dart::dynamics::WeldJoint *const weld = dynamic_cast( this->ReferenceInterface(_jointID)->joint.get()); if (weld) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// Identity JointFeatures::AttachFixedJoint( const Identity &_childID, const BaseLink3dPtr &_parent, const std::string &_name) { auto linkInfo = this->ReferenceInterface(_childID); DartBodyNode *const bn = linkInfo->link.get(); dart::dynamics::WeldJoint::Properties properties; properties.mName = _name; auto *const parentBn = _parent ? this->ReferenceInterface( _parent->FullIdentity())->link.get() : nullptr; if (bn->getParentJoint()->getType() != "FreeJoint") { // child already has a parent joint // TODO(scpeters): use a WeldJointConstraint between the two bodies return this->GenerateInvalidId(); } { auto skeleton = bn->getSkeleton(); if (skeleton) { bn->setName(skeleton->getName() + '/' + linkInfo->name); } } const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); // TODO(addisu) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after moveTo. bn->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } ///////////////////////////////////////////////// Identity JointFeatures::CastToFreeJoint( const Identity &_jointID) const { auto *const freeJoint = dynamic_cast( this->ReferenceInterface(_jointID)->joint.get()); if (freeJoint) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// void JointFeatures::SetFreeJointRelativeTransform( const Identity &_jointID, const Pose3d &_pose) { static_cast( this->ReferenceInterface(_jointID)->joint.get()) ->setRelativeTransform(_pose); } ///////////////////////////////////////////////// Identity JointFeatures::CastToRevoluteJoint( const Identity &_jointID) const { dart::dynamics::RevoluteJoint *const revolute = dynamic_cast( this->ReferenceInterface(_jointID)->joint.get()); if (revolute) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// AngularVector3d JointFeatures::GetRevoluteJointAxis( const Identity &_jointID) const { return static_cast( this->ReferenceInterface(_jointID)->joint.get())->getAxis(); } ///////////////////////////////////////////////// void JointFeatures::SetRevoluteJointAxis( const Identity &_jointID, const AngularVector3d &_axis) { static_cast( this->ReferenceInterface(_jointID)->joint.get()) ->setAxis(_axis); } ///////////////////////////////////////////////// Identity JointFeatures::AttachRevoluteJoint( const Identity &_childID, const BaseLink3dPtr &_parent, const std::string &_name, const AngularVector3d &_axis) { auto linkInfo = this->ReferenceInterface(_childID); DartBodyNode *const bn = linkInfo->link.get(); dart::dynamics::RevoluteJoint::Properties properties; properties.mName = _name; properties.mAxis = _axis; auto *const parentBn = _parent ? this->ReferenceInterface( _parent->FullIdentity())->link.get() : nullptr; if (bn->getParentJoint()->getType() != "FreeJoint") { // child already has a parent joint // TODO(scpeters): use a WeldJointConstraint between the two bodies return this->GenerateInvalidId(); } { auto skeleton = bn->getSkeleton(); if (skeleton) { bn->setName(skeleton->getName() + '/' + linkInfo->name); } } const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); // TODO(addisu) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after moveTo. bn->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } ///////////////////////////////////////////////// Identity JointFeatures::CastToPrismaticJoint( const Identity &_jointID) const { dart::dynamics::PrismaticJoint *prismatic = dynamic_cast( this->ReferenceInterface(_jointID)->joint.get()); if (prismatic) return this->GenerateIdentity(_jointID, this->Reference(_jointID)); return this->GenerateInvalidId(); } ///////////////////////////////////////////////// LinearVector3d JointFeatures::GetPrismaticJointAxis( const Identity &_jointID) const { return static_cast( this->ReferenceInterface(_jointID)->joint.get())->getAxis(); } ///////////////////////////////////////////////// void JointFeatures::SetPrismaticJointAxis( const Identity &_jointID, const LinearVector3d &_axis) { static_cast( this->ReferenceInterface(_jointID)->joint.get()) ->setAxis(_axis); } ///////////////////////////////////////////////// Identity JointFeatures::AttachPrismaticJoint( const Identity &_childID, const BaseLink3dPtr &_parent, const std::string &_name, const LinearVector3d &_axis) { auto linkInfo = this->ReferenceInterface(_childID); DartBodyNode *const bn = linkInfo->link.get(); dart::dynamics::PrismaticJoint::Properties properties; properties.mName = _name; properties.mAxis = _axis; auto *const parentBn = _parent ? this->ReferenceInterface( _parent->FullIdentity())->link.get() : nullptr; if (bn->getParentJoint()->getType() != "FreeJoint") { // child already has a parent joint // TODO(scpeters): use a WeldJointConstraint between the two bodies return this->GenerateInvalidId(); } { auto skeleton = bn->getSkeleton(); if (skeleton) { bn->setName(skeleton->getName() + '/' + linkInfo->name); } } const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); // TODO(addisu) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after moveTo. bn->incrementVersion(); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } ///////////////////////////////////////////////// Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame( const Identity &_id) const { auto &joint = this->ReferenceInterface(_id)->joint; auto *childBn = joint->getChildBodyNode(); if (nullptr == childBn) { ignerr << "Joint [" << joint->getName() << "] does not have a child link. Unable to get transmitted wrench.\n"; return {}; } const Eigen::Vector6d transmittedWrenchInBody = childBn->getBodyForce(); // C - Child body frame // J - Joint frame // X_CJ - Pose of joint in child body frame const Eigen::Isometry3d X_CJ = joint->getTransformFromChildBodyNode(); const Eigen::Vector6d transmittedWrenchInJoint = dart::math::dAdT(X_CJ, transmittedWrenchInBody); Wrench3d wrenchOut; wrenchOut.torque = transmittedWrenchInJoint.head<3>(); wrenchOut.force = transmittedWrenchInJoint.tail<3>(); return wrenchOut; } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/ShapeFeatures.hh0000664000175000017500000001562014143524174024666 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_SHAPEFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_SHAPEFEATURES_HH_ #include #include #include #include #include #include #include #include #include #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { struct ShapeFeatureList : FeatureList< GetShapeKinematicProperties, SetShapeKinematicProperties, #if DART_VERSION_AT_LEAST(6, 10, 0) GetShapeFrictionPyramidSlipCompliance, SetShapeFrictionPyramidSlipCompliance, #endif GetShapeBoundingBox, GetBoxShapeProperties, // dartsim cannot yet update shape properties without reloading the model into // the world // SetBoxShapeProperties, AttachBoxShapeFeature, GetCapsuleShapeProperties, // SetCapsulerShapeProperties, AttachCapsuleShapeFeature, GetCylinderShapeProperties, // SetCylinderShapeProperties, AttachCylinderShapeFeature, GetEllipsoidShapeProperties, // SetEllipsoidShapeProperties, AttachEllipsoidShapeFeature, GetSphereShapeProperties, // SetSphereShapeProperties, AttachSphereShapeFeature, heightmap::GetHeightmapShapeProperties, // heightmap::SetHeightmapShapeProperties, heightmap::AttachHeightmapShapeFeature, mesh::GetMeshShapeProperties, // mesh::SetMeshShapeProperties, mesh::AttachMeshShapeFeature, GetPlaneShapeProperties, // SetPlaneShapeProperties, AttachPlaneShapeFeature > { }; class ShapeFeatures : public virtual Base, public virtual Implements3d { // ----- Kinematic Properties ----- public: Pose3d GetShapeRelativeTransform( const Identity &_shapeID) const override; public: void SetShapeRelativeTransform( const Identity &_shapeID, const Pose3d &_pose) override; // ----- Box Features ----- public: Identity CastToBoxShape( const Identity &_shapeID) const override; public: LinearVector3d GetBoxShapeSize( const Identity &_boxID) const override; public: Identity AttachBoxShape( const Identity &_linkID, const std::string &_name, const LinearVector3d &_size, const Pose3d &_pose) override; // ----- Capsule Features ----- public: Identity CastToCapsuleShape( const Identity &_shapeID) const override; public: double GetCapsuleShapeRadius( const Identity &_capsuleID) const override; public: double GetCapsuleShapeLength( const Identity &_capsuleID) const override; public: Identity AttachCapsuleShape( const Identity &_linkID, const std::string &_name, double _radius, double _length, const Pose3d &_pose) override; // ----- Cylinder Features ----- public: Identity CastToCylinderShape( const Identity &_shapeID) const override; public: double GetCylinderShapeRadius( const Identity &_cylinderID) const override; public: double GetCylinderShapeHeight( const Identity &_cylinderID) const override; public: Identity AttachCylinderShape( const Identity &_linkID, const std::string &_name, double _radius, double _height, const Pose3d &_pose) override; // ----- Ellipsoid Features ----- public: Identity CastToEllipsoidShape( const Identity &_shapeID) const override; public: Vector3d GetEllipsoidShapeRadii( const Identity &_ellipsoidID) const override; public: Identity AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, const Vector3d _radii, const Pose3d &_pose) override; // ----- Sphere Features ----- public: Identity CastToSphereShape( const Identity &_shapeID) const override; public: double GetSphereShapeRadius( const Identity &_sphereID) const override; public: Identity AttachSphereShape( const Identity &_linkID, const std::string &_name, double _radius, const Pose3d &_pose) override; // ----- Heightmap Features ----- public: Identity CastToHeightmapShape( const Identity &_shapeID) const override; public: LinearVector3d GetHeightmapShapeSize( const Identity &_heightmapID) const override; public: Identity AttachHeightmapShape( const Identity &_linkID, const std::string &_name, const common::HeightmapData &_heightmapData, const Pose3d &_pose, const LinearVector3d &_size, int _subSampling) override; // ----- Mesh Features ----- public: Identity CastToMeshShape( const Identity &_shapeID) const override; public: LinearVector3d GetMeshShapeSize( const Identity &_meshID) const override; public: LinearVector3d GetMeshShapeScale( const Identity &_meshID) const override; public: Identity AttachMeshShape( const Identity &_linkID, const std::string &_name, const ignition::common::Mesh &_mesh, const Pose3d &_pose, const LinearVector3d &_scale) override; // ----- Boundingbox Features ----- public: AlignedBox3d GetShapeAxisAlignedBoundingBox( const Identity &_shapeID) const override; // ----- Plane Features ----- public: Identity CastToPlaneShape( const Identity &_shapeID) const override; public: LinearVector3d GetPlaneShapeNormal( const Identity &_planeID) const override; public: LinearVector3d GetPlaneShapePoint( const Identity &_planeID) const override; public: Identity AttachPlaneShape( const Identity &_linkID, const std::string &_name, const LinearVector3d &_normal, const LinearVector3d &_point) override; #if DART_VERSION_AT_LEAST(6, 10, 0) // ----- Friction Features ----- public: virtual double GetShapeFrictionPyramidPrimarySlipCompliance( const Identity &_shapeID) const override; public: virtual double GetShapeFrictionPyramidSecondarySlipCompliance( const Identity &_shapeID) const override; public: virtual bool SetShapeFrictionPyramidPrimarySlipCompliance( const Identity &_shapeID, double _value) override; public: virtual bool SetShapeFrictionPyramidSecondarySlipCompliance( const Identity &_shapeID, double _value) override; #endif }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/FreeGroupFeatures.cc0000664000175000017500000002044114143524174025507 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include "FreeGroupFeatures.hh" #include #include #include namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// Identity FreeGroupFeatures::FindFreeGroupForModel( const Identity &_modelID) const { const auto modelInfo = this->models.at(_modelID); // Verify that the model qualifies as a FreeGroup const dart::dynamics::ConstSkeletonPtr &skeleton = modelInfo->model; // If there are no bodies at all in this model, then the FreeGroup functions // will not work properly, so we'll just reject these cases. if (skeleton->getNumBodyNodes() == 0 && modelInfo->nestedModels.size() == 0) return this->GenerateInvalidId(); // Verify that all root joints are FreeJoints for (std::size_t i = 0; i < skeleton->getNumTrees(); ++i) { if (skeleton->getRootJoint(i)->getType() != dart::dynamics::FreeJoint::getStaticType()) { return this->GenerateInvalidId(); } } for (const auto &nestedModel : modelInfo->nestedModels) { // Check that each nested model with BodyNodes or nested models has valid // free groups by recursively calling FindFreeGroupForModel. Nested models // without BodyNodes or their own nested models are disallowed by the // SDFormat spec, yet may occur if all BodyNodes are moved out of a skeleton // when creating joints. In this case, skip such a skeleton instead of // returning an error. auto nestedModelInfo = this->models.at(nestedModel); if (nestedModelInfo->model->getNumBodyNodes() > 0 || nestedModelInfo->nestedModels.size() > 0) { if (!this->FindFreeGroupForModel( this->GenerateIdentity(nestedModel, nestedModelInfo))) { return this->GenerateInvalidId(); } } } // TODO(MXG): When the dartsim plugin supports closed-loop constraints, verify // that this model is not attached to the world or any other models. If it's // attached to anything external, then we should return an invalid identity. return _modelID; } ///////////////////////////////////////////////// Identity FreeGroupFeatures::FindFreeGroupForLink( const Identity &_linkID) const { const dart::dynamics::BodyNode* bn = this->links.at(_linkID)->link; // Move towards the root of the tree looking for a FreeJoint while (bn) { if (bn->getParentJoint()->getType() == dart::dynamics::FreeJoint::getStaticType()) { break; } bn = bn->getParentBodyNode(); } if (bn == nullptr) return this->GenerateInvalidId(); // TODO(MXG): When the dartsim plugin supports closed-loop constraints, verify // that this sub-tree does not have any constraints that attach it to any // links outside of the tree. return this->GenerateIdentity(this->links.IdentityOf(bn)); } ///////////////////////////////////////////////// Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const { const FreeGroupInfo &info = GetCanonicalInfo(_groupID); return this->GenerateIdentity(this->links.IdentityOf(info.link)); } ///////////////////////////////////////////////// FreeGroupFeatures::FreeGroupInfo FreeGroupFeatures::GetCanonicalInfo( const Identity &_groupID) const { const auto model_it = this->models.idToObject.find(_groupID); if (model_it != this->models.idToObject.end()) { const auto &modelInfo = model_it->second; if (modelInfo->model->getNumBodyNodes() > 0) { return FreeGroupInfo{ modelInfo->model->getRootBodyNode(), modelInfo->model.get()}; } else { // If the skeleton doesn't have any BodyNodes, we recursively search for // a root BodyNode in the first nested model that has a non-zero number of // BodyNodes. Since all root joints have been verified to be FreeJoints in // FindFreeGroupForModel, we are guaranteed that the BodyNode found in // this way is the root link for the free group. for (const auto &nestedModel : modelInfo->nestedModels) { auto freeGroupInfo = this->GetCanonicalInfo(this->GenerateIdentity(nestedModel)); if (freeGroupInfo.link != nullptr) return freeGroupInfo; } // Error return {}; } } return FreeGroupInfo{this->links.at(_groupID)->link, nullptr}; } ///////////////////////////////////////////////// void FreeGroupFeatures::SetFreeGroupWorldPose( const Identity &_groupID, const PoseType &_pose) { const FreeGroupInfo &info = GetCanonicalInfo(_groupID); if (!info.model) { if (nullptr != info.link) { static_cast(info.link->getParentJoint()) ->setTransform(_pose); } else { ignerr << "No link for free group with id [" << _groupID.id << "] found. SetFreeGroupWorldPose failed." << std::endl; } return; } const Eigen::Isometry3d tfChange = _pose * info.link->getWorldTransform().inverse(); for (std::size_t i = 0; i < info.model->getNumTrees(); ++i) { auto *bn = info.model->getRootBodyNode(i); const Eigen::Isometry3d new_tf = tfChange * bn->getTransform(); static_cast(bn->getParentJoint()) ->setTransform(new_tf); } auto modelInfo = this->models.at(_groupID); for (const auto &nestedModel : modelInfo->nestedModels) { auto nestedModelIdentity = this->GenerateIdentity(nestedModel); const FreeGroupInfo &nestedInfo = GetCanonicalInfo(nestedModelIdentity); // If nestedInfo.link is a nullptr, we skip this model because means the // BodyNodes in this skeleton have been moved to another skeleton and their // pose update will be handled there. if (nullptr != nestedInfo.link && nestedInfo.link != info.link) { this->SetFreeGroupWorldPose(nestedModelIdentity, tfChange * nestedInfo.link->getTransform()); } } } ///////////////////////////////////////////////// void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( const Identity &_groupID, const LinearVelocity &_linearVelocity) { const FreeGroupInfo &info = GetCanonicalInfo(_groupID); if (!info.model) { static_cast(info.link->getParentJoint()) ->setLinearVelocity(_linearVelocity); return; } const Eigen::Vector3d delta_v = _linearVelocity - info.link->getLinearVelocity(); for (std::size_t i = 0; i < info.model->getNumTrees(); ++i) { auto *bn = info.model->getRootBodyNode(i); const Eigen::Vector3d new_v = bn->getLinearVelocity() + delta_v; static_cast(bn->getParentJoint()) ->setLinearVelocity(new_v); } } ///////////////////////////////////////////////// void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( const Identity &_groupID, const AngularVelocity &_angularVelocity) { const FreeGroupInfo &info = GetCanonicalInfo(_groupID); if (!info.model) { static_cast(info.link->getParentJoint()) ->setAngularVelocity(_angularVelocity); return; } const Eigen::Vector3d delta_w = _angularVelocity - info.link->getAngularVelocity(); const Eigen::Vector3d origin = info.link->getTransform().translation(); for (std::size_t i = 0; i < info.model->getNumTrees(); ++i) { auto *bn = info.model->getRootBodyNode(i); const Eigen::Vector3d r = bn->getTransform().translation() - origin; const Eigen::Vector3d v = bn->getLinearVelocity(); const Eigen::Vector3d w = bn->getAngularVelocity(); dart::dynamics::FreeJoint *fj = static_cast(bn->getParentJoint()); fj->setLinearVelocity(v + delta_w.cross(r)); fj->setAngularVelocity(w + delta_w); } } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/CustomMeshShape.hh0000664000175000017500000000242514143524174025176 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_ #include #include namespace ignition { namespace physics { namespace dartsim { /// \brief This class creates a custom derivative of dartsim's MeshShape class /// which allows an ignition::common::Mesh to be converted into a MeshShape that /// can be used by dartsim. class CustomMeshShape : public dart::dynamics::MeshShape { public: CustomMeshShape( const ignition::common::Mesh &_input, const Eigen::Vector3d &_scale); }; } } } #endif // IGNITION_PHYSICS_DARTSIM_SRC_CUSTOMMESHSHAPE_HH_ ign-physics-ignition-physics5_5.1.0/dartsim/src/JointFeatures.hh0000664000175000017500000001366314143524174024716 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_SRC_JOINTFEATURES_HH_ #define IGNITION_PHYSICS_DARTSIM_SRC_JOINTFEATURES_HH_ #include #include #include #include #include #include #include "Base.hh" namespace ignition { namespace physics { namespace dartsim { struct JointFeatureList : FeatureList< GetBasicJointState, SetBasicJointState, GetBasicJointProperties, SetJointTransformFromParentFeature, SetJointTransformToChildFeature, DetachJointFeature, SetFreeJointRelativeTransformFeature, AttachFixedJointFeature, SetRevoluteJointProperties, GetRevoluteJointProperties, AttachRevoluteJointFeature, SetPrismaticJointProperties, GetPrismaticJointProperties, AttachPrismaticJointFeature, SetJointVelocityCommandFeature, SetJointPositionLimitsFeature, SetJointVelocityLimitsFeature, SetJointEffortLimitsFeature, GetJointTransmittedWrench > { }; class JointFeatures : public virtual Base, public virtual Implements3d { // ----- Get Basic Joint State ----- public: double GetJointPosition( const Identity &_id, const std::size_t _dof) const override; public: double GetJointVelocity( const Identity &_id, const std::size_t _dof) const override; public: double GetJointAcceleration( const Identity &_id, const std::size_t _dof) const override; public: double GetJointForce( const Identity &_id, const std::size_t _dof) const override; public: Pose3d GetJointTransform(const Identity &_id) const override; // ----- Set Basic Joint State ----- public: void SetJointPosition( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointVelocity( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointAcceleration( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointForce( const Identity &_id, const std::size_t _dof, const double _value) override; // ----- Get Basic Joint Properties ----- public: std::size_t GetJointDegreesOfFreedom( const Identity &_id) const override; public: Pose3d GetJointTransformFromParent( const Identity &_id) const override; public: Pose3d GetJointTransformToChild( const Identity &_id) const override; // ----- Set Basic Joint Properties ----- public: void SetJointTransformFromParent( const Identity &_id, const Pose3d &_pose) override; public: void SetJointTransformToChild( const Identity &_id, const Pose3d &_pose) override; // ----- Detach Joint ----- public: void DetachJoint(const Identity &_jointId) override; // ----- Fixed Joint ----- public: Identity CastToFixedJoint( const Identity &_jointID) const override; public: Identity AttachFixedJoint( const Identity &_childID, const BaseLink3dPtr &_parent, const std::string &_name) override; // ----- Free Joint ----- public: Identity CastToFreeJoint( const Identity &_jointID) const override; public: void SetFreeJointRelativeTransform( const Identity &_jointID, const Pose3d &_pose) override; // ----- Revolute Joint ----- public: Identity CastToRevoluteJoint( const Identity &_jointID) const override; public: AngularVector3d GetRevoluteJointAxis( const Identity &_jointID) const override; public: void SetRevoluteJointAxis( const Identity &_jointID, const AngularVector3d &_axis) override; public: Identity AttachRevoluteJoint( const Identity &_childID, const BaseLink3dPtr &_parent, const std::string &_name, const AngularVector3d &_axis) override; // ----- Prismatic Joint ----- public: Identity CastToPrismaticJoint( const Identity &_jointID) const override; public: LinearVector3d GetPrismaticJointAxis( const Identity &_jointID) const override; public: void SetPrismaticJointAxis( const Identity &_jointID, const LinearVector3d &_axis) override; public: Identity AttachPrismaticJoint( const Identity &_childID, const BaseLink3dPtr &_parent, const std::string &_name, const LinearVector3d &_axis) override; // ----- Joint Commands ----- public: void SetJointVelocityCommand( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointMinPosition( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointMaxPosition( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointMinVelocity( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointMaxVelocity( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointMinEffort( const Identity &_id, const std::size_t _dof, const double _value) override; public: void SetJointMaxEffort( const Identity &_id, const std::size_t _dof, const double _value) override; // ----- Transmitted wrench ----- public: Wrench3d GetJointTransmittedWrenchInJointFrame( const Identity &_id) const override; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/EntityManagementFeatures.cc0000664000175000017500000006710114143524174027066 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include "EntityManagementFeatures.hh" #include #include #include #include #include #include #include #include #include namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// /// This class filters collision based on a bitmask: /// Each objects has a bitmask. If the bitwise-and of two objects' bitmasks /// evaluates to 0, then collisions between them are ignored. class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter { public: using DartCollisionConstPtr = const dart::collision::CollisionObject*; public: using DartShapeConstPtr = const dart::dynamics::ShapeNode*; private: std::unordered_map bitmaskMap; public: bool ignoresCollision( DartCollisionConstPtr _object1, DartCollisionConstPtr _object2) const override { auto shapeNode1 = _object1->getShapeFrame()->asShapeNode(); auto shapeNode2 = _object2->getShapeFrame()->asShapeNode(); if (dart::collision::BodyNodeCollisionFilter::ignoresCollision( _object1, _object2)) return true; auto shape1Iter = bitmaskMap.find(shapeNode1); auto shape2Iter = bitmaskMap.find(shapeNode2); if (shape1Iter != bitmaskMap.end() && shape2Iter != bitmaskMap.end() && ((shape1Iter->second & shape2Iter->second) == 0)) return true; return false; } public: void SetIgnoredCollision(DartShapeConstPtr _shapePtr, const uint16_t _mask) { bitmaskMap[_shapePtr] = _mask; } public: uint16_t GetIgnoredCollision(DartShapeConstPtr _shapePtr) const { auto shapeIter = bitmaskMap.find(_shapePtr); if (shapeIter != bitmaskMap.end()) return shapeIter->second; return 0xff; } public: void RemoveIgnoredCollision(DartShapeConstPtr _shapePtr) { auto shapeIter = bitmaskMap.find(_shapePtr); if (shapeIter != bitmaskMap.end()) bitmaskMap.erase(shapeIter); } public: void RemoveSkeletonCollisions(dart::dynamics::SkeletonPtr _skelPtr) { for (std::size_t i = 0; i < _skelPtr->getNumShapeNodes(); ++i) { auto shapePtr = _skelPtr->getShapeNode(i); this->RemoveIgnoredCollision(shapePtr); } } public: virtual ~BitmaskContactFilter() = default; }; /// Utility functions ///////////////////////////////////////////////// /// TODO (addisu): There's a lot of code duplication in model removal code, such /// as RemoveModelByIndex, where we call GetFilterPtr followed by /// RemoveSkeletonCollisions(model). To de-duplicate this, move this logic into /// RemoveModelImpl. To do that, we need to move GetFilterPtr into Base.hh. static const std::shared_ptr GetFilterPtr( const EntityManagementFeatures* _emf, std::size_t _worldID) { const auto world = _emf->worlds.at(_worldID); // We need to cast the base class pointer to the derived class const auto filterPtr = std::static_pointer_cast( world->getConstraintSolver()->getCollisionOption() .collisionFilter); return filterPtr; } ///////////////////////////////////////////////// static std::size_t GetWorldOfShapeNode(const EntityManagementFeatures *_emf, const dart::dynamics::ShapeNode* _shapeNode) { // Get the body of the shape node const auto bn = _shapeNode->getBodyNodePtr(); // Get the body node's skeleton const auto skelPtr = bn->getSkeleton(); // Now find the skeleton's model const std::size_t modelID = _emf->models.objectToID.at(skelPtr); // And the world containing the model return _emf->GetWorldOfModelImpl(modelID); } ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetEngineName( const Identity &/*_engineID*/) const { static const std::string engineName = "dartsim-" DART_VERSION; return engineName; } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetEngineIndex( const Identity &/*_engineID*/) const { // The dartsim plugin does not make a distinction between different engine // indexes. return 0; } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetWorldCount( const Identity &/*_engineID*/) const { return worlds.size(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetWorld( const Identity &, std::size_t _worldIndex) const { const std::size_t id = this->worlds.indexInContainerToID.begin()->second[_worldIndex]; return this->GenerateIdentity(id, this->worlds.idToObject.at(id)); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetWorld( const Identity &, const std::string &_worldName) const { const std::size_t id = this->worlds.IdentityOf(_worldName); return this->GenerateIdentity(id, this->worlds.idToObject.at(id)); } ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetWorldName( const Identity &_worldID) const { return this->ReferenceInterface(_worldID)->getName(); } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetWorldIndex( const Identity &_worldID) const { // TODO(anyone) this will throw if the world has been removed return this->worlds.idToIndexInContainer.at(_worldID); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetEngineOfWorld( const Identity &/*_worldID*/) const { return this->GenerateIdentity(0); } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetModelCount( const Identity &_worldID) const { // dart::simulation::World::getNumSkeletons returns all the skeletons in the // world, including nested ones. We use the size of the indexInContainerToID // vector associated with the _worldID to determine the number of models // that are direct children of the world. return this->models.indexInContainerToID.at(_worldID).size(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetModel( const Identity &_worldID, const std::size_t _modelIndex) const { const auto &indexInContainerToID = this->models.indexInContainerToID.at(_worldID); if (_modelIndex >= indexInContainerToID.size()) { return this->GenerateInvalidId(); } const std::size_t modelID = indexInContainerToID[_modelIndex]; // If the model doesn't exist in "models", it means the containing entity has // been removed. if (this->models.HasEntity(modelID)) { return this->GenerateIdentity(modelID, this->models.at(modelID)); } else { return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetModel( const Identity &_worldID, const std::string &_modelName) const { const DartSkeletonPtr &model = this->ReferenceInterface(_worldID)->getSkeleton(_modelName); // If the model doesn't exist in "models", it means the containing entity has // been removed. if (this->models.HasEntity(model)) { const std::size_t modelID = this->models.IdentityOf(model); return this->GenerateIdentity(modelID, this->models.at(modelID)); } else { return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetModelName( const Identity &_modelID) const { return this->ReferenceInterface(_modelID)->localName; } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetModelIndex( const Identity &_modelID) const { // TODO(anyone) this will throw if the model has been removed. The alternative // is to first check if the model exists, but what should we return if it // doesn't exist return this->models.idToIndexInContainer.at(_modelID); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetWorldOfModel( const Identity &_modelID) const { auto worldID = this->GetWorldOfModelImpl(_modelID); if (worldID != INVALID_ENTITY_ID ) { return this->GenerateIdentity(worldID, this->worlds.at(worldID)); } return this->GenerateInvalidId(); } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetNestedModelCount( const Identity &_modelID) const { return this->ReferenceInterface(_modelID)->nestedModels.size(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetNestedModel( const Identity &_modelID, const std::size_t _modelIndex) const { const auto modelInfo = this->ReferenceInterface(_modelID); if (_modelIndex >= modelInfo->nestedModels.size()) { return this->GenerateInvalidId(); } const auto nestedModelID = modelInfo->nestedModels[_modelIndex]; // If the model doesn't exist in "models", it means the containing entity has // been removed. if (this->models.HasEntity(nestedModelID)) { return this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID)); } else { return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetNestedModel( const Identity &_modelID, const std::string &_modelName) const { const auto modelInfo = this->ReferenceInterface(_modelID); const std::string fullName = ::sdf::JoinName(modelInfo->model->getName(), _modelName); if (this->models.HasEntity(_modelID)) { auto worldID = this->GetWorldOfModelImpl(_modelID); auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); if (nullptr == nestedSkel) { return this->GenerateInvalidId(); } const std::size_t nestedModelID = this->models.IdentityOf(nestedSkel); return this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID)); } else { return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetLinkCount( const Identity &_modelID) const { return this->ReferenceInterface(_modelID) ->links.size(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetLink( const Identity &_modelID, const std::size_t _linkIndex) const { auto modelInfo = this->ReferenceInterface(_modelID); if (_linkIndex >= modelInfo->links.size()) return this->GenerateInvalidId(); const auto &linkInfo = modelInfo->links[_linkIndex]; // If the link doesn't exist in "links", it means the containing entity has // been removed. if (this->links.HasEntity(linkInfo->link)) { const std::size_t linkID = this->links.IdentityOf(linkInfo->link); return this->GenerateIdentity(linkID, this->links.at(linkID)); } else { // TODO(addisu) It's not clear what to do when `GetLink` is called on a // model that has been removed. Right now we are returning an invalid // identity, but that could cause a segfault if the use doesn't check if // returned value before using it. return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetLink( const Identity &_modelID, const std::string &_linkName) const { const auto &modelInfo = this->ReferenceInterface(_modelID); for (const auto &linkInfo : modelInfo->links) { if (_linkName == linkInfo->name) { // If the link doesn't exist in "links", it means the containing entity // has been removed. if (this->links.HasEntity(linkInfo->link)) { const std::size_t linkID = this->links.IdentityOf(linkInfo->link); return this->GenerateIdentity(linkID, this->links.at(linkID)); } else { // TODO(addisu) It's not clear what to do when `GetLink` is called on a // model that has been removed. Right now we are returning an invalid // identity, but that could cause a segfault if the user doesn't check // the returned value before using it. return this->GenerateInvalidId(); } } } return this->GenerateInvalidId(); } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetJointCount( const Identity &_modelID) const { return this->ReferenceInterface(_modelID)->model->getNumJoints(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetJoint( const Identity &_modelID, const std::size_t _jointIndex) const { DartJoint *const joint = this->ReferenceInterface(_modelID)->model->getJoint( _jointIndex); // If the joint doesn't exist in "joints", it means the containing entity has // been removed. if (this->joints.HasEntity(joint)) { const std::size_t jointID = this->joints.IdentityOf(joint); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } else { // TODO(addisu) It's not clear what to do when `GetJoint` is called on a // model that has been removed. Right now we are returning an invalid // identity, but that could cause a segfault if the use doesn't check if // returned value before using it. return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetJoint( const Identity &_modelID, const std::string &_jointName) const { DartJoint *const joint = this->ReferenceInterface(_modelID)->model->getJoint( _jointName); // If the joint doesn't exist in "joints", it means the containing entity has // been removed. if (this->joints.HasEntity(joint)) { const std::size_t jointID = this->joints.IdentityOf(joint); return this->GenerateIdentity(jointID, this->joints.at(jointID)); } else { // TODO(addisu) It's not clear what to do when `GetJoint` is called on a // model that has been removed. Right now we are returning an invalid // identity, but that could cause a segfault if the use doesn't check if // returned value before using it. return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetLinkName( const Identity &_linkID) const { return this->ReferenceInterface(_linkID)->name; } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetLinkIndex( const Identity &_linkID) const { return this->links.idToIndexInContainer.at(_linkID); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetModelOfLink( const Identity &_linkID) const { const std::size_t modelID = this->links.idToContainerID.at(_linkID); // If the model containing the link doesn't exist in "models", it means this // link belongs to a removed model. if (this->models.HasEntity(modelID)) { return this->GenerateIdentity(modelID, this->models.at(modelID)); } else { return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetShapeCount( const Identity &_linkID) const { return this->ReferenceInterface(_linkID)->link->getNumShapeNodes(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetShape( const Identity &_linkID, const std::size_t _shapeIndex) const { DartShapeNode *const sn = this->ReferenceInterface(_linkID)->link->getShapeNode( _shapeIndex); // If the shape doesn't exist in "shapes", it means the containing entity has // been removed. if (this->shapes.HasEntity(sn)) { const std::size_t shapeID = this->shapes.IdentityOf(sn); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } else { // TODO(addisu) It's not clear what to do when `GetShape` is called on a // link that has been removed. Right now we are returning an invalid // identity, but that could cause a segfault if the use doesn't check if // returned value before using it. return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetShape( const Identity &_linkID, const std::string &_shapeName) const { auto bn = this->ReferenceInterface(_linkID)->link; DartShapeNode *const sn = bn->getSkeleton()->getShapeNode( bn->getName() + ":" + _shapeName); // If the shape doesn't exist in "shapes", it means the containing entity has // been removed. if (this->shapes.HasEntity(sn)) { const std::size_t shapeID = this->shapes.IdentityOf(sn); return this->GenerateIdentity(shapeID, this->shapes.at(shapeID)); } else { // TODO(addisu) It's not clear what to do when `GetShape` is called on a // link that has been removed. Right now we are returning an invalid // identity, but that could cause a segfault if the use doesn't check if // returned value before using it. return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetJointName( const Identity &_jointID) const { return this->ReferenceInterface(_jointID)->joint->getName(); } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetJointIndex( const Identity &_jointID) const { return this->ReferenceInterface(_jointID) ->joint->getJointIndexInSkeleton(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetModelOfJoint( const Identity &_jointID) const { const DartSkeletonPtr &model = this->ReferenceInterface(_jointID)->joint->getSkeleton(); // If the model containing the joint doesn't exist in "models", it means this // joint belongs to a removed model. if (this->models.HasEntity(model)) { const std::size_t modelID = this->models.IdentityOf(model); return this->GenerateIdentity(modelID, this->models.at(modelID)); } else { return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetShapeName( const Identity &_shapeID) const { const auto shapeInfo = this->ReferenceInterface(_shapeID); return shapeInfo->name; } ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetShapeIndex( const Identity &_shapeID) const { const auto shapeInfo = this->ReferenceInterface(_shapeID); return shapeInfo->node->getIndexInBodyNode(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetLinkOfShape( const Identity &_shapeID) const { auto shapeInfo = this->ReferenceInterface(_shapeID); DartBodyNode *const bn = shapeInfo->node->getBodyNodePtr(); // If the link containing the shape doesn't exist in "links", it means this // shape belongs to a removed link. if (this->links.HasEntity(bn)) { const std::size_t linkID = this->links.IdentityOf(bn); return this->GenerateIdentity(linkID, this->links.at(linkID)); } else { return this->GenerateInvalidId(); } } ///////////////////////////////////////////////// bool EntityManagementFeatures::RemoveModelByIndex(const Identity &_worldID, std::size_t _modelIndex) { auto *const world = this->ReferenceInterface(_worldID); const DartSkeletonPtr &model = world->getSkeleton(_modelIndex); if (model != nullptr && this->models.HasEntity(model)) { auto filterPtr = GetFilterPtr(this, _worldID); filterPtr->RemoveSkeletonCollisions(model); return this->RemoveModelImpl(_worldID, this->models.IdentityOf(model)); } return false; } ///////////////////////////////////////////////// bool EntityManagementFeatures::RemoveModelByName(const Identity &_worldID, const std::string &_modelName) { auto *const world = this->ReferenceInterface(_worldID); const DartSkeletonPtr &model = world->getSkeleton(_modelName); if (model != nullptr && this->models.HasEntity(model)) { auto filterPtr = GetFilterPtr(this, _worldID); filterPtr->RemoveSkeletonCollisions(model); return this->RemoveModelImpl(_worldID, this->models.IdentityOf(model)); } return false; } ///////////////////////////////////////////////// bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { if (this->models.HasEntity(_modelID)) { auto worldID = this->GetWorldOfModelImpl(_modelID); auto model = this->models.at(_modelID)->model; auto filterPtr = GetFilterPtr(this, worldID); filterPtr->RemoveSkeletonCollisions(model); return this->RemoveModelImpl(worldID, _modelID); } return false; } ///////////////////////////////////////////////// bool EntityManagementFeatures::ModelRemoved(const Identity &_modelID) const { return !this->models.HasEntity(_modelID); } ///////////////////////////////////////////////// bool EntityManagementFeatures::RemoveNestedModelByIndex( const Identity &_modelID, std::size_t _nestedModelIndex) { auto modelInfo = this->ReferenceInterface(_modelID); if (_nestedModelIndex >= modelInfo->nestedModels.size()) { return this->GenerateInvalidId(); } const auto nestedModelID = modelInfo->nestedModels[_nestedModelIndex]; if (this->models.HasEntity(nestedModelID)) { const auto worldID = this->GetWorldOfModelImpl(nestedModelID); const auto model = this->models.at(nestedModelID)->model; const auto filterPtr = GetFilterPtr(this, worldID); filterPtr->RemoveSkeletonCollisions(model); return this->RemoveModelImpl(worldID, nestedModelID); } return false; } ///////////////////////////////////////////////// bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID, const std::string &_modelName) { auto modelInfo = this->ReferenceInterface(_modelID); const std::string fullName = ::sdf::JoinName(modelInfo->model->getName(), _modelName); if (this->models.HasEntity(_modelID)) { auto worldID = this->GetWorldOfModelImpl(_modelID); auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); if (nullptr == nestedSkel || !this->models.HasEntity(nestedSkel)) { return false; } const std::size_t nestedModelID = this->models.IdentityOf(nestedSkel); const auto filterPtr = GetFilterPtr(this, worldID); filterPtr->RemoveSkeletonCollisions(nestedSkel); return this->RemoveModelImpl(worldID, nestedModelID); } return false; } ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyWorld( const Identity &/*_engineID*/, const std::string &_name) { const auto &world = std::make_shared(_name); world->getConstraintSolver()->setCollisionDetector( dart::collision::OdeCollisionDetector::create()); // TODO(anyone) We need a machanism to configure maxNumContacts at runtime. auto &collOpt = world->getConstraintSolver()->getCollisionOption(); collOpt.maxNumContacts = 10000; world->getConstraintSolver()->getCollisionOption().collisionFilter = std::make_shared(); const std::size_t worldID = this->AddWorld(world, _name); return this->GenerateIdentity(worldID, this->worlds.at(worldID)); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyModel( const Identity &_worldID, const std::string &_name) { dart::dynamics::SkeletonPtr model = dart::dynamics::Skeleton::create(_name); dart::dynamics::SimpleFramePtr modelFrame = dart::dynamics::SimpleFrame::createShared( dart::dynamics::Frame::World(), _name + "_frame"); const auto [modelID, modelInfo] = this->AddModel({model, _name, modelFrame, ""}, _worldID); // NOLINT return this->GenerateIdentity(modelID, this->models.at(modelID)); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyNestedModel( const Identity &_parentModelID, const std::string &_name) { // find the world assocated with the model auto worldID = this->GetWorldOfModelImpl(_parentModelID); const auto &skel = this->models.at(_parentModelID)->model; const std::string modelFullName = ::sdf::JoinName(skel->getName(), _name); dart::dynamics::SkeletonPtr model = dart::dynamics::Skeleton::create(modelFullName); dart::dynamics::SimpleFramePtr modelFrame = dart::dynamics::SimpleFrame::createShared( dart::dynamics::Frame::World(), modelFullName + "_frame"); auto [modelID, modelInfo] = this->AddNestedModel( {model, _name, modelFrame, ""}, _parentModelID, worldID); // NOLINT return this->GenerateIdentity(modelID, this->models.at(modelID)); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyLink( const Identity &_modelID, const std::string &_name) { auto model = this->ReferenceInterface(_modelID)->model; dart::dynamics::FreeJoint::Properties prop_fj; prop_fj.mName = _name + "_FreeJoint"; DartBodyNode::Properties prop_bn; prop_bn.mName = _name; DartBodyNode *bn = model->createJointAndBodyNodePair( nullptr, prop_fj, prop_bn).second; auto worldID = this->GetWorldOfModelImpl(_modelID); if (worldID == INVALID_ENTITY_ID) { ignerr << "World of model [" << model->getName() << "] could not be found when creating link [" << _name << "]\n"; return this->GenerateInvalidId(); } auto world = this->worlds.at(worldID); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(model->getName(), bn->getName())); const std::size_t linkID = this->AddLink(bn, fullName, _modelID); return this->GenerateIdentity(linkID, this->links.at(linkID)); } void EntityManagementFeatures::SetCollisionFilterMask( const Identity &_shapeID, const uint16_t _mask) { const auto shapeNode = this->ReferenceInterface(_shapeID)->node; const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); const auto filterPtr = GetFilterPtr(this, worldID); filterPtr->SetIgnoredCollision(shapeNode, _mask); } uint16_t EntityManagementFeatures::GetCollisionFilterMask( const Identity &_shapeID) const { const auto shapeNode = this->ReferenceInterface(_shapeID)->node; const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); const auto filterPtr = GetFilterPtr(this, worldID); return filterPtr->GetIgnoredCollision(shapeNode); } void EntityManagementFeatures::RemoveCollisionFilterMask( const Identity &_shapeID) { const auto shapeNode = this->ReferenceInterface(_shapeID)->node; const std::size_t worldID = GetWorldOfShapeNode(this, shapeNode); const auto filterPtr = GetFilterPtr(this, worldID); filterPtr->RemoveIgnoredCollision(shapeNode); } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/KinematicsFeatures_TEST.cc0000664000175000017500000000775314143524174026552 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 3.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include // Features #include #include #include #include #include #include #include #include #include #include #include "test/Utils.hh" using namespace ignition; using TestFeatureList = ignition::physics::FeatureList< physics::ForwardStep, physics::GetEntities, physics::JointFrameSemantics, physics::LinkFrameSemantics, physics::sdf::ConstructSdfModel, physics::sdf::ConstructSdfWorld >; using TestEnginePtr = physics::Engine3dPtr; class KinematicsFeaturesFixture : public ::testing::Test { protected: void SetUp() override { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); this->engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, this->engine); } protected: TestEnginePtr engine; }; // Test joint frame semantics TEST_F(KinematicsFeaturesFixture, JointFrameSemantics) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "string_pendulum.sdf"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); ASSERT_NE(nullptr, world); auto model = world->GetModel("pendulum"); ASSERT_NE(nullptr, model); auto pivotJoint = model->GetJoint("pivot"); ASSERT_NE(nullptr, pivotJoint); auto childLink = model->GetLink("bob"); ASSERT_NE(nullptr, childLink); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; for (std::size_t i = 0; i < 100; ++i) { world->Step(output, state, input); } // Pose of Child link (C) in Joint frame (J) physics::Pose3d X_JC = physics::Pose3d::Identity(); X_JC.translate(physics::Vector3d(0, 0, -1)); // Notation: Using F_WJ for the frame data of frame J (joint) relative to // frame W (world). auto F_WJ = pivotJoint->FrameDataRelativeToWorld(); physics::FrameData3d F_WCexpected = F_WJ; physics::Vector3d pendulumArmInWorld = F_WJ.pose.rotation() * X_JC.translation(); F_WCexpected.pose = F_WJ.pose * X_JC; // angular acceleration of the child link is the same as the joint, so we // don't need to assign a new value. // Note that the joint's linear velocity and linear acceleration are zero, so // they are omitted here. F_WCexpected.linearAcceleration = F_WJ.angularAcceleration.cross(pendulumArmInWorld) + F_WJ.angularVelocity.cross( F_WJ.angularVelocity.cross(pendulumArmInWorld)); F_WCexpected.linearVelocity = F_WJ.angularVelocity.cross(pendulumArmInWorld); auto childLinkFrameData = childLink->FrameDataRelativeToWorld(); EXPECT_TRUE( physics::test::Equal(F_WCexpected, childLinkFrameData, 1e-6)); } ///////////////////////////////////////////////// int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/Base.hh0000664000175000017500000003606514143524174023007 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DARTSIM_BASE_HH_ #define IGNITION_PHYSICS_DARTSIM_BASE_HH_ #include #include #include #include #include #include #include #include #include #include #include #include #include namespace ignition { namespace physics { namespace dartsim { /// \brief The structs ModelInfo, LinkInfo, JointInfo, and ShapeInfo are used /// for two reasons: /// 1) Holding extra information such as the name or offset /// that will be different from the underlying engine /// 2) Wrap shared pointers to DART entities. Since these shared pointers (eg. /// dart::dynamics::BodyNodePtr) are different from std::shared_ptr, we /// cannot use them directly as parameters to GenerateIdentity. Instead we /// create a std::shared_ptr of the struct that wraps the corresponding DART /// shared pointer. struct LinkInfo { dart::dynamics::BodyNodePtr link; /// \brief It may be necessary for dartsim to rename a BodyNode (eg. when /// moving the BodyNode to a new skeleton), so we store the Gazebo-specified /// name of the Link here. std::string name; }; struct ModelInfo { dart::dynamics::SkeletonPtr model; std::string localName; dart::dynamics::SimpleFramePtr frame; std::string canonicalLinkName; std::vector> links {}; std::vector nestedModels = {}; }; struct JointInfo { dart::dynamics::JointPtr joint; dart::dynamics::SimpleFramePtr frame; }; struct ShapeInfo { dart::dynamics::ShapeNodePtr node; /// \brief dartsim has more strict name uniqueness rules than Gazebo, so we /// store the Gazebo-specified name of the Shape here. std::string name; /// \brief This is added because sometimes the relative transform of a shape /// according to Gazebo specifications may be different than the relative /// transform of a shape within the dartsim specifications. This is the offset /// from the Gazebo specs to the dartsim specs, i.e. T_g * tf_offset = T_d /// where T_g is the relative transform according to Gazebo and T_d is the /// relative transform according to dartsim. Eigen::Isometry3d tf_offset = Eigen::Isometry3d::Identity(); }; template struct EntityStorage { /// \brief Map from an entity ID to its corresponding object std::unordered_map idToObject; /// \brief Map from an object pointer (or other unique key) to its entity ID std::unordered_map objectToID; using IndexMap = std::unordered_map>; /// \brief The key represents the parent ID. The value represents a vector of /// the objects' IDs. The key of the vector is the object's index within its /// container. This is used by World and Model objects, which don't know their /// own indices within their containers as well as Links, whose indices might /// change when constructing joints. /// /// The container type for World is Engine. /// The container type for Model is World. /// The container type for Link is Model. /// /// Joints are contained in Models, but they know their own indices within /// their Models, so we do not need to use this field for Joints IndexMap indexInContainerToID; /// \brief Map from an entity ID to its index within its container std::unordered_map idToIndexInContainer; /// \brief Map from an entity ID to the ID of its container std::unordered_map idToContainerID; Value1 &operator[](const std::size_t _id) { return idToObject[_id]; } Value1 &at(const std::size_t _id) { return idToObject.at(_id); } const Value1 &at(const std::size_t _id) const { return idToObject.at(_id); } Value1 &at(const Key2 &_key) { return idToObject.at(objectToID.at(_key)); } const Value1 &at(const Key2 &_key) const { return idToObject.at(objectToID.at(_key)); } std::size_t size() const { return idToObject.size(); } std::size_t IdentityOf(const Key2 &_key) const { return objectToID.at(_key); } bool HasEntity(const Key2 &_key) const { return objectToID.find(_key) != objectToID.end(); } bool HasEntity(const std::size_t _id) const { return idToObject.find(_id) != idToObject.end(); } bool RemoveEntity(const Key2 &_key) { auto entIter = this->objectToID.find(_key); if (entIter!= this->objectToID.end()) { std::size_t entId = entIter->second; // Check if we are keeping track of the index of this entity in its // container auto contIter = this->idToContainerID.find(entId); if (contIter != this->idToContainerID.end()) { std::size_t contId = contIter->second; std::size_t entIndex = this->idToIndexInContainer.at(entId); // house keeping // The key in indexInContainerToID is the index of the vector so erasing // the element automatically decrements the index of the rest of the // elements of the vector. The indices in idToIndexInContainer, however, // are stored as numbers (as values in the map). We need to decrement // all the indices greater than the index of the model we are removing. for (auto indIter = this->indexInContainerToID[contId].begin() + entIndex + 1; indIter != this->indexInContainerToID[contId].end(); ++indIter) { // decrement the index (the value of the map) --this->idToIndexInContainer[*indIter]; } this->idToIndexInContainer.erase(entId); this->indexInContainerToID[contId].erase( this->indexInContainerToID[contId].begin() + entIndex); this->idToContainerID.erase(entId); } this->objectToID.erase(entIter); this->idToObject.erase(entId); return true; } return false; } }; class Base : public Implements3d> { public: using DartWorld = dart::simulation::World; public: using DartWorldPtr = dart::simulation::WorldPtr; public: using DartSkeletonPtr = dart::dynamics::SkeletonPtr; public: using DartConstSkeletonPtr = dart::dynamics::ConstSkeletonPtr; public: using DartSkeleton = dart::dynamics::Skeleton; public: using DartBodyNode = dart::dynamics::BodyNode; public: using DartBodyNodePtr = dart::dynamics::BodyNodePtr; public: using DartJoint = dart::dynamics::Joint; public: using DartJointPtr = dart::dynamics::JointPtr; public: using DartShapeNode = dart::dynamics::ShapeNode; public: using DartShapeNodePtr = std::shared_ptr; public: using ModelInfoPtr = std::shared_ptr; public: using LinkInfoPtr = std::shared_ptr; public: using JointInfoPtr = std::shared_ptr; public: using ShapeInfoPtr = std::shared_ptr; public: inline Identity InitiateEngine(std::size_t /*_engineID*/) override { this->GetNextEntity(); // Create a 0th entry in this map this->worlds.indexInContainerToID.insert( std::make_pair(0u, std::vector())); // dartsim does not have multiple "engines" return this->GenerateIdentity(0); } public: inline std::size_t GetNextEntity() { return entityCount++; } public: std::size_t entityCount = 0; public: inline std::size_t AddWorld( const DartWorldPtr &_world, const std::string &_name) { const std::size_t id = this->GetNextEntity(); this->worlds.idToObject[id] = _world; this->worlds.objectToID[_name] = id; std::vector &indexInContainerToID = this->worlds.indexInContainerToID.at(0); this->worlds.idToIndexInContainer[id] = indexInContainerToID.size(); indexInContainerToID.push_back(id); this->worlds.idToContainerID[id] = 0; _world->setName(_name); this->frames[id] = dart::dynamics::Frame::World(); return id; } public: inline std::tuple AddModel( const ModelInfo &_info, const std::size_t _worldID) { const std::size_t id = this->GetNextEntity(); this->models.idToObject[id] = std::make_shared(_info); ModelInfo &entry = *this->models.idToObject[id]; this->models.objectToID[_info.model] = id; const dart::simulation::WorldPtr &world = worlds[_worldID]; std::vector &indexInContainerToID = this->models.indexInContainerToID[_worldID]; const std::size_t indexInWorld = indexInContainerToID.size(); this->models.idToIndexInContainer[id] = indexInWorld; indexInContainerToID.push_back(id); world->addSkeleton(entry.model); this->models.idToContainerID[id] = _worldID; this->frames[id] = _info.frame.get(); return std::forward_as_tuple(id, entry); } public: inline std::tuple AddNestedModel( const ModelInfo &_info, const std::size_t _parentID, const std::size_t _worldID) { const std::size_t id = this->GetNextEntity(); this->models.idToObject[id] = std::make_shared(_info); ModelInfo &entry = *this->models.idToObject[id]; this->models.objectToID[_info.model] = id; const dart::simulation::WorldPtr &world = worlds[_worldID]; auto parentModelInfo = this->models.at(_parentID); const std::size_t indexInModel = parentModelInfo->nestedModels.size(); this->models.idToIndexInContainer[id] = indexInModel; std::vector &indexInContainerToID = this->models.indexInContainerToID[_parentID]; indexInContainerToID.push_back(id); world->addSkeleton(entry.model); this->models.idToContainerID[id] = _parentID; this->frames[id] = _info.frame.get(); parentModelInfo->nestedModels.push_back(id); return {id, entry}; } public: inline std::size_t AddLink(DartBodyNode *_bn, const std::string &_fullName, std::size_t _modelID) { const std::size_t id = this->GetNextEntity(); auto linkInfo = std::make_shared(); this->links.idToObject[id] = linkInfo; linkInfo->link = _bn; // The name of the BodyNode during creation is assumed to be the // Gazebo-specified name. linkInfo->name = _bn->getName(); this->links.objectToID[_bn] = id; this->frames[id] = _bn; this->linksByName[_fullName] = _bn; this->models.at(_modelID)->links.push_back(linkInfo); // Even though DART keeps track of the index of this BodyNode in the // skeleton, the BodyNode may be moved to another skeleton when a joint is // constructed. Thus, we store the original index here. this->links.idToIndexInContainer[id] = _bn->getIndexInSkeleton(); std::vector &indexInContainerToID = this->links.indexInContainerToID[_modelID]; indexInContainerToID.push_back(id); this->links.idToContainerID[id] = _modelID; return id; } public: inline std::size_t AddJoint(DartJoint *_joint) { const std::size_t id = this->GetNextEntity(); this->joints.idToObject[id] = std::make_shared(); this->joints.idToObject[id]->joint = _joint; this->joints.objectToID[_joint] = id; dart::dynamics::SimpleFramePtr jointFrame = dart::dynamics::SimpleFrame::createShared( _joint->getChildBodyNode(), _joint->getName() + "_frame", _joint->getTransformFromChildBodyNode()); this->joints.idToObject[id]->frame = jointFrame; this->frames[id] = this->joints.idToObject[id]->frame.get(); return id; } public: inline std::size_t AddShape( const ShapeInfo &_info) { const std::size_t id = this->GetNextEntity(); this->shapes.idToObject[id] = std::make_shared(_info); this->shapes.objectToID[_info.node] = id; this->frames[id] = _info.node.get(); return id; } public: bool RemoveModelImpl(const std::size_t _worldID, const std::size_t _modelID) { const auto &world = this->worlds.at(_worldID); auto modelInfo = this->models.at(_modelID); auto skel = modelInfo->model; // Remove the contents of the skeleton from local entity storage containers for (auto &nestedModel : modelInfo->nestedModels) { this->RemoveModelImpl(_worldID, nestedModel); } modelInfo->nestedModels.clear(); for (auto &jt : skel->getJoints()) { this->joints.RemoveEntity(jt); } for (auto &bn : skel->getBodyNodes()) { for (auto &sn : bn->getShapeNodes()) { this->shapes.RemoveEntity(sn); } this->links.RemoveEntity(bn); this->linksByName.erase(::sdf::JoinName( world->getName(), ::sdf::JoinName(skel->getName(), bn->getName()))); } // If this is a nested model, remove an entry from the parent models // "nestedModels" vector auto parentID = this->models.idToContainerID.at(_modelID); if (parentID != _worldID) { auto parentModelInfo = this->models.at(parentID); const std::size_t modelIndex = this->models.idToIndexInContainer.at(_modelID); if (modelIndex >= parentModelInfo->nestedModels.size()) return false; parentModelInfo->nestedModels.erase( parentModelInfo->nestedModels.begin() + modelIndex); } this->models.RemoveEntity(skel); world->removeSkeleton(skel); return true; } public: inline std::size_t GetWorldOfModelImpl( const std::size_t &_modelID) const { if (this->models.HasEntity(_modelID)) { auto parentIt = this->models.idToContainerID.find(_modelID); if (parentIt != this->models.idToContainerID.end()) { if (this->worlds.HasEntity(parentIt->second)) { return parentIt->second; } return this->GetWorldOfModelImpl(parentIt->second); } } return this->GenerateInvalidId(); } public: EntityStorage worlds; public: EntityStorage models; public: EntityStorage links; public: EntityStorage joints; public: EntityStorage shapes; public: std::unordered_map frames; /// \brief Map from the fully qualified link name (including the world name) /// to the BodyNode object. This is useful for keeping track of BodyNodes even /// as they move to other skeletons. public: std::unordered_map linksByName; }; } } } #endif ign-physics-ignition-physics5_5.1.0/dartsim/src/CustomHeightmapShape.cc0000664000175000017500000000415014143524174026173 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include "CustomHeightmapShape.hh" #include #include #include #include namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// CustomHeightmapShape::CustomHeightmapShape( const common::HeightmapData &_input, const Eigen::Vector3d &_size, int _subSampling) : dart::dynamics::HeightmapShape() { double heightmapSizeZ = _input.MaxElevation(); const bool flipY = false; const int vertSize = (_input.Width() * _subSampling) - _subSampling + 1; math::Vector3d scale; scale.X(_size(0) / vertSize); scale.Y(_size(1) / vertSize); if (math::equal(heightmapSizeZ, 0.0)) scale.Z(1.0); else scale.Z(fabs(_size(2)) / heightmapSizeZ); auto sizeIgn = ignition::math::eigen3::convert(_size); // We need to make a copy here in order to use the non-const FillHeightMap // function common::ImageHeightmap copyData; try { const auto &image = dynamic_cast(_input); copyData.Load(image.Filename()); } catch(const std::bad_cast &) { ignerr << "Only image heightmaps are supported at the moment." << std::endl; return; } std::vector heightsFloat; copyData.FillHeightMap(_subSampling, vertSize, sizeIgn, scale, flipY, heightsFloat); this->setHeightField(vertSize, vertSize, heightsFloat); this->setScale(Vector3(scale.X(), scale.Y(), 1)); } } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/SimulationFeatures.cc0000664000175000017500000002361514143524174025743 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #ifdef DART_HAS_CONTACT_SURFACE #include #endif #include #include #include #include "ignition/physics/GetContacts.hh" #include "SimulationFeatures.hh" namespace ignition { namespace physics { namespace dartsim { void SimulationFeatures::WorldForwardStep( const Identity &_worldID, ForwardStep::Output & _h, ForwardStep::State & /*_x*/, const ForwardStep::Input & _u) { IGN_PROFILE("SimulationFeatures::WorldForwardStep"); auto *world = this->ReferenceInterface(_worldID); auto *dtDur = _u.Query(); const double tol = 1e-6; if (dtDur) { std::chrono::duration dt = *dtDur; if (std::fabs(dt.count() - world->getTimeStep()) > tol) { world->setTimeStep(dt.count()); igndbg << "Simulation timestep set to: " << world->getTimeStep() << std::endl; } } // TODO(MXG): Parse input world->step(); this->Write(_h.Get()); // TODO(MXG): Fill in state } void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const { // remove link poses from the previous iteration _changedPoses.entries.clear(); _changedPoses.entries.reserve(this->links.size()); std::unordered_map newPoses; for (const auto &[id, info] : this->links.idToObject) { // make sure the link exists if (info && info->link) { WorldPose wp; wp.pose = ignition::math::eigen3::convert( info->link->getWorldTransform()); wp.body = id; // If the link's pose is new or has changed, save this new pose and // add it to the output poses. Otherwise, keep the existing link pose auto iter = this->prevLinkPoses.find(id); if ((iter == this->prevLinkPoses.end()) || !iter->second.Pos().Equal(wp.pose.Pos(), 1e-6) || !iter->second.Rot().Equal(wp.pose.Rot(), 1e-6)) { _changedPoses.entries.push_back(wp); newPoses[id] = wp.pose; } else newPoses[id] = iter->second; } } // Save the new poses so that they can be used to check for updates in the // next iteration. Re-setting this->prevLinkPoses with the contents of // newPoses ensures that we aren't caching data for links that were removed this->prevLinkPoses = std::move(newPoses); } std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { std::vector outContacts; auto *const world = this->ReferenceInterface(_worldID); const auto colResult = world->getLastCollisionResult(); for (const auto &dtContact : colResult.getContacts()) { auto contact = this->convertContact(dtContact); if (contact) outContacts.push_back(contact.value()); } return outContacts; } std::optional SimulationFeatures::convertContact( const dart::collision::Contact& _contact) const { auto *dtCollObj1 = _contact.collisionObject1; auto *dtCollObj2 = _contact.collisionObject2; auto *dtShapeFrame1 = dtCollObj1->getShapeFrame(); auto *dtShapeFrame2 = dtCollObj2->getShapeFrame(); if (this->shapes.HasEntity(dtShapeFrame1->asShapeNode()) && this->shapes.HasEntity(dtShapeFrame2->asShapeNode())) { std::size_t shape1ID = this->shapes.IdentityOf(dtShapeFrame1->asShapeNode()); std::size_t shape2ID = this->shapes.IdentityOf(dtShapeFrame2->asShapeNode()); CompositeData extraData; // Add normal, depth and wrench to extraData. auto& extraContactData = extraData.Get(); extraContactData.force = _contact.force; extraContactData.normal = _contact.normal; extraContactData.depth = _contact.penetrationDepth; return SimulationFeatures::ContactInternal { this->GenerateIdentity(shape1ID, this->shapes.at(shape1ID)), this->GenerateIdentity(shape2ID, this->shapes.at(shape2ID)), _contact.point, extraData }; } return std::nullopt; } #ifdef DART_HAS_CONTACT_SURFACE void SimulationFeatures::AddContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID, SurfaceParamsCallback _callback) { auto *world = this->ReferenceInterface(_worldID); auto handler = std::make_shared(); handler->surfaceParamsCallback = _callback; handler->convertContact = [this](const dart::collision::Contact& _contact) { return this->convertContact(_contact); }; this->contactSurfaceHandlers[_callbackID] = handler; world->getConstraintSolver()->addContactSurfaceHandler(handler); } bool SimulationFeatures::RemoveContactPropertiesCallback( const Identity& _worldID, const std::string& _callbackID) { auto *world = this->ReferenceInterface(_worldID); if (this->contactSurfaceHandlers.find(_callbackID) != this->contactSurfaceHandlers.end()) { const auto handler = this->contactSurfaceHandlers[_callbackID]; this->contactSurfaceHandlers.erase(_callbackID); return world->getConstraintSolver()->removeContactSurfaceHandler(handler); } else { ignerr << "Could not find the contact surface handler to be removed" << std::endl; return false; } } dart::constraint::ContactSurfaceParams IgnContactSurfaceHandler::createParams( const dart::collision::Contact& _contact, const size_t _numContactsOnCollisionObject) const { auto pDart = ContactSurfaceHandler::createParams( _contact, _numContactsOnCollisionObject); if (!this->surfaceParamsCallback) return pDart; typedef SetContactPropertiesCallbackFeature F; typedef FeaturePolicy3d P; typename F::ContactSurfaceParams

pIgn; pIgn.frictionCoeff = pDart.mFrictionCoeff; pIgn.secondaryFrictionCoeff = pDart.mSecondaryFrictionCoeff; pIgn.slipCompliance = pDart.mSlipCompliance; pIgn.secondarySlipCompliance = pDart.mSecondarySlipCompliance; pIgn.restitutionCoeff = pDart.mRestitutionCoeff; pIgn.firstFrictionalDirection = pDart.mFirstFrictionalDirection; pIgn.contactSurfaceMotionVelocity = pDart.mContactSurfaceMotionVelocity; auto contactInternal = this->convertContact(_contact); if (contactInternal) { this->surfaceParamsCallback(contactInternal.value(), _numContactsOnCollisionObject, pIgn); if (pIgn.frictionCoeff) pDart.mFrictionCoeff = pIgn.frictionCoeff.value(); if (pIgn.secondaryFrictionCoeff) pDart.mSecondaryFrictionCoeff = pIgn.secondaryFrictionCoeff.value(); if (pIgn.slipCompliance) pDart.mSlipCompliance = pIgn.slipCompliance.value(); if (pIgn.secondarySlipCompliance) pDart.mSecondarySlipCompliance = pIgn.secondarySlipCompliance.value(); if (pIgn.restitutionCoeff) pDart.mRestitutionCoeff = pIgn.restitutionCoeff.value(); if (pIgn.firstFrictionalDirection) pDart.mFirstFrictionalDirection = pIgn.firstFrictionalDirection.value(); if (pIgn.contactSurfaceMotionVelocity) pDart.mContactSurfaceMotionVelocity = pIgn.contactSurfaceMotionVelocity.value(); static bool warnedRollingFrictionCoeff = false; if (!warnedRollingFrictionCoeff && pIgn.rollingFrictionCoeff) { ignwarn << "DART doesn't support rolling friction setting" << std::endl; warnedRollingFrictionCoeff = true; } static bool warnedSecondaryRollingFrictionCoeff = false; if (!warnedSecondaryRollingFrictionCoeff && pIgn.secondaryRollingFrictionCoeff) { ignwarn << "DART doesn't support secondary rolling friction setting" << std::endl; warnedSecondaryRollingFrictionCoeff = true; } static bool warnedTorsionalFrictionCoeff = false; if (!warnedTorsionalFrictionCoeff && pIgn.torsionalFrictionCoeff) { ignwarn << "DART doesn't support torsional friction setting" << std::endl; warnedTorsionalFrictionCoeff = true; } } this->lastIgnParams = pIgn; return pDart; } dart::constraint::ContactConstraintPtr IgnContactSurfaceHandler::createConstraint( dart::collision::Contact& _contact, const size_t _numContactsOnCollisionObject, const double _timeStep) const { // this call sets this->lastIgnParams auto constraint = dart::constraint::ContactSurfaceHandler::createConstraint( _contact, _numContactsOnCollisionObject, _timeStep); typedef SetContactPropertiesCallbackFeature F; typedef FeaturePolicy3d P; typename F::ContactSurfaceParams

& p = this->lastIgnParams; if (this->lastIgnParams.errorReductionParameter) constraint->setErrorReductionParameter(p.errorReductionParameter.value()); if (this->lastIgnParams.maxErrorAllowance) constraint->setErrorAllowance(p.maxErrorAllowance.value()); if (this->lastIgnParams.maxErrorReductionVelocity) constraint->setMaxErrorReductionVelocity( p.maxErrorReductionVelocity.value()); if (this->lastIgnParams.constraintForceMixing) constraint->setConstraintForceMixing(p.constraintForceMixing.value()); return constraint; } #endif } } } ign-physics-ignition-physics5_5.1.0/dartsim/src/JointFeatures_TEST.cc0000664000175000017500000013312514143524174025537 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 3.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include #include // Features #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "ignition/physics/Geometry.hh" #include "test/Utils.hh" using namespace ignition; using TestFeatureList = ignition::physics::FeatureList< physics::dartsim::RetrieveWorld, physics::AttachFixedJointFeature, physics::DetachJointFeature, physics::SetJointTransformFromParentFeature, physics::ForwardStep, physics::FreeJointCast, physics::SetFreeGroupWorldPose, physics::GetBasicJointState, physics::GetEntities, physics::GetJointTransmittedWrench, physics::JointFrameSemantics, physics::LinkFrameSemantics, physics::RevoluteJointCast, physics::SetBasicJointState, physics::SetJointVelocityCommandFeature, physics::sdf::ConstructSdfModel, physics::sdf::ConstructSdfWorld, physics::SetJointPositionLimitsFeature, physics::SetJointVelocityLimitsFeature, physics::SetJointEffortLimitsFeature >; using TestEnginePtr = physics::Engine3dPtr; class JointFeaturesFixture : public ::testing::Test { protected: void SetUp() override { ignition::plugin::Loader loader; loader.LoadLib(dartsim_plugin_LIB); ignition::plugin::PluginPtr dartsim = loader.Instantiate("ignition::physics::dartsim::Plugin"); this->engine = ignition::physics::RequestEngine3d::From(dartsim); ASSERT_NE(nullptr, this->engine); } protected: TestEnginePtr engine; }; // Test setting joint commands and verify that the joint type is set accordingly // and that the commanded velocity is acheived TEST_F(JointFeaturesFixture, JointSetCommand) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; const std::string jointName{"upper_joint"}; auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel(modelName); auto joint = model->GetJoint(jointName); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); const auto *dartBaseLink = skeleton->getBodyNode("base"); ASSERT_NE(nullptr, dartBaseLink); const auto *dartJoint = skeleton->getJoint(jointName); // Default actuatore type EXPECT_EQ(dart::dynamics::Joint::FORCE, dartJoint->getActuatorType()); // Test joint velocity command physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; // Expect negative joint velocity after 1 step without joint command world->Step(output, state, input); EXPECT_LT(joint->GetVelocity(0), 0.0); // Check that invalid velocity commands don't cause collisions to fail for (std::size_t i = 0; i < 1000; ++i) { joint->SetForce(0, std::numeric_limits::quiet_NaN()); // expect the position of the pendulum to stay aabove ground world->Step(output, state, input); EXPECT_NEAR(0.0, dartBaseLink->getWorldTransform().translation().z(), 1e-3); } joint->SetVelocityCommand(0, 1); world->Step(output, state, input); // Setting a velocity command changes the actuator type to SERVO EXPECT_EQ(dart::dynamics::Joint::SERVO, dartJoint->getActuatorType()); const std::size_t numSteps = 10; for (std::size_t i = 0; i < numSteps; ++i) { // Call SetVelocityCommand before each step joint->SetVelocityCommand(0, 1); world->Step(output, state, input); EXPECT_NEAR(1.0, joint->GetVelocity(0), 1e-6); } for (std::size_t i = 0; i < numSteps; ++i) { // expect joint to freeze in subsequent steps without SetVelocityCommand world->Step(output, state, input); EXPECT_NEAR(0.0, joint->GetVelocity(0), 1e-6); } // Check that invalid velocity commands don't cause collisions to fail for (std::size_t i = 0; i < 1000; ++i) { joint->SetVelocityCommand(0, std::numeric_limits::quiet_NaN()); // expect the position of the pendulum to stay aabove ground world->Step(output, state, input); EXPECT_NEAR(0.0, dartBaseLink->getWorldTransform().translation().z(), 1e-3); } } TEST_F(JointFeaturesFixture, JointSetPositionLimitsWithForceControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel("simple_joint_test"); auto joint = model->GetJoint("j1"); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; world->Step(output, state, input); auto pos = joint->GetPosition(0); joint->SetMinPosition(0, pos - 0.1); joint->SetMaxPosition(0, pos + 0.1); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 100); world->Step(output, state, input); } EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-3); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, -100); world->Step(output, state, input); } EXPECT_NEAR(pos - 0.1, joint->GetPosition(0), 1e-3); joint->SetMinPosition(0, pos - 0.5); joint->SetMaxPosition(0, pos + 0.5); for (std::size_t i = 0; i < 300; ++i) { joint->SetForce(0, 100); world->Step(output, state, input); } EXPECT_NEAR(pos + 0.5, joint->GetPosition(0), 1e-2); for (std::size_t i = 0; i < 300; ++i) { joint->SetForce(0, -100); world->Step(output, state, input); } EXPECT_NEAR(pos - 0.5, joint->GetPosition(0), 1e-2); joint->SetMinPosition(0, -math::INF_D); joint->SetMaxPosition(0, math::INF_D); joint->SetPosition(0, pos); for (std::size_t i = 0; i < 300; ++i) { joint->SetForce(0, 100); world->Step(output, state, input); } EXPECT_LT(pos + 0.5, joint->GetPosition(0)); } #if DART_VERSION_AT_LEAST(6, 10, 0) TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithForceControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel("simple_joint_test"); auto joint = model->GetJoint("j1"); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; world->Step(output, state, input); joint->SetMinVelocity(0, -0.25); joint->SetMaxVelocity(0, 0.5); for (std::size_t i = 0; i < 10; ++i) { joint->SetForce(0, 1000); world->Step(output, state, input); } EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 10; ++i) { joint->SetForce(0, -1000); world->Step(output, state, input); } EXPECT_NEAR(-0.25, joint->GetVelocity(0), 1e-6); // set minimum velocity above zero joint->SetMinVelocity(0, 0.25); for (std::size_t i = 0; i < 10; ++i) { joint->SetForce(0, 0); world->Step(output, state, input); } EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 10; ++i) { // make sure the minimum velocity is kept even without velocity commands world->Step(output, state, input); } EXPECT_NEAR(0.25, joint->GetVelocity(0), 1e-6); joint->SetMinVelocity(0, -0.25); joint->SetPosition(0, 0); joint->SetVelocity(0, 0); for (std::size_t i = 0; i < 10; ++i) { joint->SetForce(0, 0); world->Step(output, state, input); } EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); joint->SetMinVelocity(0, -math::INF_D); joint->SetMaxVelocity(0, math::INF_D); for (std::size_t i = 0; i < 10; ++i) { joint->SetForce(0, 1000); world->Step(output, state, input); } EXPECT_LT(0.5, joint->GetVelocity(0)); } TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithForceControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel("simple_joint_test"); auto joint = model->GetJoint("j1"); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; world->Step(output, state, input); auto pos = joint->GetPosition(0); joint->SetMinEffort(0, -1e-6); joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 1); world->Step(output, state, input); } EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, -1); world->Step(output, state, input); } EXPECT_NEAR(pos, joint->GetPosition(0), 1e-3); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); joint->SetMinEffort(0, -80); joint->SetMaxEffort(0, 80); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 1); world->Step(output, state, input); } EXPECT_LT(pos, joint->GetPosition(0)); EXPECT_LT(0, joint->GetVelocity(0)); joint->SetMinEffort(0, -math::INF_D); joint->SetMaxEffort(0, math::INF_D); joint->SetPosition(0, 0); joint->SetVelocity(0, 0); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 1); world->Step(output, state, input); } EXPECT_LT(pos, joint->GetPosition(0)); EXPECT_LT(0, joint->GetVelocity(0)); } TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithForceControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel("simple_joint_test"); auto joint = model->GetJoint("j1"); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; world->Step(output, state, input); auto pos = joint->GetPosition(0); joint->SetMinPosition(0, pos - 0.1); joint->SetMaxPosition(0, pos + 0.1); joint->SetMinVelocity(0, -0.25); joint->SetMaxVelocity(0, 0.5); joint->SetMinEffort(0, -1e-6); joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 100); world->Step(output, state, input); } EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, -100); world->Step(output, state, input); } EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); joint->SetMinEffort(0, -500); joint->SetMaxEffort(0, 1000); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 1000); world->Step(output, state, input); } // 0.05 because we go 0.1 s with max speed 0.5 EXPECT_NEAR(pos + 0.05, joint->GetPosition(0), 1e-2); EXPECT_NEAR(0.5, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 200; ++i) { joint->SetForce(0, 1000); world->Step(output, state, input); } EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); joint->SetPosition(0, pos); EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); joint->SetMinVelocity(0, -1); joint->SetMaxVelocity(0, 1); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 1000); world->Step(output, state, input); } EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); joint->SetPosition(0, pos); EXPECT_NEAR(pos, joint->GetPosition(0), 1e-2); joint->SetMinPosition(0, -1e6); joint->SetMaxPosition(0, 1e6); for (std::size_t i = 0; i < 100; ++i) { joint->SetForce(0, 1000); world->Step(output, state, input); } EXPECT_LT(pos + 0.1, joint->GetPosition(0)); EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); } // TODO(anyone): position limits do not work very well with velocity control // bug https://github.com/dartsim/dart/issues/1583 // resolved in DART 6.11.0 TEST_F(JointFeaturesFixture, DISABLED_JointSetPositionLimitsWithVelocityControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"simple_joint_test"}; const std::string jointName{"j1"}; auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel(modelName); auto joint = model->GetJoint(jointName); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; world->Step(output, state, input); auto pos = joint->GetPosition(0); joint->SetMinPosition(0, pos - 0.1); joint->SetMaxPosition(0, pos + 0.1); for (std::size_t i = 0; i < 1000; ++i) { joint->SetVelocityCommand(0, 1); world->Step(output, state, input); if (i % 500 == 499) { EXPECT_NEAR(pos + 0.1, joint->GetPosition(0), 1e-2); EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); } } } TEST_F(JointFeaturesFixture, JointSetVelocityLimitsWithVelocityControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel("simple_joint_test"); auto joint = model->GetJoint("j1"); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; joint->SetMinVelocity(0, -0.1); joint->SetMaxVelocity(0, 0.1); for (std::size_t i = 0; i < 100; ++i) { joint->SetVelocityCommand(0, 1); world->Step(output, state, input); } EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 10; ++i) { joint->SetVelocityCommand(0, 0.1); world->Step(output, state, input); } EXPECT_NEAR(0.1, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 10; ++i) { joint->SetVelocityCommand(0, -0.025); world->Step(output, state, input); } EXPECT_NEAR(-0.025, joint->GetVelocity(0), 1e-6); for (std::size_t i = 0; i < 10; ++i) { joint->SetVelocityCommand(0, -1); world->Step(output, state, input); } EXPECT_NEAR(-0.1, joint->GetVelocity(0), 1e-6); joint->SetMinVelocity(0, -math::INF_D); joint->SetMaxVelocity(0, math::INF_D); for (std::size_t i = 0; i < 100; ++i) { joint->SetVelocityCommand(0, 1); world->Step(output, state, input); } EXPECT_NEAR(1, joint->GetVelocity(0), 1e-6); } TEST_F(JointFeaturesFixture, JointSetEffortLimitsWithVelocityControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel("simple_joint_test"); auto joint = model->GetJoint("j1"); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; joint->SetMinEffort(0, -1e-6); joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 100; ++i) { joint->SetVelocityCommand(0, 1); world->Step(output, state, input); } EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); joint->SetMinEffort(0, -80); joint->SetMaxEffort(0, 80); for (std::size_t i = 0; i < 100; ++i) { joint->SetVelocityCommand(0, -1); world->Step(output, state, input); } EXPECT_NEAR(-1, joint->GetVelocity(0), 1e-6); joint->SetMinEffort(0, -math::INF_D); joint->SetMaxEffort(0, math::INF_D); for (std::size_t i = 0; i < 10; ++i) { joint->SetVelocityCommand(0, -100); world->Step(output, state, input); } EXPECT_NEAR(-100, joint->GetVelocity(0), 1e-6); } TEST_F(JointFeaturesFixture, JointSetCombinedLimitsWithVelocityControl) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel("simple_joint_test"); auto joint = model->GetJoint("j1"); // Test joint velocity command physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; joint->SetMinVelocity(0, -0.5); joint->SetMaxVelocity(0, 0.5); joint->SetMinEffort(0, -1e-6); joint->SetMaxEffort(0, 1e-6); for (std::size_t i = 0; i < 1000; ++i) { joint->SetVelocityCommand(0, 1); world->Step(output, state, input); } EXPECT_NEAR(0, joint->GetVelocity(0), 1e-6); joint->SetMinEffort(0, -1e6); joint->SetMaxEffort(0, 1e6); for (std::size_t i = 0; i < 1000; ++i) { joint->SetVelocityCommand(0, -1); world->Step(output, state, input); } EXPECT_NEAR(-0.5, joint->GetVelocity(0), 1e-6); } #endif // Test detaching joints. TEST_F(JointFeaturesFixture, JointDetach) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "test.world"); ASSERT_TRUE(errors.empty()) << errors.front(); const std::string modelName{"double_pendulum_with_base"}; const std::string upperJointName{"upper_joint"}; const std::string lowerJointName{"lower_joint"}; const std::string upperLinkName{"upper_link"}; const std::string lowerLinkName{"lower_link"}; auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); auto model = world->GetModel(modelName); auto upperLink = model->GetLink(upperLinkName); auto lowerLink = model->GetLink(lowerLinkName); auto upperJoint = model->GetJoint(upperJointName); auto lowerJoint = model->GetJoint(lowerJointName); // test Cast*Joint functions EXPECT_NE(nullptr, upperJoint->CastToRevoluteJoint()); EXPECT_NE(nullptr, lowerJoint->CastToRevoluteJoint()); EXPECT_EQ(nullptr, upperJoint->CastToFreeJoint()); EXPECT_EQ(nullptr, lowerJoint->CastToFreeJoint()); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton(modelName); ASSERT_NE(nullptr, skeleton); const auto *dartUpperLink = skeleton->getBodyNode(upperLinkName); const auto *dartLowerLink = skeleton->getBodyNode(lowerLinkName); EXPECT_EQ("RevoluteJoint", dartUpperLink->getParentJoint()->getType()); EXPECT_EQ("RevoluteJoint", dartLowerLink->getParentJoint()->getType()); const math::Pose3d initialUpperLinkPose(1, 0, 2.1, -IGN_PI/2, 0, 0); const math::Pose3d initialLowerLinkPose(1.25, 1, 2.1, -2, 0, 0); EXPECT_EQ(initialUpperLinkPose, math::eigen3::convert(dartUpperLink->getWorldTransform())); EXPECT_EQ(initialLowerLinkPose, math::eigen3::convert(dartLowerLink->getWorldTransform())); // detach lower joint lowerJoint->Detach(); EXPECT_EQ("FreeJoint", dartLowerLink->getParentJoint()->getType()); EXPECT_NE(nullptr, lowerJoint->CastToFreeJoint()); EXPECT_EQ(nullptr, lowerJoint->CastToRevoluteJoint()); // Detach() can be called again though it has no effect lowerJoint->Detach(); EXPECT_EQ("FreeJoint", dartLowerLink->getParentJoint()->getType()); EXPECT_NE(nullptr, lowerJoint->CastToFreeJoint()); EXPECT_EQ(nullptr, lowerJoint->CastToRevoluteJoint()); // expect poses to remain unchanged EXPECT_EQ(initialUpperLinkPose, math::eigen3::convert(dartUpperLink->getWorldTransform())); EXPECT_EQ(initialLowerLinkPose, math::eigen3::convert(dartLowerLink->getWorldTransform())); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; const std::size_t numSteps = 10; for (std::size_t i = 0; i < numSteps; ++i) { // step forward and expect lower link to fall world->Step(output, state, input); // expect upper link to rotate EXPECT_LT(upperJoint->GetVelocity(0), 0.0); // expect lower link to fall down without rotating math::Vector3d lowerLinkLinearVelocity = math::eigen3::convert(dartLowerLink->getLinearVelocity()); EXPECT_NEAR(0.0, lowerLinkLinearVelocity.X(), 1e-10); EXPECT_NEAR(0.0, lowerLinkLinearVelocity.Y(), 1e-10); EXPECT_GT(0.0, lowerLinkLinearVelocity.Z()); math::Vector3d lowerLinkAngularVelocity = math::eigen3::convert(dartLowerLink->getAngularVelocity()); EXPECT_EQ(math::Vector3d::Zero, lowerLinkAngularVelocity); } // now detach the upper joint too, and ensure that velocity is preserved math::Pose3d upperLinkPose = math::eigen3::convert(dartUpperLink->getWorldTransform()); math::Vector3d upperLinkLinearVelocity = math::eigen3::convert(dartUpperLink->getLinearVelocity()); math::Vector3d upperLinkAngularVelocity = math::eigen3::convert(dartUpperLink->getAngularVelocity()); // sanity check on velocity values EXPECT_LT(1e-5, upperLinkLinearVelocity.Z()); EXPECT_GT(-0.03, upperLinkAngularVelocity.X()); EXPECT_NEAR(0.0, upperLinkLinearVelocity.X(), 1e-6); EXPECT_NEAR(0.0, upperLinkLinearVelocity.Y(), 1e-6); EXPECT_NEAR(0.0, upperLinkAngularVelocity.Y(), 1e-6); EXPECT_NEAR(0.0, upperLinkAngularVelocity.Z(), 1e-6); upperJoint->Detach(); EXPECT_EQ("FreeJoint", dartUpperLink->getParentJoint()->getType()); EXPECT_NE(nullptr, upperJoint->CastToFreeJoint()); EXPECT_EQ(nullptr, upperJoint->CastToRevoluteJoint()); EXPECT_EQ(upperLinkPose, math::eigen3::convert(dartUpperLink->getWorldTransform())); EXPECT_EQ(upperLinkLinearVelocity, math::eigen3::convert(dartUpperLink->getLinearVelocity())); EXPECT_EQ(upperLinkAngularVelocity, math::eigen3::convert(dartUpperLink->getAngularVelocity())); } ///////////////////////////////////////////////// // Attach a fixed joint between links that belong to different models TEST_F(JointFeaturesFixture, JointAttachDetach) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "joint_across_models.sdf"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const std::string modelName1{"M1"}; const std::string modelName2{"M2"}; const std::string bodyName{"body"}; auto model1 = world->GetModel(modelName1); auto model2 = world->GetModel(modelName2); auto model1Body = model1->GetLink(bodyName); auto model2Body = model2->GetLink(bodyName); const dart::dynamics::SkeletonPtr skeleton1 = dartWorld->getSkeleton(modelName1); const dart::dynamics::SkeletonPtr skeleton2 = dartWorld->getSkeleton(modelName2); ASSERT_NE(nullptr, skeleton1); ASSERT_NE(nullptr, skeleton2); auto *dartBody1 = skeleton1->getBodyNode(bodyName); auto *dartBody2 = skeleton2->getBodyNode(bodyName); ASSERT_NE(nullptr, dartBody1); ASSERT_NE(nullptr, dartBody2); const math::Pose3d initialModel1Pose(0, 0, 0.25, 0, 0, 0); const math::Pose3d initialModel2Pose(0, 0, 3.0, 0, 0, 0); EXPECT_EQ(initialModel1Pose, math::eigen3::convert(dartBody1->getWorldTransform())); EXPECT_EQ(initialModel2Pose, math::eigen3::convert(dartBody2->getWorldTransform())); physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; const std::size_t numSteps = 100; for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); // Expect the model1 to stay at rest (since it's on the ground) and model2 // to start falling math::Vector3d body1LinearVelocity = math::eigen3::convert(dartBody1->getLinearVelocity()); math::Vector3d body2LinearVelocity = math::eigen3::convert(dartBody2->getLinearVelocity()); EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); // Negative z velocity EXPECT_GT(0.0, body2LinearVelocity.Z()); } const auto poseParent = dartBody1->getTransform(); const auto poseChild = dartBody2->getTransform(); auto poseParentChild = poseParent.inverse() * poseChild; auto fixedJoint = model2Body->AttachFixedJoint(model1Body); // AttachFixedJoint snaps the child body to the origin of the parent, so we // set a transform on the joint to keep the transform between the two bodies // the same as it was before they were attached fixedJoint->SetTransformFromParent(poseParentChild); // The name of the link obtained using the ign-physics API should remain the // same even though AttachFixedJoint renames the associated BodyNode. EXPECT_EQ(bodyName, model2Body->GetName()); for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); // Expect the model1 to remain at rest and model2 // to stop moving math::Vector3d body1LinearVelocity = math::eigen3::convert(dartBody1->getLinearVelocity()); math::Vector3d body2LinearVelocity = math::eigen3::convert(dartBody2->getLinearVelocity()); EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); } // now detach joint and expect model2 to start moving again fixedJoint->Detach(); // The name of the link obtained using the ign-physics API should remain the // same even though Detach renames the associated BodyNode. EXPECT_EQ(bodyName, model2Body->GetName()); for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); // Expect the model1 to remain at rest and model2 // to start moving again math::Vector3d body1LinearVelocity = math::eigen3::convert(dartBody1->getLinearVelocity()); math::Vector3d body2LinearVelocity = math::eigen3::convert(dartBody2->getLinearVelocity()); EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); // Negative z velocity EXPECT_GT(0.0, body2LinearVelocity.Z()); } // After a while, body2 should reach the ground and come to a stop for (std::size_t i = 0; i < 1000; ++i) { world->Step(output, state, input); } EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); } ///////////////////////////////////////////////// // Expectations on number of links before/after attach/detach TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "joint_across_models.sdf"); ASSERT_TRUE(errors.empty()) << errors.front(); auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const std::string modelName1{"M1"}; const std::string modelName2{"M2"}; const std::string bodyName{"body"}; auto model1 = world->GetModel(modelName1); auto model2 = world->GetModel(modelName2); auto model1Body = model1->GetLink(bodyName); auto model2Body = model2->GetLink(bodyName); // Before attaching we expect each model to have 1 link EXPECT_EQ(1u, model1->GetLinkCount()); EXPECT_EQ(1u, model2->GetLinkCount()); auto fixedJoint = model2Body->AttachFixedJoint(model1Body); // After attaching we expect each model to have 1 link EXPECT_EQ(1u, model1->GetLinkCount()); EXPECT_EQ(1u, model2->GetLinkCount()); // now detach joint and expect model2 to start moving again fixedJoint->Detach(); // After detaching we expect each model to have 1 link EXPECT_EQ(1u, model1->GetLinkCount()); EXPECT_EQ(1u, model2->GetLinkCount()); // Test that a model with the same name as a link doesn't cause problems const std::string modelName3{"body"}; auto model3 = world->GetModel(modelName3); EXPECT_EQ(1u, model3->GetLinkCount()); auto model3Body = model3->GetLink(bodyName); auto fixedJoint2 = model3Body->AttachFixedJoint(model2Body); EXPECT_EQ(1u, model2->GetLinkCount()); fixedJoint2->Detach(); // After detaching we expect each model to have 1 link EXPECT_EQ(1u, model2->GetLinkCount()); EXPECT_EQ(1u, model3->GetLinkCount()); } ///////////////////////////////////////////////// // Attach a fixed joint between links that belong to different models where one // of the models is created after a step is called TEST_F(JointFeaturesFixture, JointAttachDetachSpawnedModel) { std::string model1Str = R"( 0 0 0.1 0 0 0 0.2 0.2 0.2 )"; std::string model2Str = R"( 1 0 0.1 0 0 0 0.1 )"; physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; physics::World3dPtr world; { sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "ground.sdf"); ASSERT_TRUE(errors.empty()) << errors.front(); world = this->engine->ConstructWorld(*root.WorldByIndex(0)); ASSERT_NE(nullptr, world); } { sdf::Root root; sdf::Errors errors = root.LoadSdfString(model1Str); ASSERT_TRUE(errors.empty()) << errors.front(); ASSERT_NE(nullptr, root.Model()); world->ConstructModel(*root.Model()); } world->Step(output, state, input); { sdf::Root root; sdf::Errors errors = root.LoadSdfString(model2Str); ASSERT_TRUE(errors.empty()) << errors.front(); ASSERT_NE(nullptr, root.Model()); world->ConstructModel(*root.Model()); } const std::string modelName1{"M1"}; const std::string modelName2{"M2"}; const std::string bodyName1{"body"}; const std::string bodyName2{"chassis"}; auto model1 = world->GetModel(modelName1); auto model2 = world->GetModel(modelName2); auto model1Body = model1->GetLink(bodyName1); auto model2Body = model2->GetLink(bodyName2); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const auto skeleton1 = dartWorld->getSkeleton(modelName1); const auto skeleton2 = dartWorld->getSkeleton(modelName2); ASSERT_NE(nullptr, skeleton1); ASSERT_NE(nullptr, skeleton2); auto *dartBody1 = skeleton1->getBodyNode(bodyName1); auto *dartBody2 = skeleton2->getBodyNode(bodyName2); ASSERT_NE(nullptr, dartBody1); ASSERT_NE(nullptr, dartBody2); const auto poseParent = dartBody1->getTransform(); const auto poseChild = dartBody2->getTransform(); // Before ign-physics PR #31, uncommenting the following `step` call makes // this test pass, but commenting it out makes it fail. // world->Step(output, state, input); auto fixedJoint = model2Body->AttachFixedJoint(model1Body); // Pose of child relative to parent auto poseParentChild = poseParent.inverse() * poseChild; // We let the joint be at the origin of the child link. fixedJoint->SetTransformFromParent(poseParentChild); const std::size_t numSteps = 100; for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); } // Expect both bodies to hit the ground and stop EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3); EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); fixedJoint->Detach(); for (std::size_t i = 0; i < numSteps; ++i) { world->Step(output, state, input); } // Expect both bodies to remain in contact with the ground with zero velocity. EXPECT_NEAR(0.0, dartBody1->getLinearVelocity().z(), 1e-3); EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); } class JointTransmittedWrenchFixture : public JointFeaturesFixture { public: using WorldPtr = physics::World3dPtr; public: using ModelPtr = physics::Model3dPtr; public: using JointPtr = physics::Joint3dPtr; public: using LinkPtr = physics::Link3dPtr; public: using Vector3d = physics::Vector3d; public: using Wrench3d = physics::Wrench3d; protected: void SetUp() override { JointFeaturesFixture::SetUp(); sdf::Root root; const sdf::Errors errors = root.Load(TEST_WORLD_DIR "pendulum_joint_wrench.sdf"); ASSERT_TRUE(errors.empty()) << errors.front(); this->world = this->engine->ConstructWorld(*root.WorldByIndex(0)); ASSERT_NE(nullptr, this->world); this->model = this->world->GetModel("pendulum"); ASSERT_NE(nullptr, this->model); this->motorJoint = this->model->GetJoint("motor_joint"); ASSERT_NE(nullptr, this->motorJoint); this->sensorJoint = this->model->GetJoint("sensor_joint"); ASSERT_NE(nullptr, this->sensorJoint); this->armLink = this->model->GetLink("arm"); ASSERT_NE(nullptr, this->armLink); } public: void Step(int _iters) { for (int i = 0; i < _iters; ++i) { this->world->Step(this->output, this->state, this->input); } } public: physics::ForwardStep::Output output; public: physics::ForwardStep::State state; public: physics::ForwardStep::Input input; public: WorldPtr world; public: ModelPtr model; public: JointPtr motorJoint; public: JointPtr sensorJoint; public: LinkPtr armLink; // From SDFormat file static constexpr double kGravity = 9.8; static constexpr double kArmLinkMass = 6.0; static constexpr double kSensorLinkMass = 0.4; // MOI in the z-axis static constexpr double kSensorLinkMOI = 0.02; static constexpr double kArmLength = 1.0; }; TEST_F(JointTransmittedWrenchFixture , PendulumAtZeroAngle) { namespace test = physics::test; // Run a few steps for the constraint forces to stabilize this->Step(10); // Test wrench expressed in different frames { auto wrenchAtMotorJoint = this->motorJoint->GetTransmittedWrench(); Wrench3d expectedWrenchAtMotorJoint{ Vector3d::Zero(), {-kGravity * (kArmLinkMass + kSensorLinkMass), 0, 0}}; EXPECT_TRUE( test::Equal(expectedWrenchAtMotorJoint, wrenchAtMotorJoint, 1e-4)); } { auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( this->motorJoint->GetFrameID(), physics::FrameID::World()); Wrench3d expectedWrenchAtMotorJointInWorld{ Vector3d::Zero(), {0, 0, kGravity * (kArmLinkMass + kSensorLinkMass)}}; EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInWorld, wrenchAtMotorJointInWorld, 1e-4)); } { auto wrenchAtMotorJointInArm = this->motorJoint->GetTransmittedWrench( this->armLink->GetFrameID(), this->armLink->GetFrameID()); // The arm frame is rotated by 90° in the Y-axis of the joint frame. Wrench3d expectedWrenchAtMotorJointInArm{ Vector3d::Zero(), {0, 0, kGravity * (kArmLinkMass + kSensorLinkMass)}}; EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInArm, wrenchAtMotorJointInArm, 1e-4)); } } TEST_F(JointTransmittedWrenchFixture, PendulumInMotion) { namespace test = physics::test; // Start pendulum at 90° (parallel to the ground) and stop at about 40° // so that we have non-trivial test expectations. this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); this->Step(350); // Given the position (θ), velocity (ω), and acceleration (α) of the joint // and distance from the joint to the COM (r), the reaction forces in // the tangent direction (Ft) and normal direction (Fn) are given by: // // Ft = m * α * r + (m * g * sin(θ)) = m * (α * r + g * sin(θ)) // Fn = -m * ω² * r - (m * g * cos(θ)) = -m * (ω² * r + g * cos(θ)) { const double theta = this->motorJoint->GetPosition(0); const double omega = this->motorJoint->GetVelocity(0); // In order to get the math to work out, we need to use the joint // acceleration and transmitted wrench from the current time step with the // joint position and velocity from the previous time step. That is, we need // the position and velocity before they are integrated. this->Step(1); const double alpha = this->motorJoint->GetAcceleration(0); auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); const double armTangentForce = kArmLinkMass * ((alpha * kArmLength / 2.0) + (kGravity * sin(theta))); const double motorLinkTangentForce = kSensorLinkMass * kGravity * sin(theta); const double armNormalForce = -kArmLinkMass * ((std::pow(omega, 2) * kArmLength / 2.0) + (kGravity * cos(theta))); const double motorLinkNormalForce = -kSensorLinkMass * kGravity * cos(theta); const double tangentForce = armTangentForce + motorLinkTangentForce; const double normalForce = armNormalForce + motorLinkNormalForce; // The orientation of the joint frame is such that the normal force is // parallel to the x-axis and the tangent force is parallel to the y-axis. Wrench3d expectedWrenchAtMotorJointInJoint{ Vector3d::Zero(), {normalForce, tangentForce, 0}}; EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInJoint, wrenchAtMotorJointInJoint, 1e-4)); } // Test Wrench expressed in different frames { auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); // This is just a rotation of the wrench to be expressed in the world's // coordinate frame auto wrenchAtMotorJointInWorld = this->motorJoint->GetTransmittedWrench( this->motorJoint->GetFrameID(), physics::FrameID::World()); // The joint frame is rotated by 90° along the world's y-axis Eigen::Quaterniond R_WJ = Eigen::AngleAxisd(IGN_PI_2, Eigen::Vector3d(0, 1, 0)) * Eigen::AngleAxisd(this->motorJoint->GetPosition(0), Eigen::Vector3d(0, 0, 1)); Wrench3d expectedWrenchAtMotorJointInWorld{ Vector3d::Zero(), R_WJ * wrenchAtMotorJointInJoint.force}; EXPECT_TRUE(test::Equal(expectedWrenchAtMotorJointInWorld, wrenchAtMotorJointInWorld, 1e-4)); // This moves the point of application and changes the coordinate frame Wrench3d wrenchAtArmInArm = this->motorJoint->GetTransmittedWrench( armLink->GetFrameID(), armLink->GetFrameID()); // Notation: arm link (A), joint (J) Eigen::Isometry3d X_AJ; // Pose of joint (J) in arm link (A) as specified in the SDFormat file. X_AJ = Eigen::AngleAxisd(IGN_PI_2, Eigen::Vector3d(0, 1, 0)); X_AJ.translation() = Vector3d(0, 0, kArmLength / 2.0); Wrench3d expectedWrenchAtArmInArm; expectedWrenchAtArmInArm.force = X_AJ.linear() * wrenchAtMotorJointInJoint.force; expectedWrenchAtArmInArm.torque = X_AJ.linear() * wrenchAtMotorJointInJoint.torque + X_AJ.translation().cross(expectedWrenchAtArmInArm.force); EXPECT_TRUE(test::Equal(expectedWrenchAtArmInArm, wrenchAtArmInArm, 1e-4)); } } // Compare wrench at the motor joint with wrench from the sensor joint (a // fixed joint measuring only constraint forces). TEST_F(JointTransmittedWrenchFixture, ValidateWrenchWithSecondaryJoint) { namespace test = physics::test; // Start pendulum at 90° (parallel to the ground) and stop at about 40° // so that we have non-trivial test expectations. this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); this->Step(350); const double theta = this->motorJoint->GetPosition(0); // In order to get the math to work out, we need to use the joint // acceleration and transmitted wrench from the current time step with the // joint position and velocity from the previous time step. That is, we need // the position and velocity before they are integrated. this->Step(1); const double alpha = this->motorJoint->GetAcceleration(0); auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); auto wrenchAtSensorInSensor = this->sensorJoint->GetTransmittedWrench(); // Since sensor_link has moment of inertia, the fixed joint will transmit a // torque necessary to rotate the sensor. This is not detected by the motor // joint because no force is transmitted along the revolute axis. On the // other hand, the mass of sensor_link will contribute to the constraint // forces on the motor joint, but these won't be detected by the sensor // joint. Vector3d expectedTorqueDiff{0, 0, kSensorLinkMOI * alpha}; Vector3d expectedForceDiff{-kSensorLinkMass * kGravity * cos(theta), kSensorLinkMass * kGravity * sin(theta), 0}; Vector3d torqueDiff = wrenchAtMotorJointInJoint.torque - wrenchAtSensorInSensor.torque; Vector3d forceDiff = wrenchAtMotorJointInJoint.force - wrenchAtSensorInSensor.force; EXPECT_TRUE(test::Equal(expectedTorqueDiff, torqueDiff, 1e-4)); EXPECT_TRUE(test::Equal(expectedForceDiff, forceDiff, 1e-4)); } // Check that the transmitted wrench is affected by joint friction, stiffness // and damping TEST_F(JointTransmittedWrenchFixture, JointLosses) { // Get DART joint pointer to set joint friction, damping, etc. auto dartWorld = this->world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); auto dartModel = dartWorld->getSkeleton(this->model->GetIndex()); ASSERT_NE(nullptr, dartModel); auto dartJoint = dartModel->getJoint(this->motorJoint->GetIndex()); ASSERT_NE(nullptr, dartJoint); // Joint friction { this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); this->motorJoint->SetVelocity(0, 0); const double kFrictionCoef = 0.5; dartJoint->setCoulombFriction(0, kFrictionCoef); this->Step(10); auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); EXPECT_NEAR(kFrictionCoef, wrenchAtMotorJointInJoint.torque.z(), 1e-4); dartJoint->setCoulombFriction(0, 0.0); } // Joint damping { this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); this->motorJoint->SetVelocity(0, 0); const double kDampingCoef = 0.2; dartJoint->setDampingCoefficient(0, kDampingCoef); this->Step(100); const double omega = this->motorJoint->GetVelocity(0); this->Step(1); auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); EXPECT_NEAR(-omega * kDampingCoef, wrenchAtMotorJointInJoint.torque.z(), 1e-3); dartJoint->setDampingCoefficient(0, 0.0); } // Joint stiffness { // Note: By default, the spring reference position is 0. this->motorJoint->SetPosition(0, IGN_DTOR(30.0)); this->motorJoint->SetVelocity(0, 0); const double kSpringStiffness = 0.7; dartJoint->setSpringStiffness(0, kSpringStiffness); this->Step(1); const double theta = this->motorJoint->GetPosition(0); this->Step(1); auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); EXPECT_NEAR(-theta * kSpringStiffness, wrenchAtMotorJointInJoint.torque.z(), 1e-3); dartJoint->setSpringStiffness(0, 0.0); } } // Check that the transmitted wrench is affected by contact forces TEST_F(JointTransmittedWrenchFixture, ContactForces) { auto box = this->world->GetModel("box"); ASSERT_NE(nullptr, box); auto boxFreeGroup = box->FindFreeGroup(); ASSERT_NE(nullptr, boxFreeGroup); physics::Pose3d X_WB(Eigen::Translation3d(0, 1, 1)); boxFreeGroup->SetWorldPose(X_WB); this->motorJoint->SetPosition(0, IGN_DTOR(90.0)); // After this many steps, the pendulum is in contact with the box this->Step(1000); const double theta = this->motorJoint->GetPosition(0); // Sanity check that the pendulum is at rest EXPECT_NEAR(0.0, this->motorJoint->GetVelocity(0), 1e-3); auto wrenchAtMotorJointInJoint = this->motorJoint->GetTransmittedWrench(); // To compute the reaction forces, we consider the pivot on the contact point // between the pendulum and the box and the fact that the sum of moments about // the pivot is zero. We also note that all forces, including the reaction // forces, are in the vertical (world's z-axis) direction. // // Notation: // Fp_z: Reaction force at pendulum joint (pin) in the world's z-axis // M_b: Moment about the contact point between box and pendulum // // Fp_z = √(Fn² + Ft²) // Since all of the reaction force is in the world's // z-axis // // ∑M_b = 0 = -Fp_z * sin(θ) * (2*r) + mâ‚*g*sin(θ)*r + mâ‚‚*g*sin(θ)*(2*r) // // Fp_z = 0.5 * g * (mâ‚ + 2*mâ‚‚) // // We can then compute the tangential (Ft) and normal (Fn) components as // // Ft = Fp_z * sin(θ) // Fn = -Fp_z * cos(θ) const double reactionForceAtP = 0.5 * kGravity * (kArmLinkMass + 2 * kSensorLinkMass); Wrench3d expectedWrenchAtMotorJointInJoint{ Vector3d::Zero(), {-reactionForceAtP * cos(theta), reactionForceAtP * sin(theta), 0}}; EXPECT_TRUE(physics::test::Equal(expectedWrenchAtMotorJointInJoint, wrenchAtMotorJointInJoint, 1e-4)); } ///////////////////////////////////////////////// int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ign-physics-ignition-physics5_5.1.0/dartsim/src/KinematicsFeatures.cc0000664000175000017500000000467614143524174025714 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include "KinematicsFeatures.hh" namespace ignition { namespace physics { namespace dartsim { ///////////////////////////////////////////////// FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( const FrameID &_id) const { FrameData3d data; // The feature system should never send us the world ID. if (_id.IsWorld()) { ignerr << "Given a FrameID belonging to the world. This should not be " << "possible! Please report this bug!\n"; assert(false); return data; } const dart::dynamics::Frame *frame = SelectFrame(_id); // A missing frame ID indicates that frame semantics is not properly // implemented for the type of frame represented by the ID. if (nullptr == frame) { ignerr << "The frame ID " << _id.ID() << " was not found in the list of known frames. This should not be " "possible! Please report this bug!\n"; assert(false); return data; } data.pose = frame->getWorldTransform(); data.linearVelocity = frame->getLinearVelocity(); data.angularVelocity = frame->getAngularVelocity(); data.linearAcceleration = frame->getLinearAcceleration(); data.angularAcceleration = frame->getAngularAcceleration(); return data; } ///////////////////////////////////////////////// const dart::dynamics::Frame *KinematicsFeatures::SelectFrame( const FrameID &_id) const { const auto model_it = this->models.idToObject.find(_id.ID()); if (model_it != this->models.idToObject.end()) { // This is a model FreeGroup frame, so we'll use the first root link as the // frame return model_it->second->model->getRootBodyNode(); } auto framesIt = this->frames.find(_id.ID()); if (framesIt == this->frames.end()) { return nullptr; } return framesIt->second; } } } } ign-physics-ignition-physics5_5.1.0/include/0000775000175000017500000000000014143524174020773 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/include/CMakeLists.txt0000664000175000017500000000004314143524174023530 0ustar jriverojriveroadd_subdirectory(ignition/physics) ign-physics-ignition-physics5_5.1.0/include/ignition/0000775000175000017500000000000014143524174022613 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/include/ignition/physics/0000775000175000017500000000000014143524174024275 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/include/ignition/physics/CapsuleShape.hh0000664000175000017500000001225414143524174027177 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_CAPSULESHAPE_HH_ #define IGNITION_PHYSICS_CAPSULESHAPE_HH_ #include #include #include namespace ignition { namespace physics { IGN_PHYSICS_DECLARE_SHAPE_TYPE(CapsuleShape) class IGNITION_PHYSICS_VISIBLE GetCapsuleShapeProperties : public virtual FeatureWithRequirements { public: template class CapsuleShape : public virtual Entity { public: using Scalar = typename PolicyT::Scalar; /// \brief Get the radius of this CapsuleShape /// \return the radius of this CapsuleShape public: Scalar GetRadius() const; /// \brief Get the length along the local z-axis of this /// CapsuleShape's cylinder. /// \return the length of this CapsuleShape public: Scalar GetLength() const; }; public: template class Implementation : public virtual Feature::Implementation { public: using Scalar = typename PolicyT::Scalar; public: virtual Scalar GetCapsuleShapeRadius( const Identity &_capsuleID) const = 0; public: virtual Scalar GetCapsuleShapeLength( const Identity &_capsuleID) const = 0; }; }; ///////////////////////////////////////////////// /// \brief This feature sets the CapsuleShape properties such as /// the capsule radius and length. class IGNITION_PHYSICS_VISIBLE SetCapsuleShapeProperties : public virtual FeatureWithRequirements { public: template class CapsuleShape : public virtual Entity { public: using Scalar = typename PolicyT::Scalar; /// \brief Set the radius of this CapsuleShape /// \param[in] _radius /// The desired radius of this CapsuleShape public: void SetRadius(Scalar _radius); /// \brief Set the length of this CapsuleShape /// \param[in] _length /// The desired length of this CapsuleShape public: void SetLength(Scalar length); }; public: template class Implementation : public virtual Feature::Implementation { public: using Scalar = typename PolicyT::Scalar; public: virtual void SetCapsuleShapeRadius( const Identity &_capsuleID, Scalar _radius) = 0; public: virtual void SetCapsuleShapeLength( const Identity &_capsuleID, Scalar _length) = 0; }; }; ///////////////////////////////////////////////// /// \brief This feature constructs a new capsule shape and attaches the /// desired pose in the link frame. The pose could be defined to be the /// capsule center point in actual implementation. class IGNITION_PHYSICS_VISIBLE AttachCapsuleShapeFeature : public virtual FeatureWithRequirements { public: template class Link : public virtual Feature::Link { public: using Scalar = typename PolicyT::Scalar; public: using PoseType = typename FromPolicy::template Use; public: using ShapePtrType = CapsuleShapePtr; /// \brief Rigidly attach a CapsuleShape to this link. /// \param[in] _radius /// The radius of the capsule. /// \param[in] _length /// The length of the capsule. /// \param[in] _pose /// The desired pose of the CapsuleShape relative to the Link frame. /// \returns a ShapePtrType to the newly constructed CapsuleShape public: ShapePtrType AttachCapsuleShape( const std::string &_name = "capsule", Scalar _radius = 0.5, Scalar _length = 1.0, const PoseType &_pose = PoseType::Identity()); }; public: template class Implementation : public virtual Feature::Implementation { public: using Scalar = typename PolicyT::Scalar; public: using PoseType = typename FromPolicy::template Use; public: virtual Identity AttachCapsuleShape( const Identity &_linkID, const std::string &_name, Scalar _radius, Scalar _length, const PoseType &_pose) = 0; }; }; } } #include #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/ConstructEmpty.hh0000664000175000017500000001102414143524174027617 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_CONSTRUCTEMPTY_HH_ #define IGNITION_PHYSICS_CONSTRUCTEMPTY_HH_ #include #include namespace ignition { namespace physics { ///////////////////////////////////////////////// //! [ConstructEmptyWorld] /// \brief This feature constructs an empty world and returns its pointer /// from the current physics engine in use. class ConstructEmptyWorldFeature : public virtual Feature { public: template class Engine : public virtual Feature::Engine { public: using WorldPtrType = WorldPtr; /// \brief Construct an empty world and attach a given name to it. /// \param[in] _name /// Name of the world. /// \return /// The WorldPtrType of the constructed world. public: WorldPtrType ConstructEmptyWorld(const std::string &_name); }; public: template class Implementation : public virtual Feature::Implementation { public: virtual Identity ConstructEmptyWorld( const Identity &_engineID, const std::string &_name) = 0; }; }; //! [ConstructEmptyWorld] ///////////////////////////////////////////////// /// \brief This feature constructs an empty model and returns its pointer /// from the given world. class ConstructEmptyModelFeature : public virtual Feature { public: template class World : public virtual Feature::World { public: using ModelPtrType = ModelPtr; /// \brief Construct an empty model and attach a given name to it. /// \param[in] _name /// Name of the model. /// \return /// The ModelPtrType of the constructed model. public: ModelPtrType ConstructEmptyModel(const std::string &_name); }; public: template class Implementation : public virtual Feature::Implementation { public: virtual Identity ConstructEmptyModel( const Identity &_worldID, const std::string &_name) = 0; }; }; ///////////////////////////////////////////////// /// \brief This feature constructs an empty nested model and returns its pointer /// from the given world. class ConstructEmptyNestedModelFeature : public virtual Feature { public: template class Model : public virtual Feature::Model { public: using ModelPtrType = ModelPtr; /// \brief Construct an empty model and attach a given name to it. /// \param[in] _name /// Name of the nested model. /// \return /// The ModelPtrType of the constructed model. public: ModelPtrType ConstructEmptyNestedModel(const std::string &_name); }; public: template class Implementation : public virtual Feature::Implementation { public: virtual Identity ConstructEmptyNestedModel( const Identity &_modelID, const std::string &_name) = 0; }; }; ///////////////////////////////////////////////// /// \brief This feature constructs an empty link and returns its pointer /// from the given model. class ConstructEmptyLinkFeature : public virtual Feature { public: template class Model : public virtual Feature::Model { public: using LinkPtrType = LinkPtr; /// \brief Construct an empty link and attach a given name to it. /// \param[in] _name /// Name of the link. /// \return /// The LinkPtrType of the constructed link. public: LinkPtrType ConstructEmptyLink(const std::string &_name); }; public: template class Implementation : public virtual Feature::Implementation { public: virtual Identity ConstructEmptyLink( const Identity &_modelID, const std::string &_name) = 0; }; }; } } #include #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/FixedJoint.hh0000664000175000017500000000446314143524174026670 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_FIXEDJOINT_HH_ #define IGNITION_PHYSICS_FIXEDJOINT_HH_ #include #include namespace ignition { namespace physics { IGN_PHYSICS_DECLARE_JOINT_TYPE(FixedJoint) class IGNITION_PHYSICS_VISIBLE AttachFixedJointFeature : public virtual FeatureWithRequirements { public: template class Link : public virtual Feature::Link { public: using JointPtrType = FixedJointPtr; /// \brief Attach this link to another link using a FixedJoint. /// \param[in] _parent /// The parent link for the joint. Pass in a nullptr to attach the /// link to the world. /// \param[in] _name /// The name for this joint. /// \return A reference to the newly constructed FixedJoint. You can use /// the SetJointTransformFromParentFeature and /// SetJointTransformToChildFeature on this JointPtr to set the relative /// transform between the parent and child if your physics engine offers /// those features. public: JointPtrType AttachFixedJoint( const BaseLinkPtr &_parent, const std::string &_name = "fixed"); }; public: template class Implementation : public virtual Feature::Implementation { public: virtual Identity AttachFixedJoint( const Identity &_childID, const BaseLinkPtr &_parent, const std::string &_name) = 0; }; }; } } #include #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/GetContacts.hh0000664000175000017500000000701314143524174027035 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_GETCONTACTS_HH_ #define IGNITION_PHYSICS_GETCONTACTS_HH_ #include #include #include #include #include namespace ignition { namespace physics { /// \brief GetContactsFromLastStepFeature is a feature for retrieving the list /// of contacts generated in the previous simulation step. class IGNITION_PHYSICS_VISIBLE GetContactsFromLastStepFeature : public virtual FeatureWithRequirements { public: template struct ExtraContactDataT { using Scalar = typename PolicyT::Scalar; using VectorType = typename FromPolicy::template Use; /// \brief The contact force acting on the first body expressed /// in the world frame VectorType force; /// \brief The normal of the force acting on the first body expressed /// in the world frame VectorType normal; /// \brief The penetration depth Scalar depth; }; public: template class World : public virtual Feature::World { public: using Scalar = typename PolicyT::Scalar; public: using ShapePtrType = ShapePtr; public: using VectorType = typename FromPolicy::template Use; public: using ExtraContactData = ExtraContactDataT; public: struct ContactPoint { /// \brief Collision shape of the first body ShapePtrType collision1; /// \brief Collision shape of the second body ShapePtrType collision2; /// \brief The point of contact expressed in the world frame VectorType point; }; public: using Contact = SpecifyData< RequireData, ExpectData >; /// \brief Get contacts generated in the previous simulation step public: std::vector GetContactsFromLastStep() const; }; public: template class Implementation : public virtual Feature::Implementation { public: using Scalar = typename PolicyT::Scalar; public: using VectorType = typename FromPolicy::template Use; public: using ExtraContactData = ExtraContactDataT; public: struct ContactInternal { /// \brief Identity of the first body Identity collision1; /// \brief Identity of the second body Identity collision2; /// \brief The point of contact expressed in the world frame VectorType point; /// \brief Extra data related to contact. CompositeData extraData; }; public: virtual std::vector GetContactsFromLastStep( const Identity &_worldID) const = 0; }; }; } } #include "ignition/physics/detail/GetContacts.hh" #endif /* end of include guard: IGNITION_PHYSICS_GETCONTACTS_HH_ */ ign-physics-ignition-physics5_5.1.0/include/ignition/physics/SpecifyData.hh0000664000175000017500000003521614143524174027021 0ustar jriverojrivero/* * Copyright (C) 2017 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_SPECIFYDATA_HH_ #define IGNITION_PHYSICS_SPECIFYDATA_HH_ #include "ignition/physics/detail/PrivateSpecifyData.hh" namespace ignition { namespace physics { /// \brief ExpectData is an extension of CompositeData which indicates that /// the composite expects to be operating on the data types listed in its /// template arguments. All of the expected types will benefit from very /// high-speed operations when being accessed from an object of type /// ExpectData. The ordinary CompositeData class needs to perform /// a map lookup on the data type whenever one of its functions is called, /// but an ExpectData object does not need to perform any map lookup when /// performing an operation on T (e.g. .Get(), .Insert(), .Query(), /// .Has(), etc). /// /// Note that there is no guarantee that any of the types listed in its /// template arguments will exist in the underlying CompositeData, but it /// will still provide very high-speed access for creating and querying them /// even if they are not yet present. /// /// This template specialization implements ExpectData for a single type, /// but the ExpectData template can accept any number of types, e.g. /// ExpectData. template class ExpectData : public virtual CompositeData { /// \brief Default constructor public: ExpectData(); /// \brief Copy constructor. /// The copy constructor of the base class, CompositeData, is called /// before this copy constructor and it does the actual copying of the /// data contained in the MapData object of `_other`. Thus, this copy /// constructor simply calls the default constructor to initialize the /// MapData iterator of this object to point the appropriate iterator in /// the newly copied MapData. public: ExpectData(const ExpectData &_other); /// TODO(anyone) Implement move constructor and assignment operator. Due /// to the multiple inheritence used to implement SpcifyData, care must be /// taken when implementing move constructs to avoid calling a move /// constructor on a moved-from object. /// \brief Virtual destructor public: virtual ~ExpectData() = default; /// \brief Provides extremely high-speed access to expected data types and /// normal access to unexpected data types. /// \sa CompositeData::Get() public: template Data &Get(); /// \brief Provides extremely high-speed access to creating expected data /// types and normal access to unexpected data types. /// \sa CompositeData::InsertOrAssign() public: template InsertResult InsertOrAssign(Args&&... _args); /// \brief Provides extremely high-speed access to getting or creating /// expected data types and normal access to unexpected data types. /// \sa CompositeData::Insert() public: template InsertResult Insert(Args&&... _args); /// \brief Provides extremely high-speed access for removing expected data /// types and normal access for unexpected data types. /// \sa CompositeData::Remove() public: template bool Remove(); /// \brief Provides extremely high-speed access for querying expected data /// types and normal access for unexpected data types. /// \sa CompositeData::Query() public: template Data *Query(const QueryMode _mode = QueryMode::NORMAL); /// \brief Const-qualified version of Query /// \sa CompositeData::Query() public: template const Data *Query(const QueryMode _mode = QueryMode::NORMAL) const; /// \brief Provides extremely high-speed access for checking the existence /// of expected data types and normal access for unexpected data types. /// \sa CompositeData::Has() const public: template bool Has() const; /// \brief Provides extremely high-speed access to the status of expected /// data types and normal access for unexpected data types. /// \sa CompositeData::StatusOf() const public: template DataStatus StatusOf() const; /// \brief Provides extremely high-speed access for unquerying expected /// data types and normal access for unexpected data types. /// \sa CompositeData::Unquery() const public: template bool Unquery() const; /// \brief Provides extremely high-speed access for making expected data /// types required and normal access for unexpected data types. /// \sa CompositeData::MakeRequired() public: template Data &MakeRequired(Args&&... _args); /// \brief Provides extremely high-speed access for checking whether /// expected data types are required and normal access for unexpected data /// types. /// \sa CompositeData::Requires() const public: template bool Requires() const; /// \brief Returns true if the Data type is expected. Data types for which /// this is true will be able to enjoy extremely high-speed access. /// \sa CompositeData::Expects() public: template static constexpr bool Expects(); /// \brief Provides the implementation for delegating the functions /// provided by the ExpectData class protected: detail::PrivateExpectData privateExpectData; // This allows outside users to identify this expected data type at // compile time, which can be useful for doing template metaprogramming. public: using Specification = ExpectData; public: using ExpectedData = Expected; public: using RequiredData = void; public: using SubSpecification1 = void; public: using SubSpecification2 = void; }; /// \brief RequireData is an extension of ExpectData which indicates that /// the composite requires the existence of any data types that are listed /// in its template arguments. /// /// Each data type that is listed in the template arguments will be /// instantiated upon construction of the RequireData object, and /// none of them can be removed from that object or from any reference to /// that object. /// /// Objects that are listed in the template arguments will also benefit from /// extremely high-speed accessibility when they are accessed using an /// object of the type RequireData. This is similar to ExpectData. /// /// RequireData also offers a const-qualified Get() function. /// /// This template specialization implements RequireData for a single type, /// but the RequireData template can accept any number of types, e.g. /// RequireData. template class RequireData : public virtual ExpectData { // This allows outside users to identify this Required data type at // compile time, which can be useful for doing template metaprogramming. public: using Specification = RequireData; public: using RequiredData = Required; /// \brief Default constructor. Will initialize the Required data type /// using its default constructor. public: RequireData(); /// \brief Virtual destructor public: virtual ~RequireData() = default; // Do not shadow the ExpectData implementation using ExpectData::Get; /// \brief Provides extremely high-speed const-qualified reference access /// to data types that are required. /// /// NOTE: For data types that are not required, this will throw a /// compilation error, and you should use Query() const instead. /// \sa CompositeData::Get() public: template const Data &Get() const; /// \brief Returns true if the Data type is always required by this more /// highly specified CompositeData type. public: template static constexpr bool AlwaysRequires(); /// \brief Provides the implementation for delegating the functions /// provided by the RequireData class protected: detail::PrivateRequireData privateRequireData; }; /// \brief The SpecifyData class allows you to form combinations of data /// specifications. In other words, you can freely mix invocations to /// RequireData and ExpectData. Example usage: /// /// \code /// using namespace ignition::physics; /// /// using MyInputSpecifications = SpecifyData< /// RequireData< /// DesiredPositionInput, /// DesiredVelocityInput>, /// ExpectData< /// ExternalForceInput, /// ProximitySensorInput, /// ForceTorqueSensorInput> >; /// \endcode /// /// This would define a CompositeData which is required to contain a /// DesiredPositionInput data structure and a DesiredVelocityInput data /// structure. It is also optimized for ExternalForceInput, /// ProximitySensorInput, and ForceTorqueSensorInput data structures, but /// they are optional. Whether a data structure is specified as expected or /// required, you will be able to get extremely high-speed access to it /// through an object that has the type of MyInputSpecifications. /// /// Specifications can also be composed together. For example, if there is /// another specification like: /// /// \code /// using ComplianceInputSpecifications = SpecifyData< /// RequireData< /// ProximitySensorInput, /// ComplianceParameters>, /// ExpectData< /// ForceTorqueSensorInput, /// CameraSensorInput> >; /// \endcode /// /// then you can combine these specifications: /// /// \code /// using CombinedInputSpecifications = SpecifyData< /// MyInputSpecifications, /// ComplianceInputSpecifications>; /// \endcode /// /// Note that RequireData takes precedence over ExpectData, so /// ProximitySensorInput will be promoted to Required when the two /// specifications are combined. /// /// \sa ExpectData /// \sa RequireData template class SpecifyData; // ----------------------- Utility Templates ---------------------------- /// \brief This provides an upper limit on the number of expected data types /// in a CompositeData specification. This is an upper limit because when a /// data type is specified multiple times within a specficiation, it will /// be counted multiple times. As long as each data type is only specified /// once, it will provide an exact count. /// /// This is a constexpr so it will be evaluated at compile-time and behaves /// like a constant literal. template constexpr std::size_t CountUpperLimitOfExpectedData(); /// \brief Same as CountUpperLimitOfExpectedData() except it will count /// required data instead. template constexpr std::size_t CountUpperLimitOfRequiredData(); /// \brief Same as CountUpperLimitOfExpectedData() except you can specify /// what kind of data to count using SpecFinder. SpecFinder must accept a /// data specification (or void) as a template argument and provide a type /// called Data. See FindExpected and FindRequired below for examples. template class SpecFinder> constexpr std::size_t CountUpperLimitOfSpecifiedData(); /// \brief This allows us to specify that we are interested in expected /// data while performing template metaprogramming. /// /// This is currently used by CountUpperLimitOfExpectedData() template struct FindExpected { using Data = typename Specification::ExpectedData; }; /// \brief This specialization handles the terminating case where we have /// reached a leaf node in the specification tree. /// \private template <> struct FindExpected { using Data = void; }; /// \brief This allows us to specify that we are interested in required /// data while performing template metaprogramming. /// /// This is currently used by CountUpperLimitOfRequiredData() template struct FindRequired { using Data = typename Specification::RequiredData; }; /// \brief This specialization handles the terminating case where we have /// reached a leaf node in the specification tree. /// \private template <> struct FindRequired { using Data = void; }; /// \brief Provides a constexpr field named `value` whose value is true if /// and only if Data is expected by Specification template struct IsExpectedBy : std::integral_constant< bool, Specification::template Expects() > { }; /// \brief This template specialization allows us to provide a false `value` /// when given a void Specification. /// \private template struct IsExpectedBy : std::false_type { }; /// \brief Provides a constexpr field named `value` whose value is true if /// and only if Data is required by Specification template struct IsRequiredBy : std::integral_constant< bool, Specification::template AlwaysRequires() > { }; /// \brief This template specialization allows us to provide a false `value` /// when given a void Specification. /// \private template struct IsRequiredBy : std::false_type { }; } } #include "ignition/physics/detail/SpecifyData.hh" #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/EllipsoidShape.hh0000664000175000017500000001150114143524174027521 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_ELLIPSOIDSHAPE_HH_ #define IGNITION_PHYSICS_ELLIPSOIDSHAPE_HH_ #include #include #include namespace ignition { namespace physics { IGN_PHYSICS_DECLARE_SHAPE_TYPE(EllipsoidShape) class IGNITION_PHYSICS_VISIBLE GetEllipsoidShapeProperties : public virtual FeatureWithRequirements { public: template class EllipsoidShape : public virtual Entity { public: using Dimensions = typename FromPolicy::template Use; /// \brief Get the radius of this EllipsoidShape /// \return the radius of this EllipsoidShape public: Dimensions GetRadii() const; }; public: template class Implementation : public virtual Feature::Implementation { public: using Dimensions = typename FromPolicy::template Use; public: virtual Dimensions GetEllipsoidShapeRadii( const Identity &_ellipsoidID) const = 0; }; }; ///////////////////////////////////////////////// /// \brief This feature sets the EllipsoidShape properties such as /// the ellipsoid radii. class IGNITION_PHYSICS_VISIBLE SetEllipsoidShapeProperties : public virtual FeatureWithRequirements { public: template class EllipsoidShape : public virtual Entity { public: using Dimensions = typename FromPolicy::template Use; /// \brief Set the radius of this EllipsoidShape /// \param[in] _radii /// The desired radius of this EllipsoidShape public: void SetRadii(Dimensions _radii); }; public: template class Implementation : public virtual Feature::Implementation { public: using Dimensions = typename FromPolicy::template Use; public: virtual void SetEllipsoidShapeRadii( const Identity &_ellipsoidID, Dimensions _radii) = 0; }; }; ///////////////////////////////////////////////// /// \brief This feature constructs a new ellipsoid shape and attaches the /// desired pose in the link frame. The pose could be defined to be the /// ellipsoid center point in actual implementation. class IGNITION_PHYSICS_VISIBLE AttachEllipsoidShapeFeature : public virtual FeatureWithRequirements { public: template class Link : public virtual Feature::Link { public: using Dimensions = typename FromPolicy::template Use; public: using PoseType = typename FromPolicy::template Use; public: using ShapePtrType = EllipsoidShapePtr; /// \brief Rigidly attach a EllipsoidShape to this link. /// \param[in] _radii /// The radius of the ellipsoid. /// \param[in] _pose /// The desired pose of the EllipsoidShape relative to the Link frame. /// \returns a ShapePtrType to the newly constructed EllipsoidShape public: ShapePtrType AttachEllipsoidShape( const std::string &_name = "ellipsoid", Dimensions _radii = Dimensions::Constant(1.0), const PoseType &_pose = PoseType::Identity()); }; public: template class Implementation : public virtual Feature::Implementation { public: using Dimensions = typename FromPolicy::template Use; public: using PoseType = typename FromPolicy::template Use; public: virtual Identity AttachEllipsoidShape( const Identity &_linkID, const std::string &_name, Dimensions _radii, const PoseType &_pose) = 0; }; }; } } #include #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/detail/0000775000175000017500000000000014143524174025537 5ustar jriverojriveroign-physics-ignition-physics5_5.1.0/include/ignition/physics/detail/CapsuleShape.hh0000664000175000017500000000530714143524174030442 0ustar jriverojrivero/* * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DETAIL_CAPSULESHAPE_HH_ #define IGNITION_PHYSICS_DETAIL_CAPSULESHAPE_HH_ #include #include namespace ignition { namespace physics { ///////////////////////////////////////////////// template auto GetCapsuleShapeProperties::CapsuleShape ::GetRadius() const -> Scalar { return this->template Interface() ->GetCapsuleShapeRadius(this->identity); } ///////////////////////////////////////////////// template auto GetCapsuleShapeProperties::CapsuleShape ::GetLength() const -> Scalar { return this->template Interface() ->GetCapsuleShapeLength(this->identity); } ///////////////////////////////////////////////// template void SetCapsuleShapeProperties::CapsuleShape ::SetRadius(Scalar _radius) { this->template Interface() ->SetCapsuleShapeRadius(this->identity, _radius); } ///////////////////////////////////////////////// template void SetCapsuleShapeProperties::CapsuleShape ::SetLength(Scalar _length) { this->template Interface() ->SetCapsuleShapeLength(this->identity, _length); } ///////////////////////////////////////////////// template auto AttachCapsuleShapeFeature::Link ::AttachCapsuleShape( const std::string &_name, Scalar _radius, Scalar _length, const PoseType &_pose) -> ShapePtrType { return ShapePtrType(this->pimpl, this->template Interface() ->AttachCapsuleShape( this->identity, _name, _radius, _length, _pose)); } } } #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/detail/ConstructEmpty.hh0000664000175000017500000000460314143524174031066 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DETAIL_CONSTRUCTEMPTY_HH_ #define IGNITION_PHYSICS_DETAIL_CONSTRUCTEMPTY_HH_ #include #include namespace ignition { namespace physics { ///////////////////////////////////////////////// template auto ConstructEmptyWorldFeature::Engine ::ConstructEmptyWorld(const std::string &_name) -> WorldPtrType { return WorldPtrType(this->pimpl, this->template Interface() ->ConstructEmptyWorld(this->identity, _name)); } ///////////////////////////////////////////////// template auto ConstructEmptyModelFeature::World ::ConstructEmptyModel(const std::string &_name) -> ModelPtrType { return ModelPtrType(this->pimpl, this->template Interface() ->ConstructEmptyModel(this->identity, _name)); } ///////////////////////////////////////////////// template auto ConstructEmptyNestedModelFeature::Model ::ConstructEmptyNestedModel(const std::string &_name) -> ModelPtrType { return ModelPtrType(this->pimpl, this->template Interface() ->ConstructEmptyNestedModel(this->identity, _name)); } ///////////////////////////////////////////////// template auto ConstructEmptyLinkFeature::Model ::ConstructEmptyLink(const std::string &_name) -> LinkPtrType { return LinkPtrType(this->pimpl, this->template Interface() ->ConstructEmptyLink(this->identity, _name)); } } } #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/detail/FixedJoint.hh0000664000175000017500000000242514143524174030126 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DETAIL_FIXEDJOINT_HH_ #define IGNITION_PHYSICS_DETAIL_FIXEDJOINT_HH_ #include #include namespace ignition { namespace physics { ///////////////////////////////////////////////// template auto AttachFixedJointFeature::Link::AttachFixedJoint( const BaseLinkPtr &_parent, const std::string &_name) -> JointPtrType { return JointPtrType(this->pimpl, this->template Interface() ->AttachFixedJoint(this->identity, _parent, _name)); } } } #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/detail/GetContacts.hh0000664000175000017500000000476714143524174030314 0ustar jriverojrivero/* * Copyright (C) 2019 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DETAIL_GETCONTACTS_HH_ #define IGNITION_PHYSICS_DETAIL_GETCONTACTS_HH_ #include #include #include namespace ignition { namespace physics { ///////////////////////////////////////////////// template auto GetContactsFromLastStepFeature::World< PolicyT, FeaturesT>::GetContactsFromLastStep() const -> std::vector { auto contactsInternal = this->template Interface() ->GetContactsFromLastStep(this->identity); // contactInternal provides the identities of collision1 and collision2. Here, // we create a new vector and create ShapePtrs out of those identities. Seems // inefficient, but I don't know if there's a better way to handle this. std::vector output; output.reserve(contactsInternal.size()); for (auto &contact : contactsInternal) { ContactPoint contactPoint{ShapePtrType(this->pimpl, contact.collision1), ShapePtrType(this->pimpl, contact.collision2), contact.point}; // Note: Using emplace_back and using Get on the resulting reference seems // to be the only way to add contacts into the vector. Using push_back like // the following does not work: // Contact contactOutput // contactOutput.template Get() = std::move(contactPoint); // output.push_back(contactOutput); // auto &contactOutput = output.emplace_back(); contactOutput.template Get() = std::move(contactPoint); auto *extraContactData = contact.extraData.template Query(); if (extraContactData) { contactOutput.template Get() = std::move(*extraContactData); } } return output; } } // namespace physics } // namespace ignition #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/detail/DeclareDerivedType.hh0000664000175000017500000001142314143524174031565 0ustar jriverojrivero/* * Copyright (C) 2018 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DETAIL_DECLAREDERIVEDTYPE_HH_ #define IGNITION_PHYSICS_DETAIL_DECLAREDERIVEDTYPE_HH_ #include #include #include #define DETAIL_IGN_PHYSICS_PREDEFINE_DERIVED_POLICY(Derived, P) \ template \ using Derived ## P = Derived; \ template \ using Derived ## P ## Ptr = \ ::ignition::physics::EntityPtr>; \ template \ using Const ## Derived ## P ## Ptr = \ ::ignition::physics::EntityPtr>; #define DETAIL_IGN_PHYSICS_DECLARE_DERIVED_TYPE(Base, Derived) \ struct Derived ## Cast : public virtual ::ignition::physics::Feature \ { \ IGN_PHYSICS_CREATE_SELECTOR(Derived) \ class Derived ## Identifier { }; \ \ public: template \ class Using : \ public virtual ::ignition::physics::detail::ExtractAPI< \ Select ## Derived, FeaturesT>:: \ template type, \ public virtual ::ignition::physics::detail::ExtractAPI< \ ::ignition::physics::detail:: Select ## Base, FeaturesT>:: \ template type \ { \ public: using Identifier = Derived ## Identifier; \ public: using UpcastIdentifiers = std::tuple< \ Derived ## Identifier, \ /* Allow this derived type to be upcast to plain base types */ \ ::ignition::physics::detail:: Base ## Identifier>; \ public: using EntityBase = \ ::ignition::physics::Entity; \ \ public: Using(const std::shared_ptr &_pimpl, \ const ::ignition::physics::Identity &_identity) \ : EntityBase(_pimpl, _identity) { } \ public: Using() = default; \ public: Using(const Using&) = default; \ public: Using(Using&&) noexcept = default; \ \ /* We customize these operators because of virtual inheritance */ \ public: Using &operator=(const Using &_other) \ { \ static_cast(*this) = _other; \ return *this; \ } \ public: Using &operator=(Using &&_other) noexcept \ { \ static_cast(*this) = std::move(_other); \ return *this; \ } \ }; \ \ template \ class Base : \ public virtual ::ignition::physics::Feature:: Base \ { \ public: using CastReturnType = \ ::ignition::physics::EntityPtr>; \ public: using ConstCastReturnType = \ ::ignition::physics::EntityPtr>; \ public: CastReturnType CastTo ## Derived() \ { \ return CastReturnType(this->pimpl, \ this->template Interface() \ ->CastTo ## Derived(this->identity)); \ } \ \ public: ConstCastReturnType \ CastTo ## Derived() const \ { \ return ConstCastReturnType(this->pimpl, \ this->template Interface() \ ->CastTo ## Derived(this->identity)); \ } \ }; \ \ template \ class Implementation \ : public virtual ::ignition::physics::Feature::Implementation \ { \ public: virtual ::ignition::physics::Identity CastTo ## Derived( \ const Identity &_id) const = 0; \ }; \ }; \ \ template \ using Derived = Derived ## Cast::Using; \ template \ using Derived ## Ptr = \ ::ignition::physics::EntityPtr>; \ template \ using Const ## Derived ## Ptr = \ ::ignition::physics::EntityPtr>; \ DETAIL_IGN_PHYSICS_PREDEFINE_DERIVED_POLICY(Derived, 3d) \ DETAIL_IGN_PHYSICS_PREDEFINE_DERIVED_POLICY(Derived, 2d) \ DETAIL_IGN_PHYSICS_PREDEFINE_DERIVED_POLICY(Derived, 3f) \ DETAIL_IGN_PHYSICS_PREDEFINE_DERIVED_POLICY(Derived, 2f) #endif ign-physics-ignition-physics5_5.1.0/include/ignition/physics/detail/SpecifyData.hh0000664000175000017500000004655714143524174030275 0ustar jriverojrivero/* * Copyright (C) 2017 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #ifndef IGNITION_PHYSICS_DETAIL_SPECIFYDATA_HH_ #define IGNITION_PHYSICS_DETAIL_SPECIFYDATA_HH_ #include #include #include "ignition/physics/SpecifyData.hh" namespace ignition { namespace physics { ///////////////////////////////////////////////// template ExpectData::ExpectData() : CompositeData(), privateExpectData( this->dataMap.insert( std::make_pair(typeid(Expected).name(), CompositeData::DataEntry())).first) { // Do nothing } ///////////////////////////////////////////////// template ExpectData::ExpectData(const ExpectData &) : ExpectData() { // Do nothing } ///////////////////////////////////////////////// template template Data &ExpectData::Get() { return this->ExpectData::privateExpectData .Get(this, detail::type()); } ///////////////////////////////////////////////// template template auto ExpectData::InsertOrAssign(Args&&... _args) -> InsertResult { return this->ExpectData::privateExpectData .InsertOrAssign(this, detail::type(), std::forward(_args)...); } ///////////////////////////////////////////////// template template auto ExpectData::Insert(Args&&... _args) -> InsertResult { return this->ExpectData::privateExpectData .Insert(this, detail::type(), std::forward(_args)...); } ///////////////////////////////////////////////// template template bool ExpectData::Remove() { return this->ExpectData::privateExpectData .Remove(this, detail::type()); } ///////////////////////////////////////////////// template template Data *ExpectData::Query(const QueryMode _mode) { return this->ExpectData::privateExpectData .Query(this, detail::type(), _mode); } ///////////////////////////////////////////////// template template const Data *ExpectData::Query(const QueryMode _mode) const { return this->ExpectData::privateExpectData .Query(this, detail::type(), _mode); } ///////////////////////////////////////////////// template template bool ExpectData::Has() const { return this->ExpectData::privateExpectData .Has(this, detail::type()); } ///////////////////////////////////////////////// template template CompositeData::DataStatus ExpectData::StatusOf() const { return this->ExpectData::privateExpectData .StatusOf(this, detail::type()); } ///////////////////////////////////////////////// template template bool ExpectData::Unquery() const { return this->ExpectData::privateExpectData .Unquery(this, detail::type()); } ///////////////////////////////////////////////// template template Data &ExpectData::MakeRequired(Args&&... _args) { return this->ExpectData::privateExpectData .MakeRequired( this, detail::type(), std::forward(_args)...); } ///////////////////////////////////////////////// template template bool ExpectData::Requires() const { return this->ExpectData::privateExpectData .Requires(this, detail::type()); } ///////////////////////////////////////////////// template template constexpr bool ExpectData::Expects() { return detail::PrivateExpectData::Expects(detail::type()); } ///////////////////////////////////////////////// template RequireData::RequireData() : CompositeData(), ExpectData() { CompositeData::DataEntry &entry = this->ExpectData::privateExpectData .expectedIterator->second; // Create the required data in its designated map entry, and mark it as // required for runtime checking. entry.data = std::unique_ptr(new MakeCloneable()); entry.required = true; ++CompositeData::numEntries; } ///////////////////////////////////////////////// template template const Data &RequireData::Get() const { const CompositeData::MapOfData::iterator &it = this->ExpectData::privateExpectData .expectedIterator; return this->RequireData::privateRequireData.Get( this, it, detail::type()); } ///////////////////////////////////////////////// template template constexpr bool RequireData::AlwaysRequires() { return detail::PrivateRequireData::AlwaysRequires( detail::type()); } namespace detail { /// The following templates deal with finding leaf specifiers within a /// specification tree. An example of a leaf specifier is ExpectData /// or RequireData. Casting a complex specification to its relevant /// leaf specifier allows us to utilize the extremely high-speed access /// functions provided by the leaf specifier. // Forward declaration template class Condition> struct SelectSpecifierIfAvailable; /// \brief Default definition for this template. This definition will be /// invoked when S1 is true and some sub-specifications exist. /// /// The fact that S1 is true means that SubSpec1 met the conditions for /// the type of specifier that we are looking for. Therefore, we will /// branch into S1 and search for the exact leaf specifier that we want. template class Condition, typename SubSpec1, typename SubSpec2, bool S1> struct SelectSpecifierIfAvailableImpl { using Specifier = typename SelectSpecifierIfAvailable< Data, SubSpec1, Condition>::Specifier; }; /// \brief Specialized definition which is invoked when S1 is false and /// some sub-specifications exist. /// /// The fact that S1 is false means that it would be futile to branch /// into SubSpec1. Therefore, we will instead branch into SubSpec2 in the /// hopes that it contains the specification that we want (even though it /// might not actually contain what we want). template class Condition, typename SubSpec1, typename SubSpec2> struct SelectSpecifierIfAvailableImpl< Data, Specification, Condition, SubSpec1, SubSpec2, false> { using Specifier = typename SelectSpecifierIfAvailable< Data, SubSpec2, Condition>::Specifier; }; /// \brief Specialized definition which is invoked when we've reached a /// leaf specification. Either this is the specification that we want, or /// else the specification that we want does not exist in the original /// specification. template class Condition> struct SelectSpecifierIfAvailableImpl< Data, Specification, Condition, void, void, false> { using Specifier = Specification; }; /// \brief Find the leaf specifier within Specification which meets the /// Condition for Data. If Specification is not able to satisfy the /// Condition, then this will simply provide the last leaf specifier /// within Specification, whatever that may be. template class Condition> struct SelectSpecifierIfAvailable { using Specifier = typename SelectSpecifierIfAvailableImpl< Data, Specification, Condition, typename Specification::SubSpecification1, typename Specification::SubSpecification2, Condition::value> ::Specifier; }; /// \brief A version of SelectSpecifierIfAvailable that returns the leaf /// specifier which expects Data. template struct SelectExpectorIfAvailable { using Expector = typename SelectSpecifierIfAvailable< Data, Specification, IsExpectedBy>::Specifier; }; /// \brief A version of SelectSpecifierIfAvailable that returns the leaf /// specifier which requires Data template struct SelectRequirerIfAvailable { using Requirer = typename SelectSpecifierIfAvailable< Data, Specification, IsRequiredBy>::Specifier; }; } ///////////////////////////////////////////////// /// \private template class SpecifyData : public virtual DataSpec { // We provide this template specialization in case someone // wants to use the SpecifyData<~> format with only one specification, // e.g.: // // using MySpecification = SpecifyData>; /// \brief Virtual destructor public: virtual ~SpecifyData() = default; }; /// \brief Create a function which casts this to the relevant leaf specifier /// and then invokes the requested function on it. #define DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( \ ReturnType, Function, Suffix, CastTo, Args) \ ReturnType Function Suffix \ { \ using Expector = typename detail::SelectExpectorIfAvailable< \ T, Specification>::Expector; \ return static_cast(this)->template Function Args; \ } ///////////////////////////////////////////////// /// \brief Create a fork in the binary specification tree /// \private template class SpecifyData : public virtual DataSpec1, public virtual DataSpec2 { public: virtual ~SpecifyData() = default; // These allow outside users to identify the specifications at compile // time, which can be useful for template metaprogramming. public: using Specification = SpecifyData; public: using SubSpecification1 = DataSpec1; public: using SubSpecification2 = DataSpec2; // These allow us to use SFINAE to tunnel down into deeper specifications public: using ExpectedData = void; public: using RequiredData = void; template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( T&, Get, (), Expector, ()) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( CompositeData::InsertResult, InsertOrAssign, (Args&&... args), Expector, (std::forward(args)...)) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( CompositeData::InsertResult, Insert, (Args&&... args), Expector, (std::forward(args)...)) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( bool, Remove, (), Expector, ()) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( T*, Query, (const CompositeData::QueryMode mode = CompositeData::QueryMode::NORMAL), Expector, (mode)) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( const T*, Query, (const CompositeData::QueryMode mode = CompositeData::QueryMode::NORMAL) const, const Expector, (mode)) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( bool, Has, () const, const Expector, ()) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( CompositeData::DataStatus, StatusOf, () const, const Expector, ()) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( bool, Unquery, () const, const Expector, ()) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( T&, MakeRequired, (Args&&... args), Expector, (std::forward(args)...)) template DETAIL_IGN_PHYSICS_SPECIFYDATA_DISPATCH( bool, Requires, () const, const Expector, ()) template static constexpr bool Expects() { return ( DataSpec1::template Expects() || DataSpec2::template Expects()); } template const T &Get() const { static_assert(AlwaysRequires(), IGNITION_PHYSICS_CONST_GET_ERROR); using Requirer = typename detail::SelectRequirerIfAvailable< T, Specification>::Requirer; return static_cast(this)->template Get(); } template static constexpr bool AlwaysRequires() { return ( DataSpec1::template AlwaysRequires() || DataSpec2::template AlwaysRequires()); } }; ///////////////////////////////////////////////// /// Use the SpecifyData fork to combine these expectations /// \private template class ExpectData : public virtual SpecifyData, ExpectData> { public: virtual ~ExpectData() = default; }; ///////////////////////////////////////////////// /// Use the SpecifyData fork to combine these requirements /// \private template class RequireData : public virtual SpecifyData, RequireData> { public: virtual ~RequireData() = default; }; namespace detail { /// This will get called if the specification has both a Data specified /// and also contains sub-specifications. In our current implementation, /// this never happens, but we can provide this just in case we change the /// implementation. template class SpecFinder> struct SpecificationDataCounterImpl { public: static constexpr std::size_t Count() { return 1 + SpecificationDataCounterImpl< typename SpecFinder::Data, typename SubSpec1::SubSpecification1, typename SubSpec1::SubSpecification2, SpecFinder>::Count() + SpecificationDataCounterImpl< typename SpecFinder::Data, typename SubSpec2::SubSpecification1, typename SubSpec2::SubSpecification2, SpecFinder>::Count(); } }; /// This will get called if the specification does not specify data but /// does provide sub-specifications template class SpecFinder> struct SpecificationDataCounterImpl { public: static constexpr std::size_t Count() { return 0 + SpecificationDataCounterImpl< typename SpecFinder::Data, typename SubSpec1::SubSpecification1, typename SubSpec1::SubSpecification2, SpecFinder>::Count() + SpecificationDataCounterImpl< typename SpecFinder::Data, typename SubSpec2::SubSpecification1, typename SubSpec2::SubSpecification2, SpecFinder>::Count(); } }; /// This will get called if the specification specifies data but does not /// provide sub-specifications. We count the data once and terminate /// this branch. template class SpecFinder> struct SpecificationDataCounterImpl { public: static constexpr std::size_t Count() { return 1; } }; /// This will get called if the specification does not specify data and /// also does not provide sub-specifications. We count nothing and /// terminate this branch. template