diff --git a/.clangd b/.clangd new file mode 100644 index 0000000000..1ee2795421 --- /dev/null +++ b/.clangd @@ -0,0 +1,9 @@ +If: + PathMatch: .*\.hxx +CompileFlags: + Add: + - -DPINOCCHIO_LSP +--- +Diagnostics: + Suppress: + - unused-includes diff --git a/development/convention.md b/development/convention.md new file mode 100644 index 0000000000..584105036c --- /dev/null +++ b/development/convention.md @@ -0,0 +1,87 @@ +# Coding convention + +## Headers + +Pinocchio has 4 kinds of headers: +- Public headers: `xxx.hpp` +- Private headers: + - Declaration or declaration and definition when not spited: `xxx.hxx` + - Definition: `xxx-def.hxx` + - Explicit template instantiation: `xxx-inst.hxx` + +Since Pinocchio is mostly a header only library, include cycles are easy to create. + +As a trivial example: + +`a.hpp`: +``` +#pragma once +#include "b.hpp" + +struct A +{ + B* b; +}; +``` + +`b.hpp`: +``` +#pragma once +#include "a.hpp" + +struct B +{ + A* a; +}; +``` + +If we build a file that include `a.hpp` we will have the following error: +```bash +b.hpp:5:3: error: ‘A’ does not name a type +``` +Since `a.hpp` is already included, the guard prevent to include it again +in `b.hpp` and `A` is never defined. +This kind of error can be really tricky to debug, +especially in header only libraries that can have big include chain. + +To prevent this we apply the following rule: +- Private header can't include anything +- Public header can include private header + +Public headers will have the duty to include anything needed by private ones it exposes. +This will prevent cycles since transitive includes are now forbidden. + +This approach have two drawbacks: +- Transitive include allow to save some code, we will have a lot of redundant code in public headers +- In private headers, language server will know no symbols (since the dependencies are not included) + +To overcome the second issue, we can create the following code snippet to add after the guard of any private header: +```cpp +#ifdef PINOCCHIO_LSP +#include "xxx.hpp" +#endif // PINOCCHIO_LSP +``` +Where `xxx.hpp` is the public header that expose this private header. + +By setting the following `.clangd` file at the project root the LSP will be able to see the private header dependencies: +```yaml +If: + PathMatch: .*\.hxx +CompileFlags: + Add: + - -DPINOCCHIO_LSP +``` + +### Guard + +Pinocchio doesn't use `pragma once` as a guard. Instead, we use the file path converted into snake_case. + +As an example `include/pinocchio/spatial/se3.hpp` will have the following guard: +```cpp +#ifndef __pinocchio_spatial_se3_hpp__ +#define __pinocchio_spatial_se3_hpp__ + +// ... + +#endif // ifndef __pinocchio_spatial_se3_hpp__ +``` diff --git a/development/scripts/misc/refactor_headers.py b/development/scripts/misc/refactor_headers.py new file mode 100755 index 0000000000..133362d7ae --- /dev/null +++ b/development/scripts/misc/refactor_headers.py @@ -0,0 +1,453 @@ +#! /usr/bin/env python3 +# Refactor headers according to +# https://github.com/stack-of-tasks/pinocchio/pull/2572 +# Run test with python -m doctest -v refactor_headers.py + +import re +import argparse +import subprocess + +from pathlib import Path + +GUARD_PATTERN = re.compile("^#ifndef __(.*)__$", re.MULTILINE) +INCLUDE_PATTERN = re.compile(r"^#include ([\"<].*[\">])\n", re.MULTILINE) + +TEST_CPP_CONTENT_GOOD_GUARD = """// +// Copyright (c) 2015-2024 CNRS INRIA +// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// + +#ifndef __pinocchio_python_spatial_se3_hpp__ +#define __pinocchio_python_spatial_se3_hpp__ + +#include +#include + +#include "pinocchio/spatial/se3.hpp" +#include "pinocchio/spatial/explog.hpp" +// ... +#endif // ifndef __pinocchio_python_spatial_se3_hpp__""" + +TEST_CPP_CONTENT_BAD_GUARD = """// +// Copyright (c) 2015-2024 CNRS INRIA +// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// + +#ifndef _pinocchio_python_spatial_se3_hpp__ +#define _pinocchio_python_spatial_se3_hpp__ +// ... +#endif // ifndef _pinocchio_python_spatial_se3i_hpp__""" + +HPP_MODULE_INST_GUARD = """ +/// Explicit template instantiation +#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION + #include "{module_inst}" +#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION +""" + +HPP_MODULE = """// +// Copyright (c) 2025 INRIA +// + +#ifndef {guard} +#define {guard} + +// Module dependencies +{dep_includes} + +// Module headers +{module_includes} + +{module_inst_include} + +#endif // ifndef {guard} +""" + +LSP_GUARD = """#ifdef PINOCCHIO_LSP +#include "{module_hpp}" +#endif // PINOCCHIO_LSP +""" + + +def find_guard(content: str) -> None | str: + """Find guard in a C++ header file. + :ivar content: File content to parse. + :return: Guard name if found (without __), None if no guard is found. + >>> find_guard(TEST_CPP_CONTENT_GOOD_GUARD) + 'pinocchio_python_spatial_se3_hpp' + >>> find_guard(TEST_CPP_CONTENT_BAD_GUARD) + """ + match = GUARD_PATTERN.search(content) + if match: + return match.group(1) + return None + + +def update_guard(content: str, old_guard_name, new_guard_name: str) -> None | str: + """Replace guards in a C++ header file. + :ivar content: File content to parse. + :ivar old_guard_name: Guard to replace. + :ivar new_guard_name: New guard name. + :return: New content if the 3 guards are changed, None otherwise. + >>> res = update_guard(TEST_CPP_CONTENT_GOOD_GUARD, "pinocchio_python_spatial_se3_hpp", "pinocchio_python_spatial_se3_def_hpp") + >>> print(res) + // + // Copyright (c) 2015-2024 CNRS INRIA + // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. + // + + #ifndef __pinocchio_python_spatial_se3_def_hpp__ + #define __pinocchio_python_spatial_se3_def_hpp__ + + #include + #include + + #include "pinocchio/spatial/se3.hpp" + #include "pinocchio/spatial/explog.hpp" + // ... + #endif // ifndef __pinocchio_python_spatial_se3_def_hpp__ + >>> update_guard(TEST_CPP_CONTENT_BAD_GUARD, "pinocchio_python_spatial_se3_hpp", "pinocchio_python_spatial_se3_def_hpp") + """ + (new_content, nr_sub) = re.subn(f"{old_guard_name}", new_guard_name, content) + if nr_sub == 3: + return new_content + return None + + +def remove_includes(content: str, module_header: str) -> (str, list[str]): + """Remove includes, add LSP guard instead and return them. + :ivar content: File content to parse. + :ivar module_header: Module header path (without < or ") + :return: New content and list of removed includes (path with < or ") + >>> res = remove_includes(TEST_CPP_CONTENT_GOOD_GUARD, "pinocchio/bindings/python/spatial/se3.hpp") + >>> print(res[0]) + // + // Copyright (c) 2015-2024 CNRS INRIA + // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. + // + + #ifndef __pinocchio_python_spatial_se3_hpp__ + #define __pinocchio_python_spatial_se3_hpp__ + + #ifdef PINOCCHIO_LSP + #include "pinocchio/bindings/python/spatial/se3.hpp" + #endif // PINOCCHIO_LSP + + // ... + #endif // ifndef __pinocchio_python_spatial_se3_hpp__ + >>> print(res[1]) + ['', '', '"pinocchio/spatial/se3.hpp"', '"pinocchio/spatial/explog.hpp"'] + """ + includes = INCLUDE_PATTERN.findall(content) + if len(includes) == 0: + print("\tNo include, PINOCCHIO_LSP will not be added") + tmp_pattern = "$$$$$ BLBLBBLLB #####" + new_content = INCLUDE_PATTERN.sub(tmp_pattern, content) + new_content2 = new_content.replace( + tmp_pattern, LSP_GUARD.format(module_hpp=module_header), 1 + ) + new_content3 = new_content2.replace(tmp_pattern, "") + return new_content3, includes + + +def create_hpp_module( + guard: str, + dependencies_includes: list[str], + module_includes: list[str], + module_inst_include: str | None, +) -> str: + """Create a module content. + :ivar guard: Guard name. + :ivar dependencies_includes: Module dependencies include paths (path with < or "). + :ivar module_includes: Module internal include paths (path without < or "). + :ivar module_inst: Module explicit template instantiation (path without < or "). + :return: Module content. + >>> res = create_hpp_module("__pinocchio_python_spatial_se3_hpp__",\ + ['', '', '"pinocchio/spatial/se3.hpp"', '"pinocchio/spatial/explog.hpp"'],\ + ['pinocchio/bindings/python/spatial/se3_decl.hxx', 'pinocchio/bindings/python/spatial/se3_def.hxx'],\ + None) + >>> print(res) + // + // Copyright (c) 2025 INRIA + // + + #ifndef __pinocchio_python_spatial_se3_hpp__ + #define __pinocchio_python_spatial_se3_hpp__ + + // Module dependencies + #include + #include + #include "pinocchio/spatial/se3.hpp" + #include "pinocchio/spatial/explog.hpp" + + // Module headers + #include "pinocchio/bindings/python/spatial/se3_decl.hxx" + #include "pinocchio/bindings/python/spatial/se3_def.hxx" + + + + #endif // ifndef __pinocchio_python_spatial_se3_hpp__ + + >>> res = create_hpp_module("__pinocchio_python_spatial_se3_hpp__",\ + ['', '', '"pinocchio/spatial/se3.hpp"', '"pinocchio/spatial/explog.hpp"'],\ + ['pinocchio/bindings/python/spatial/se3_decl.hxx', 'pinocchio/bindings/python/spatial/se3_def.hxx'],\ + "pinocchio/bindings/python/spatial/se3_inst.hxx") + >>> print(res) + // + // Copyright (c) 2025 INRIA + // + + #ifndef __pinocchio_python_spatial_se3_hpp__ + #define __pinocchio_python_spatial_se3_hpp__ + + // Module dependencies + #include + #include + #include "pinocchio/spatial/se3.hpp" + #include "pinocchio/spatial/explog.hpp" + + // Module headers + #include "pinocchio/bindings/python/spatial/se3_decl.hxx" + #include "pinocchio/bindings/python/spatial/se3_def.hxx" + + + /// Explicit template instantiation + #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION + #include "pinocchio/bindings/python/spatial/se3_inst.hxx" + #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION + + + #endif // ifndef __pinocchio_python_spatial_se3_hpp__ + + """ + deps = "\n".join([f"#include {d}" for d in dependencies_includes]) + modules = "\n".join([f'#include "{d}"' for d in module_includes]) + module_inst = "" + if module_inst_include is not None: + module_inst = HPP_MODULE_INST_GUARD.format(module_inst=module_inst_include) + return HPP_MODULE.format( + guard=guard, + dep_includes=deps, + module_includes=modules, + module_inst_include=module_inst, + ) + + +def guard_from_path(path: Path) -> str: + """Compute the guard from the relative path + >>> guard_from_path(Path("pinocchio/algorithm/kinematics.hxx")) + 'pinocchio_algorithm_kinematics_hxx' + """ + guard_content = "_".join( + map(lambda x: x.replace(".", "_").replace("-", "_"), path.parts) + ) + return f"{guard_content}" + + +def update_content( + path: Path, + module_path: Path, + new_guard: str, + dry: bool = False, +) -> list[str]: + """Update path content with the following rule: + :ivar path: File to update. + :ivar module_path: Module path relative to root directory. + :ivar new_guard: New guard. + :ivar dry: Don modify the file. + - Update guards + - Remove includes + - Add clangd_hack + """ + content = "" + with path.open() as path_desc: + content = path_desc.read() + old_guard = find_guard(content) + if old_guard is None: + raise RuntimeError(f"{path} doesn't contains valid guards") + print(f"\tNew guard: {new_guard}") + content = update_guard(content, old_guard, new_guard) + if content is None: + raise RuntimeError(f"{path} doesn't have three guards") + content, includes = remove_includes(content, str(module_path)) + if not dry: + with path.open("w") as path: + path.write(content) + return includes, old_guard + + +def update_module(module_path: Path, include_root_path: Path, dry: bool = False): + """Apply new convention to a module + :ivar module_path: Path to the hpp (module entry point) to refactor. Must be an absolute path. + :ivar include_root_path: Path to the include directory. Me be an absolute path. + """ + assert module_path.is_absolute() + assert include_root_path.is_absolute() + + print(f"Update {module_path}") + + module_path_rel = module_path.relative_to(include_root_path) + module_old_def = module_path.parent / Path(f"{module_path.stem}.hxx") + module_old_inst = module_path.parent / Path(f"{module_path.stem}.txx") + module_decl = module_path.parent / Path(f"{module_path.stem}.hxx") + module_def = module_path.parent / Path(f"{module_path.stem}-def.hxx") + module_inst = module_path.parent / Path(f"{module_path.stem}-inst.hxx") + + dependency_includes = [] + module_includes = [] + + # We must process module_old_def first because his old name + # clash with module_decl new name. + if module_old_def.exists(): + print(f"\tConvert {module_old_def} into {module_def}") + def_includes, _ = update_content( + module_old_def, + module_path_rel, + guard_from_path(module_def.relative_to(include_root_path)), + dry, + ) + dependency_includes += def_includes + if not dry: + subprocess.run( + [ + "git", + "-C", + str(include_root_path), + "mv", + str(module_old_def), + str(module_def), + ] + ) + module_includes.append(str(module_def.relative_to(include_root_path))) + + print(f"\tConvert {module_path} into {module_decl}") + # Declaration/Definition should be before definition + module_includes.insert(0, str(module_decl.relative_to(include_root_path))) + module_inst_include = None + + decl_includes, module_guard = update_content( + module_path, + module_path_rel, + guard_from_path(module_decl.relative_to(include_root_path)), + dry, + ) + dependency_includes += decl_includes + if not dry: + subprocess.run( + [ + "git", + "-C", + str(include_root_path), + "mv", + str(module_path), + str(module_decl), + ] + ) + + if module_old_inst.exists(): + print(f"\tConvert {module_old_inst} into {module_inst}") + inst_includes, _ = update_content( + module_old_inst, + module_path_rel, + guard_from_path(module_inst.relative_to(include_root_path)), + dry, + ) + dependency_includes += inst_includes + if not dry: + subprocess.run( + [ + "git", + "-C", + str(include_root_path), + "mv", + str(module_old_inst), + str(module_inst), + ] + ) + module_inst_include = str(module_inst.relative_to(include_root_path)) + + print(f"\tCreate new module {module_path}") + print(f"\tNew module guard: {module_guard}") + print(f"\tNew module dependencies: {', '.join(dependency_includes)}") + print(f"\tNew module includes: {', '.join(module_includes)}") + print(f"\tNew module inst: {module_inst_include}") + module_content = create_hpp_module( + module_guard, dependency_includes, module_includes, module_inst_include + ) + if not dry: + with module_path.open("w") as module_path_desc: + module_path_desc.write(module_content) + print() + + +def is_dir(dir: str) -> Path: + p = Path(dir) + if not p.is_dir(): + raise argparse.ArgumentTypeError(f"{dir} is not a directory") + return p + + +def is_file(file: str) -> Path: + p = Path(file) + if not p.is_file(): + raise argparse.ArgumentTypeError(f"{file} is not a file") + return p + + +def argument_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Refactor headers in a Pinocchio subdirectory" + ) + parser.add_argument("include_directory", help="Include directory", type=is_dir) + parser.add_argument("--dry", help="Dry run", action="store_true") + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument( + "-m", "--module", help="Module to convert", type=is_file, default=None + ) + group.add_argument( + "-d", "--dir", help="Directory to convert", type=is_dir, default=None + ) + return parser + + +def main(args: list[str]): + """ + Walk in a directory + Find all xxx.hpp + Rename it into xxx.hxx + Update guards + Remove includes + Add clangd_hack + if xxx.hxx associated + Rename into xxx-def.hxx + Update guards + Remove includes + Add clangd_hack + if xxx.txx associated + Rename into xxx-inst.hxx + Update guards + Remove includes + Add clangd_hack + Create xxx.hpp + Add guards + Add includes form decl, def and inst + include decl, def and inst + """ + parser = argument_parser() + args = parser.parse_args(args) + + if args.module is not None: + update_module( + args.module.absolute(), args.include_directory.absolute(), args.dry + ) + elif args.dir is not None: + for m in args.dir.glob("*.hpp"): + update_module(m.absolute(), args.include_directory.absolute(), args.dry) + else: + raise RuntimeError("module or dir argument must be provided") + + +if __name__ == "__main__": + import sys + + main(sys.argv[1:]) diff --git a/include/pinocchio/spatial/se3-base-def.hxx b/include/pinocchio/spatial/se3-base-def.hxx new file mode 100644 index 0000000000..4410bc427d --- /dev/null +++ b/include/pinocchio/spatial/se3-base-def.hxx @@ -0,0 +1,225 @@ +// +// Copyright (c) 2015-2025 CNRS INRIA +// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// + +#ifndef __pinocchio_spatial_se3_base_def_hxx__ +#define __pinocchio_spatial_se3_base_def_hxx__ + +#ifdef PINOCCHIO_LSP + #include "pinocchio/spatial/se3-base.hpp" +#endif + +namespace pinocchio +{ + /** \brief Base class for rigid transformation. + * + * The rigid transform aMb can be seen in two ways: + * + * - given a point p expressed in frame B by its coordinate vector \f$ ^bp \f$, \f$ ^aM_b \f$ + * computes its coordinates in frame A by \f$ ^ap = {}^aM_b {}^bp \f$. + * - \f$ ^aM_b \f$ displaces a solid S centered at frame A into the solid centered in + * B. In particular, the origin of A is displaced at the origin of B: + * \f$^aM_b {}^aA = {}^aB \f$. + + * The rigid displacement is stored as a rotation matrix and translation vector by: + * \f$ ^aM_b x = {}^aR_b x + {}^aAB \f$ + * where \f$^aAB\f$ is the vector from origin A to origin B expressed in coordinates A. + * + * \cheatsheet \f$ {}^aM_c = {}^aM_b {}^bM_c \f$ + * + * \ingroup pinocchio_spatial + */ + template + struct SE3Base : NumericalBase + { + PINOCCHIO_SE3_TYPEDEF_TPL(Derived); + + Derived & derived() + { + return *static_cast(this); + } + const Derived & derived() const + { + return *static_cast(this); + } + + Derived & const_cast_derived() const + { + return *const_cast(&derived()); + } + + ConstAngularRef rotation() const + { + return derived().rotation_impl(); + } + ConstLinearRef translation() const + { + return derived().translation_impl(); + } + AngularRef rotation() + { + return derived().rotation_impl(); + } + LinearRef translation() + { + return derived().translation_impl(); + } + void rotation(const AngularType & R) + { + derived().rotation_impl(R); + } + void translation(const LinearType & t) + { + derived().translation_impl(t); + } + + HomogeneousMatrixType toHomogeneousMatrix() const + { + return derived().toHomogeneousMatrix_impl(); + } + operator HomogeneousMatrixType() const + { + return toHomogeneousMatrix(); + } + + /** + * @brief The action matrix \f$ {}^aX_b \f$ of \f$ {}^aM_b \f$. + * + * With \f$ {}^aM_b = \left( \begin{array}{cc} R & t \\ 0 & 1 \\ \end{array} \right) \f$, + * \f[ + * {}^aX_b = \left( \begin{array}{cc} R & \hat{t} R \\ 0 & R \\ \end{array} \right) + * \f] + * + * \cheatsheet \f$ {}^a\nu_c = {}^aX_b {}^b\nu_c \f$ + */ + ActionMatrixType toActionMatrix() const + { + return derived().toActionMatrix_impl(); + } + operator ActionMatrixType() const + { + return toActionMatrix(); + } + + template + void toActionMatrix(const Eigen::MatrixBase & action_matrix) const + { + derived().toActionMatrix_impl(action_matrix); + } + + /** + * @brief The action matrix \f$ {}^bX_a \f$ of \f$ {}^aM_b \f$. + * \sa toActionMatrix() + */ + ActionMatrixType toActionMatrixInverse() const + { + return derived().toActionMatrixInverse_impl(); + } + + template + void toActionMatrixInverse(const Eigen::MatrixBase & action_matrix_inverse) const + { + derived().toActionMatrixInverse_impl(action_matrix_inverse.const_cast_derived()); + } + + ActionMatrixType toDualActionMatrix() const + { + return derived().toDualActionMatrix_impl(); + } + + void disp(std::ostream & os) const + { + static_cast(this)->disp_impl(os); + } + + template + void toDualActionMatrix(const Eigen::MatrixBase & dual_action_matrix) const + { + derived().toDualActionMatrix_impl(dual_action_matrix); + } + + typename SE3GroupAction::ReturnType operator*(const Derived & m2) const + { + return derived().__mult__(m2); + } + + /// ay = aXb.act(by) + template + typename SE3GroupAction::ReturnType act(const D & d) const + { + return derived().act_impl(d); + } + + /// by = aXb.actInv(ay) + template + typename SE3GroupAction::ReturnType actInv(const D & d) const + { + return derived().actInv_impl(d); + } + + bool operator==(const Derived & other) const + { + return derived().isEqual(other); + } + + bool operator!=(const Derived & other) const + { + return !(*this == other); + } + + bool isApprox( + const Derived & other, + const Scalar & prec = Eigen::NumTraits::dummy_precision()) const + { + return derived().isApprox_impl(other, prec); + } + + friend std::ostream & operator<<(std::ostream & os, const SE3Base & X) + { + X.disp(os); + return os; + } + + /// + /// \returns true if *this is approximately equal to the identity placement, within the + /// precision given by prec. + /// + bool isIdentity( + const typename traits::Scalar & prec = + Eigen::NumTraits::Scalar>::dummy_precision()) const + { + return derived().isIdentity(prec); + } + + /// + /// \returns true if the rotational part of *this is a rotation matrix (normalized columns), + /// within the precision given by prec. + /// + bool isNormalized(const Scalar & prec = Eigen::NumTraits::dummy_precision()) const + { + return derived().isNormalized(prec); + } + + /// + /// \brief Normalize *this in such a way the rotation part of *this lies on SO(3). + /// + void normalize() + { + derived().normalize(); + } + + /// + /// \returns a Normalized version of *this, in such a way the rotation part of the returned + /// transformation lies on SO(3). + /// + PlainType normalized() const + { + derived().normalized(); + } + + }; // struct SE3Base + +} // namespace pinocchio + +#endif // ifndef __pinocchio_spatial_se3_base_def_hxx__ diff --git a/include/pinocchio/spatial/se3-base.hpp b/include/pinocchio/spatial/se3-base.hpp index 97304ffb33..5f7340af4b 100644 --- a/include/pinocchio/spatial/se3-base.hpp +++ b/include/pinocchio/spatial/se3-base.hpp @@ -6,216 +6,7 @@ #ifndef __pinocchio_spatial_se3_base_hpp__ #define __pinocchio_spatial_se3_base_hpp__ -namespace pinocchio -{ - /** \brief Base class for rigid transformation. - * - * The rigid transform aMb can be seen in two ways: - * - * - given a point p expressed in frame B by its coordinate vector \f$ ^bp \f$, \f$ ^aM_b \f$ - * computes its coordinates in frame A by \f$ ^ap = {}^aM_b {}^bp \f$. - * - \f$ ^aM_b \f$ displaces a solid S centered at frame A into the solid centered in - * B. In particular, the origin of A is displaced at the origin of B: - * \f$^aM_b {}^aA = {}^aB \f$. - - * The rigid displacement is stored as a rotation matrix and translation vector by: - * \f$ ^aM_b x = {}^aR_b x + {}^aAB \f$ - * where \f$^aAB\f$ is the vector from origin A to origin B expressed in coordinates A. - * - * \cheatsheet \f$ {}^aM_c = {}^aM_b {}^bM_c \f$ - * - * \ingroup pinocchio_spatial - */ - template - struct SE3Base : NumericalBase - { - PINOCCHIO_SE3_TYPEDEF_TPL(Derived); - - Derived & derived() - { - return *static_cast(this); - } - const Derived & derived() const - { - return *static_cast(this); - } - - Derived & const_cast_derived() const - { - return *const_cast(&derived()); - } - - ConstAngularRef rotation() const - { - return derived().rotation_impl(); - } - ConstLinearRef translation() const - { - return derived().translation_impl(); - } - AngularRef rotation() - { - return derived().rotation_impl(); - } - LinearRef translation() - { - return derived().translation_impl(); - } - void rotation(const AngularType & R) - { - derived().rotation_impl(R); - } - void translation(const LinearType & t) - { - derived().translation_impl(t); - } - - HomogeneousMatrixType toHomogeneousMatrix() const - { - return derived().toHomogeneousMatrix_impl(); - } - operator HomogeneousMatrixType() const - { - return toHomogeneousMatrix(); - } - - /** - * @brief The action matrix \f$ {}^aX_b \f$ of \f$ {}^aM_b \f$. - * - * With \f$ {}^aM_b = \left( \begin{array}{cc} R & t \\ 0 & 1 \\ \end{array} \right) \f$, - * \f[ - * {}^aX_b = \left( \begin{array}{cc} R & \hat{t} R \\ 0 & R \\ \end{array} \right) - * \f] - * - * \cheatsheet \f$ {}^a\nu_c = {}^aX_b {}^b\nu_c \f$ - */ - ActionMatrixType toActionMatrix() const - { - return derived().toActionMatrix_impl(); - } - operator ActionMatrixType() const - { - return toActionMatrix(); - } - - template - void toActionMatrix(const Eigen::MatrixBase & action_matrix) const - { - derived().toActionMatrix_impl(action_matrix); - } - - /** - * @brief The action matrix \f$ {}^bX_a \f$ of \f$ {}^aM_b \f$. - * \sa toActionMatrix() - */ - ActionMatrixType toActionMatrixInverse() const - { - return derived().toActionMatrixInverse_impl(); - } - - template - void toActionMatrixInverse(const Eigen::MatrixBase & action_matrix_inverse) const - { - derived().toActionMatrixInverse_impl(action_matrix_inverse.const_cast_derived()); - } - - ActionMatrixType toDualActionMatrix() const - { - return derived().toDualActionMatrix_impl(); - } - - void disp(std::ostream & os) const - { - static_cast(this)->disp_impl(os); - } - - template - void toDualActionMatrix(const Eigen::MatrixBase & dual_action_matrix) const - { - derived().toDualActionMatrix_impl(dual_action_matrix); - } - - typename SE3GroupAction::ReturnType operator*(const Derived & m2) const - { - return derived().__mult__(m2); - } - - /// ay = aXb.act(by) - template - typename SE3GroupAction::ReturnType act(const D & d) const - { - return derived().act_impl(d); - } - - /// by = aXb.actInv(ay) - template - typename SE3GroupAction::ReturnType actInv(const D & d) const - { - return derived().actInv_impl(d); - } - - bool operator==(const Derived & other) const - { - return derived().isEqual(other); - } - - bool operator!=(const Derived & other) const - { - return !(*this == other); - } - - bool isApprox( - const Derived & other, - const Scalar & prec = Eigen::NumTraits::dummy_precision()) const - { - return derived().isApprox_impl(other, prec); - } - - friend std::ostream & operator<<(std::ostream & os, const SE3Base & X) - { - X.disp(os); - return os; - } - - /// - /// \returns true if *this is approximately equal to the identity placement, within the - /// precision given by prec. - /// - bool isIdentity( - const typename traits::Scalar & prec = - Eigen::NumTraits::Scalar>::dummy_precision()) const - { - return derived().isIdentity(prec); - } - - /// - /// \returns true if the rotational part of *this is a rotation matrix (normalized columns), - /// within the precision given by prec. - /// - bool isNormalized(const Scalar & prec = Eigen::NumTraits::dummy_precision()) const - { - return derived().isNormalized(prec); - } - - /// - /// \brief Normalize *this in such a way the rotation part of *this lies on SO(3). - /// - void normalize() - { - derived().normalize(); - } - - /// - /// \returns a Normalized version of *this, in such a way the rotation part of the returned - /// transformation lies on SO(3). - /// - PlainType normalized() const - { - derived().normalized(); - } - - }; // struct SE3Base - -} // namespace pinocchio +#include "pinocchio/spatial/se3-def.hxx" +#include "pinocchio/spatial/se3-base-def.hxx" #endif // ifndef __pinocchio_spatial_se3_base_hpp__ diff --git a/include/pinocchio/spatial/se3-def.hxx b/include/pinocchio/spatial/se3-def.hxx new file mode 100644 index 0000000000..9255dd62fe --- /dev/null +++ b/include/pinocchio/spatial/se3-def.hxx @@ -0,0 +1,47 @@ +// +// Copyright (c) 2015-2025 CNRS +// + +#ifndef __pinocchio_spatial_se3_def_hxx__ +#define __pinocchio_spatial_se3_def_hxx__ + +#ifdef PINOCCHIO_LSP + #include "pinocchio/spatial/se3.hpp" +#endif + +#define PINOCCHIO_SE3_TYPEDEF_GENERIC(Derived, TYPENAME) \ + typedef TYPENAME traits::Scalar Scalar; \ + typedef TYPENAME traits::AngularType AngularType; \ + typedef TYPENAME traits::LinearType LinearType; \ + typedef TYPENAME traits::AngularRef AngularRef; \ + typedef TYPENAME traits::LinearRef LinearRef; \ + typedef TYPENAME traits::ConstAngularRef ConstAngularRef; \ + typedef TYPENAME traits::ConstLinearRef ConstLinearRef; \ + typedef TYPENAME traits::ActionMatrixType ActionMatrixType; \ + typedef TYPENAME traits::HomogeneousMatrixType HomogeneousMatrixType; \ + typedef TYPENAME traits::PlainType PlainType; \ + enum \ + { \ + Options = traits::Options, \ + LINEAR = traits::LINEAR, \ + ANGULAR = traits::ANGULAR \ + } + +#define PINOCCHIO_SE3_TYPEDEF_TPL(Derived) PINOCCHIO_SE3_TYPEDEF_GENERIC(Derived, typename) + +#define PINOCCHIO_SE3_TYPEDEF(Derived) \ + PINOCCHIO_SE3_TYPEDEF_GENERIC(Derived, PINOCCHIO_MACRO_EMPTY_ARG) + +namespace pinocchio +{ + + /* Type returned by the "se3Action" and "se3ActionInverse" functions. */ + template + struct SE3GroupAction + { + typedef D ReturnType; + }; + +} // namespace pinocchio + +#endif // ifndef __pinocchio_spatial_se3_def_hxx__ diff --git a/include/pinocchio/spatial/se3-tpl-def.hxx b/include/pinocchio/spatial/se3-tpl-def.hxx new file mode 100644 index 0000000000..aa04bc22a7 --- /dev/null +++ b/include/pinocchio/spatial/se3-tpl-def.hxx @@ -0,0 +1,451 @@ +// +// Copyright (c) 2015-2025 CNRS INRIA +// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. +// + +#ifndef __pinocchio_spatial_se3_tpl_def_hxx__ +#define __pinocchio_spatial_se3_tpl_def_hxx__ + +#ifdef PINOCCHIO_LSP + #include "pinocchio/spatial/se3-tpl.hpp" +#endif + +namespace pinocchio +{ + template + struct traits> + { + enum + { + Options = _Options, + LINEAR = 0, + ANGULAR = 3 + }; + typedef _Scalar Scalar; + typedef Eigen::Matrix Vector3; + typedef Eigen::Matrix Vector4; + typedef Eigen::Matrix Vector6; + typedef Eigen::Matrix Matrix3; + typedef Eigen::Matrix Matrix4; + typedef Eigen::Matrix Matrix6; + typedef Matrix3 AngularType; + typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef; + typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef; + typedef Vector3 LinearType; + typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef; + typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef; + typedef Matrix6 ActionMatrixType; + typedef Matrix4 HomogeneousMatrixType; + typedef SE3Tpl PlainType; + }; // traits SE3Tpl + + template + struct SE3Tpl : public SE3Base> + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl); + typedef SE3Base> Base; + typedef Eigen::Quaternion Quaternion; + typedef typename traits::Vector3 Vector3; + typedef typename traits::Matrix3 Matrix3; + typedef typename traits::Matrix4 Matrix4; + typedef typename traits::Vector4 Vector4; + typedef typename traits::Matrix6 Matrix6; + + using Base::rotation; + using Base::translation; + + SE3Tpl() + : rot() + , trans(){}; + + template + SE3Tpl( + const Eigen::QuaternionBase & quat, + const Eigen::MatrixBase & trans) + : rot(quat.matrix()) + , trans(trans) + { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) + } + + template + SE3Tpl(const Eigen::MatrixBase & R, const Eigen::MatrixBase & trans) + : rot(R) + , trans(trans){EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3)} + + SE3Tpl(const SE3Tpl & other) + { + *this = other; + } + + template + explicit SE3Tpl(const SE3Tpl & other) + { + *this = other.template cast(); + } + + template + explicit SE3Tpl(const Eigen::MatrixBase & m) + : rot(m.template block<3, 3>(LINEAR, LINEAR)) + , trans(m.template block<3, 1>(LINEAR, ANGULAR)) + { + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, 4, 4); + } + + explicit SE3Tpl(int) + : rot(AngularType::Identity()) + , trans(LinearType::Zero()) + { + } + + template + SE3Tpl(const SE3Tpl & clone) + : rot(clone.rotation()) + , trans(clone.translation()) + { + } + + template + SE3Tpl & operator=(const SE3Tpl & other) + { + rot = other.rotation(); + trans = other.translation(); + return *this; + } + + /// + /// \brief Copy assignment operator. + /// + /// \param[in] other SE3 to copy + /// + SE3Tpl & operator=(const SE3Tpl & other) + { + rot = other.rotation(); + trans = other.translation(); + return *this; + } + + static SE3Tpl Identity() + { + return SE3Tpl(1); + } + + SE3Tpl & setIdentity() + { + rot.setIdentity(); + trans.setZero(); + return *this; + } + + /// aXb = bXa.inverse() + SE3Tpl inverse() const + { + return SE3Tpl(rot.transpose(), -rot.transpose() * trans); + } + + static SE3Tpl Random() + { + return SE3Tpl().setRandom(); + } + + SE3Tpl & setRandom() + { + Quaternion q; + quaternion::uniformRandom(q); + rot = q.matrix(); + trans.setRandom(); + + return *this; + } + + HomogeneousMatrixType toHomogeneousMatrix_impl() const + { + HomogeneousMatrixType M; + M.template block<3, 3>(LINEAR, LINEAR) = rot; + M.template block<3, 1>(LINEAR, ANGULAR) = trans; + M.template block<1, 3>(ANGULAR, LINEAR).setZero(); + M(3, 3) = 1; + return M; + } + + /// Vb.toVector() = bXa.toMatrix() * Va.toVector() + template + void toActionMatrix_impl(const Eigen::MatrixBase & action_matrix) const + { + typedef Eigen::Block Block3; + + Matrix6Like & M = action_matrix.const_cast_derived(); + M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot; + M.template block<3, 3>(ANGULAR, LINEAR).setZero(); + Block3 B = M.template block<3, 3>(LINEAR, ANGULAR); + + B.col(0) = trans.cross(rot.col(0)); + B.col(1) = trans.cross(rot.col(1)); + B.col(2) = trans.cross(rot.col(2)); + } + + ActionMatrixType toActionMatrix_impl() const + { + ActionMatrixType res; + toActionMatrix_impl(res); + return res; + } + + template + void + toActionMatrixInverse_impl(const Eigen::MatrixBase & action_matrix_inverse) const + { + typedef Eigen::Block Block3; + + Matrix6Like & M = action_matrix_inverse.const_cast_derived(); + M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = + rot.transpose(); + Block3 C = M.template block<3, 3>(ANGULAR, LINEAR); // used as temporary + Block3 B = M.template block<3, 3>(LINEAR, ANGULAR); + +#define PINOCCHIO_INTERNAL_COMPUTATION(axis_id, v3_in, v3_out, R, res) \ + CartesianAxis::cross(v3_in, v3_out); \ + res.col(axis_id).noalias() = R.transpose() * v3_out; + + PINOCCHIO_INTERNAL_COMPUTATION(0, trans, C.col(0), rot, B); + PINOCCHIO_INTERNAL_COMPUTATION(1, trans, C.col(0), rot, B); + PINOCCHIO_INTERNAL_COMPUTATION(2, trans, C.col(0), rot, B); + +#undef PINOCCHIO_INTERNAL_COMPUTATION + + C.setZero(); + } + + ActionMatrixType toActionMatrixInverse_impl() const + { + ActionMatrixType res; + toActionMatrixInverse_impl(res); + return res; + } + + template + void toDualActionMatrix_impl(const Eigen::MatrixBase & dual_action_matrix) const + { + typedef Eigen::Block Block3; + + Matrix6Like & M = dual_action_matrix.const_cast_derived(); + M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot; + M.template block<3, 3>(LINEAR, ANGULAR).setZero(); + Block3 B = M.template block<3, 3>(ANGULAR, LINEAR); + + B.col(0) = trans.cross(rot.col(0)); + B.col(1) = trans.cross(rot.col(1)); + B.col(2) = trans.cross(rot.col(2)); + } + + ActionMatrixType toDualActionMatrix_impl() const + { + ActionMatrixType res; + toDualActionMatrix_impl(res); + return res; + } + + void disp_impl(std::ostream & os) const + { + os << " R =\n" << rot << std::endl << " p = " << trans.transpose() << std::endl; + } + + /// --- GROUP ACTIONS ON M6, F6 and I6 --- + + /// ay = aXb.act(by) + template + typename SE3GroupAction::ReturnType act_impl(const D & d) const + { + return d.se3Action(*this); + } + + /// by = aXb.actInv(ay) + template + typename SE3GroupAction::ReturnType actInv_impl(const D & d) const + { + return d.se3ActionInverse(*this); + } + + template + typename EigenDerived::PlainObject + actOnEigenObject(const Eigen::MatrixBase & p) const + { + return (rotation() * p + translation()).eval(); + } + + template + Vector3 actOnEigenObject(const Eigen::MapBase & p) const + { + return Vector3(rotation() * p + translation()); + } + + template + typename EigenDerived::PlainObject + actInvOnEigenObject(const Eigen::MatrixBase & p) const + { + return (rotation().transpose() * (p - translation())).eval(); + } + + template + Vector3 actInvOnEigenObject(const Eigen::MapBase & p) const + { + return Vector3(rotation().transpose() * (p - translation())); + } + + Vector3 act_impl(const Vector3 & p) const + { + return Vector3(rotation() * p + translation()); + } + + Vector3 actInv_impl(const Vector3 & p) const + { + return Vector3(rotation().transpose() * (p - translation())); + } + + template + SE3Tpl act_impl(const SE3Tpl & m2) const + { + return SE3Tpl(rot * m2.rotation(), translation() + rotation() * m2.translation()); + } + + template + SE3Tpl actInv_impl(const SE3Tpl & m2) const + { + return SE3Tpl( + rot.transpose() * m2.rotation(), rot.transpose() * (m2.translation() - translation())); + } + + template + SE3Tpl __mult__(const SE3Tpl & m2) const + { + return this->act_impl(m2); + } + + template + bool isEqual(const SE3Tpl & m2) const + { + return (rotation() == m2.rotation() && translation() == m2.translation()); + } + + template + bool isApprox_impl( + const SE3Tpl & m2, + const Scalar & prec = Eigen::NumTraits::dummy_precision()) const + { + return rotation().isApprox(m2.rotation(), prec) + && translation().isApprox(m2.translation(), prec); + } + + bool isIdentity(const Scalar & prec = Eigen::NumTraits::dummy_precision()) const + { + return rotation().isIdentity(prec) && translation().isZero(prec); + } + + ConstAngularRef rotation_impl() const + { + return rot; + } + AngularRef rotation_impl() + { + return rot; + } + void rotation_impl(const AngularType & R) + { + rot = R; + } + ConstLinearRef translation_impl() const + { + return trans; + } + LinearRef translation_impl() + { + return trans; + } + void translation_impl(const LinearType & p) + { + trans = p; + } + + /// \returns An expression of *this with the Scalar type casted to NewScalar. + template + SE3Tpl cast() const + { + typedef SE3Tpl ReturnType; + ReturnType res(rot.template cast(), trans.template cast()); + + // During the cast, it may appear that the matrix is not normalized correctly. + // Force the normalization of the rotation part of the matrix. + internal::cast_call_normalize_method::run(res); + return res; + } + + bool isNormalized(const Scalar & prec = Eigen::NumTraits::dummy_precision()) const + { + return isUnitary(rot, prec); + } + + void normalize() + { + rot = orthogonalProjection(rot); + } + + PlainType normalized() const + { + PlainType res(*this); + res.normalize(); + return res; + } + + /// + /// \brief Linear interpolation on the SE3 manifold. + /// + /// \param[in] A Initial transformation. + /// \param[in] B Target transformation. + /// \param[in] alpha Interpolation factor in [0 ... 1]. + /// + /// \returns An interpolated transformation between A and B. + /// + /// \note This is similar to the SLERP operation which acts initially for rotation but applied + /// here to rigid transformation. + /// + template + static SE3Tpl Interpolate(const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha); + + protected: + AngularType rot; + LinearType trans; + + }; // class SE3Tpl + + namespace internal + { + template + struct cast_call_normalize_method, Scalar, Scalar> + { + template + static void run(T &) + { + } + }; + + template + struct cast_call_normalize_method, NewScalar, Scalar> + { + template + static void run(T & self) + { + if ( + pinocchio::cast(Eigen::NumTraits::epsilon()) + > Eigen::NumTraits::epsilon()) + self.normalize(); + } + }; + + } // namespace internal + +} // namespace pinocchio + +#endif // ifndef __pinocchio_spatial_se3_tpl_def_hxx__ diff --git a/include/pinocchio/spatial/se3-tpl.hpp b/include/pinocchio/spatial/se3-tpl.hpp index 8eabc5742d..6fd93030f8 100644 --- a/include/pinocchio/spatial/se3-tpl.hpp +++ b/include/pinocchio/spatial/se3-tpl.hpp @@ -1,456 +1,23 @@ // -// Copyright (c) 2015-2021 CNRS INRIA +// Copyright (c) 2015-2025 CNRS INRIA // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // #ifndef __pinocchio_spatial_se3_tpl_hpp__ #define __pinocchio_spatial_se3_tpl_hpp__ +#include + +#include "pinocchio/fwd.hpp" #include "pinocchio/spatial/fwd.hpp" -#include "pinocchio/spatial/se3-base.hpp" +// TODO: to hxx ? #include "pinocchio/math/quaternion.hpp" #include "pinocchio/math/rotation.hpp" #include "pinocchio/spatial/cartesian-axis.hpp" -#include - -namespace pinocchio -{ - template - struct traits> - { - enum - { - Options = _Options, - LINEAR = 0, - ANGULAR = 3 - }; - typedef _Scalar Scalar; - typedef Eigen::Matrix Vector3; - typedef Eigen::Matrix Vector4; - typedef Eigen::Matrix Vector6; - typedef Eigen::Matrix Matrix3; - typedef Eigen::Matrix Matrix4; - typedef Eigen::Matrix Matrix6; - typedef Matrix3 AngularType; - typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef; - typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef; - typedef Vector3 LinearType; - typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef; - typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef; - typedef Matrix6 ActionMatrixType; - typedef Matrix4 HomogeneousMatrixType; - typedef SE3Tpl PlainType; - }; // traits SE3Tpl - - template - struct SE3Tpl : public SE3Base> - { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl); - typedef SE3Base> Base; - typedef Eigen::Quaternion Quaternion; - typedef typename traits::Vector3 Vector3; - typedef typename traits::Matrix3 Matrix3; - typedef typename traits::Matrix4 Matrix4; - typedef typename traits::Vector4 Vector4; - typedef typename traits::Matrix6 Matrix6; - - using Base::rotation; - using Base::translation; - - SE3Tpl() - : rot() - , trans() {}; - - template - SE3Tpl( - const Eigen::QuaternionBase & quat, - const Eigen::MatrixBase & trans) - : rot(quat.matrix()) - , trans(trans) - { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) - } - - template - SE3Tpl(const Eigen::MatrixBase & R, const Eigen::MatrixBase & trans) - : rot(R) - , trans(trans){EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3)} - - SE3Tpl(const SE3Tpl & other) - { - *this = other; - } - - template - explicit SE3Tpl(const SE3Tpl & other) - { - *this = other.template cast(); - } - - template - explicit SE3Tpl(const Eigen::MatrixBase & m) - : rot(m.template block<3, 3>(LINEAR, LINEAR)) - , trans(m.template block<3, 1>(LINEAR, ANGULAR)) - { - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, 4, 4); - } - - explicit SE3Tpl(int) - : rot(AngularType::Identity()) - , trans(LinearType::Zero()) - { - } - - template - SE3Tpl(const SE3Tpl & clone) - : rot(clone.rotation()) - , trans(clone.translation()) - { - } - - template - SE3Tpl & operator=(const SE3Tpl & other) - { - rot = other.rotation(); - trans = other.translation(); - return *this; - } - - /// - /// \brief Copy assignment operator. - /// - /// \param[in] other SE3 to copy - /// - SE3Tpl & operator=(const SE3Tpl & other) - { - rot = other.rotation(); - trans = other.translation(); - return *this; - } - - static SE3Tpl Identity() - { - return SE3Tpl(1); - } - - SE3Tpl & setIdentity() - { - rot.setIdentity(); - trans.setZero(); - return *this; - } - - /// aXb = bXa.inverse() - SE3Tpl inverse() const - { - return SE3Tpl(rot.transpose(), -rot.transpose() * trans); - } - - static SE3Tpl Random() - { - return SE3Tpl().setRandom(); - } - - SE3Tpl & setRandom() - { - Quaternion q; - quaternion::uniformRandom(q); - rot = q.matrix(); - trans.setRandom(); - - return *this; - } - - HomogeneousMatrixType toHomogeneousMatrix_impl() const - { - HomogeneousMatrixType M; - M.template block<3, 3>(LINEAR, LINEAR) = rot; - M.template block<3, 1>(LINEAR, ANGULAR) = trans; - M.template block<1, 3>(ANGULAR, LINEAR).setZero(); - M(3, 3) = 1; - return M; - } - - /// Vb.toVector() = bXa.toMatrix() * Va.toVector() - template - void toActionMatrix_impl(const Eigen::MatrixBase & action_matrix) const - { - typedef Eigen::Block Block3; - - Matrix6Like & M = action_matrix.const_cast_derived(); - M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot; - M.template block<3, 3>(ANGULAR, LINEAR).setZero(); - Block3 B = M.template block<3, 3>(LINEAR, ANGULAR); - - B.col(0) = trans.cross(rot.col(0)); - B.col(1) = trans.cross(rot.col(1)); - B.col(2) = trans.cross(rot.col(2)); - } - - ActionMatrixType toActionMatrix_impl() const - { - ActionMatrixType res; - toActionMatrix_impl(res); - return res; - } - - template - void - toActionMatrixInverse_impl(const Eigen::MatrixBase & action_matrix_inverse) const - { - typedef Eigen::Block Block3; - - Matrix6Like & M = action_matrix_inverse.const_cast_derived(); - M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = - rot.transpose(); - Block3 C = M.template block<3, 3>(ANGULAR, LINEAR); // used as temporary - Block3 B = M.template block<3, 3>(LINEAR, ANGULAR); - -#define PINOCCHIO_INTERNAL_COMPUTATION(axis_id, v3_in, v3_out, R, res) \ - CartesianAxis::cross(v3_in, v3_out); \ - res.col(axis_id).noalias() = R.transpose() * v3_out; - - PINOCCHIO_INTERNAL_COMPUTATION(0, trans, C.col(0), rot, B); - PINOCCHIO_INTERNAL_COMPUTATION(1, trans, C.col(0), rot, B); - PINOCCHIO_INTERNAL_COMPUTATION(2, trans, C.col(0), rot, B); - -#undef PINOCCHIO_INTERNAL_COMPUTATION - - C.setZero(); - } - - ActionMatrixType toActionMatrixInverse_impl() const - { - ActionMatrixType res; - toActionMatrixInverse_impl(res); - return res; - } - - template - void toDualActionMatrix_impl(const Eigen::MatrixBase & dual_action_matrix) const - { - typedef Eigen::Block Block3; - - Matrix6Like & M = dual_action_matrix.const_cast_derived(); - M.template block<3, 3>(ANGULAR, ANGULAR) = M.template block<3, 3>(LINEAR, LINEAR) = rot; - M.template block<3, 3>(LINEAR, ANGULAR).setZero(); - Block3 B = M.template block<3, 3>(ANGULAR, LINEAR); - - B.col(0) = trans.cross(rot.col(0)); - B.col(1) = trans.cross(rot.col(1)); - B.col(2) = trans.cross(rot.col(2)); - } - - ActionMatrixType toDualActionMatrix_impl() const - { - ActionMatrixType res; - toDualActionMatrix_impl(res); - return res; - } - - void disp_impl(std::ostream & os) const - { - os << " R =\n" << rot << std::endl << " p = " << trans.transpose() << std::endl; - } - - /// --- GROUP ACTIONS ON M6, F6 and I6 --- - - /// ay = aXb.act(by) - template - typename SE3GroupAction::ReturnType act_impl(const D & d) const - { - return d.se3Action(*this); - } - - /// by = aXb.actInv(ay) - template - typename SE3GroupAction::ReturnType actInv_impl(const D & d) const - { - return d.se3ActionInverse(*this); - } - - template - typename EigenDerived::PlainObject - actOnEigenObject(const Eigen::MatrixBase & p) const - { - return (rotation() * p + translation()).eval(); - } - - template - Vector3 actOnEigenObject(const Eigen::MapBase & p) const - { - return Vector3(rotation() * p + translation()); - } - - template - typename EigenDerived::PlainObject - actInvOnEigenObject(const Eigen::MatrixBase & p) const - { - return (rotation().transpose() * (p - translation())).eval(); - } - - template - Vector3 actInvOnEigenObject(const Eigen::MapBase & p) const - { - return Vector3(rotation().transpose() * (p - translation())); - } - - Vector3 act_impl(const Vector3 & p) const - { - return Vector3(rotation() * p + translation()); - } - - Vector3 actInv_impl(const Vector3 & p) const - { - return Vector3(rotation().transpose() * (p - translation())); - } - - template - SE3Tpl act_impl(const SE3Tpl & m2) const - { - return SE3Tpl(rot * m2.rotation(), translation() + rotation() * m2.translation()); - } - - template - SE3Tpl actInv_impl(const SE3Tpl & m2) const - { - return SE3Tpl( - rot.transpose() * m2.rotation(), rot.transpose() * (m2.translation() - translation())); - } - - template - SE3Tpl __mult__(const SE3Tpl & m2) const - { - return this->act_impl(m2); - } - - template - bool isEqual(const SE3Tpl & m2) const - { - return (rotation() == m2.rotation() && translation() == m2.translation()); - } - - template - bool isApprox_impl( - const SE3Tpl & m2, - const Scalar & prec = Eigen::NumTraits::dummy_precision()) const - { - return rotation().isApprox(m2.rotation(), prec) - && translation().isApprox(m2.translation(), prec); - } - - bool isIdentity(const Scalar & prec = Eigen::NumTraits::dummy_precision()) const - { - return rotation().isIdentity(prec) && translation().isZero(prec); - } - - ConstAngularRef rotation_impl() const - { - return rot; - } - AngularRef rotation_impl() - { - return rot; - } - void rotation_impl(const AngularType & R) - { - rot = R; - } - ConstLinearRef translation_impl() const - { - return trans; - } - LinearRef translation_impl() - { - return trans; - } - void translation_impl(const LinearType & p) - { - trans = p; - } - - /// \returns An expression of *this with the Scalar type casted to NewScalar. - template - SE3Tpl cast() const - { - typedef SE3Tpl ReturnType; - ReturnType res(rot.template cast(), trans.template cast()); - - // During the cast, it may appear that the matrix is not normalized correctly. - // Force the normalization of the rotation part of the matrix. - internal::cast_call_normalize_method::run(res); - return res; - } - - bool isNormalized(const Scalar & prec = Eigen::NumTraits::dummy_precision()) const - { - return isUnitary(rot, prec); - } - - void normalize() - { - rot = orthogonalProjection(rot); - } - - PlainType normalized() const - { - PlainType res(*this); - res.normalize(); - return res; - } - - /// - /// \brief Linear interpolation on the SE3 manifold. - /// - /// \param[in] A Initial transformation. - /// \param[in] B Target transformation. - /// \param[in] alpha Interpolation factor in [0 ... 1]. - /// - /// \returns An interpolated transformation between A and B. - /// - /// \note This is similar to the SLERP operation which acts initially for rotation but applied - /// here to rigid transformation. - /// - template - static SE3Tpl Interpolate(const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha); - - protected: - AngularType rot; - LinearType trans; - - }; // class SE3Tpl - - namespace internal - { - template - struct cast_call_normalize_method, Scalar, Scalar> - { - template - static void run(T &) - { - } - }; - - template - struct cast_call_normalize_method, NewScalar, Scalar> - { - template - static void run(T & self) - { - if ( - pinocchio::cast(Eigen::NumTraits::epsilon()) - > Eigen::NumTraits::epsilon()) - self.normalize(); - } - }; - - } // namespace internal - -} // namespace pinocchio +#include "pinocchio/spatial/se3-def.hxx" +#include "pinocchio/spatial/se3-base-def.hxx" +#include "pinocchio/spatial/se3-tpl-def.hxx" #endif // ifndef __pinocchio_spatial_se3_tpl_hpp__ diff --git a/include/pinocchio/spatial/se3.hpp b/include/pinocchio/spatial/se3.hpp index 19aee1eb31..ca5828dd99 100644 --- a/include/pinocchio/spatial/se3.hpp +++ b/include/pinocchio/spatial/se3.hpp @@ -6,42 +6,10 @@ #define __pinocchio_spatial_se3_hpp__ #include "pinocchio/spatial/fwd.hpp" -#include "pinocchio/macros.hpp" - -#define PINOCCHIO_SE3_TYPEDEF_GENERIC(Derived, TYPENAME) \ - typedef TYPENAME traits::Scalar Scalar; \ - typedef TYPENAME traits::AngularType AngularType; \ - typedef TYPENAME traits::LinearType LinearType; \ - typedef TYPENAME traits::AngularRef AngularRef; \ - typedef TYPENAME traits::LinearRef LinearRef; \ - typedef TYPENAME traits::ConstAngularRef ConstAngularRef; \ - typedef TYPENAME traits::ConstLinearRef ConstLinearRef; \ - typedef TYPENAME traits::ActionMatrixType ActionMatrixType; \ - typedef TYPENAME traits::HomogeneousMatrixType HomogeneousMatrixType; \ - typedef TYPENAME traits::PlainType PlainType; \ - enum \ - { \ - Options = traits::Options, \ - LINEAR = traits::LINEAR, \ - ANGULAR = traits::ANGULAR \ - } - -#define PINOCCHIO_SE3_TYPEDEF_TPL(Derived) PINOCCHIO_SE3_TYPEDEF_GENERIC(Derived, typename) -#define PINOCCHIO_SE3_TYPEDEF(Derived) \ - PINOCCHIO_SE3_TYPEDEF_GENERIC(Derived, PINOCCHIO_MACRO_EMPTY_ARG) - -namespace pinocchio -{ - - /* Type returned by the "se3Action" and "se3ActionInverse" functions. */ - template - struct SE3GroupAction - { - typedef D ReturnType; - }; +#include "pinocchio/macros.hpp" -} // namespace pinocchio +#include "pinocchio/spatial/se3-def.hxx" #include "pinocchio/spatial/se3-base.hpp" #include "pinocchio/spatial/se3-tpl.hpp"