├── bindings
    ├── pycontact
    │   ├── jax
    │   │   ├── __init__.py
    │   │   └── solvers.py
    │   ├── torch
    │   │   ├── __init__.py
    │   │   └── solvers.py
    │   ├── utils
    │   │   ├── __init__.py
    │   │   ├── callbacks.py
    │   │   ├── bench.py
    │   │   └── pin_utils.py
    │   ├── __init__.py
    │   └── inverse_dynamics.py
    ├── contactbench.hh
    ├── context-cppad.hh
    ├── context.hh
    └── CMakeLists.txt
├── .gitignore
├── include
    └── contactbench
    │   ├── bindings
    │       └── context.hh
    │   ├── context.hpp
    │   ├── contact-problem.txx
    │   ├── delassus-wrapper.txx
    │   ├── solvers.txx
    │   ├── helpers.hpp
    │   ├── macros.hpp
    │   ├── fwd.hpp
    │   ├── math.hpp
    │   ├── delassus-wrapper.hxx
    │   ├── delassus-wrapper.hpp
    │   ├── contact-problem.hpp
    │   └── friction-constraint.hpp
├── .gitmodules
├── tests
    ├── python
    │   ├── data
    │   │   ├── 2cubes_t0.npz
    │   │   └── sliding_cube.npz
    │   ├── test_friction_constraint.py
    │   ├── test_collisions.py
    │   ├── test_jax_solvers.py
    │   ├── test_delassus_wrapper.py
    │   ├── test_contact_problem.py
    │   ├── test_solvers_grad.py
    │   └── test_pytorch_solvers.py
    ├── autodiff
    │   ├── CMakeLists.txt
    │   └── friction-constraints-cppad.cpp
    ├── CMakeLists.txt
    └── friction-constraints.cpp
├── src
    ├── contact-problem.cpp
    ├── delassus-wrapper.cpp
    └── solvers.cpp
├── setup.py
├── examples
    ├── utils
    │   └── visualize.py
    ├── ur5.py
    ├── humanoid.py
    ├── solve_ncp.py
    ├── 2cubes.py
    ├── hand.py
    ├── solo.py
    ├── ball.py
    └── cube.py
├── CITATION.cff
├── LICENSE
├── README.md
└── CMakeLists.txt


/bindings/pycontact/jax/__init__.py:
--------------------------------------------------------------------------------
1 | from . import solvers
2 | 


--------------------------------------------------------------------------------
/bindings/pycontact/torch/__init__.py:
--------------------------------------------------------------------------------
1 | from . import solvers
2 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | examples/log/
2 | *.pyc
3 | build*/
4 | compile_commands.json
5 | 


--------------------------------------------------------------------------------
/include/contactbench/bindings/context.hh:
--------------------------------------------------------------------------------
1 | /home/qlelidec/Documents/willow/diffcontact-bench/bindings/context.hh


--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "cmake"]
2 | 	path = cmake
3 | 	url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
4 | 


--------------------------------------------------------------------------------
/bindings/pycontact/utils/__init__.py:
--------------------------------------------------------------------------------
1 | from . import bench
2 | from . import callbacks
3 | from . import pin_utils
4 | 


--------------------------------------------------------------------------------
/tests/python/data/2cubes_t0.npz:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Simple-Robotics/ContactBench/HEAD/tests/python/data/2cubes_t0.npz


--------------------------------------------------------------------------------
/tests/python/data/sliding_cube.npz:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Simple-Robotics/ContactBench/HEAD/tests/python/data/sliding_cube.npz


--------------------------------------------------------------------------------
/bindings/pycontact/__init__.py:
--------------------------------------------------------------------------------
1 | from .libpycontact import *
2 | from . import simulators
3 | from . import torch
4 | from . import jax
5 | from . import utils
6 | 


--------------------------------------------------------------------------------
/include/contactbench/context.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "contactbench/fwd.hpp"
 4 | 
 5 | namespace contactbench {
 6 | namespace context {
 7 | 
 8 | using Scalar = double;
 9 | 
10 | } // namespace context
11 | } // namespace contactbench
12 | 


--------------------------------------------------------------------------------
/src/contact-problem.cpp:
--------------------------------------------------------------------------------
1 | #include "contactbench/contact-problem.hpp"
2 | 
3 | namespace contactbench {
4 | 
5 | template struct ContactProblem<context::Scalar, IceCreamCone>;
6 | template struct ContactProblem<context::Scalar, PyramidCone>;
7 | 
8 | } // namespace contactbench
9 | 


--------------------------------------------------------------------------------
/bindings/contactbench.hh:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "contactbench/bindings/context.hh"
 4 | 
 5 | namespace contactbench {
 6 | namespace context {
 7 | 
 8 | void expose_contact_bench();
 9 | void expose_vector_types();
10 | 
11 | } // namespace context
12 | } // namespace contactbench
13 | 


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
 1 | from setuptools import setup
 2 | 
 3 | setup(
 4 |     name="contact-bench",
 5 |     version="1.0",
 6 |     description="A benchmark of contact solvers.",
 7 |     packages=["pycontact"],
 8 |     author="Quentin Le Lidec",
 9 |     author_email="quentin.le-lidec@inria.fr,",
10 | )
11 | 


--------------------------------------------------------------------------------
/src/delassus-wrapper.cpp:
--------------------------------------------------------------------------------
 1 | #include "contactbench/contact-problem.hpp"
 2 | 
 3 | namespace contactbench {
 4 | 
 5 | template struct DelassusBase<context::Scalar>;
 6 | template struct DelassusDense<context::Scalar>;
 7 | template struct DelassusPinocchio<context::Scalar>;
 8 | 
 9 | } // namespace contactbench
10 | 


--------------------------------------------------------------------------------
/examples/utils/visualize.py:
--------------------------------------------------------------------------------
 1 | def sub_sample(xs, duration, fps):
 2 |     nb_frames = len(xs)
 3 |     nb_subframes = int(duration*fps)
 4 |     if nb_frames<nb_subframes:
 5 |         return xs
 6 |     else:
 7 |         step = nb_frames//nb_subframes
 8 |         xs_sub = [xs[i] for i in range(0,nb_frames, step)]
 9 |         return xs_sub
10 | 


--------------------------------------------------------------------------------
/include/contactbench/contact-problem.txx:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "contactbench/contact-problem.hpp"
 4 | 
 5 | namespace contactbench {
 6 | 
 7 | extern template struct ContactProblem<context::Scalar, IceCreamCone>;
 8 | extern template struct ContactProblem<context::Scalar, PyramidCone>;
 9 | 
10 | 
11 | } // namespace contactbench
12 | 


--------------------------------------------------------------------------------
/include/contactbench/delassus-wrapper.txx:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "contactbench/delassus-wrapper.hpp"
 4 | 
 5 | namespace contactbench {
 6 | 
 7 | extern template struct DelassusBase<context::Scalar>;
 8 | extern template struct DelassusDense<context::Scalar>;
 9 | extern template struct DelassusPinocchio<context::Scalar>;
10 | 
11 | } // namespace contactbench
12 | 


--------------------------------------------------------------------------------
/CITATION.cff:
--------------------------------------------------------------------------------
 1 | cff-version: 1.0.0
 2 | message: "If you use this software, please cite it as below."
 3 | authors:
 4 |   - family-names: Le Lidec
 5 |     given-names: Quentin
 6 |   - family-names: Jallet
 7 |     given-names: Wilson
 8 |   - family-names: Montaut
 9 |     given-names: Louis
10 |   - family-names: Schmid
11 |     given-names: Cordelia
12 |   - family-names: Laptev
13 |     given-names: Ivan
14 |   - family-names: Carpentier
15 |     given-names: Justin
16 | title: "Contact Models in Robotics: a Comparative Analysis"
17 | version: 1.0.0
18 | date-released: 2021-08-11
19 | 


--------------------------------------------------------------------------------
/tests/autodiff/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | macro(add_cppad_contact_bench_unit_test name)
 2 |   include_directories(SYSTEM ${cppad_INCLUDE_DIR})
 3 |   add_contact_bench_unit_test(${name} PACKAGES cppad)
 4 |   add_dependencies(test-cpp-cppad test-cpp-${name})
 5 |   target_link_libraries(test-cpp-${name} PUBLIC ${cppad_LIBRARY})
 6 | endmacro(add_cppad_contact_bench_unit_test)
 7 | 
 8 | if(BUILD_WITH_AUTODIFF_SUPPORT)
 9 |   add_custom_target(test-cpp-cppad)
10 |   set_target_properties(test-cpp-cppad PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
11 | 
12 |   add_cppad_contact_bench_unit_test(friction-constraints-cppad)
13 |   add_cppad_contact_bench_unit_test(delassus-cppad)
14 |   add_cppad_contact_bench_unit_test(contact-problem-cppad)
15 |   add_cppad_contact_bench_unit_test(solvers-cppad)
16 | endif(BUILD_WITH_AUTODIFF_SUPPORT)
17 | 


--------------------------------------------------------------------------------
/bindings/context-cppad.hh:
--------------------------------------------------------------------------------
 1 | #ifndef CONTACT_BENCH_BINDINGS_CONTEXT_CPPAD_H
 2 | #define CONTACT_BENCH_BINDINGS_CONTEXT_CPPAD_H
 3 | 
 4 | #include <pinocchio/autodiff/cppad.hpp>
 5 | #include "contactbench/fwd.hpp"
 6 | 
 7 | namespace contactbench {
 8 | namespace context {
 9 | 
10 | using T = CppAD::AD<double>;
11 | 
12 | CONTACTBENCH_EIGEN_TYPEDEFS(T);
13 | 
14 | using IceCreamCone = contactbench::IceCreamCone<T>;
15 | template <template <typename> class C>
16 | using ContactProblem = contactbench::ContactProblem<T, C>;
17 | 
18 | template <template <typename> class C>
19 | using BaseSolver = contactbench::BaseSolver<T, C>;
20 | 
21 | using CCPBaseSolver = contactbench::CCPBaseSolver<T>;
22 | 
23 | template <template <typename> class C>
24 | using NPCPGSSolver = contactbench::NPCPGSSolver<C>;
25 | 
26 | } // namespace context
27 | } // namespace contactbench
28 | #endif
29 | 


--------------------------------------------------------------------------------
/src/solvers.cpp:
--------------------------------------------------------------------------------
 1 | #include "contactbench/solvers.hpp"
 2 | 
 3 | namespace contactbench {
 4 | 
 5 | template struct BaseSolver<context::Scalar, IceCreamCone>;
 6 | template struct BaseSolver<context::Scalar, PyramidCone>;
 7 | template struct DualBaseSolver<context::Scalar, IceCreamCone>;
 8 | template struct DualBaseSolver<context::Scalar, PyramidCone>;
 9 | template struct NCPPGSSolver<context::Scalar, IceCreamCone>;
10 | template struct NCPPGSSolver<context::Scalar, PyramidCone>;
11 | template struct LCPQPSolver<context::Scalar>;
12 | template struct NCPStagProjSolver<IceCreamCone>;
13 | template struct NCPStagProjSolver<PyramidCone>;
14 | template struct CCPBaseSolver<context::Scalar>;
15 | template struct CCPPGSSolver<context::Scalar>;
16 | template struct CCPADMMSolver<context::Scalar>;
17 | template struct CCPADMMPrimalSolver<context::Scalar>;
18 | template struct CCPNewtonPrimalSolver<context::Scalar>;
19 | template struct RaisimSolver<context::Scalar>;
20 | template struct RaisimCorrectedSolver<context::Scalar>;
21 | 
22 | } // namespace contactbench
23 | 


--------------------------------------------------------------------------------
/include/contactbench/solvers.txx:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "contactbench/solvers.hpp"
 4 | 
 5 | namespace contactbench {
 6 | 
 7 | extern template struct BaseSolver<context::Scalar, IceCreamCone>;
 8 | extern template struct BaseSolver<context::Scalar, PyramidCone>;
 9 | extern template struct DualBaseSolver<context::Scalar, IceCreamCone>;
10 | extern template struct DualBaseSolver<context::Scalar, PyramidCone>;
11 | extern template struct NCPPGSSolver<context::Scalar, IceCreamCone>;
12 | extern template struct NCPPGSSolver<context::Scalar, PyramidCone>;
13 | extern template struct LCPQPSolver<context::Scalar>;
14 | extern template struct NCPStagProjSolver< IceCreamCone>;
15 | extern template struct NCPStagProjSolver< PyramidCone>;
16 | extern template struct CCPBaseSolver<context::Scalar>;
17 | extern template struct CCPPGSSolver<context::Scalar>;
18 | extern template struct CCPADMMSolver<context::Scalar>;
19 | extern template struct CCPADMMPrimalSolver<context::Scalar>;
20 | extern template struct CCPNewtonPrimalSolver<context::Scalar>;
21 | extern template struct RaisimSolver<context::Scalar>;
22 | extern template struct RaisimCorrectedSolver<context::Scalar>;
23 | 
24 | 
25 | } // namespace contactbench
26 | 


--------------------------------------------------------------------------------
/include/contactbench/helpers.hpp:
--------------------------------------------------------------------------------
 1 | #include <cmath>
 2 | #include <type_traits>
 3 | #ifdef DIFFCONTACT_WITH_CPPAD
 4 |   #include <cppad/cppad.hpp>
 5 | #endif
 6 | 
 7 | namespace contactbench {
 8 | template <typename T>
 9 | typename std::enable_if<std::is_same<T, double>::value, double>::type
10 | cos(const T &x) {
11 |   return std::cos(x);
12 | }
13 | 
14 | template <typename T>
15 | typename std::enable_if<std::is_same<T, double>::value, double>::type
16 | sin(const T &x) {
17 |   return std::sin(x);
18 | }
19 | 
20 | template <typename T>
21 | typename std::enable_if<std::is_same<T, double>::value, double>::type
22 | abs(const T &x) {
23 |   return std::abs(x);
24 | }
25 | 
26 | #ifdef DIFFCONTACT_WITH_CPPAD
27 | template <typename T>
28 | typename std::enable_if<std::is_same<T, CppAD::AD<double>>::value,
29 |                         CppAD::AD<double>>::type
30 | cos(const T &x) {
31 |   return CppAD::cos(x);
32 | }
33 | 
34 | template <typename T>
35 | typename std::enable_if<std::is_same<T, CppAD::AD<double>>::value,
36 |                         CppAD::AD<double>>::type
37 | sin(const T &x) {
38 |   return CppAD::sin(x);
39 | }
40 | 
41 | template <typename T>
42 | typename std::enable_if<std::is_same<T, CppAD::AD<double>>::value,
43 |                         CppAD::AD<double>>::type
44 | abs(const T &x) {
45 |   return fabs(x);
46 | }
47 | #endif
48 | } // namespace contactbench
49 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 2-Clause License
 2 | 
 3 | Copyright (c) 2024, INRIA
 4 | 
 5 | Redistribution and use in source and binary forms, with or without
 6 | modification, are permitted provided that the following conditions are met:
 7 | 
 8 | 1. Redistributions of source code must retain the above copyright notice, this
 9 |    list of conditions and the following disclaimer.
10 | 
11 | 2. Redistributions in binary form must reproduce the above copyright notice,
12 |    this list of conditions and the following disclaimer in the documentation
13 |    and/or other materials provided with the distribution.
14 | 
15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 | 


--------------------------------------------------------------------------------
/include/contactbench/macros.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #if __cplusplus >= 201703L
 4 | #define CONTACTBENCH_WITH_CPP_17
 5 | #endif
 6 | 
 7 | #if __cplusplus >= 201402L
 8 | #define CONTACTBENCH_WITH_CPP_14
 9 | #endif
10 | 
11 | #ifdef CONTACTBENCH_EIGEN_CHECK_MALLOC
12 | #define CONTACTBENCH_EIGEN_ALLOW_MALLOC(allowed)                               \
13 |   ::Eigen::internal::set_is_malloc_allowed(allowed)
14 | #else
15 | #define CONTACTBENCH_EIGEN_ALLOW_MALLOC(allowed)
16 | #endif
17 | 
18 | /// @brief Entering performance-critical code.
19 | #define CONTACTBENCH_NOMALLOC_BEGIN CONTACTBENCH_EIGEN_ALLOW_MALLOC(false)
20 | /// @brief Exiting performance-critical code.
21 | #define CONTACTBENCH_NOMALLOC_END CONTACTBENCH_EIGEN_ALLOW_MALLOC(true)
22 | 
23 | #define CONTACTBENCH_INLINE inline __attribute__((always_inline))
24 | 
25 | /// @brief Unused function parameters
26 | #define CONTACTBENCH_UNUSED(x) (void)(x)
27 | 
28 | 
29 | /// @brief Macro to static-cast to size_t i.e unsigned long
30 | #define CAST_UL(i) static_cast<size_t>(i)
31 | 
32 | #ifdef CONTACTBENCH_WITH_CPP_17
33 | #define CONTACTBENCH_MAYBE_UNUSED [[maybe_unused]]
34 | #elif defined(_MSC_VER) && !defined(__clang__)
35 | #define CONTACTBENCH_MAYBE_UNUSED
36 | #else
37 | #define CONTACTBENCH_MAYBE_UNUSED __attribute__((__unused__))
38 | #endif
39 | 
40 | #define CONTACTBENCH_RUNTIME_ERROR(msg) throw std::runtime_error(msg)
41 | 
42 | #define CONTACTBENCH_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)                        \
43 |   std::vector<T, Eigen::aligned_allocator<T>>
44 | 


--------------------------------------------------------------------------------
/bindings/context.hh:
--------------------------------------------------------------------------------
 1 | #ifndef CONTACT_BENCH_BINDINGS_CONTEXT_H
 2 | #define CONTACT_BENCH_BINDINGS_CONTEXT_H
 3 | 
 4 | #include "contactbench/fwd.hpp"
 5 | 
 6 | namespace contactbench {
 7 | namespace context {
 8 | 
 9 | using T = double;
10 | 
11 | CONTACTBENCH_EIGEN_TYPEDEFS(T);
12 | 
13 | using FrictionConstraint = contactbench::FrictionConstraint<T>;
14 | 
15 | using IceCreamCone = contactbench::IceCreamCone<T>;
16 | 
17 | using PyramidCone = contactbench::PyramidCone<T>;
18 | 
19 | using DelassusPinocchio = contactbench::DelassusPinocchio<T>;
20 | 
21 | using DelassusDense = contactbench::DelassusDense<T>;
22 | 
23 | using Statistics = contactbench::Statistics<T>;
24 | 
25 | using ContactSolverSettings = contactbench::ContactSolverSettings<T>;
26 | 
27 | template <template <typename> class C>
28 | using ContactProblem = contactbench::ContactProblem<T, C>;
29 | 
30 | template <template <typename> class C>
31 | using BaseSolver = contactbench::BaseSolver<T, C>;
32 | 
33 | using CCPBaseSolver = contactbench::CCPBaseSolver<T>;
34 | 
35 | template <template <typename> class C>
36 | using NCPPGSSolver = contactbench::NCPPGSSolver<T, C>;
37 | 
38 | using LCPQPSolver = contactbench::LCPQPSolver<T>;
39 | 
40 | template <template <typename> class C>
41 | using NCPStagProjSolver = contactbench::NCPStagProjSolver<C>;
42 | 
43 | using CCPPGSSolver = contactbench::CCPPGSSolver<T>;
44 | 
45 | using CCPADMMSolver = contactbench::CCPADMMSolver<T>;
46 | 
47 | using CCPADMMPrimalSolver = contactbench::CCPADMMPrimalSolver<T>;
48 | 
49 | using CCPNewtonPrimalSolver = contactbench::CCPNewtonPrimalSolver<T>;
50 | 
51 | using RaisimSolver = contactbench::RaisimSolver<T>;
52 | using RaisimCorrectedSolver = contactbench::RaisimCorrectedSolver<T>;
53 | 
54 | } // namespace context
55 | } // namespace contactbench
56 | #endif
57 | 


--------------------------------------------------------------------------------
/include/contactbench/fwd.hpp:
--------------------------------------------------------------------------------
 1 | #include <stdexcept>
 2 | 
 3 | #include "./macros.hpp"
 4 | 
 5 | // fwd-declaration of namespace
 6 | namespace contactbench {
 7 | 
 8 | // fwd FrictionConstraint
 9 | template <typename T> struct FrictionConstraint;
10 | 
11 | // fwd IceCreamCone
12 | template <typename T> struct IceCreamCone;
13 | 
14 | // fwd PyramidCone
15 | template <typename T> struct PyramidCone;
16 | 
17 | // fwd DelassusBase
18 | template <typename T> struct DelassusBase;
19 | 
20 | // fwd DelassusPinocchio
21 | template <typename T> struct DelassusPinocchio;
22 | 
23 | // fwd DelassusDense
24 | template <typename T> struct DelassusDense;
25 | 
26 | // fwd ContactProblem
27 | template <typename T, template <typename> class ConstraintTpl>
28 | struct ContactProblem;
29 | 
30 | // fwd BaseSolver
31 | template <typename T, template <typename> class ConstraintTpl>
32 | struct BaseSolver;
33 | 
34 | // fwd NCPPGSSolver
35 | template <typename T, template <typename> class ConstraintTpl>
36 | struct NCPPGSSolver;
37 | 
38 | template <typename T> struct LCPQPSolver;
39 | 
40 | // fwd NCPStagProjSolver
41 | template <template <typename> class ConstraintTpl> struct NCPStagProjSolver;
42 | 
43 | // fwd CCPBaseSolver
44 | template <typename T> struct CCPBaseSolver;
45 | 
46 | // fwd CCPPGSSolver
47 | template <typename T> struct CCPPGSSolver;
48 | template <typename T> struct CCPADMMSolver;
49 | template <typename T> struct CCPADMMPrimalSolver;
50 | template <typename T> struct CCPNewtonPrimalSolver;
51 | 
52 | // fwd
53 | template <typename T> struct RaisimSolver;
54 | template <typename T> struct RaisimCorrectedSolver;
55 | 
56 | // fwd
57 | template <typename T> struct Statistics;
58 | template <typename T> struct ContactSolverSettings;
59 | 
60 | } // namespace contactbench
61 | 
62 | #include "contactbench/math.hpp"
63 | 
64 | #include "contactbench/context.hpp"
65 | 


--------------------------------------------------------------------------------
/include/contactbench/math.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef CONTACT_BENCH_MATH_H
 2 | #define CONTACT_BENCH_MATH_H
 3 | 
 4 | #include <Eigen/Core>
 5 | 
 6 | #define CONTACTBENCH_EIGEN_TYPEDEFS_WITH_OPTIONS(Scalar, Options)              \
 7 |   using MatrixXs = Eigen::Matrix<Scalar, -1, -1, Options>;                     \
 8 |   using RowMatrixXs = Eigen::Matrix<Scalar, -1, -1, Eigen::RowMajor>;          \
 9 |   using Matrix2s = Eigen::Matrix<Scalar, 2, 2, Options>;                       \
10 |                                                                                \
11 |   using Matrix3s = Eigen::Matrix<Scalar, 3, 3, Options>;                       \
12 |   using Matrix4s = Eigen::Matrix<Scalar, 4, 4, Options>;                       \
13 |   using Matrix24s = Eigen::Matrix<Scalar, 2, 4, Options>;                      \
14 |   using Matrix34s = Eigen::Matrix<Scalar, 3, 4, Options>;                      \
15 |   using Matrix36s = Eigen::Matrix<Scalar, 3, 6, Options>;                      \
16 |   template <int N> using Vector = Eigen::Matrix<Scalar, N, 1, Options>;        \
17 |   using Vector2s = Vector<2>;                                                  \
18 |   using Vector3s = Vector<3>;                                                  \
19 |   using Vector4s = Vector<4>;                                                  \
20 |   using VectorXs = Vector<Eigen::Dynamic>;                                     \
21 |   using MatrixRef = Eigen::Ref<MatrixXs>;                                      \
22 |   using VectorRef = Eigen::Ref<VectorXs>
23 | 
24 | #define CONTACTBENCH_EIGEN_TYPEDEFS(Scalar)                                    \
25 |   CONTACTBENCH_EIGEN_TYPEDEFS_WITH_OPTIONS(Scalar, Eigen::ColMajor)
26 | 
27 | namespace contactbench {
28 | 
29 | template <typename Scalar, int Options = Eigen::ColMajor> struct math_types {
30 |   CONTACTBENCH_EIGEN_TYPEDEFS_WITH_OPTIONS(Scalar, Options);
31 | };
32 | 
33 | } // namespace contactbench
34 | 
35 | #endif
36 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Contact Bench
 2 | 
 3 | C++ implementation of various contact models and solvers used for robotics simulation.
 4 | 
 5 | ### The contact problem
 6 | 
 7 | The contact problem is formulated as a Non-linear Complementarity Problem (NCP):
 8 | 
 9 | $$
10 | \begin{align}
11 | &c = G \lambda + g \\
12 | &\mathcal{K} \ni \lambda \perp c + {\Phi}(c) \in \mathcal{K} \\
13 | \end{align}
14 | $$
15 | 
16 | Several algorithms solving this problem or a relaxation of it are implemented and empirically evaluated.
17 | We also benchmark various algorithms for gradient computation: automatic and implicit differentiation.
18 | 
19 | ### Requirements
20 | We use [Pinocchio](https://github.com/stack-of-tasks/pinocchio) for free dynamics and Delassus computation. Collision detection and its gradients are performed with [hppfcl](https://github.com/humanoid-path-planner/hpp-fcl). The QP solver [ProxSuite](https://github.com/Simple-Robotics/proxsuite) is used in some contact models and to compute derivatives. Automatic differentiation is implemented via [CppAD](https://github.com/coin-or/CppAD).
21 | 
22 | 
23 | ### Citing this work
24 | If you find this helpful work, please cite the related paper:
25 | ```
26 | @article{lelidec2023contact,
27 |   title={Contact Models in Robotics: a Comparative Analysis},
28 |   author={Le Lidec, Quentin and Jallet, Wilson and Montaut, Louis and Laptev, Ivan and Schmid, Cordelia and Carpentier, Justin}
29 | }
30 | ```
31 | 
32 | ### Credits
33 | 
34 | The following people have been involved in the development of **Contact Bench**:
35 | 
36 | -   [Quentin Le Lidec](https://quentinll.github.io) (Inria): main developer and manager of the project
37 | -   [Justin Carpentier](https://jcarpent.github.io) (Inria): manager of the project
38 | -   [Wilson Jallet](https://github.com/ManifoldFR) (LAAS-CNRS/Inria): core developer
39 | -   [Fabian Schramm](https://github.com/fabinsch) (Inria): core developer
40 | -   [Louis Montaut](https://github.com/lmontaut) (Inria): core developper
41 | 


--------------------------------------------------------------------------------
/tests/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | macro(add_test_cflags target flag)
 2 |   set_property(TARGET "test-cpp-${target}" APPEND_STRING PROPERTY COMPILE_FLAGS " ${flag}")
 3 | endmacro(add_test_cflags)
 4 | 
 5 | macro(add_contact_bench_unit_test NAME)
 6 |   set(options HEADER_ONLY)
 7 | 
 8 |   set(PKGS ${unit_test_PACKAGES})
 9 | 
10 |   set(TEST_NAME "test-cpp-${NAME}")
11 |   add_unit_test(${TEST_NAME} ${NAME}.cpp)
12 |   set_target_properties(${TEST_NAME} PROPERTIES LINKER_LANGUAGE CXX)
13 |   target_include_directories(${TEST_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
14 | 
15 |   add_test_cflags(${NAME} "-DBOOST_TEST_DYN_LINK")
16 |   set(MODULE_NAME "${NAME}Test")
17 |   string(REGEX REPLACE "-" "_" MODULE_NAME ${MODULE_NAME})
18 |   add_test_cflags(${NAME} "-DBOOST_TEST_MODULE=${MODULE_NAME}")
19 |   if(WIN32)
20 |     add_test_cflags(${NAME} "-DNOMINMAX -D_USE_MATH_DEFINES")
21 |   endif(WIN32)
22 | 
23 |   target_link_libraries(${TEST_NAME} PUBLIC ${PROJECT_NAME})
24 |   modernize_target_link_libraries(${TEST_NAME} SCOPE PRIVATE
25 |     TARGETS Boost::unit_test_framework
26 |     LIBRARIES ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
27 | endmacro(add_contact_bench_unit_test)
28 | 
29 | find_package(Boost REQUIRED COMPONENTS unit_test_framework)
30 | 
31 | if(BUILD_WITH_AUTODIFF_SUPPORT)
32 |   add_subdirectory(autodiff)
33 | endif()
34 | 
35 | add_contact_bench_unit_test(friction-constraints)
36 | add_contact_bench_unit_test(delassus)
37 | add_contact_bench_unit_test(contact-problem)
38 | add_contact_bench_unit_test(solvers)
39 | add_contact_bench_unit_test(solvers-derivatives)
40 | 
41 | 
42 | if(BUILD_PYTHON_INTERFACE)
43 |   file(GLOB_RECURSE ${PROJECT_NAME}_PYTHON_UNITTEST *.py)
44 | 
45 |   foreach(TEST_FILE ${${PROJECT_NAME}_PYTHON_UNITTEST})
46 |     get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE)
47 |     string(REGEX REPLACE "${PROJECT_SOURCE_DIR}/" "" TEST_FILE ${TEST_FILE})
48 |     add_python_unit_test("test-py-${TEST_NAME}" "${TEST_FILE}"
49 |                          "bindings")
50 |   endforeach(TEST_FILE ${${PROJECT_NAME}_PYTHON_UNITTEST})
51 | endif(BUILD_PYTHON_INTERFACE)
52 | 


--------------------------------------------------------------------------------
/tests/python/test_friction_constraint.py:
--------------------------------------------------------------------------------
 1 | from pycontact import IceCreamCone, PyramidCone
 2 | import numpy as np
 3 | 
 4 | def test_ICC_init():
 5 |     cone = IceCreamCone(0.8)
 6 |     assert True
 7 | 
 8 | 
 9 | def test_ICC_project():
10 |     cone = IceCreamCone(0.8)
11 |     lam = np.ones(3)
12 |     lam_out = np.zeros(3)
13 |     cone.project(lam, lam_out)
14 |     lam2 = lam.copy()
15 |     lam_out2 = np.zeros(3)
16 |     IceCreamCone.project(.8,lam2,lam_out2)
17 |     assert np.allclose(lam_out, lam_out2)
18 | 
19 | def test_ICC_projectDual():
20 |     mu = 0.8
21 |     cone = IceCreamCone(mu)
22 |     lam = np.zeros(3)
23 |     lam[0], lam[2] = 1e3, 1.0
24 |     lam_out = np.zeros(3)
25 |     cone.projectDual(lam, lam_out)
26 |     assert lam_out[0] == lam_out[2] * 1.0 / mu
27 | 
28 | 
29 | def test_ICC_isInside():
30 |     cone = IceCreamCone(0.8)
31 |     lam = np.zeros(3)
32 |     lam[2] = 1.
33 |     isin = cone.isInside(lam,1e-4)
34 |     isin2 = IceCreamCone.isInside(.8,lam, 1e-4)
35 |     assert isin and isin2
36 |     lam[0] = 2.
37 |     isin = cone.isInside(lam,1e-4)
38 |     isin2 = IceCreamCone.isInside(.8, lam,1e-4)
39 |     assert (not isin) and (not isin2)
40 | 
41 | def test_ICC_contactComplementarity():
42 |     cone = IceCreamCone(0.8)
43 |     lam = np.ones(3)
44 |     v = np.zeros(3)
45 |     comp = cone.computeContactComplementarity(lam, v)
46 |     assert True
47 | 
48 | 
49 | def test_ICC_conicComplementarity():
50 |     cone = IceCreamCone(0.8)
51 |     lam = np.ones(3)
52 |     v = np.zeros(3)
53 |     comp = cone.computeConicComplementarity(lam, v)
54 |     assert True
55 | 
56 | 
57 | def test_ICC_signoriniComplementarity():
58 |     cone = IceCreamCone(0.8)
59 |     lam = np.ones(3)
60 |     v = np.zeros(3)
61 |     comp = cone.computeSignoriniComplementarity(lam, v)
62 |     assert True
63 | 
64 | 
65 | def test_PC_init():
66 |     cone = PyramidCone(0.8)
67 |     assert True
68 | 
69 | 
70 | def test_PC_project_horizontal():
71 |     cone = PyramidCone(0.8)
72 |     lam = np.ones(3)
73 |     lam_out = np.zeros(3)
74 |     cone.projectHorizontal(lam, lam_out)
75 |     assert lam_out[0] == 0.8 and lam_out[1] == 0.8
76 | 
77 | 
78 | if __name__ == '__main__':
79 |     import sys
80 |     import pytest
81 |     sys.exit(pytest.main(sys.argv))
82 | 


--------------------------------------------------------------------------------
/bindings/pycontact/utils/callbacks.py:
--------------------------------------------------------------------------------
 1 | import abc
 2 | from pycontact.utils.bench import compute_ncp, compute_dual_feas
 3 | import matplotlib.pyplot as plt
 4 | 
 5 | 
 6 | class CallBackBase(abc.ABC):
 7 |     """Base class for callbacks in python contact solvers."""
 8 |     def __init__(self) -> None:
 9 |         super().__init__()
10 | 
11 |     @abc.abstractclassmethod
12 |     def call(self, **kwargs):
13 |         pass
14 | 
15 | 
16 | class LoggerCallback(CallBackBase):
17 |     def __init__(self) -> None:
18 |         CallBackBase.__init__(self)
19 |         self.ncp_criterion = []
20 |         self.prim_feas = []
21 |         self.dual_feas = []
22 |         self.comp = []
23 | 
24 |     def call(self, G, g, lam, mu, prim_feas, dual_feas, comp, **kwargs):
25 |         self.ncp_criterion += [compute_ncp([G], [g], [lam], [mu])]
26 |         self.prim_feas += [prim_feas]
27 |         self.dual_feas += [dual_feas]
28 |         self.comp += [comp]
29 | 
30 |     def reset(self):
31 |         self.ncp_criterion = []
32 |         self.prim_feas = []
33 |         self.dual_feas = []
34 |         self.comp = []
35 | 
36 |     def plot(self, figs=None, axs=None, block=True):
37 |         if figs is None:
38 |             figs, axs = plt.subplots(2, 2, figsize=(20, 15))
39 |             showing = True
40 |         else:
41 |             for i in range(len(axs)):
42 |                 for j in range(len(axs[i])):
43 |                     axs[i][j].clear()
44 |             showing = False
45 |         iters = [t for t in range(len(self.comp))]
46 |         axs[0][0].semilogy(iters, self.ncp_criterion)
47 |         axs[0][0].set_xlabel("iterations")
48 |         axs[0][0].set_ylabel("Contact complementarity")
49 | 
50 |         axs[0][1].semilogy(iters, self.comp)
51 |         axs[0][1].set_xlabel("iterations")
52 |         axs[0][1].set_ylabel("Problem complementarity")
53 | 
54 |         axs[1][0].semilogy(iters, self.prim_feas)
55 |         axs[1][0].set_xlabel("iterations")
56 |         axs[1][0].set_ylabel("Primal feasibility")
57 | 
58 |         axs[1][1].semilogy(iters, self.dual_feas)
59 |         axs[1][1].set_xlabel("iterations")
60 |         axs[1][1].set_ylabel("Dual feasibility")
61 | 
62 |         plt.draw()
63 |         if showing:
64 |             plt.tight_layout()
65 |             plt.show(block=block)
66 |         return figs, axs
67 | 
68 |     def close_plot(self):
69 |         plt.close(self.figs)
70 | 


--------------------------------------------------------------------------------
/bindings/pycontact/inverse_dynamics.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import pinocchio as pin
 3 | from pycontact.utils.pin_utils import  computeContacts, computeDFreeVelocity
 4 | from pycontact import ContactProblem
 5 | 
 6 | 
 7 | def inverse_contact_dynamics(model,
 8 |         data,
 9 |         geom_model,
10 |         geom_data,
11 |         q,
12 |         v,
13 |         v_ref,
14 |         dt,
15 |         maxIter=100,
16 |         th=1e-6,
17 |         rho = 1e-4,
18 |         ccp = False):
19 |     """Compute the inverse dynamics of a system with contacts"""
20 |     pin.updateGeometryPlacements(model, data, geom_model, geom_data, q)
21 |     M = pin.crba(model, data, q)
22 |     pin.computeCollisions(geom_model, geom_data, False)
23 |     J, Del, M, R, signed_dist, mus, comps, els, active_cols, contact_points, contact_joints, contact_placements = computeContacts(
24 |         model, data, geom_model, geom_data
25 |     )
26 |     nc = len(mus)
27 |     if nc >0:
28 |         R_comp = np.zeros(3*nc)
29 |         for i in range(nc):
30 |             R_comp[3*i+2] = comps[i]
31 |         R_prox = R_comp + rho * np.ones_like(R_comp)
32 |         R_prox_sqrt = np.sqrt(R_prox)
33 |         problem = ContactProblem(Del, np.zeros(3*nc), [mus[i]*R_prox_sqrt[3*i]/R_prox_sqrt[3*i+2] for i in range(nc)], R_prox)
34 |         problem2 = ContactProblem(Del, np.zeros(3*nc), mus, R_comp)
35 |         lam = np.zeros(3*nc)
36 |         lam_proj = lam.copy()
37 |         c_ref = J @ v_ref
38 |         c_ref += signed_dist/dt
39 |         c_cor = np.zeros_like(lam)
40 |         sig_cor = np.zeros_like(lam)
41 |         c_proj = c_cor.copy()
42 |         problem.computeDeSaxceCorrection(c_ref, c_cor)
43 |         n_iter = maxIter
44 |         for i in range(maxIter):
45 |             lam_pred = lam.copy()
46 |             problem.project(-(c_cor - rho * lam_pred)/R_prox_sqrt, lam)
47 |             lam /= R_prox_sqrt
48 |             sig = c_ref + R_comp*lam
49 |             if ccp:
50 |                 c_cor = c_ref + R_comp*lam
51 |             else:
52 |                 problem.computeDeSaxceCorrection(sig, sig_cor)
53 |                 c_cor = sig_cor - R_comp*lam
54 |             problem2.project(lam, lam_proj)
55 |             prim_res = lam - lam_proj
56 |             problem2.projectDual(sig_cor, c_proj)
57 |             dual_res = sig_cor - c_proj
58 |             contact_comp = problem2.computeConicComplementarity(lam, sig_cor)
59 |             stop = np.max([np.linalg.norm(prim_res, ord = np.inf), np.linalg.norm(dual_res, ord = np.inf), contact_comp])
60 |             if stop < th:
61 |                 n_iter = i+1
62 |                 break
63 |         has_converged = False if n_iter==maxIter else True
64 |         tau_contact = J.T @ lam
65 |     else:
66 |         lam = np.zeros(0)
67 |         has_converged=True
68 |         n_iter = 0
69 |         tau_contact = np.zeros(model.nv)
70 |     acc_ref = (v_ref - v)/dt
71 |     tau = pin.rnea(model, data, q, v, acc_ref) - tau_contact
72 |     return tau, lam/dt
73 | 


--------------------------------------------------------------------------------
/bindings/pycontact/jax/solvers.py:
--------------------------------------------------------------------------------
 1 | from functools import partial
 2 | 
 3 | try:
 4 |     import jax.numpy as jnp
 5 |     from jax import custom_jvp
 6 |     jax_available = True
 7 | except ImportError:
 8 |     jax_available = False
 9 | 
10 | import numpy as np
11 | from pycontact import LCPQPSolver, NCPPGSSolver, CCPADMMSolver, LinearContactProblem, ContactProblem, ContactSolverSettings
12 | 
13 | 
14 | if jax_available:
15 |     @partial(custom_jvp, nondiff_argnums=(3,4,5,6))
16 |     def contact_solver_layer(G, g, mus, max_iter, eps, contact_problem_type, contact_solver_type):
17 |         batch_size = g.shape[0]
18 |         lam =jnp.zeros(g.shape, dtype = g.dtype)
19 |         settings = ContactSolverSettings()
20 |         settings.max_iter_ = max_iter
21 |         settings.th_stop_ = eps
22 |         for i in range(batch_size):
23 |             Gi,gi,musi = np.array(G[i,:,:]), np.array(g[i,:,:]), np.array(mus[i,:,0])
24 |             probi = contact_problem_type(Gi, gi, musi.tolist())
25 |             solveri = contact_solver_type()
26 |             solveri.setProblem(probi)
27 |             lam_ws = np.zeros(len(gi))
28 |             solveri.solve(probi, lam_ws, settings)
29 |             lam.at[i,:,0].set(jnp.array(solveri.getSolution().copy()))
30 |         return lam
31 | 
32 |     @contact_solver_layer.defjvp
33 |     def contact_solver_layer_jvp(max_iter, eps, contact_problem_type, contact_solver_type, primals, tangents):
34 |         G, g, mus  = primals
35 |         batch_size = g.shape[0]
36 |         nc = mus.shape[1]
37 |         G_dot, g_dot, mus_dot = tangents
38 |         nparam = mus_dot.shape[2]
39 |         lam =jnp.zeros(g.shape, dtype = g.dtype)
40 |         lam_dot = jnp.zeros_like(lam)
41 |         settings = ContactSolverSettings()
42 |         settings.max_iter_ = max_iter
43 |         settings.th_stop_ = eps
44 |         for i in range(batch_size):
45 |             Gi,gi,musi = np.array(G[i,:,:]), np.array(g[i,:,:]), np.array(mus[i,:,0])
46 |             probi = contact_problem_type(Gi, gi, musi.tolist())
47 |             solveri = contact_solver_type()
48 |             solveri.setProblem(probi)
49 |             lam_ws = np.zeros(len(gi))
50 |             solveri.solve(probi, lam_ws, settings)
51 |             lam_i  = solveri.getSolution().copy()
52 |             lam.at[i,:,0].set(jnp.array(lam_i))
53 |             v_i = solveri.getDualSolution().copy()
54 |             G_dot_i, g_dot_i, mus_dot_i = np.array(G_dot[i]), np.array(g_dot[i]), np.array(mus_dot[i])
55 |             solveri.jvp(probi, lam_i, v_i, G_dot_i, g_dot_i, mus_dot_i, eps, eps_reg=0.)
56 |             lam_dot.at[i,:,:].set(jnp.array(solveri.getdlamdtheta().copy()))
57 |         return lam, lam_dot
58 | 
59 |     ccp_layer = lambda G, g, mus, max_iter, eps : contact_solver_layer( G, g, mus, max_iter, eps, ContactProblem, CCPADMMSolver)
60 |     # lcp_layer = lambda G, g, mus, max_iter, eps : contact_solver_layer( G, g, mus, max_iter, eps, LinearContactProblem, LCPQPSolver)
61 | else:
62 |     print("JAX not available, skipping JAX solvers")


--------------------------------------------------------------------------------
/bindings/pycontact/utils/bench.py:
--------------------------------------------------------------------------------
 1 | from typing import List
 2 | import numpy as np
 3 | from pycontact import IceCreamCone
 4 | 
 5 | 
 6 | def compute_contact_vel(J: List, v: List):
 7 |     Jv = []
 8 |     Jv = [None if len(J[t]) == 0 else J[t] @ v[t] for t in range(len(J))]
 9 |     return Jv
10 | 
11 | 
12 | def compute_contact_torque(J: List, lam: List):
13 |     Jlam = [
14 |         np.zeros(3) if len(J[i]) == 0 else np.dot(J[i].T, lam[i])[:3, 0]
15 |         for i in range(len(J))
16 |     ]
17 |     return Jlam
18 | 
19 | 
20 | def compute_signorini(J: List, lam: List, v: List):
21 |     signorini = []
22 |     lamn = []
23 |     vc = compute_contact_vel(J, v)
24 |     for t in range(len(lam)):
25 |         nc = int(len(lam[t]) / 3)
26 |         lamn = np.zeros(nc)
27 |         vn = np.zeros(nc)
28 |         for i in range(nc):
29 |             lamn[i] = lam[t][3 * i + 2]
30 |             vn[i] = vc[t][3 * i + 2]
31 |         if nc > 0:
32 |             Jlamvn = lamn * vn
33 |             signorini += [np.linalg.norm(Jlamvn, ord=np.inf)]
34 |         else:
35 |             signorini += [0.0]
36 |     return signorini
37 | 
38 | 
39 | def compute_mdp(J: List, v: List, lam: List, mu: float):
40 |     mdp_gap = []
41 |     vc = compute_contact_vel(J, v)
42 |     for t in range(len(lam)):
43 |         nc = int(len(lam[t]) / 3)
44 |         gaps = np.zeros(nc)
45 |         for i in range(nc):
46 |             gaps[i] = mu * np.abs(lam[t][3 * i + 2]) - np.linalg.norm(
47 |                 lam[t][3 * i : 3 * i + 2]
48 |             )
49 |         if nc > 0:
50 |             mdp_gap += [np.linalg.norm(gaps * vc[t], ord=np.inf)]
51 |         else:
52 |             mdp_gap += [0.0]
53 |     return mdp_gap
54 | 
55 | 
56 | def phi(v, mu):
57 |     norm_t = np.linalg.norm(v[:2])
58 |     ph = np.expand_dims(np.array([0.0, 0.0, mu * norm_t]), axis=-1)
59 |     return ph
60 | 
61 | 
62 | def compute_ncp(G: List, g: List, lam: List, mu: List):
63 |     ncp_crit = []
64 |     for t in range(len(lam)):
65 |         nc = int(len(lam[t]) / 3)
66 |         if nc > 0:
67 |             v_t = G[t] @ lam[t] + g[t]
68 |             dual = v_t.copy()
69 |             for i in range(nc):
70 |                 dual[3 * i : 3 * (i + 1)] += phi(v_t[3 * i : 3 * (i + 1)], mu[t][i])
71 | 
72 |             ncp_crit += [np.abs((dual.T @ lam[t])[0, 0])]
73 |         else:
74 |             ncp_crit += [0.0]
75 |     return ncp_crit
76 | 
77 | 
78 | def compute_dual_feas(G: List, g: List, lam: List, mu: List):
79 |     dual_feas = []
80 |     for t in range(len(lam)):
81 |         nc = int(len(lam[t]) / 3)
82 |         if nc > 0:
83 |             v_t = G[t] @ lam[t] + g[t]
84 |             dual = v_t.copy()
85 |             for i in range(nc):
86 |                 dual[3 * i : 3 * (i + 1)] += phi(v_t[3 * i : 3 * (i + 1)], mu[t][i])
87 |             dual_proj = dual.copy()
88 |             for i in range(nc):
89 |                 dual_cone = IceCreamCone(1.0 / mu[t][i])
90 |                 dual_proj[3 * i : 3 * (i + 1)] = dual_cone.project(
91 |                     dual[3 * i : 3 * (i + 1)]
92 |                 )
93 |             dual_feas += [np.linalg.norm(dual - dual_proj, ord=np.inf)]
94 |         else:
95 |             dual_feas += [0.0]
96 |     return dual_feas
97 | 


--------------------------------------------------------------------------------
/tests/python/test_collisions.py:
--------------------------------------------------------------------------------
 1 | import hppfcl
 2 | import numpy as np
 3 | import pinocchio as pin
 4 | 
 5 | 
 6 | def test_normal_hppfcl():
 7 |     n = np.array([0, 0, 1])
 8 |     d = 0
 9 |     plane = hppfcl.Halfspace(n, d)
10 |     T1 = hppfcl.Transform3f.Identity()
11 | 
12 |     r = 0.5
13 |     ellipsoid = hppfcl.Ellipsoid(np.ones(3) * r)
14 | 
15 |     req = hppfcl.CollisionRequest()
16 |     res = hppfcl.CollisionResult()
17 |     T2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 0.25]))
18 | 
19 |     col = hppfcl.collide(plane, T1, ellipsoid, T2, req, res)
20 |     assert col
21 |     if col:
22 |         c = res.getContacts()[0]
23 |         assert np.allclose(c.normal, n, 1e-3)
24 | 
25 | 
26 | def test_normal_pin():
27 |     n = np.array([0, 0, 1])
28 |     d = 0
29 |     rmodel = pin.Model()
30 |     rdata = rmodel.createData()
31 | 
32 |     geom_model = pin.GeometryModel()
33 |     plane_shape = hppfcl.Halfspace(n, d)
34 |     T1 = pin.SE3(np.eye(3), np.zeros(3))
35 |     plane = pin.GeometryObject("plane", 0, 0, T1, plane_shape)
36 |     plane_id = geom_model.addGeometryObject(plane)
37 |     r = 0.5
38 |     ellipsoid_shape = hppfcl.Ellipsoid(np.ones(3) * r)
39 |     T2 = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.25]))
40 |     geometry = pin.GeometryObject("ellipsoid", 0, 0, T2, ellipsoid_shape)
41 |     geom_id = geom_model.addGeometryObject(geometry)
42 | 
43 |     ell_plane = pin.CollisionPair(plane_id, geom_id)
44 |     geom_model.addCollisionPair(ell_plane)
45 |     geom_data = geom_model.createData()
46 | 
47 |     q = pin.neutral(rmodel)
48 |     pin.updateGeometryPlacements(rmodel, rdata, geom_model, geom_data, q)
49 |     pin.computeCollisions(geom_model, geom_data, False)
50 |     res = geom_data.collisionResults[0]
51 |     assert res.isCollision()
52 |     contacts = res.getContacts()
53 |     contact = contacts[0]
54 |     normal = contact.normal
55 |     assert np.allclose(normal, n, 1e-3)
56 | 
57 | def test_normal_pin_inverse():
58 |     n = np.array([0, 0, 1])
59 |     d = 0
60 |     rmodel = pin.Model()
61 |     rdata = rmodel.createData()
62 | 
63 |     geom_model = pin.GeometryModel()
64 |     plane_shape = hppfcl.Halfspace(n, d)
65 |     T1 = pin.SE3(np.eye(3), np.zeros(3))
66 |     plane = pin.GeometryObject("plane", 0, 0, T1, plane_shape)
67 |     plane_id = geom_model.addGeometryObject(plane)
68 |     r = 0.5
69 |     ellipsoid_shape = hppfcl.Ellipsoid(np.ones(3) * r)
70 |     T2 = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.25]))
71 |     geometry = pin.GeometryObject("ellipsoid", 0, 0, T2, ellipsoid_shape)
72 |     geom_id = geom_model.addGeometryObject(geometry)
73 | 
74 |     ell_plane = pin.CollisionPair(geom_id, plane_id)
75 |     geom_model.addCollisionPair(ell_plane)
76 |     geom_data = geom_model.createData()
77 | 
78 |     q = pin.neutral(rmodel)
79 |     pin.updateGeometryPlacements(rmodel, rdata, geom_model, geom_data, q)
80 |     pin.computeCollisions(geom_model, geom_data, False)
81 |     res = geom_data.collisionResults[0]  # get collision pair results
82 |     assert res.isCollision()
83 |     contacts = res.getContacts()
84 |     contact = contacts[0]
85 |     normal = contact.normal
86 |     assert np.allclose(normal, -n, 1e-3)
87 | 
88 | 
89 | if __name__ == '__main__':
90 |     import sys
91 |     import pytest
92 |     sys.exit(pytest.main(sys.argv))
93 | 


--------------------------------------------------------------------------------
/tests/python/test_jax_solvers.py:
--------------------------------------------------------------------------------
 1 | 
 2 | from pycontact.jax.solvers import ccp_layer
 3 | from jax.example_libraries.optimizers import adam
 4 | from jax import value_and_grad, jvp, jacfwd, vmap
 5 | import jax.numpy as jnp
 6 | from jax.lax import stop_gradient
 7 | from jax.test_util import check_grads
 8 | 
 9 | import numpy as np
10 | from pathlib import Path
11 | import os
12 | 
13 | test_python_path = Path(os.path.dirname(os.path.realpath(__file__)))
14 | 
15 | def test_contact_solver_init():
16 |     ccp_layer
17 | 
18 | 
19 | def test_ccpadmm_solver_jvp():
20 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
21 |     G = npzfile["G"]
22 |     g = npzfile["g"]
23 |     mus = npzfile["mus"]
24 |     nc = len(mus)
25 |     G = jnp.array(G)[None]
26 |     g = jnp.array(g)[None]
27 |     mus = jnp.array(mus)[None, :, None]
28 | 
29 |     def compute_loss(G,g,mus,lam_tar):
30 |         mus2 = jnp.square(mus)
31 |         max_iter = 1000
32 |         eps = 1e-7
33 |         lam = ccp_layer(G,g,mus2, max_iter, eps)
34 |         loss = 0.5*jnp.square(lam- lam_tar).sum()
35 |         return loss
36 | 
37 | 
38 |     mus2 = jnp.square(stop_gradient(mus)*0.9)
39 |     lam_tar = ccp_layer(G,g,mus2, 1000, 1e-7).clone()
40 |     # lam_tar = torch.zeros_like(g)
41 |     loss = compute_loss(G, g, mus, lam_tar)
42 | 
43 |     G2, g2, mus2 = G.clone(), g.clone(), mus.clone()
44 |     delta = 1e-8
45 |     for i in range(nc):
46 |         mus2.at[0,i,0].add(delta)
47 |         loss2 = compute_loss(G,g,mus2, lam_tar)
48 |         dl_dmusi = (loss2-loss)/delta
49 |         # assert np.abs(dl_dmusi - mus.grad[0,i,0].detach().numpy())< 1e-3
50 |         mus2.at[0,i,0].add(-delta)
51 | 
52 |     for i in range(3*nc):
53 |         g2.at[0,i,0].add(delta)
54 |         loss2 = compute_loss(G,g2,mus, lam_tar)
55 |         dl_dgi = (loss2-loss)/delta
56 |         # assert np.abs(dl_dgi - g.grad[0,i,0].detach().numpy())< 1e-3
57 |         g2.at[0,i,0].add(-delta)
58 | 
59 |     for i in range(3*nc):
60 |         for j in range(3*nc):
61 |             G2.at[0,i,j].add(delta)
62 |             loss2 = compute_loss(G2,g,mus, lam_tar)
63 |             dl_dGij = (loss2-loss)/delta
64 |             # assert np.abs(dl_dGij - G.grad[0,i,j].detach().numpy())< 1e-3
65 |             G2.at[0,i,j].add(-delta)
66 | 
67 |     lr = 5e-5
68 |     # lr = 5e-4
69 |     opt_init, opt_update, get_params = adam(lr)
70 |     opt_state = opt_init(mus)
71 |     def step(step, opt_state):
72 |         musb = get_params(opt_state)
73 |         grads = jacfwd(compute_loss, argnums=2)(G, g, musb, lam_tar)
74 |         value = 0.
75 |         # value, grads = jvp(compute_loss, (G, g, musb, lam_tar),(jnp.zeros_like(G), jnp.zeros_like(g), jnp.identity(nc)[None, :,:], jnp.zeros_like(lam_tar)))
76 |         # _jvp = lambda s: jvp(compute_loss, (G, g, musb, lam_tar),(jnp.zeros_like(G), jnp.zeros_like(g), s, jnp.zeros_like(lam_tar)))[1]
77 |         # grads = vmap(_jvp, in_axes=1)(jnp.identity(nc)[None, :,:])
78 |         # value = 0.
79 |         # value, grads = value_and_grad(compute_loss)(G,g, musb, lam_tar)
80 |         opt_state = opt_update(step, grads, opt_state)
81 |         return value, opt_state
82 |     print("mus", mus)
83 |     loss = compute_loss(G, g, mus, lam_tar)
84 |     print("init loss",loss)
85 |     for i in range(40000):
86 |         value, opt_state = step(i, opt_state)
87 | 
88 | 
89 | 
90 | 
91 | 
92 | if __name__ == '__main__':
93 |     import sys
94 |     import pytest
95 |     # sys.exit(pytest.main(sys.argv))
96 |     test_ccpadmm_solver_jvp()
97 | 


--------------------------------------------------------------------------------
/bindings/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | add_custom_target(python)
  2 | set_target_properties(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
  3 | 
  4 | if(GENERATE_PYTHON_STUBS)
  5 |   include(${PROJECT_SOURCE_DIR}/cmake/stubs.cmake)
  6 | endif(GENERATE_PYTHON_STUBS)
  7 | 
  8 | # Name of the Python library
  9 | set(LIBRARY_NAME pycontact)
 10 | 
 11 | 
 12 | set(${LIBRARY_NAME}_HEADERS
 13 |   "contactbench.hh"
 14 |   "context.hh"
 15 | )
 16 | 
 17 | set(${LIBRARY_NAME}_SOURCES
 18 |   contactbench.cc
 19 | )
 20 | 
 21 | message(STATUS "Curr source dir: ${CMAKE_CURRENT_SOURCE_DIR}")
 22 | message(STATUS "Curr binary dir: ${CMAKE_CURRENT_BINARY_DIR}")
 23 | message(STATUS "HEADERS = ${${LIBRARY_NAME}_HEADERS}")
 24 | 
 25 | add_library(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_HEADERS})
 26 | target_include_directories(${LIBRARY_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS})
 27 | 
 28 | add_dependencies(python ${LIBRARY_NAME})
 29 | add_header_group(${LIBRARY_NAME}_HEADERS)
 30 | add_source_group(${LIBRARY_NAME}_SOURCES)
 31 | 
 32 | target_link_boost_python(${LIBRARY_NAME} PUBLIC)
 33 | target_link_libraries(${LIBRARY_NAME} PUBLIC
 34 |   ${PROJECT_NAME}
 35 |   eigenpy::eigenpy
 36 |   Boost::system)
 37 | 
 38 | set_target_properties(${LIBRARY_NAME} PROPERTIES
 39 |   PREFIX "lib"
 40 |   SUFFIX "${PYTHON_EXT_SUFFIX}"
 41 |   LIBRARY_OUTPUT_NAME ${LIBRARY_NAME}
 42 |   LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bindings/${LIBRARY_NAME}"
 43 |   PUBLIC_HEADER "${${LIBRARY_NAME}_HEADERS}"
 44 |   VERSION ${PROJECT_VERSION}
 45 |   )
 46 | 
 47 | if(UNIX AND NOT APPLE)
 48 |   set_target_properties(${LIBRARY_NAME} PROPERTIES INSTALL_RPATH "\$ORIGIN/../../..")
 49 | endif()
 50 | 
 51 | if(IS_ABSOLUTE ${PYTHON_SITELIB})
 52 |   set(ABSOLUTE_PYTHON_SITELIB ${PYTHON_SITELIB})
 53 | else()
 54 |   set(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB})
 55 | endif()
 56 | set(${LIBRARY_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${LIBRARY_NAME})
 57 | 
 58 | set(PY_HEADERS_INSTALL_DIR ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}/bindings)
 59 | 
 60 | install(TARGETS ${LIBRARY_NAME}
 61 |         PUBLIC_HEADER DESTINATION ${PY_HEADERS_INSTALL_DIR}
 62 |         LIBRARY DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR})
 63 | 
 64 | # --- INSTALL SCRIPTS
 65 | set(PYTHON_FILES
 66 |   __init__.py
 67 |   solvers.py
 68 |   simulators.py
 69 |   inverse_dynamics.py
 70 |   utils/__init__.py
 71 |   utils/bench.py
 72 |   utils/pin_utils.py
 73 |   utils/callbacks.py
 74 |   torch/__init__.py
 75 |   torch/solvers.py
 76 |   jax/__init__.py
 77 |   jax/solvers.py
 78 |   )
 79 | 
 80 | if(GENERATE_PYTHON_STUBS)
 81 |   load_stubgen()
 82 |   generate_stubs(${CMAKE_BINARY_DIR}/python ${LIBRARY_NAME} ${ABSOLUTE_PYTHON_SITELIB})
 83 | endif(GENERATE_PYTHON_STUBS)
 84 | 
 85 | file(MAKE_DIRECTORY
 86 |      ${${LIBRARY_NAME}_INSTALL_DIR}/utils)
 87 | 
 88 | file(MAKE_DIRECTORY
 89 |      ${${LIBRARY_NAME}_INSTALL_DIR}/torch)
 90 | 
 91 | file(MAKE_DIRECTORY
 92 |      ${${LIBRARY_NAME}_INSTALL_DIR}/jax)
 93 | 
 94 | foreach(python ${PYTHON_FILES})
 95 |   PYTHON_BUILD(${LIBRARY_NAME} ${python})
 96 |   get_filename_component(pysubmodule ${python} PATH)
 97 |   get_filename_component(pyname ${python} NAME)
 98 |   set(MODULE_NAME ${LIBRARY_NAME}/${pysubmodule})
 99 | 
100 |   python_install_on_site(${MODULE_NAME} ${pyname})
101 | endforeach(python)
102 | 
103 | macro(create_symlink target linkname)
104 |     execute_process(
105 |         COMMAND ln -sf "${target}" "${linkname}"
106 |         RESULT_VARIABLE HEADER_LINK_STATUS
107 |         ERROR_VARIABLE HEADER_LINK_ERROR
108 |     )
109 |     if(NOT "${HEADER_LINK_STATUS}" EQUAL 0)
110 |         message(FATAL_ERROR "Symlinking headers failed:\n${HEADER_LINK_ERROR}")
111 |     endif()
112 | endmacro()
113 | 
114 | file(MAKE_DIRECTORY ${CMAKE_BINARY_DIR}/include/${PROJECT_NAME}/bindings)
115 | 
116 | if(BUILD_WITH_AUTODIFF_SUPPORT)
117 |   target_compile_definitions(${LIBRARY_NAME} PUBLIC DIFFCONTACT_WITH_CPPAD)
118 |   target_include_directories(${LIBRARY_NAME} PUBLIC ${cppad_INCLUDE_DIR})
119 | endif(BUILD_WITH_AUTODIFF_SUPPORT)
120 | 
121 | create_symlink(
122 |   ${CMAKE_CURRENT_SOURCE_DIR}/context.hh
123 |   ${CMAKE_BINARY_DIR}/include/${PROJECT_NAME}/bindings/context.hh)
124 | 


--------------------------------------------------------------------------------
/tests/python/test_delassus_wrapper.py:
--------------------------------------------------------------------------------
  1 | import example_robot_data as erd
  2 | import pinocchio as pin
  3 | import numpy as np
  4 | from pycontact import DelassusPinocchio
  5 | 
  6 | def create_models_and_datas():
  7 |     robot = erd.load("solo12")
  8 |     model = robot.model
  9 |     data = model.createData()
 10 |     nc = 3
 11 |     njoint = len(model.joints)
 12 |     contact_models = []
 13 |     contact_datas = []
 14 |     for i in range(nc):
 15 |         id1 = 0
 16 |         id2 = np.random.randint(1,njoint)
 17 |         placement_i1 = pin.SE3.Identity()
 18 |         placement_i2 = pin.SE3.Identity()
 19 |         contact_model_i = pin.RigidConstraintModel(
 20 |             pin.ContactType.CONTACT_3D,
 21 |             model,
 22 |             id2,
 23 |             placement_i2,
 24 |             id1,
 25 |             placement_i1,
 26 |             pin.ReferenceFrame.LOCAL,
 27 |         )
 28 |         contact_models += [contact_model_i]
 29 |         contact_data_i = contact_model_i.createData()
 30 |         contact_datas += [contact_data_i]
 31 |     q = pin.neutral(model)
 32 |     M = pin.crba(model, data, q)
 33 |     return model, data, contact_models, contact_datas
 34 | 
 35 | def test_delassus_pinocchio_init():
 36 |     model, data, contact_models, contact_datas = create_models_and_datas()
 37 |     Del = DelassusPinocchio(model, data, contact_models, contact_datas)
 38 |     assert(True)
 39 | 
 40 | def test_delassus_pinocchio_evaluate():
 41 |     model, data, contact_models, contact_datas = create_models_and_datas()
 42 |     Del = DelassusPinocchio(model, data, contact_models, contact_datas)
 43 |     Del.computeChol(1e-6)
 44 |     Del.evaluateDel()
 45 |     G = Del.G_
 46 |     chol = pin.ContactCholeskyDecomposition(model, contact_models)
 47 |     mu = 1e-9
 48 |     chol.compute(model, data, contact_models, contact_datas, mu) #mu should be set to 0 in order to get the exact Delassus
 49 |     G_test = chol.getInverseOperationalSpaceInertiaMatrix() - mu*np.eye(3*Del.nc_)
 50 |     assert(np.allclose(G, G_test))
 51 | 
 52 | def test_delassus_pinocchio_applyOnTheRight():
 53 |     model, data, contact_models, contact_datas = create_models_and_datas()
 54 |     Del = DelassusPinocchio(model, data, contact_models, contact_datas)
 55 |     Del.computeChol(1e-6)
 56 |     Del.evaluateDel()
 57 |     lam = np.zeros(3*Del.nc_)
 58 |     lam_out = np.zeros(3*Del.nc_)
 59 |     Del.applyOnTheRight(lam, lam_out)
 60 |     chol = pin.ContactCholeskyDecomposition(model, contact_models)
 61 |     mu = 1e-9
 62 |     chol.compute(model, data, contact_models, contact_datas, mu) #mu should be set to 0 in order to get the exact Delassus
 63 |     G_test = chol.getInverseOperationalSpaceInertiaMatrix() - mu*np.eye(3*Del.nc_)
 64 |     lam_out2 = G_test @ lam
 65 |     assert(np.allclose(lam_out,lam_out2))
 66 | 
 67 | def test_delassus_pinocchio_computeChol():
 68 |     model, data, contact_models, contact_datas = create_models_and_datas()
 69 |     Del = DelassusPinocchio(model, data, contact_models, contact_datas)
 70 |     Del.computeChol(1e-9)
 71 |     assert(True)
 72 | 
 73 | def test_delassus_pinocchio_updateChol():
 74 |     model, data, contact_models, contact_datas = create_models_and_datas()
 75 |     Del = DelassusPinocchio(model, data, contact_models, contact_datas)
 76 |     Del.computeChol(1e-9)
 77 |     Del.updateChol(1e-7)
 78 |     assert(True)
 79 | 
 80 | def test_delassus_pinocchio_solve():
 81 |     model, data, contact_models, contact_datas = create_models_and_datas()
 82 |     nc = len(contact_models)
 83 |     Del = DelassusPinocchio(model, data, contact_models, contact_datas)
 84 |     Del.computeChol(1e-9)
 85 |     lam = np.zeros(3*nc)
 86 |     lam_out = np.zeros(3*nc)
 87 |     Del.solve(lam, lam_out)
 88 |     chol = pin.ContactCholeskyDecomposition(model, contact_models)
 89 |     mu = 1e-9
 90 |     chol.compute(model, data, contact_models, contact_datas, mu) #mu should be set to 0 in order to get the exact Delassus
 91 |     G_test = chol.getInverseOperationalSpaceInertiaMatrix() - mu*np.eye(3*nc)
 92 |     lam_out2 = np.linalg.inv(G_test+mu*np.eye(3*nc))@ lam
 93 |     assert(np.allclose(lam_out, lam_out2))
 94 | 
 95 | 
 96 | if __name__ == '__main__':
 97 |     import sys
 98 |     import pytest
 99 |     sys.exit(pytest.main(sys.argv))
100 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.10)
  2 | 
  3 | set(PROJECT_NAME contactbench)
  4 | set(PROJECT_DESCRIPTION "Contact benchmark")
  5 | set(PROJECT_URL "https://github.com/quentinll/diffcontact-bench")
  6 | set(PROJECT_CUSTOM_HEADER_EXTENSION "hpp")
  7 | # set(CMAKE_CXX_STANDARD 11)
  8 | set(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE)
  9 | set(INSTALL_DOCUMENTATION OFF CACHE BOOL ""  FORCE)
 10 | 
 11 | # Disable -Werror on Unix for now.
 12 | set(CXX_DISABLE_WERROR True)
 13 | set(CMAKE_VERBOSE_MAKEFILE True)
 14 | 
 15 | # Set CMake Policies
 16 | if(POLICY CMP0068)
 17 |   cmake_policy(SET CMP0068 NEW)
 18 | endif(POLICY CMP0068)
 19 | 
 20 | # Check if the submodule cmake have been initialized
 21 | if(NOT EXISTS "${CMAKE_CURRENT_LIST_DIR}/cmake/base.cmake")
 22 |   message(
 23 |     FATAL_ERROR
 24 |       "\nPlease run the following command first:\ngit submodule update --init\n"
 25 |   )
 26 | endif()
 27 | 
 28 | # Include jrl-cmake submodules
 29 | include(${CMAKE_CURRENT_LIST_DIR}/cmake/base.cmake)
 30 | include(${CMAKE_CURRENT_LIST_DIR}/cmake/ide.cmake)
 31 | include(${CMAKE_CURRENT_LIST_DIR}/cmake/apple.cmake)
 32 | include(${CMAKE_CURRENT_LIST_DIR}/cmake/boost.cmake)
 33 | include(${CMAKE_CURRENT_LIST_DIR}/cmake/python.cmake)
 34 | include(CMakeDependentOption)
 35 | include(FetchContent)
 36 | 
 37 | compute_project_args(PROJECT_ARGS LANGUAGES CXX)
 38 | project(${PROJECT_NAME} ${PROJECT_ARGS})
 39 | set(CMAKE_VERBOSE_MAKEFILE OFF)
 40 | 
 41 | # If needed, set CMake policy for APPLE systems
 42 | apply_default_apple_configuration()
 43 | 
 44 | # Options
 45 | option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
 46 | option(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the automatic differentiation support (via CppAD)" OFF)
 47 | option(GENERATE_PYTHON_STUBS "Generate the Python stubs associated to the Python library" OFF )
 48 | option(ENABLE_TEMPLATE_INSTANTIATION "Template instantiation of the main library" ON)
 49 | option(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
 50 | option(CHECK_RUNTIME_MALLOC "Check if some memory allocations are performed at runtime" OFF)
 51 | 
 52 | if(INITIALIZE_WITH_NAN)
 53 |   message(STATUS "Initialize with NaN all the Eigen entries.")
 54 |   add_definitions(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
 55 | endif(INITIALIZE_WITH_NAN)
 56 | 
 57 | if(ENABLE_TEMPLATE_INSTANTIATION)
 58 |   message(STATUS "Template instantiation of the main library")
 59 |   add_definitions(-DCONTACTBENCH_ENABLE_TEMPLATE_INSTANTIATION)
 60 | endif(ENABLE_TEMPLATE_INSTANTIATION)
 61 | 
 62 | if(CHECK_RUNTIME_MALLOC)
 63 |   Message(STATUS "Check if some memory allocations are performed at runtime.")
 64 |   Add_definitions(-DCONTACTBENCH_EIGEN_CHECK_MALLOC)
 65 |   Add_definitions(-DEIGEN_RUNTIME_NO_MALLOC)
 66 | endif(CHECK_RUNTIME_MALLOC)
 67 | 
 68 | set_boost_default_options()
 69 | export_boost_default_options()
 70 | if(WIN32)
 71 |   add_project_dependency(Boost REQUIRED COMPONENTS chrono thread date_time serialization)
 72 | elseif(WIN32)
 73 |   add_project_dependency(Boost REQUIRED chrono serialization)
 74 | endif(WIN32)
 75 | 
 76 | # MAIN LIBRARY
 77 | file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS
 78 |      ${PROJECT_SOURCE_DIR}/include/contactbench/*.hpp
 79 |      ${PROJECT_SOURCE_DIR}/include/contactbench/*.hxx)
 80 | 
 81 | if(ENABLE_TEMPLATE_INSTANTIATION)
 82 |   set(LIB_TEMPLATE_SOURCES
 83 |     ${PROJECT_SOURCE_DIR}/src/solvers.cpp
 84 |     ${PROJECT_SOURCE_DIR}/src/contact-problem.cpp
 85 |     ${PROJECT_SOURCE_DIR}/src/delassus-wrapper.cpp
 86 |     )
 87 |   set(LIB_TEMPLATE_DECLARATIONS
 88 |     ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/solvers.txx
 89 |     ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/contact-problem.txx
 90 |     ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/delassus-wrapper.txx
 91 |     )
 92 |     list(APPEND ${PROJECT_NAME}_HEADERS ${LIB_TEMPLATE_DECLARATIONS})
 93 |     list(APPEND ${PROJECT_NAME}_SOURCES ${LIB_TEMPLATE_SOURCES})
 94 | endif()
 95 | 
 96 | add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
 97 | #adding dependencies
 98 | add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5")
 99 | add_project_dependency(pinocchio REQUIRED)
100 | add_project_dependency(proxsuite REQUIRED)
101 | target_link_libraries(
102 |   ${PROJECT_NAME}
103 |   PUBLIC
104 |   PUBLIC Eigen3::Eigen
105 |   PUBLIC pinocchio::pinocchio
106 |   PUBLIC proxsuite::proxsuite
107 | )
108 | set_target_properties(${PROJECT_NAME}
109 |   PROPERTIES
110 |   LINKER_LANGUAGE CXX)
111 | 
112 | # Tests
113 | if(BUILD_TESTING)
114 |   add_subdirectory(tests)
115 | endif()
116 | 
117 | # Python bindings
118 | if(BUILD_PYTHON_INTERFACE)
119 |   add_project_dependency(eigenpy 2.9.0 REQUIRED)
120 |   set(PYTHON_COMPONENTS Interpreter Development.Module)
121 |   set(PY_NAME py${PROJECT_NAME})
122 |   set(${PY_NAME}_INSTALL_DIR ${PYTHON_SITELIB}/${PY_NAME})
123 | 
124 |   add_project_dependency(Boost REQUIRED COMPONENTS system)
125 |   add_subdirectory(bindings)
126 | endif()
127 | 
128 | # target_compile_definitions(${PROJECT_NAME} PRIVATE -DPINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION)
129 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color")
130 | 
131 | 
132 | # AD support
133 | set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/find-external/CppAD/" ${CMAKE_MODULE_PATH})
134 | if(BUILD_WITH_AUTODIFF_SUPPORT)
135 |   add_project_dependency(cppad 20180000.0 REQUIRED PKG_CONFIG_REQUIRES "cppad >= 20180000.0")
136 |   target_compile_definitions(${PROJECT_NAME} PUBLIC DIFFCONTACT_WITH_CPPAD)
137 |   target_link_libraries(${PROJECT_NAME} PUBLIC ${cppad_LIBRARY})
138 | endif(BUILD_WITH_AUTODIFF_SUPPORT)
139 | 
140 | # Install library
141 | install(
142 |   TARGETS ${PROJECT_NAME}
143 |   EXPORT ${TARGETS_EXPORT_NAME}
144 |   INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
145 |   LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
146 | )
147 | 


--------------------------------------------------------------------------------
/examples/ur5.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from tap import Tap
  4 | from utils.models import create_ur5, addBall
  5 | from utils.visualize import sub_sample
  6 | from pycontact.simulators import (
  7 |     CCPADMMSimulator,
  8 |     CCPPGSSimulator,
  9 |     NCPPGSSimulator,
 10 |     PinNCPADMMSimulator,
 11 |     NCPStagProjSimulator,
 12 |     RaisimSimulator,
 13 | )
 14 | from pycontact.utils.pin_utils import inverse_kinematics_trans
 15 | from pycontact.inverse_dynamics import inverse_contact_dynamics
 16 | import meshcat
 17 | import os
 18 | from pathlib import Path
 19 | 
 20 | 
 21 | class Args(Tap):
 22 |     id: bool = False
 23 |     wall: bool = False
 24 |     ball: bool = False
 25 |     display: bool = False
 26 |     record: bool = False
 27 |     plot: bool = False
 28 |     seed: int = 1234
 29 |     save: bool = False
 30 |     debug: bool = False
 31 | 
 32 |     def process_args(self):
 33 |         if self.record:
 34 |             self.display = True
 35 | 
 36 | 
 37 | args = Args().parse_args()
 38 | 
 39 | np.random.seed(args.seed)
 40 | pin.seed(args.seed)
 41 | 
 42 | mu = 0.9  # friction parameter
 43 | comp = 10000
 44 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = create_ur5(
 45 |     mu, comp = comp, add_ball=True, add_wall=args.wall
 46 | )
 47 | 
 48 | if args.ball:
 49 |     a = 0.1
 50 |     m = 1e-4
 51 |     model, geom_model, visual_model, data, geom_data, visual_data, actuation = addBall(model, geom_model, visual_model, actuation,a, m, mu, 0.)
 52 |     model.qinit[6:9] = np.array([.85,0.1,a/2])
 53 | 
 54 | # Number of time steps
 55 | T = 10
 56 | # T = 1
 57 | # time steps
 58 | dt = 1e-3
 59 | 
 60 | # Physical parameters of the contact problem
 61 | # Kb = 1e-4  # Baumgarte
 62 | Kb = 1.0  # Baumgarte
 63 | eps = 0.0  # elasticity
 64 | 
 65 | # initial state
 66 | frame_id = model.getFrameId("wrist_3_link")
 67 | if args.wall:
 68 |     # frame_trans = np.array([0.0,0.42,0.7])
 69 |     frame_trans = np.array([0.0,0.43,0.7])
 70 | else:
 71 |     frame_trans = np.array([0.0,0.62,0.08])
 72 | q0 = inverse_kinematics_trans(model, data, frame_trans, frame_id)
 73 | v0 = np.zeros(model.nv)
 74 | q, v = q0.copy(), v0.copy()
 75 | 
 76 | 
 77 | # simulator
 78 | # simulator = NCPPGSSimulator()
 79 | simulator = PinNCPADMMSimulator()
 80 | # simulator = NCPStagProjSimulator()
 81 | # simulator = CCPPGSSimulator()
 82 | # simulator = CCPADMMSimulator()
 83 | # simulator = RaisimSimulator()
 84 | 
 85 | 
 86 | # record quantities during trajectory
 87 | xs = [np.concatenate((q0, v0))]
 88 | lams = []
 89 | lams_id = []
 90 | Js = []
 91 | us = []
 92 | Rs = []
 93 | es = []
 94 | Gs = []
 95 | gs = []
 96 | contact_points = []
 97 | 
 98 | c_ref = np.array([0.05,0.0,0.0])
 99 | for t in range(T):
100 |     if args.id:
101 |         v_ref = np.zeros(model.nv)
102 |         J_wrist = pin.computeJointJacobian(model, data, q, model.getJointId("wrist_3_joint"))
103 |         rot_wrist = data.oMi[model.getJointId("wrist_3_joint")].rotation
104 |         v_ref, _, _, _ = np.linalg.lstsq(rot_wrist@J_wrist[:3,:], c_ref, rcond = None)
105 |         print("v_ref", v_ref)
106 |         tau, lam_id = inverse_contact_dynamics(model,data, geom_model, geom_data, q, v, v_ref, dt, 100, 1e-10, rho = 1e-8, ccp = False)
107 |         # print("v_ref", v_ref)
108 |     else:
109 |         tau = np.zeros(model.nv)
110 |     fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
111 |     q, v = simulator.step(
112 |         model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 200, 1e-6
113 |     )
114 |     if args.debug:
115 |         print("t=", t)
116 |         simulator.plotCallback(block=False)
117 |         input("Press enter for next step!")
118 |     lams += [simulator.lam]
119 |     if args.id:
120 |         lams_id += [lam_id]
121 |     Js += [simulator.J]
122 |     Rs += [simulator.R]
123 |     es += [simulator.signed_dist]
124 |     Gs += [simulator.Del]
125 |     gs += [simulator.g]
126 |     xs += [np.concatenate((q, v))]
127 |     us += [tau]
128 |     contact_points += [simulator.contact_points]
129 | 
130 | if args.debug:
131 |     simulator.close_plot_callback()
132 | 
133 | 
134 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
135 | os.makedirs(dir_path / "log", exist_ok=True)
136 | os.makedirs(dir_path / "log/ur5/", exist_ok=True)
137 | if args.id:
138 |     os.makedirs(dir_path / "log/ur5/id/", exist_ok=True)
139 | 
140 | if args.save:
141 |     if args.id:
142 |         np.savez(
143 |             dir_path/"log/ur5/id/contact_forces.npz", lams = lams, lams_id = lams_id
144 |         )
145 | 
146 | 
147 | if args.display:
148 |     from pinocchio.visualize import MeshcatVisualizer
149 | 
150 |     # visualize the trajectory
151 |     vizer = MeshcatVisualizer(model, geom_model, visual_model)
152 |     vizer.initViewer(open=True, loadModel=True)
153 | 
154 |     vizer.viewer["plane"].set_object(meshcat.geometry.Box(np.array([10, 10, 0.1])))
155 |     placement = np.eye(4)
156 |     placement[:3, 3] = np.array([0, 0, -0.05])
157 |     vizer.viewer["plane"].set_transform(placement)
158 |     vizer.displayCollisions(True)
159 |     vizer.display(q0)
160 | 
161 |     numrep = 2
162 |     cp = [1.6, 0.0, 0.6]
163 |     # camera positions for visualization
164 |     cps_ = [cp.copy() for _ in range(numrep)]
165 |     rps_ = [np.zeros(3)]*numrep
166 |     qs = [x[:model.nq] for x in xs]
167 |     vs = [x[model.nq:] for x in xs]
168 | 
169 |     max_fps = 30.
170 |     fps = min([max_fps,1./dt])
171 |     qs = sub_sample(qs,dt*T, fps)
172 |     vs = sub_sample(vs,dt*T, fps)
173 | 
174 |     def get_callback(i: int):
175 |         def _callback(t):
176 |             vizer.setCameraPosition(cps_[i])
177 |             vizer.setCameraTarget(rps_[i])
178 |             pin.forwardKinematics(model, vizer.data, qs[t], vs[t])
179 |             # vizer.drawFrameVelocities(base_link_id)
180 | 
181 |         return _callback
182 | 
183 |     input("[Press enter]")
184 | 
185 |     if args.record:
186 |         ctx = vizer.create_video_ctx(dir_path/"log/ur5/simulation.mp4", fps=fps)
187 |     else:
188 |         import contextlib
189 | 
190 |         ctx = contextlib.nullcontext()
191 |     with ctx:
192 |         for i in range(numrep):
193 |             vizer.play(
194 |                 qs,
195 |                 1./fps,
196 |                 get_callback(i)
197 |             )
198 | 


--------------------------------------------------------------------------------
/tests/friction-constraints.cpp:
--------------------------------------------------------------------------------
  1 | // #define BOOST_TEST_MODULE FRICTION_CONSTRAINTS
  2 | #include "contactbench/friction-constraint.hpp"
  3 | #include "vector"
  4 | 
  5 | #include <iostream>
  6 | #include <boost/test/unit_test.hpp>
  7 | #include <boost/utility/binary.hpp>
  8 | 
  9 | BOOST_AUTO_TEST_SUITE(FRICTION_CONSTRAINTS)
 10 | 
 11 | namespace cb = contactbench;
 12 | using T = double;
 13 | CONTACTBENCH_EIGEN_TYPEDEFS(T);
 14 | 
 15 | BOOST_AUTO_TEST_CASE(ice_cream_cone_init) {
 16 |   cb::IceCreamCone<T> cone(.84);
 17 |   BOOST_CHECK(!std::isnan(cone.mu_));
 18 | }
 19 | 
 20 | // BOOST_AUTO_TEST_CASE(ice_cream_projection) {
 21 | //   cb::IceCreamCone<T> cone(.84);
 22 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
 23 | //   Eigen::Vector3d x_out;
 24 | //   cone.project(x, x_out);
 25 | //   BOOST_CHECK(x_out.isApprox(x));
 26 | //   x(2) = 1.;
 27 | //   x(0) = 1.08;
 28 | //   cone.project(x, x_out);
 29 | //   BOOST_CHECK(std::abs(x_out(0) - (x_out(2) * cone.mu_)) < 1e-3);
 30 | //   for (int i = 0; i < 10; i++) {
 31 | //     x = Eigen::Vector3d::Random();
 32 | //     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
 33 | //     Eigen::Vector3d x_out2;
 34 | //     cone.project(x, x_out);
 35 | //     cone.project(x_out, x_out2);
 36 | //     BOOST_CHECK(x_out.isApprox(x_out2));
 37 | //     BOOST_CHECK(std::abs(x_out.dot(x_out - x)) < 1e-6);
 38 | //   }
 39 | // }
 40 | 
 41 | // BOOST_AUTO_TEST_CASE(ice_cream_dual_projection) {
 42 | //   cb::IceCreamCone<T> cone(.84);
 43 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
 44 | //   Eigen::Vector3d x_out;
 45 | //   cone.projectDual(x, x_out);
 46 | //   BOOST_CHECK(x_out.isApprox(x));
 47 | //   x(2) = 1.;
 48 | //   x(0) = 1e3;
 49 | //   cone.projectDual(x, x_out);
 50 | //   BOOST_CHECK(std::abs(x_out(0) - (x_out(2) * 1. / cone.mu_)) < 1e-3);
 51 | //   for (int i = 0; i < 10; i++) {
 52 | //     x = Eigen::Vector3d::Random();
 53 | //     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
 54 | //     Eigen::Vector3d x_out2;
 55 | //     cone.projectDual(x, x_out);
 56 | //     cone.projectDual(x_out, x_out2);
 57 | //     BOOST_CHECK(x_out.isApprox(x_out2));
 58 | //     BOOST_CHECK(std::abs(x_out.dot(x_out - x)) < 1e-6);
 59 | //   }
 60 | // }
 61 | 
 62 | // BOOST_AUTO_TEST_CASE(ice_cream_circle_projection) {
 63 | //   cb::IceCreamCone<T> cone(.37);
 64 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
 65 | //   Eigen::Vector3d x_out;
 66 | //   cone.projectHorizontal(x, x_out);
 67 | //   BOOST_CHECK(x_out.isApprox(x));
 68 | //   x(2) = 1.59;
 69 | //   x(0) = 3.46;
 70 | //   cone.projectHorizontal(x, x_out);
 71 | //   BOOST_CHECK(x_out(0) == x(2) * cone.mu_);
 72 | //   for (int i = 0; i < 10; i++) {
 73 | //     x = Eigen::Vector3d::Random();
 74 | //     x[2] = std::abs(x[2]);
 75 | //     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
 76 | //     Eigen::Vector3d x_out2;
 77 | //     cone.projectHorizontal(x, x_out);
 78 | //     cone.projectHorizontal(x_out, x_out2);
 79 | //     BOOST_CHECK(x_out.isApprox(x_out2));
 80 | //     x_out2[0] = 0.;
 81 | //     x_out2[1] = 0.;
 82 | //     BOOST_CHECK(std::abs(x_out2.dot(x_out - x)) < 1e-6);
 83 | //   }
 84 | // }
 85 | 
 86 | // BOOST_AUTO_TEST_CASE(ice_cream_circle_projection2) {
 87 | //   cb::IceCreamCone<T> cone(.37);
 88 | //   Eigen::Vector2d x = Eigen::Vector2d::Zero();
 89 | //   double xn = 0.;
 90 | //   Eigen::Vector2d x_out;
 91 | //   cone.projectHorizontal(x, xn, x_out);
 92 | //   BOOST_CHECK(x_out.isApprox(x));
 93 | //   xn = 1.59;
 94 | //   x(0) = 3.46;
 95 | //   cone.projectHorizontal(x, xn, x_out);
 96 | //   BOOST_CHECK(x_out(0) == xn * cone.mu_);
 97 | //   for (int i = 0; i < 10; i++) {
 98 | //     x = Eigen::Vector2d::Random();
 99 | //     xn = (1. / (1. + std::rand())) * 10.;
100 | //     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
101 | //     Eigen::Vector2d x_out2;
102 | //     cone.projectHorizontal(x, xn, x_out);
103 | //     cone.projectHorizontal(x_out, xn, x_out2);
104 | //     BOOST_CHECK(x_out.isApprox(x_out2));
105 | //   }
106 | // }
107 | 
108 | // BOOST_AUTO_TEST_CASE(ice_cream_circle_contact_complementarity) {
109 | //   cb::IceCreamCone<T> cone(.37);
110 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
111 | //   Eigen::Vector3d v = Eigen::Vector3d::Zero();
112 | //   T comp = cone.computeContactComplementarity(x, v);
113 | //   BOOST_CHECK(comp == 0.);
114 | // }
115 | 
116 | // BOOST_AUTO_TEST_CASE(ice_cream_circle_signorini_complementarity) {
117 | //   cb::IceCreamCone<T> cone(.37);
118 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
119 | //   Eigen::Vector3d v = Eigen::Vector3d::Zero();
120 | //   T comp = cone.computeSignoriniComplementarity(x, v);
121 | //   BOOST_CHECK(comp == 0.);
122 | // }
123 | 
124 | // BOOST_AUTO_TEST_CASE(ice_cream_circle_conic_complementarity) {
125 | //   MatrixXs A = MatrixXs::Zero(3, 3);
126 | //   VectorXs b = VectorXs::Zero(3);
127 | //   cb::IceCreamCone<T> cone(.37);
128 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
129 | //   Eigen::Vector3d v = Eigen::Vector3d::Zero();
130 | //   T comp = cone.computeConicComplementarity(x, v);
131 | //   BOOST_CHECK(comp == 0.);
132 | // }
133 | 
134 | // BOOST_AUTO_TEST_CASE(pyramid_cone_init) {
135 | //   cb::PyramidCone<T> cone(.84);
136 | //   BOOST_CHECK(!std::isnan(cone.mu_));
137 | // }
138 | 
139 | // BOOST_AUTO_TEST_CASE(pyramid_cone_horizontal_projection) {
140 | //   cb::PyramidCone<T> cone(.37);
141 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
142 | //   Eigen::Vector3d x_out;
143 | //   cone.projectHorizontal(x, x_out);
144 | //   BOOST_CHECK(x_out.isApprox(x));
145 | //   x(2) = 1.59;
146 | //   x(0) = 3.46;
147 | //   cone.projectHorizontal(x, x_out);
148 | //   BOOST_CHECK(x_out(0) == x(2) * cone.mu_);
149 | // }
150 | 
151 | // BOOST_AUTO_TEST_CASE(pyramid_cone_horizontal_projection2) {
152 | //   cb::PyramidCone<T> cone(.37);
153 | //   Eigen::Vector2d x = Eigen::Vector2d::Zero();
154 | //   double xn = 0.;
155 | //   Eigen::Vector2d x_out;
156 | //   cone.projectHorizontal(x, xn, x_out);
157 | //   BOOST_CHECK(x_out.isApprox(x));
158 | //   xn = 1.59;
159 | //   x(0) = 3.46;
160 | //   cone.projectHorizontal(x, xn, x_out);
161 | //   BOOST_CHECK(x_out(0) == xn * cone.mu_);
162 | // }
163 | 
164 | // BOOST_AUTO_TEST_CASE(pyramid_computeCoordinatesInD) {
165 | //   double mu = .37;
166 | //   cb::PyramidCone<T> cone(mu);
167 | //   Eigen::Vector3d x = Eigen::Vector3d::Zero();
168 | //   Eigen::Vector4d x_out;
169 | //   cone.computeCoordinatesInD(x, x_out);
170 | //   BOOST_CHECK(x_out.isZero(0.));
171 | //   x(2) = 1.59;
172 | //   x(0) = mu * x(2);
173 | //   cone.computeCoordinatesInD(x, x_out);
174 | //   BOOST_CHECK(x_out(1) == 0.);
175 | //   BOOST_CHECK(x_out(2) == 0.);
176 | //   BOOST_CHECK(std::abs(x_out(0) - x_out(3)) < 1e-5);
177 | // }
178 | 
179 | BOOST_AUTO_TEST_SUITE_END()
180 | 


--------------------------------------------------------------------------------
/bindings/pycontact/torch/solvers.py:
--------------------------------------------------------------------------------
  1 | try:
  2 |     import torch
  3 |     from torch.autograd import Function
  4 |     torch_available = True
  5 | except ImportError:
  6 |     torch_available = False
  7 | 
  8 | import numpy as np
  9 | from pycontact import LCPQPSolver, NCPPGSSolver, CCPPGSSolver, LinearContactProblem, ContactProblem, ContactSolverSettings
 10 | 
 11 | if torch_available:
 12 |     class ContactLayer(Function):
 13 |         @staticmethod
 14 |         def forward(ctx,G, g, mus, max_iter, eps, contact_problem_type, contact_solver_type):
 15 |             batch_size = g.size()[0]
 16 |             lam =torch.zeros(g.size(), dtype = g.dtype)
 17 |             ctx.probs = []
 18 |             ctx.solvers = []
 19 |             settings = ContactSolverSettings()
 20 |             settings.max_iter_ = max_iter
 21 |             settings.th_stop_ = eps
 22 |             for i in range(batch_size):
 23 |                 Gi,gi,musi = G[i,:,:].detach().numpy(),g[i,:,:].detach().numpy(), mus[i,:,0].detach().numpy()
 24 |                 probi = contact_problem_type(Gi, gi, musi.tolist())
 25 |                 solveri = contact_solver_type()
 26 |                 solveri.setProblem(probi)
 27 |                 lam_ws = np.zeros(len(gi))
 28 |                 solveri.solve(probi, lam_ws, settings)
 29 |                 lam[i,:,0] = torch.from_numpy(solveri.getSolution().copy())
 30 |                 ctx.probs += [probi]
 31 |                 ctx.solvers += [solveri]
 32 |             ctx.save_for_backward(G,g,mus)
 33 |             return lam
 34 | 
 35 |         @staticmethod
 36 |         def backward(ctx, grad_l):
 37 |             '''
 38 |             Compute derivatives of the solution of the contact problem
 39 |             via implicit differentiation.
 40 |             '''
 41 |             G,g,mus = ctx.saved_tensors
 42 |             batch_size = g.size()[0]
 43 |             grad_G, grad_g, grad_mus = None, None, None
 44 |             if ctx.needs_input_grad[0] or ctx.needs_input_grad[1] or ctx.needs_input_grad[2]:
 45 |                 grad_G =  torch.zeros(G.size(), dtype = g.dtype)
 46 |                 grad_g = torch.zeros(g.size(), dtype = g.dtype)
 47 |                 grad_mus = torch.zeros(mus.size(), dtype = g.dtype)
 48 |                 for i in range(batch_size):
 49 |                     solveri = ctx.solvers[i]
 50 |                     probi = ctx.probs[i]
 51 |                     grad_li = grad_l[i,:,:].detach().numpy()
 52 |                     solveri.vjp(probi, grad_li)
 53 |                     grad_mus[i] = torch.from_numpy(solveri.getdLdmus().copy()).unsqueeze(-1)
 54 |                     grad_G[i] = torch.from_numpy(solveri.getdLdDel().copy())
 55 |                     grad_g[i] = torch.from_numpy(solveri.getdLdg().copy()).unsqueeze(-1)
 56 |             return grad_G, grad_g, grad_mus, None, None, None, None
 57 | 
 58 |     lcp_layer = lambda G, g, mus, max_iter, eps : ContactLayer.apply( G, g, mus, max_iter, eps, LinearContactProblem, LCPQPSolver)
 59 |     ccp_layer = lambda G, g, mus, max_iter, eps : ContactLayer.apply( G, g, mus, max_iter, eps, ContactProblem, CCPPGSSolver)
 60 | 
 61 | 
 62 |     class ContactLayerApproxBackward(Function):
 63 |         @staticmethod
 64 |         def forward(ctx,G, g, mus, max_iter, eps, contact_problem_type, contact_solver_type):
 65 |             ContactLayer.forward(ctx, G, g, mus, max_iter, eps, contact_problem_type, contact_solver_type)
 66 | 
 67 |         @staticmethod
 68 |         def backward(ctx, grad_l):
 69 |             '''
 70 |             Compute derivatives of the solution of the contact problem
 71 |             via approximate linear backward.
 72 |             '''
 73 |             G,g,mus = ctx.saved_tensors
 74 |             batch_size = g.size()[0]
 75 |             grad_G, grad_g, grad_mus = None, None, None
 76 |             if ctx.needs_input_grad[0] or ctx.needs_input_grad[1] or ctx.needs_input_grad[2]:
 77 |                 grad_G =  torch.zeros(G.size(), dtype = g.dtype)
 78 |                 grad_g = torch.zeros(g.size(), dtype = g.dtype)
 79 |                 grad_mus = torch.zeros(mus.size(), dtype = g.dtype)
 80 |                 for i in range(batch_size):
 81 |                     solveri = ctx.solvers[i]
 82 |                     probi = ctx.probs[i]
 83 |                     grad_li = grad_l[i,:,:].detach().numpy()
 84 |                     solveri.vjp_approx(probi, grad_li)
 85 |                     grad_mus[i] = torch.from_numpy(solveri.getdLdmus().copy()).unsqueeze(-1)
 86 |                     grad_G[i] = torch.from_numpy(solveri.getdLdDel().copy())
 87 |                     grad_g[i] = torch.from_numpy(solveri.getdLdg().copy()).unsqueeze(-1)
 88 |             return grad_G, grad_g, grad_mus, None, None, None, None
 89 | 
 90 |     ncp_layer = lambda G, g, mus, max_iter, eps : ContactLayerApproxBackward.apply( G, g, mus, max_iter, eps, ContactProblem, NCPPGSSolver)
 91 | 
 92 |     class ContactLayerADBackward(Function):
 93 |         @staticmethod
 94 |         def forward(ctx,G, g, mus, max_iter, eps, contact_problem_type, contact_solver_type):
 95 |             ContactLayer.forward(ctx, G, g, mus, max_iter, eps, contact_problem_type, contact_solver_type)
 96 | 
 97 |         @staticmethod
 98 |         def backward(ctx, grad_l):
 99 |             '''
100 |             Compute derivatives of the solution of the contact problem
101 |             via automatic differentiation
102 |             '''
103 |             G,g,mus = ctx.saved_tensors
104 |             batch_size = g.size()[0]
105 |             grad_G, grad_g, grad_mus = None, None, None
106 | 
107 |             if ctx.needs_input_grad[0] or ctx.needs_input_grad[1] or ctx.needs_input_grad[2]:
108 |                 grad_G =  torch.zeros(G.size(), dtype = g.dtype)
109 |                 grad_g = torch.zeros(g.size(), dtype = g.dtype)
110 |                 grad_mus = torch.zeros(mus.size(), dtype = g.dtype)
111 |                 for i in range(batch_size):
112 |                     solveri = ctx.solvers[i]
113 |                     probi = ctx.probs[i]
114 |                     grad_li = grad_l[i,:,:].detach().numpy()
115 |                     solveri.vjp_cppad(probi, grad_li)
116 |                     grad_mus[i] = torch.from_numpy(solveri.getdLdmus().copy()).unsqueeze(-1)
117 |                     grad_G[i] = torch.from_numpy(solveri.getdLdDel().copy())
118 |                     grad_g[i] = torch.from_numpy(solveri.getdLdg().copy()).unsqueeze(-1)
119 |             return grad_G, grad_g, grad_mus, None, None, None, None
120 | 
121 |     ncp_ad_layer_ = lambda G, g, mus, max_iter, eps : ContactLayerADBackward.apply( G, g, mus, max_iter, eps, ContactProblem, NCPPGSSolver)
122 |     ccp_ad_layer_ = lambda G, g, mus, max_iter, eps : ContactLayerADBackward.apply( G, g, mus, max_iter, eps, ContactProblem, CCPPGSSolver)
123 | 
124 | else:
125 |     print("Torch not available, skipping torch solvers")


--------------------------------------------------------------------------------
/tests/python/test_contact_problem.py:
--------------------------------------------------------------------------------
  1 | from pycontact import ContactProblem, DelassusPinocchio
  2 | import pinocchio as pin
  3 | import example_robot_data as erd
  4 | import numpy as np
  5 | 
  6 | 
  7 | def test_ContactProblem():
  8 |     nc = np.random.randint(1, 10)
  9 |     A = np.random.rand(3 * nc, 3 * nc)
 10 |     b = np.random.random(3 * nc)
 11 |     mus = [0.8] * nc
 12 |     prob = ContactProblem(A, b, mus)
 13 |     for i in range(nc):
 14 |         assert prob.contact_constraints_[i].mu_ == mus[i]
 15 | 
 16 | 
 17 | def test_ContactProblem_project():
 18 |     nc = np.random.randint(1, 10)
 19 |     A = np.random.rand(3 * nc, 3 * nc)
 20 |     b = np.random.random(3 * nc)
 21 |     mus = [0.8] * nc
 22 |     prob = ContactProblem(A, b, mus)
 23 |     lam = np.random.rand(b.shape[0])
 24 |     lam_proj = np.zeros_like(lam)
 25 |     lam_proj2 = np.zeros_like(lam)
 26 |     prob.project(lam, lam_proj)
 27 |     ContactProblem.project(mus, lam, lam_proj2)
 28 |     for i in range(nc):
 29 |         lami_proj = np.zeros(3)
 30 |         prob.contact_constraints_[i].project(lam[3*i:3*i+3], lami_proj)
 31 |         assert (lami_proj == lam_proj[3*i:3*i+3]).all()
 32 |         assert (lami_proj == lam_proj2[3*i:3*i+3]).all()
 33 | 
 34 | def test_ContactProblem_projectDual():
 35 |     nc = np.random.randint(1, 10)
 36 |     A = np.random.rand(3 * nc, 3 * nc)
 37 |     b = np.random.random(3 * nc)
 38 |     mus = [0.8] * nc
 39 |     prob = ContactProblem(A, b, mus)
 40 |     lam = np.random.rand(b.shape[0])
 41 |     lam_proj = np.zeros_like(lam)
 42 |     prob.projectDual(lam, lam_proj)
 43 |     for i in range(nc):
 44 |         lami_proj = np.zeros(3)
 45 |         prob.contact_constraints_[i].projectDual(lam[3*i:3*i+3], lami_proj)
 46 |         assert (lami_proj == lam_proj[3*i:3*i+3]).all()
 47 | 
 48 | 
 49 | def test_ContactProblem_computeContactComplementarity():
 50 |     nc = np.random.randint(1, 10)
 51 |     A = np.random.rand(3 * nc, 3 * nc)
 52 |     b = np.random.random(3 * nc)
 53 |     mus = [0.8] * nc
 54 |     prob = ContactProblem(A, b, mus)
 55 |     lam = np.random.random(3 * nc)
 56 |     prob.computeContactComplementarity(lam)
 57 |     assert True
 58 | 
 59 | 
 60 | def test_ContactProblem_computeContactComplementarity2():
 61 |     nc = np.random.randint(1, 10)
 62 |     A = np.random.rand(3 * nc, 3 * nc)
 63 |     b = np.random.random(3 * nc)
 64 |     mus = [0.8] * nc
 65 |     prob = ContactProblem(A, b, mus)
 66 |     lam = np.random.random(3 * nc)
 67 |     v = np.random.random(3 * nc)
 68 |     prob.computeContactComplementarity(lam, v)
 69 |     assert True
 70 | 
 71 | 
 72 | def test_ContactProblem_computeSignoriniComplementarity():
 73 |     nc = np.random.randint(1, 10)
 74 |     A = np.random.rand(3 * nc, 3 * nc)
 75 |     b = np.random.random(3 * nc)
 76 |     mus = [0.8] * nc
 77 |     prob = ContactProblem(A, b, mus)
 78 |     lam = np.random.random(3 * nc)
 79 |     prob.computeSignoriniComplementarity(lam)
 80 |     assert True
 81 | 
 82 | 
 83 | def test_ContactProblem_computeSignoriniComplementarity2():
 84 |     nc = np.random.randint(1, 10)
 85 |     A = np.random.rand(3 * nc, 3 * nc)
 86 |     b = np.random.random(3 * nc)
 87 |     mus = [0.8] * nc
 88 |     prob = ContactProblem(A, b, mus)
 89 |     lam = np.random.random(3 * nc)
 90 |     v = np.random.random(3 * nc)
 91 |     prob.computeSignoriniComplementarity(lam, v)
 92 |     assert True
 93 | 
 94 | 
 95 | def test_ContactProblem_computeConicComplementarity():
 96 |     nc = np.random.randint(1, 10)
 97 |     A = np.random.rand(3 * nc, 3 * nc)
 98 |     b = np.random.random(3 * nc)
 99 |     mus = [0.8] * nc
100 |     prob = ContactProblem(A, b, mus)
101 |     lam = np.random.random(3 * nc)
102 |     prob.computeConicComplementarity(lam)
103 |     assert True
104 | 
105 | 
106 | def test_ContactProblem_computeConicComplementarity2():
107 |     nc = np.random.randint(1, 10)
108 |     A = np.random.rand(3 * nc, 3 * nc)
109 |     b = np.random.random(3 * nc)
110 |     mus = [0.8] * nc
111 |     prob = ContactProblem(A, b, mus)
112 |     lam = np.random.random(3 * nc)
113 |     v = np.random.random(3 * nc)
114 |     prob.computeConicComplementarity(lam, v)
115 |     assert True
116 | 
117 | 
118 | def test_ContactProblem_computePerContactContactComplementarity():
119 |     nc = np.random.randint(1, 10)
120 |     A = np.random.rand(3 * nc, 3 * nc)
121 |     b = np.random.random(3 * nc)
122 |     mus = [0.8] * nc
123 |     prob = ContactProblem(A, b, mus)
124 |     i = np.random.randint(0, nc)
125 |     lam = np.random.random(3 * nc)
126 |     v = A[3 * i : 3 * (i + 1)] @ lam + b[3 * i : 3 * (i + 1)]
127 |     prob.computePerContactContactComplementarity(i, lam[3 * i : 3 * (i + 1)], v)
128 |     assert True
129 | 
130 | 
131 | def test_ContactProblem_computePerContactSignoriniComplementarity():
132 |     nc = np.random.randint(1, 10)
133 |     A = np.random.rand(3 * nc, 3 * nc)
134 |     b = np.random.random(3 * nc)
135 |     mus = [0.8] * nc
136 |     prob = ContactProblem(A, b, mus)
137 |     i = np.random.randint(0, nc)
138 |     lam = np.random.random(3 * nc)
139 |     v = A[3 * i : 3 * (i + 1)] @ lam + b[3 * i : 3 * (i + 1)]
140 |     prob.computePerContactSignoriniComplementarity(i, lam[3 * i : 3 * (i + 1)], v)
141 |     assert True
142 | 
143 | 
144 | def test_ContactProblem_computePerContactConicComplementarity():
145 |     nc = np.random.randint(1, 10)
146 |     A = np.random.rand(3 * nc, 3 * nc)
147 |     b = np.random.random(3 * nc)
148 |     mus = [0.8] * nc
149 |     prob = ContactProblem(A, b, mus)
150 |     i = np.random.randint(0, nc)
151 |     lam = np.random.random(3 * nc)
152 |     v = A[3 * i : 3 * (i + 1)] @ lam + b[3 * i : 3 * (i + 1)]
153 |     prob.computePerContactConicComplementarity(i, lam[3 * i : 3 * (i + 1)], v)
154 |     assert True
155 | 
156 | 
157 | def create_models_and_datas():
158 |     robot = erd.load("solo12")
159 |     model = robot.model
160 |     data = model.createData()
161 |     nc = 3
162 |     njoint = len(model.joints)
163 |     contact_models = []
164 |     contact_datas = []
165 |     for i in range(nc):
166 |         id1 = 0
167 |         id2 = np.random.randint(1,njoint)
168 |         placement_i1 = pin.SE3.Identity()
169 |         placement_i2 = pin.SE3.Identity()
170 |         contact_model_i = pin.RigidConstraintModel(
171 |             pin.ContactType.CONTACT_3D,
172 |             model,
173 |             id2,
174 |             placement_i2,
175 |             id1,
176 |             placement_i1,
177 |             pin.ReferenceFrame.LOCAL,
178 |         )
179 |         contact_models += [contact_model_i]
180 |         contact_data_i = contact_model_i.createData()
181 |         contact_datas += [contact_data_i]
182 |     q = pin.neutral(model)
183 |     M = pin.crba(model, data, q)
184 |     return model, data, contact_models, contact_datas
185 | 
186 | def test_constructor_delassus():
187 |     model, data, contact_models, contact_datas = create_models_and_datas()
188 |     Del = DelassusPinocchio(model, data, contact_models, contact_datas)
189 |     nc = Del.nc_
190 |     g = np.zeros(3*nc)
191 |     mus = [0.9]*nc
192 |     prob = ContactProblem(Del,g,mus)
193 | 
194 | 
195 | if __name__ == '__main__':
196 |     import sys
197 |     import pytest
198 |     sys.exit(pytest.main(sys.argv))
199 | 


--------------------------------------------------------------------------------
/examples/humanoid.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from tap import Tap
  4 | from utils.models import create_humanoid, addBall, addHumanoid
  5 | from utils.visualize import sub_sample
  6 | from pycontact.simulators import (
  7 |     NCPPGSSimulator,
  8 |     CCPADMMSimulator,
  9 |     CCPPGSSimulator,
 10 |     RaisimSimulator,
 11 |     RaisimCorrectedSimulator,
 12 |     NCPStagProjSimulator,
 13 |     CCPNewtonPrimalSimulator,
 14 |     PinNCPADMMSimulator
 15 | )
 16 | import meshcat
 17 | import os
 18 | from pathlib import Path
 19 | from tqdm import trange
 20 | 
 21 | 
 22 | class Args(Tap):
 23 |     perturb: bool = False
 24 |     ball: bool = False
 25 |     twotalos: bool = False
 26 |     display: bool = False
 27 |     display_collision: bool = False
 28 |     timings: bool = False
 29 |     record: bool = False
 30 |     plot: bool = False
 31 |     seed: int = 1234
 32 |     save: bool = False
 33 |     debug: bool = False
 34 | 
 35 |     def process_args(self):
 36 |         if self.record:
 37 |             self.display = True
 38 | 
 39 | 
 40 | args = Args().parse_args()
 41 | 
 42 | np.random.seed(args.seed)
 43 | pin.seed(args.seed)
 44 | 
 45 | mu = 0.9  # friction parameter
 46 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = create_humanoid(
 47 |     mu,
 48 |     0.,
 49 |     0.,
 50 |     True,
 51 |     True,
 52 |     args.twotalos
 53 | )
 54 | if args.ball:
 55 |     model, geom_model, visual_model, data, geom_data, visual_data, actuation = addBall(model, geom_model, visual_model, actuation,0.1,1e-3, mu, 0.)
 56 |     model.qinit[-7:-5] = np.array([0.,2.])
 57 | if args.twotalos:
 58 |     model, geom_model, visual_model, data, geom_data, visual_data, actuation = addHumanoid(model, geom_model, visual_model, actuation, mu, 0. , 1, True, True, True)
 59 |     model.qref[7] = 2.1
 60 |     place = pin.SE3.Identity()
 61 |     place.rotation[:,0] = np.array([-1.,0.,0.])
 62 |     place.rotation[:,1] = np.array([0.,-1.,0.])
 63 |     model.qref[-4:] = pin.SE3ToXYZQUAT(place)[-4:]
 64 |     model.qinit = model.qref.copy()
 65 | 
 66 | 
 67 | # Number of time steps
 68 | T = 2000
 69 | # time steps
 70 | dt = 1*1e-3
 71 | 
 72 | # Physical parameters of the contact problem
 73 | Kb = 1e-4  # Baumgarte
 74 | eps = 0.0  # elasticity
 75 | 
 76 | # initial state
 77 | q0 = model.qinit
 78 | v0 = np.zeros(model.nv)
 79 | q, v = q0.copy(), v0.copy()
 80 | 
 81 | 
 82 | # simulator
 83 | warm_start = True
 84 | # simulator = NCPPGSSimulator(warm_start=warm_start, timings = args.timings)
 85 | # simulator = CCPNewtonPrimalSimulator(warm_start=warm_start, timings = args.timings)
 86 | simulator = PinNCPADMMSimulator(warm_start=warm_start, timings = args.timings)
 87 | # simulator = CCPPGSSimulator(warm_start=warm_start, timings = args.timings)
 88 | # simulator = CCPADMMSimulator(warm_start=warm_start, timings = args.timings)
 89 | # simulator = RaisimSimulator(warm_start=warm_start, timings = args.timings)
 90 | # simulator = RaisimCorrectedSimulator(warm_start=warm_start, timings = args.timings)
 91 | # simulator = NCPStagProjSimulator(warm_start=warm_start, timings = args.timings)
 92 | 
 93 | simulator.setSimulation(model, data, geom_model, geom_data)
 94 | 
 95 | # record quantities during trajectory
 96 | xs = [np.concatenate((q0, v0))]
 97 | lams = []
 98 | Js = []
 99 | us = []
100 | Rs = []
101 | es = []
102 | Gs = []
103 | gs = []
104 | if args.timings:
105 |     timings = []
106 | if args.perturb:
107 |     def ext_torque(t):
108 |         fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
109 |         noise_intensity = 75.
110 |         freq = 2.
111 |         linear_force =(np.array([np.sin(freq*t), np.sin(freq*t+1.), np.sin(freq*t+2)]))*noise_intensity
112 |         fext[1] = pin.Force(np.concatenate((linear_force,np.zeros(3))))
113 |         return fext
114 | else:
115 |     def ext_torque(t):
116 |         fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
117 |         return fext
118 | for t in trange(T):
119 |     tau = np.zeros(model.nv)
120 |     fext = ext_torque(t*dt)
121 |     q, v = simulator.step(
122 |         model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 10000, 1e-6, rel_th_stop = 1e-12
123 |     )
124 |     if args.debug:
125 |         print("t=", t)
126 |         simulator.plotCallback(block=False)
127 |         input("Press enter for next step!")
128 |     # print( "stop_", simulator.solver.stop_)
129 |     # print( "rel_stop_", simulator.solver.rel_stop_)
130 |     # print( "comp_reg_", simulator.solver.comp_reg_)
131 |     lams += [simulator.lam]
132 |     Js += [simulator.J]
133 |     Rs += [simulator.R]
134 |     es += [simulator.signed_dist]
135 |     # Gs += [simulator.G]
136 |     gs += [simulator.g]
137 |     xs += [np.concatenate((q, v))]
138 |     us += [tau]
139 |     if args.timings:
140 |         timings += [simulator.solve_time]
141 | 
142 | if args.debug:
143 |     simulator.close_plot_callback()
144 | 
145 | 
146 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
147 | os.makedirs(dir_path / "log", exist_ok=True)
148 | os.makedirs(dir_path / "log/talos/", exist_ok=True)
149 | 
150 | if args.save:
151 |     if simulator.Del is not None:
152 |         simulator.Del.evaluateDel()
153 |         np.savez(
154 |             dir_path /"log/talos/contact_problem.npz", G=simulator.Del.G_, g=simulator.g, mus=simulator.mus
155 |         )
156 |     if args.timings:
157 |         np.save(dir_path/"log/talos/timings.npy", timings)
158 | 
159 | if args.plot:
160 |     if args.save:
161 |         np.savez(
162 |             dir_path /"log/talos/traj.npz", Gs=Gs, gs=gs, lams=lams, xs =xs, us= us
163 |         )
164 | 
165 | 
166 | if args.display:
167 |     from pinocchio.visualize import MeshcatVisualizer
168 |     # visualize the trajectory
169 |     vizer = MeshcatVisualizer(model, geom_model, visual_model)
170 |     vizer.initViewer(open=True, loadModel=True)
171 | 
172 |     vizer.viewer["plane"].set_object(meshcat.geometry.Box(np.array([20, 20, 0.1])))
173 |     placement = np.eye(4)
174 |     placement[:3, 3] = np.array([0, 0, -0.05])
175 |     # vizer.displayVisual(True)
176 |     if args.display_collision:
177 |         # vizer.displayVisual(False)
178 |         vizer.displayCollisions(True)
179 |     vizer.display(q0)
180 | 
181 |     numrep = 2
182 |     if args.twotalos:
183 |         cp = [2.6, 1.0, 1.5]
184 |     else:
185 |         cp = [2.6, 0.0, 1.5]
186 |     # camera positions for visualization
187 |     cps_ = [cp.copy() for _ in range(numrep)]
188 |     rps_ = [np.zeros(3)]*numrep
189 |     qs = [x[:model.nq] for x in xs]
190 |     vs = [x[model.nq:] for x in xs]
191 | 
192 |     max_fps = 30.
193 |     fps = min([max_fps,1./dt])
194 |     qs = sub_sample(qs,dt*T, fps)
195 |     vs = sub_sample(vs,dt*T, fps)
196 | 
197 |     def get_callback(i: int):
198 |         def _callback(t):
199 |             vizer.setCameraPosition(cps_[i])
200 |             vizer.setCameraTarget(rps_[i])
201 |             pin.forwardKinematics(model, vizer.data, qs[t], vs[t])
202 |             # vizer.drawFrameVelocities(base_link_id)
203 | 
204 |         return _callback
205 | 
206 |     input("[Press enter]")
207 | 
208 |     if args.record:
209 |         ctx = vizer.create_video_ctx(dir_path/"log/solo/simulation.mp4", fps=fps)
210 |     else:
211 |         import contextlib
212 | 
213 |         ctx = contextlib.nullcontext()
214 |     with ctx:
215 |         for i in range(numrep):
216 |             vizer.play(
217 |                 qs,
218 |                 1./fps,
219 |                 get_callback(i)
220 |             )
221 | 


--------------------------------------------------------------------------------
/examples/solve_ncp.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | from utils.models import build_solo_problem, build_talos_problem, build_allegro_hand_problem, build_cube_problem
  3 | from pycontact import ContactProblem, NCPPGSSolver, CCPPGSSolver, CCPADMMSolver, RaisimSolver, NCPStagProjSolver, LCPPGSSolver, CCPNewtonPrimalSolver, ContactSolverSettings
  4 | from tap import Tap
  5 | import os
  6 | from pathlib import Path
  7 | 
  8 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
  9 | os.makedirs(dir_path / "log/timings", exist_ok=True)
 10 | 
 11 | 
 12 | class Args(Tap):
 13 |     filename: str = None
 14 |     save: bool = False
 15 |     plot: bool = False
 16 |     timings: bool = False
 17 |     accuracy : bool = False
 18 |     stats : bool = False
 19 |     solo: bool = False
 20 |     talos: bool = False
 21 |     hand: bool = False
 22 |     cube: bool = False
 23 |     # savedir: str = "log/ncp/others"
 24 | 
 25 |     def process_args(self):
 26 |         # if self.filename is not None:
 27 |         #     if not os.path.exists(self.filename):
 28 |         #         raise ValueError("filename does not exist.")
 29 |         # else:
 30 |         #     raise ValueError("filename does not exist.")
 31 |         return
 32 | 
 33 | 
 34 | args = Args().parse_args()
 35 | 
 36 | if args.filename is not None:
 37 |     npzfile = np.load(dir_path / args.filename)
 38 |     G = npzfile["G"]
 39 |     g = npzfile["g"]
 40 |     mus = npzfile["mus"].tolist()
 41 |     prob = ContactProblem(G, g, mus)
 42 | elif args.solo:
 43 |     prob = build_solo_problem(dense = False, drag = True)
 44 |     if args.timings:
 45 |         os.makedirs(dir_path / "log/timings/solo", exist_ok=True)
 46 |         dir_file = Path(dir_path / "log/timings/solo")
 47 |     if args.stats:
 48 |         os.makedirs(dir_path / "log/iterations/solo", exist_ok=True)
 49 |         dir_file = Path(dir_path / "log/iterations/solo")
 50 | elif args.talos:
 51 |     prob = build_talos_problem(dense = False)
 52 |     if args.timings:
 53 |         os.makedirs(dir_path / "log/timings/talos", exist_ok=True)
 54 |         dir_file = Path(dir_path / "log/timings/talos")
 55 |     if args.stats:
 56 |         os.makedirs(dir_path / "log/iterations/talos", exist_ok=True)
 57 |         dir_file = Path(dir_path / "log/iterations/talos")
 58 | elif args.hand:
 59 |     prob = build_allegro_hand_problem(dense = False)
 60 |     if args.timings:
 61 |         os.makedirs(dir_path / "log/timings/hand", exist_ok=True)
 62 |         dir_file = Path(dir_path / "log/timings/hand")
 63 |     if args.stats:
 64 |         os.makedirs(dir_path / "log/iterations/hand", exist_ok=True)
 65 |         dir_file = Path(dir_path / "log/iterations/hand")
 66 | elif args.cube:
 67 |     prob = build_cube_problem(dense = False, drag = True)
 68 |     if args.timings:
 69 |         os.makedirs(dir_path / "log/timings/cube", exist_ok=True)
 70 |         dir_file = Path(dir_path / "log/timings/cube")
 71 |     if args.stats:
 72 |         os.makedirs(dir_path / "log/iterations/cube", exist_ok=True)
 73 |         dir_file = Path(dir_path / "log/iterations/cube")
 74 | else:
 75 |     raise Exception(
 76 |         "A npz file containing a contact problem (G, g, and mus) must be provided"
 77 |     )
 78 | 
 79 | nc = int(len(prob.g_) / 3)
 80 | 
 81 | # solver = NCPPGSSolver()
 82 | # solver = CCPPGSSolver()
 83 | # solver = LCPPGSSolver()
 84 | # solver = RaisimSolver()
 85 | # solver = CCPADMMSolver()
 86 | solver = CCPNewtonPrimalSolver()
 87 | # solver = NCPStagProjSolver()
 88 | solver.setProblem(prob)
 89 | eps_reg = 1e-0
 90 | stats = args.stats
 91 | max_iter =  10000
 92 | contact_settings = ContactSolverSettings()
 93 | contact_settings.max_iter_ = 100
 94 | contact_settings.th_stop_ = 1e-6
 95 | contact_settings.timings_ = False
 96 | contact_settings.statistics_ = stats
 97 | contact_settings.rel_th_stop_ = 1e-12
 98 | solver.solve(prob, np.zeros((3 * nc, 1)),settings = contact_settings)
 99 | # solver.solve(prob, np.zeros((3 * nc, 1)), max_iter, 1e-12, rel_th_stop=1e-12, statistics=stats)
100 | print(solver.n_iter_)
101 | print(solver.ncp_comp_reg_)
102 | 
103 | if args.save:
104 |     lam = solver.getSolution()
105 |     v = np.zeros_like(prob.g_)
106 |     prob.Del_.evaluateDel()
107 |     v = prob.Del_.G_ @ lam
108 |     v += prob.g_
109 |     R = np.eye(3*nc)*eps_reg
110 |     lam_stiction_analytical = - np.linalg.inv(prob.Del_.G_ + R) @ prob.g_
111 |     v_stiction_analytical = prob.Del_.G_ @ lam_stiction_analytical + prob.g_
112 |     np.savez(dir_file/"forces_velocities.npz", lam = lam, v= v,  eps_reg = eps_reg)
113 |     np.savez(dir_file/"analytical_forces_velocities.npz", lam = lam_stiction_analytical, v= v_stiction_analytical,  eps_reg = eps_reg)
114 |     if args.stats:
115 |         np.save(dir_file/"stop.npy", solver.stats_.stop_)
116 |         np.save(dir_file/"prim_feas.npy", solver.stats_.prim_feas_)
117 |         np.save(dir_file/"dual_feas.npy", solver.stats_.dual_feas_)
118 |         np.save(dir_file/"comp.npy", solver.stats_.comp_)
119 |         np.save(dir_file/"ncp_comp.npy", solver.stats_.ncp_comp_)
120 | 
121 | 
122 | if args.timings:
123 |     import timeit
124 |     timer = timeit.Timer(lambda : solver.solve(prob, np.zeros((3 * nc, 1)), 100, 1e-6))
125 |     timings = timer.repeat(repeat = 100, number = 1)
126 |     if args.save:
127 |         np.save(dir_file/"timings.npy", timings)
128 | 
129 | if args.plot:
130 | 
131 |     import matplotlib.pyplot as plt
132 |     import matplotlib
133 | 
134 |     matplotlib.use("agg")
135 |     matplotlib.rcParams["pdf.fonttype"] = 42
136 |     matplotlib.rcParams["ps.fonttype"] = 42
137 | 
138 |     fig = plt.figure()
139 |     iters = [t + 1 for t in range(len(solver.stats_.stop_))]
140 |     plt.semilogy(iters, solver.stats_.stop_, label="Stopping crit", marker="+")
141 |     plt.semilogy(iters, solver.stats_.rel_stop_, label="Rel stopping crit", marker="+")
142 |     plt.xlabel("iterations")
143 |     plt.ylabel("Contact complementarity")
144 |     plt.tight_layout()
145 |     if args.save:
146 |         plt.savefig(dir_file / "contact_comp.pdf")
147 |     plt.close()
148 | 
149 |     fig = plt.figure()
150 |     iters = [t + 1 for t in range(len(solver.stats_.ncp_comp_))]
151 |     plt.semilogy(iters, solver.stats_.ncp_comp_, marker="+")
152 |     plt.xlabel("iterations")
153 |     plt.ylabel("Contact complementarity")
154 |     plt.tight_layout()
155 |     if args.save:
156 |         plt.savefig(dir_file / "contact_comp.pdf")
157 |     plt.close()
158 | 
159 |     fig = plt.figure()
160 |     iters = [t + 1 for t in range(len(solver.stats_.comp_))]
161 |     plt.semilogy(iters, solver.stats_.comp_, marker="+")
162 |     plt.xlabel("iterations")
163 |     plt.ylabel("Problem complementarity")
164 |     plt.tight_layout()
165 |     if args.save:
166 |         plt.savefig(dir_file / "prob_comp.pdf")
167 |     plt.close()
168 | 
169 |     fig = plt.figure()
170 |     iters = [t + 1 for t in range(len(solver.stats_.prim_feas_))]
171 |     plt.semilogy(iters, solver.stats_.prim_feas_, marker="+")
172 |     plt.xlabel("iterations")
173 |     plt.ylabel("Primal feasibility")
174 |     plt.tight_layout()
175 |     if args.save:
176 |         plt.savefig(dir_file / "prim_feas.pdf")
177 |     plt.close()
178 | 
179 |     fig = plt.figure()
180 |     iters = [t + 1 for t in range(len(solver.stats_.dual_feas_))]
181 |     plt.semilogy(iters, solver.stats_.dual_feas_, marker="+")
182 |     plt.xlabel("iterations")
183 |     plt.ylabel("Dual feasibility")
184 |     plt.tight_layout()
185 |     if args.save:
186 |         plt.savefig(dir_file / "dual_feas.pdf")
187 |     plt.close()
188 | 


--------------------------------------------------------------------------------
/include/contactbench/delassus-wrapper.hxx:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | #include "contactbench/delassus-wrapper.hpp"
  3 | #include <Eigen/src/Core/Ref.h>
  4 | 
  5 | namespace contactbench {
  6 | namespace {
  7 | 
  8 | using Eigen::MatrixBase;
  9 | using Eigen::Ref;
 10 | } // namespace
 11 | 
 12 | template <typename T>
 13 | DelassusDense<T>::DelassusDense(const Ref<const MatrixXs> &G) : Base(G) {
 14 |   G_llt_.resize(3 * nc_, 3 * nc_);
 15 |   llt_ = Eigen::LLT<MatrixXs>(3 * nc_);
 16 |   R_reg_.resize(3 * nc_);
 17 |   Gvpow_.resize(3 * nc_);
 18 |   vpow_.resize(3 * nc_);
 19 |   err_vpow_.resize(3 * nc_);
 20 | }
 21 | 
 22 | template <typename T> void DelassusDense<T>::evaluateDel() {}
 23 | 
 24 | template <typename T> void DelassusDense<T>::evaluateDiagDel() {}
 25 | 
 26 | template <typename T>
 27 | void DelassusDense<T>::applyOnTheRight(const Ref<const VectorXs> &x,
 28 |                                        VectorRef x_out_) const {
 29 |   x_out_.noalias() = G_ * x;
 30 | }
 31 | 
 32 | template <typename T>
 33 | void DelassusDense<T>::applyPerContactOnTheRight(int i,
 34 |                                                  const Ref<const VectorXs> &x,
 35 |                                                  VectorRef x_out_) const {
 36 |   // evaluateDel should be called beforehand
 37 |   assert(i < nc_);
 38 |   x_out_.noalias() = G_.template middleRows<3>(3 * i) * x;
 39 | }
 40 | 
 41 | template <typename T>
 42 | void DelassusDense<T>::applyPerContactNormalOnTheRight(
 43 |     int i, const Ref<const VectorXs> &x, T &x_out_) const {
 44 |   // evaluateDel should be called beforehand
 45 |   assert(i < nc_);
 46 |   x_out_ = (G_.row(3 * i + 2) * x).value();
 47 | }
 48 | 
 49 | template <typename T>
 50 | void DelassusDense<T>::applyPerContactTangentOnTheRight(
 51 |     int i, const Ref<const VectorXs> &x, VectorRef x_out_) const {
 52 |   // evaluateDel should be called beforehand
 53 |   assert(i < nc_);
 54 |   x_out_.noalias() = G_.template middleRows<2>(3 * i) * x;
 55 | }
 56 | 
 57 | // template <typename T>
 58 | // T DelassusDense<T>::computeLargestEigenValue(const int max_it, const T tol) {
 59 | //   // vpow_.setOnes();
 60 | //   // vpow_ /= std::sqrt(vpow_.size());
 61 | //   // for (int i = 0; i < max_it; i++) {
 62 | //   //   applyOnTheRight(vpow_, Gvpow_);
 63 | //   //   // Gvpow_.noalias() = G * vpow_;
 64 | //   //   vpow_ = Gvpow_;
 65 | //   //   vpow_.normalize();
 66 | //   // };
 67 | //   // applyOnTheRight(vpow_, Gvpow_);
 68 | //   // // Gvpow_.noalias() = G_ * vpow_;
 69 | //   // T l_max = vpow_.dot(Gvpow_);
 70 | //   // return l_max;
 71 | //   T lam = proxsuite::proxqp::dense::power_iteration<T>(G_, Gvpow_, vpow_,
 72 | //                                                        err_vpow_, tol, max_it);
 73 | //   return lam;
 74 | // }
 75 | 
 76 | template <typename T> void DelassusDense<T>::computeChol(T mu) {
 77 |   G_llt_ = G_;
 78 |   G_llt_.diagonal().array() += mu;
 79 |   llt_.compute(G_llt_);
 80 |   R_reg_ = mu * VectorXs::Ones(3 * nc_);
 81 | }
 82 | 
 83 | template <typename T>
 84 | void DelassusDense<T>::computeChol(const Ref<const VectorXs> &mus) {
 85 |   G_llt_ = G_;
 86 |   G_llt_.diagonal() += mus;
 87 |   llt_.compute(G_llt_);
 88 |   R_reg_ = mus;
 89 | }
 90 | 
 91 | template <typename T> void DelassusDense<T>::updateChol(T mu) {
 92 |   G_llt_ = G_;
 93 |   G_llt_.diagonal().array() += mu;
 94 |   llt_.compute(G_llt_);
 95 |   R_reg_ = mu * VectorXs::Ones(3 * nc_);
 96 | }
 97 | 
 98 | template <typename T>
 99 | void DelassusDense<T>::updateChol(const Ref<const VectorXs> &mus) {
100 |   G_llt_ = G_;
101 |   G_llt_.diagonal() += mus;
102 |   llt_.compute(G_llt_);
103 |   R_reg_ = mus;
104 | }
105 | 
106 | template <typename T>
107 | void DelassusDense<T>::solve(const Ref<const VectorXs> &x,
108 |                              VectorRef x_out_) const {
109 |   x_out_ = llt_.solve(x);
110 | }
111 | 
112 | template <typename T> void DelassusDense<T>::solveInPlace(VectorRef x) const {
113 |   llt_.solveInPlace(x);
114 | }
115 | 
116 | template <typename T>
117 | DelassusPinocchio<T>::DelassusPinocchio(
118 |     const Model &model, const Data &data,
119 |     const RigidConstraintModelVector &contact_models,
120 |     const RigidConstraintDataVector &contact_datas)
121 |     : Base(), model_(model), data_(data), contact_models_(contact_models),
122 |       contact_datas_(contact_datas) {
123 |   contact_chol_ = ContactCholeskyDecomposition(model_, contact_models_);
124 |   nc_ = (int)contact_chol_.numContacts();
125 |   G_.resize(3 * nc_, 3 * nc_);
126 |   R_reg_.resize(3 * nc_);
127 | }
128 | 
129 | template <typename T> void DelassusPinocchio<T>::evaluateDel() {
130 |   // computeChol should be called beforehand
131 |   G_ = contact_chol_.getInverseOperationalSpaceInertiaMatrix();
132 |   G_.diagonal() -= R_reg_;
133 | }
134 | 
135 | template <typename T> void DelassusPinocchio<T>::evaluateDiagDel() {
136 |   // TODO
137 |   G_ = contact_chol_.getInverseOperationalSpaceInertiaMatrix();
138 |   G_.diagonal() -= R_reg_;
139 | }
140 | 
141 | template <typename T>
142 | void DelassusPinocchio<T>::applyOnTheRight(const Ref<const VectorXs> &x,
143 |                                            VectorRef x_out_) const {
144 |   // evaluateDel should be called beforehand
145 |   x_out_.noalias() = G_ * x;
146 | }
147 | 
148 | template <typename T>
149 | void DelassusPinocchio<T>::applyPerContactOnTheRight(
150 |     int i, const Ref<const VectorXs> &x, VectorRef x_out_) const {
151 |   // evaluateDel should be called beforehand
152 |   assert(i < nc_);
153 |   x_out_.noalias() = G_.template middleRows<3>(3 * i) * x;
154 | }
155 | 
156 | template <typename T>
157 | void DelassusPinocchio<T>::applyPerContactNormalOnTheRight(
158 |     int i, const Ref<const VectorXs> &x, T &x_out_) const {
159 |   // evaluateDel should be called beforehand
160 |   assert(i < nc_);
161 |   x_out_ = (G_.row(3 * i + 2) * x).value();
162 | }
163 | 
164 | template <typename T>
165 | void DelassusPinocchio<T>::applyPerContactTangentOnTheRight(
166 |     int i, const Ref<const VectorXs> &x, VectorRef x_out_) const {
167 |   // evaluateDel should be called beforehand
168 |   assert(i < nc_);
169 |   x_out_.noalias() = G_.template middleRows<2>(3 * i) * x;
170 | }
171 | 
172 | // template <typename T>
173 | // T DelassusPinocchio<T>::computeLargestEigenValue(const int max_it,
174 | //                                                  const T rel_tol) {
175 | //   auto chol_expr = contact_chol_.getDelassusCholeskyExpression();
176 | //   return chol_expr.computeLargestEigenValue(true, max_it, rel_tol);
177 | // }
178 | 
179 | template <typename T> void DelassusPinocchio<T>::computeChol(T mu) {
180 |   contact_chol_.compute(model_, data_, contact_models_, contact_datas_, mu);
181 |   R_reg_ = mu * VectorXs::Ones(3 * nc_);
182 | }
183 | 
184 | template <typename T>
185 | void DelassusPinocchio<T>::computeChol(const Ref<const VectorXs> &mus) {
186 |   contact_chol_.compute(model_, data_, contact_models_, contact_datas_, mus);
187 |   R_reg_ = mus;
188 | }
189 | 
190 | template <typename T> void DelassusPinocchio<T>::updateChol(T mu) {
191 |   contact_chol_.updateDamping(mu);
192 |   R_reg_ = mu * VectorXs::Ones(3 * nc_);
193 | }
194 | 
195 | template <typename T>
196 | void DelassusPinocchio<T>::updateChol(const Ref<const VectorXs> &mus) {
197 |   contact_chol_.updateDamping(mus);
198 |   R_reg_ = mus;
199 | }
200 | 
201 | template <typename T>
202 | void DelassusPinocchio<T>::solve(const Ref<const VectorXs> &x,
203 |                                  VectorRef x_out_) const {
204 |   auto chol_expr = contact_chol_.getDelassusCholeskyExpression();
205 |   x_out_ = chol_expr.solve(x);
206 | }
207 | 
208 | template <typename T>
209 | void DelassusPinocchio<T>::solveInPlace(VectorRef x) const {
210 |   auto chol_expr = contact_chol_.getDelassusCholeskyExpression();
211 |   chol_expr.solveInPlace(x);
212 | }
213 | 
214 | } // namespace contactbench
215 | 


--------------------------------------------------------------------------------
/tests/python/test_solvers_grad.py:
--------------------------------------------------------------------------------
  1 | from pycontact import CCPADMMSolver, ContactProblem, ContactSolverSettings
  2 | import numpy as np
  3 | from pathlib import Path
  4 | import os
  5 | 
  6 | test_python_path = Path(os.path.dirname(os.path.realpath(__file__)))
  7 | 
  8 | 
  9 | def test_ccpadmm_solver_jvp():
 10 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
 11 |     G = npzfile["G"]
 12 |     g = npzfile["g"]
 13 |     mus = npzfile["mus"]
 14 |     nc = len(mus)
 15 |     solver = CCPADMMSolver()
 16 | 
 17 |     def compute_loss_and_grad(G,g,mus,lam_tar):
 18 |         nc = len(mus)
 19 |         mus2 = np.square(mus)
 20 |         prob = ContactProblem(G, g, mus2.tolist())
 21 |         solver.setProblem(prob)
 22 |         max_iter = 1000
 23 |         eps = 1e-7
 24 |         settings = ContactSolverSettings()
 25 |         settings.max_iter_ = max_iter
 26 |         settings.th_stop_ = eps
 27 |         lam0 = np.zeros_like(lam_tar)
 28 |         solver.solve(prob, lam0, settings)
 29 |         lam = solver.getSolution().copy()
 30 |         v = solver.getDualSolution().copy()
 31 |         value = 0.5*np.square(lam- lam_tar).sum()
 32 |         dG_dtheta = np.zeros_like(G)
 33 |         dg_dtheta = np.zeros((3*nc, nc))
 34 |         dmus_dtheta = np.eye(nc)
 35 |         dmus2_dtheta =  (2*np.diag(mus)) @ dmus_dtheta
 36 |         solver.jvp(prob, lam, v, dG_dtheta, dg_dtheta, dmus2_dtheta)
 37 |         dlam_dtheta = solver.getdlamdtheta().copy()
 38 |         grads = (lam - lam_tar).T @ dlam_dtheta.T
 39 |         return value, grads, dlam_dtheta
 40 | 
 41 |     def compute_loss(G,g,mus,lam_tar):
 42 |         mus2 = np.square(mus)
 43 |         prob = ContactProblem(G, g, mus2.tolist())
 44 |         solver.setProblem(prob)
 45 |         max_iter = 1000
 46 |         eps = 1e-7
 47 |         settings = ContactSolverSettings()
 48 |         settings.max_iter_ = max_iter
 49 |         settings.th_stop_ = eps
 50 |         lam0 = np.zeros_like(lam_tar)
 51 |         solver.solve(prob, lam0, settings)
 52 |         lam = solver.getSolution().copy()
 53 |         v = solver.getDualSolution().copy()
 54 |         value = 0.5*np.square(lam- lam_tar).sum()
 55 |         return value
 56 | 
 57 |     mus2 = np.square(mus*0.9)
 58 |     prob_tar = ContactProblem(G, g, mus2.tolist())
 59 |     solver.setProblem(prob_tar)
 60 |     lam0 = np.zeros_like(g)
 61 |     settings = ContactSolverSettings()
 62 |     settings.max_iter_ = 1000
 63 |     settings.th_stop_ = 1e-7
 64 |     solver.solve(prob_tar, lam0, settings)
 65 |     lam_tar = solver.getSolution().copy()
 66 | 
 67 |     loss = compute_loss(G, g, mus, lam_tar)
 68 | 
 69 |     G2, g2, mus2 = G.copy(), g.copy(), mus.copy()
 70 |     delta = 1e-8
 71 |     for i in range(nc):
 72 |         mus2[i,] += delta
 73 |         loss2 = compute_loss(G,g,mus2, lam_tar)
 74 |         dl_dmusi = (loss2-loss)/delta
 75 |         # assert np.abs(dl_dmusi - mus.grad[0,i,0].detach().numpy())< 1e-3
 76 |         mus2[i] += -delta
 77 | 
 78 |     for i in range(3*nc):
 79 |         g2[i,0] += (delta)
 80 |         loss2 = compute_loss(G,g2,mus, lam_tar)
 81 |         dl_dgi = (loss2-loss)/delta
 82 |         # assert np.abs(dl_dgi - g.grad[0,i,0].detach().numpy())< 1e-3
 83 |         g2[i,0] += (-delta)
 84 | 
 85 |     for i in range(3*nc):
 86 |         for j in range(3*nc):
 87 |             G2[i,j] += (delta)
 88 |             loss2 = compute_loss(G2,g,mus, lam_tar)
 89 |             dl_dGij = (loss2-loss)/delta
 90 |             # assert np.abs(dl_dGij - G.grad[0,i,j].detach().numpy())< 1e-3
 91 |             G2[i,j] += (-delta)
 92 | 
 93 |     lr = 5e-5
 94 |     def SGD_step(i, mus):
 95 |         value, grads, _ = compute_loss_and_grad(G, g, mus, lam_tar)
 96 |         mus += -lr * grads
 97 |         return mus
 98 | 
 99 |     def GN_step(i, mus, rho = 1e-7):
100 |         value, grads, J = compute_loss_and_grad(G, g, mus, lam_tar)
101 |         dtheta = - np.linalg.inv(J.T @ J +rho*np.eye(nc)) @ grads
102 |         alpha = 1.
103 |         exp_dec = .5* dtheta.dot(grads)
104 |         for i in range(100):
105 |             dtheta_try = alpha*dtheta
106 |             mus_try = mus +dtheta_try
107 |             value_try = compute_loss(G, g, mus_try, lam_tar)
108 |             if value_try < value + exp_dec:
109 |                 mus = mus_try
110 |                 break
111 |             else:
112 |                 alpha *=0.5
113 |         return mus
114 |     print("mus", mus)
115 |     loss = compute_loss(G, g, mus, lam_tar)
116 |     print("init loss",loss)
117 |     for i in range(100):
118 |         mus = GN_step(i, mus)
119 | 
120 | 
121 | def test_ccpadmm_solver_vjp():
122 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
123 |     G = npzfile["G"]
124 |     g = npzfile["g"]
125 |     mus = npzfile["mus"]
126 |     nc = len(mus)
127 |     solver = CCPADMMSolver()
128 | 
129 |     def compute_loss_and_grad(G,g,mus,lam_tar):
130 |         nc = len(mus)
131 |         mus2 = np.square(mus)
132 |         prob = ContactProblem(G, g, mus2.tolist())
133 |         solver.setProblem(prob)
134 |         max_iter = 1000
135 |         eps = 1e-7
136 |         settings = ContactSolverSettings()
137 |         settings.max_iter_ = max_iter
138 |         settings.th_stop_ = eps
139 |         lam0 = np.zeros_like(lam_tar)
140 |         solver.solve(prob, lam0, settings)
141 |         lam = solver.getSolution().copy()
142 |         v = solver.getDualSolution().copy()
143 |         value = 0.5*np.square(lam- lam_tar).sum()
144 |         dl_dlam = (lam - lam_tar).T
145 |         solver.vjp(prob, dl_dlam, settings)
146 |         dl_dmus2 = solver.getdLdmus()
147 |         grads =  dl_dmus2 @ 2*np.diag(mus)
148 |         return value, grads
149 | 
150 |     def compute_loss(G,g,mus,lam_tar):
151 |         mus2 = np.square(mus)
152 |         prob = ContactProblem(G, g, mus2.tolist())
153 |         solver.setProblem(prob)
154 |         max_iter = 1000
155 |         eps = 1e-7
156 |         settings = ContactSolverSettings()
157 |         settings.max_iter_ = max_iter
158 |         settings.th_stop_ = eps
159 |         lam0 = np.zeros_like(lam_tar)
160 |         solver.solve(prob, lam0, settings)
161 |         lam = solver.getSolution().copy()
162 |         v = solver.getDualSolution().copy()
163 |         value = 0.5*np.square(lam- lam_tar).sum()
164 |         return value
165 | 
166 |     mus2 = np.square(mus*0.9)
167 |     prob_tar = ContactProblem(G, g, mus2)
168 |     solver.setProblem(prob_tar)
169 |     lam0 = np.zeros_like(lam_tar)
170 |     settings = ContactSolverSettings()
171 |     settings.max_iter_ = 1000
172 |     settings.th_stop_ = 1e-7
173 |     solver.solve(prob_tar, lam0, settings)
174 |     lam_tar = solver.getSolution().copy()
175 | 
176 |     loss = compute_loss(G, g, mus, lam_tar)
177 | 
178 |     G2, g2, mus2 = G.copy(), g.copy(), mus.copy()
179 |     delta = 1e-8
180 |     for i in range(nc):
181 |         mus2[i] += delta
182 |         loss2 = compute_loss(G,g,mus2, lam_tar)
183 |         dl_dmusi = (loss2-loss)/delta
184 |         # assert np.abs(dl_dmusi - mus.grad[0,i,0].detach().numpy())< 1e-3
185 |         mus2[i] += -delta
186 | 
187 |     for i in range(3*nc):
188 |         g2[i,0] += (delta)
189 |         loss2 = compute_loss(G,g2,mus, lam_tar)
190 |         dl_dgi = (loss2-loss)/delta
191 |         # assert np.abs(dl_dgi - g.grad[0,i,0].detach().numpy())< 1e-3
192 |         g2[i,0] += (-delta)
193 | 
194 |     for i in range(3*nc):
195 |         for j in range(3*nc):
196 |             G2[i,j] += (delta)
197 |             loss2 = compute_loss(G2,g,mus, lam_tar)
198 |             dl_dGij = (loss2-loss)/delta
199 |             # assert np.abs(dl_dGij - G.grad[0,i,j].detach().numpy())< 1e-3
200 |             G2[i,j] += (-delta)
201 | 
202 |     lr = 5e-5
203 |     def SGD_step(i,mus):
204 |         value, grads = compute_loss_and_grad(G, g, mus, lam_tar)
205 |         mus += -lr * grads
206 |         return mus
207 |     print("mus", mus)
208 |     loss = compute_loss(G, g, mus, lam_tar)
209 |     print("init loss",loss)
210 |     for i in range(100):
211 |         mus = SGD_step(i, mus)
212 | 
213 | 
214 | if __name__ == '__main__':
215 |     import sys
216 |     import pytest
217 |     # sys.exit(pytest.main(sys.argv))
218 |     test_ccpadmm_solver_jvp()
219 | 


--------------------------------------------------------------------------------
/examples/2cubes.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from tap import Tap
  4 | from utils.models import create_cubes, addCube
  5 | from utils.visualize import sub_sample
  6 | from pycontact.simulators import (
  7 |     CCPADMMSimulator,
  8 |     CCPADMMPrimalSimulator,
  9 |     CCPNewtonPrimalSimulator,
 10 |     PyCCPCVXSimulator,
 11 |     CCPPGSSimulator,
 12 |     NCPPGSSimulator,
 13 |     NCPStagProjSimulator,
 14 |     RaisimSimulator,
 15 |     PinNCPADMMSimulator
 16 | )
 17 | 
 18 | import meshcat
 19 | import os
 20 | from pathlib import Path
 21 | 
 22 | 
 23 | class Args(Tap):
 24 |     drag: bool = False
 25 |     display: bool = False
 26 |     record: bool = False
 27 |     plot: bool = False
 28 |     seed: int = 1234
 29 |     save: bool = False
 30 |     debug: bool = False
 31 | 
 32 |     def process_args(self):
 33 |         if self.record:
 34 |             self.display = True
 35 | 
 36 | 
 37 | args = Args().parse_args()
 38 | 
 39 | np.random.seed(args.seed)
 40 | pin.seed(args.seed)
 41 | 
 42 | 
 43 | a = 0.2  # size of cube
 44 | m1 = 1e-3  # mass of cube 1
 45 | m2 = 1e3  # mass of cube 2
 46 | mu1 = 0.9  # friction parameter between cube and floor
 47 | mu2 = 0.95 # friction parameter between the 2 cubes
 48 | el = 0.
 49 | comp = 0.
 50 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = create_cubes(
 51 |     [a], [m1], mu1, el
 52 | )
 53 | 
 54 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = addCube(model, geom_model, visual_model, actuation, a, m2, mu2,el, comp, color = np.array([1.0,0.2,0.2,1.0]))
 55 | 
 56 | # Number of time steps
 57 | T = 100
 58 | # T = 1
 59 | # time steps
 60 | dt = 1e-3
 61 | 
 62 | # Physical parameters of the contact problem
 63 | Kb = 1e-4*0.  # Baumgarte
 64 | eps = 0.0  # elasticity
 65 | 
 66 | # initial state
 67 | q0 = pin.neutral(model)
 68 | q0[2] = a / 2 + a/50.
 69 | q0[9] = 3. * a / 2 + 3*a/50.
 70 | v0 = np.zeros(model.nv)
 71 | q, v = q0.copy(), v0.copy()
 72 | 
 73 | 
 74 | # simulator
 75 | # simulator = NCPPGSSimulator(statistics = True)
 76 | # simulator = CCPPGSSimulator(statistics = True)
 77 | # simulator = NCPStagProjSimulator()
 78 | # simulator = RaisimSimulator(statistics = True)
 79 | simulator = PinNCPADMMSimulator(warm_start=True)
 80 | # simulator = CCPNewtonPrimalSimulator(statistics=True)
 81 | # simulator = CCPADMMSimulator(statistics=True)
 82 | # simulator = PyCCPCVXSimulator()
 83 | 
 84 | simulator.setSimulation(model, data, geom_model, geom_data)
 85 | 
 86 | 
 87 | # record quantities during trajectory
 88 | xs = [np.concatenate((q0, v0))]
 89 | lams = []
 90 | Js = []
 91 | us = []
 92 | Rs = []
 93 | es = []
 94 | Gs = []
 95 | gs = []
 96 | ncp_crits = []
 97 | signorini_crits =[]
 98 | comp_crits = []
 99 | if args.drag:
100 |     def ext_torque(t):
101 |         return np.min([.5*(mu1+mu2)*9.81*(m1+m2), t*4*9.81*(m1+m2)])
102 | else:
103 |     def ext_torque(t):
104 |         return 0.
105 | for t in range(T):
106 |     tau_act = np.zeros(actuation.shape[1])
107 |     tau = actuation @ tau_act
108 |     fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
109 |     fext[1].linear[1] = ext_torque(t*dt)
110 |     fext[1] = pin.Force(data.oMi[1].actionInverse @ fext[1])
111 |     q, v = simulator.step(
112 |         model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 100, 1e-12, rel_th_stop = 1e-12# , eps_reg = 0.
113 |     )
114 |     if args.debug:
115 |         print("t=", t)
116 |         simulator.plotCallback(block=False)
117 |         input("Press enter for next step!")
118 |     lams += [simulator.lam]
119 |     Js += [simulator.J]
120 |     Rs += [simulator.R]
121 |     es += [simulator.signed_dist]
122 |     Gs += [simulator.Del.G_]
123 |     gs += [simulator.g]
124 |     xs += [np.concatenate((q, v))]
125 |     us += [tau]
126 |     if len(simulator.solver.stats_.ncp_comp_)>0:
127 |         ncp_crits += [simulator.solver.stats_.ncp_comp_[-1]]
128 |         comp_crits += [simulator.solver.stats_.comp_[-1]]
129 |         signorini_crits += [simulator.solver.stats_.sig_comp_[-1]]
130 |     else:
131 |         ncp_crits += [np.nan]
132 |         comp_crits += [np.nan]
133 |         signorini_crits += [np.nan]
134 | 
135 | if args.debug:
136 |     simulator.close_plot_callback()
137 | 
138 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
139 | os.makedirs(dir_path / "log", exist_ok=True)
140 | os.makedirs(dir_path / "log/2cubes/", exist_ok=True)
141 | 
142 | if args.save:
143 |     np.savez(
144 |         dir_path /"log/2cubes/contact_problem.npz",
145 |         G=simulator.Del.G_,
146 |         g=simulator.g,
147 |         mus=simulator.mus,
148 |     )
149 | 
150 | if args.plot:  # plotting quantities accross time_steps
151 |     import matplotlib.pyplot as plt
152 |     import matplotlib
153 | 
154 |     matplotlib.use("agg")
155 |     matplotlib.rcParams["pdf.fonttype"] = 42
156 |     matplotlib.rcParams["ps.fonttype"] = 42
157 | 
158 |     steps = [t for t in range(T)]
159 |     pen_err = []
160 |     for ei in es:
161 |         nci = len(ei)//3
162 |         pen_erri = 0.
163 |         for j in range(nci):
164 |             pen_erri = max(pen_erri, ei[3*j+2])
165 |         if nci==0:
166 |             pen_erri = np.nan
167 |         pen_err+=[pen_erri]
168 | 
169 |     pen_err = np.array(pen_err)
170 |     plt.figure()
171 |     plt.plot(steps, pen_err, marker="+")
172 |     plt.xlabel("time step")
173 |     plt.ylabel("Penetration error")
174 |     if args.save:
175 |         plt.savefig(dir_path / "log/2cubes/pen_error.pdf")
176 |     plt.close()
177 | 
178 |     plt.figure()
179 |     plt.plot(steps, ncp_crits, marker="+")
180 |     plt.xlabel("time step")
181 |     plt.ylabel("Contact complementarity")
182 |     if args.save:
183 |         plt.savefig(dir_path / "log/2cubes/ncp_comp.pdf")
184 |     plt.close()
185 | 
186 |     plt.figure()
187 |     plt.plot(steps, signorini_crits, marker="+")
188 |     plt.xlabel("time step")
189 |     plt.ylabel("Signorini complementarity")
190 |     if args.save:
191 |         plt.savefig(dir_path / "log/2cubes/sig_comp.pdf")
192 |     plt.close()
193 | 
194 |     plt.figure()
195 |     plt.plot(steps, comp_crits, marker="+")
196 |     plt.xlabel("time step")
197 |     plt.ylabel("Problem complementarity")
198 |     if args.save:
199 |         plt.savefig(dir_path / "log/2cubes/prob_comp.pdf")
200 |     plt.close()
201 | 
202 |     if args.save:
203 |         np.save(dir_path /"log/2cubes/pen_err.npy",pen_err)
204 |         np.save(dir_path /"log/2cubes/sig_comp.npy",signorini_crits)
205 |         np.save(dir_path /"log/2cubes/ncp_comp.npy",ncp_crits)
206 |         print(np.max(ncp_crits))
207 |         np.save(dir_path /"log/2cubes/prob_comp.npy",comp_crits)
208 | 
209 | 
210 | if args.display:
211 |     from pinocchio.visualize import MeshcatVisualizer
212 | 
213 |     # visualize the trajectory
214 |     vizer = MeshcatVisualizer(model, geom_model, visual_model)
215 |     vizer.initViewer(open=True, loadModel=True)
216 | 
217 |     vizer.viewer["plane"].set_object(meshcat.geometry.Box(np.array([10, 10, 0.1])))
218 |     placement = np.eye(4)
219 |     placement[:3, 3] = np.array([0, 0, -0.05])
220 |     vizer.viewer["plane"].set_transform(placement)
221 |     vizer.display(q0)
222 | 
223 |     numrep = 2
224 |     cp = [0.8, 0.0, 0.2]
225 |     # camera positions for visualization
226 |     cps_ = [cp.copy() for _ in range(numrep)]
227 |     rps_ = [np.zeros(3)]*numrep
228 |     qs = [x[:model.nq] for x in xs]
229 |     vs = [x[model.nq:] for x in xs]
230 | 
231 |     max_fps = 30.
232 |     fps = min([max_fps,1./dt])
233 |     qs = sub_sample(qs,dt*T, fps)
234 |     vs = sub_sample(vs,dt*T, fps)
235 | 
236 |     def get_callback(i: int):
237 |         def _callback(t):
238 |             vizer.setCameraPosition(cps_[i])
239 |             vizer.setCameraTarget(rps_[i])
240 |             pin.forwardKinematics(model, vizer.data, qs[t], vs[t])
241 |             # vizer.drawFrameVelocities(base_link_id)
242 | 
243 |         return _callback
244 | 
245 |     input("[Press enter]")
246 | 
247 |     if args.record:
248 |         ctx = vizer.create_video_ctx(dir_path/"log/2cubes/simulation.mp4", fps=fps)
249 |     else:
250 |         import contextlib
251 | 
252 |         ctx = contextlib.nullcontext()
253 |     with ctx:
254 |         for i in range(numrep):
255 |             vizer.play(
256 |                 qs,
257 |                 1./fps,
258 |                 get_callback(i)
259 |             )
260 | 


--------------------------------------------------------------------------------
/include/contactbench/delassus-wrapper.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef CONTACT_BENCH_DELASSUS_H
  2 | #define CONTACT_BENCH_DELASSUS_H
  3 | 
  4 | #include <Eigen/Core>
  5 | #include "contactbench/fwd.hpp"
  6 | #include <pinocchio/multibody/model.hpp>
  7 | #include <pinocchio/multibody/data.hpp>
  8 | #include <pinocchio/algorithm/contact-cholesky.hpp>
  9 | #include <pinocchio/algorithm/contact-info.hpp>
 10 | #include <proxsuite/proxqp/dense/dense.hpp>
 11 | #include <proxsuite/proxqp/dense/helpers.hpp>
 12 | 
 13 | namespace contactbench {
 14 | 
 15 | template <typename T> struct DelassusBase {
 16 |   CONTACTBENCH_EIGEN_TYPEDEFS(T);
 17 | 
 18 |   int nc_;
 19 | 
 20 |   MatrixXs G_;
 21 | 
 22 |   VectorXs R_reg_;
 23 | 
 24 |   DelassusBase(){};
 25 |   DelassusBase(MatrixXs G) {
 26 |     G_ = G;
 27 |     nc_ = (int)G_.cols() / 3;
 28 |   };
 29 |   virtual ~DelassusBase() = default;
 30 | 
 31 |   virtual void evaluateDel() = 0;
 32 | 
 33 |   virtual void evaluateDiagDel() = 0;
 34 | 
 35 |   virtual void applyOnTheRight(const Eigen::Ref<const VectorXs> &x,
 36 |                                Eigen::Ref<VectorXs> x_out_) const = 0;
 37 | 
 38 |   virtual void applyPerContactOnTheRight(int i,
 39 |                                          const Eigen::Ref<const VectorXs> &x,
 40 |                                          Eigen::Ref<VectorXs> x_out_) const = 0;
 41 | 
 42 |   virtual void
 43 |   applyPerContactNormalOnTheRight(int i, const Eigen::Ref<const VectorXs> &x,
 44 |                                   T &x_out_) const = 0;
 45 | 
 46 |   virtual void
 47 |   applyPerContactTangentOnTheRight(int i, const Eigen::Ref<const VectorXs> &x,
 48 |                                    Eigen::Ref<VectorXs> x_out_) const = 0;
 49 | 
 50 |   // virtual T computeLargestEigenValue(const int max_it = 10,
 51 |   //                                    const T rel_tol = 1e-8) = 0;
 52 | 
 53 |   virtual void computeChol(T mu) = 0;
 54 | 
 55 |   virtual void computeChol(const Eigen::Ref<const VectorXs> &mus) = 0;
 56 | 
 57 |   virtual void updateChol(T mu) = 0;
 58 | 
 59 |   virtual void updateChol(const Eigen::Ref<const VectorXs> &mus) = 0;
 60 | 
 61 |   virtual void solve(const Eigen::Ref<const VectorXs> &x,
 62 |                      Eigen::Ref<VectorXs> x_out_) const = 0;
 63 | 
 64 |   virtual void solveInPlace(Eigen::Ref<VectorXs> x) const = 0;
 65 | 
 66 |   virtual bool isDense() const = 0;
 67 | };
 68 | 
 69 | template <typename T> struct DelassusDense : DelassusBase<T> {
 70 |   CONTACTBENCH_EIGEN_TYPEDEFS(T);
 71 |   using Base = DelassusBase<T>;
 72 |   using Base::G_;
 73 |   using Base::nc_;
 74 |   using Base::R_reg_;
 75 | 
 76 |   // int nc_;
 77 | 
 78 |   // MatrixXs G_;
 79 | 
 80 |   // T mu_;
 81 | 
 82 |   MatrixXs G_llt_;
 83 |   Eigen::LLT<MatrixXs> llt_;
 84 | 
 85 |   DelassusDense(){};
 86 | 
 87 |   DelassusDense(const Eigen::Ref<const MatrixXs> &G);
 88 | 
 89 |   void evaluateDel();
 90 | 
 91 |   void evaluateDiagDel();
 92 | 
 93 |   void applyOnTheRight(const Eigen::Ref<const VectorXs> &x,
 94 |                        Eigen::Ref<VectorXs> x_out) const;
 95 | 
 96 |   void applyPerContactOnTheRight(int i, const Eigen::Ref<const VectorXs> &x,
 97 |                                  Eigen::Ref<VectorXs> x_out_) const;
 98 | 
 99 |   void applyPerContactNormalOnTheRight(int i,
100 |                                        const Eigen::Ref<const VectorXs> &x,
101 |                                        T &x_out_) const;
102 | 
103 |   void applyPerContactTangentOnTheRight(int i,
104 |                                         const Eigen::Ref<const VectorXs> &x,
105 |                                         Eigen::Ref<VectorXs> x_out_) const;
106 | 
107 |   // T computeLargestEigenValue(const int max_it = 10, const T tol = 1e-8);
108 | 
109 |   void computeChol(T mu);
110 | 
111 |   void computeChol(const Eigen::Ref<const VectorXs> &mus);
112 | 
113 |   void updateChol(T mu);
114 | 
115 |   void updateChol(const Eigen::Ref<const VectorXs> &mus);
116 | 
117 |   void solve(const Eigen::Ref<const VectorXs> &x,
118 |              Eigen::Ref<VectorXs> x_out) const;
119 | 
120 |   void solveInPlace(Eigen::Ref<VectorXs> x) const;
121 | 
122 |   virtual bool isDense() const{
123 |     return true;
124 |   }
125 | 
126 | protected:
127 |   VectorXs vpow_, Gvpow_, err_vpow_;
128 | };
129 | 
130 | namespace {
131 | namespace pin = pinocchio;
132 | }
133 | 
134 | template <typename T> struct DelassusPinocchio : DelassusBase<T> {
135 |   CONTACTBENCH_EIGEN_TYPEDEFS(T);
136 |   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
137 |   static constexpr int Options = Eigen::ColMajor;
138 |   using Model = pin::ModelTpl<T>;
139 |   using Data = pin::DataTpl<T>;
140 |   using RigidConstraintModel = pin::RigidConstraintModelTpl<T, Options>;
141 |   using RigidConstraintData = pin::RigidConstraintDataTpl<T, Options>;
142 |   using RigidConstraintModelVector =
143 |       CONTACTBENCH_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel);
144 |   using RigidConstraintDataVector =
145 |       CONTACTBENCH_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData);
146 |   using ContactCholeskyDecomposition =
147 |       pin::ContactCholeskyDecompositionTpl<T, Options>;
148 |   // pin::cholesky::ContactCholeskyDecompositionTpl<T, Options>;
149 | 
150 |   Model model_;
151 |   Data data_;
152 |   RigidConstraintModelVector contact_models_;
153 |   RigidConstraintDataVector contact_datas_;
154 |   ContactCholeskyDecomposition contact_chol_;
155 | 
156 |   // int nc_;
157 | 
158 |   // MatrixXs G_;
159 | 
160 |   // T mu_;
161 |   using Base = DelassusBase<T>;
162 |   using Base::G_;
163 |   using Base::nc_;
164 |   using Base::R_reg_;
165 |   DelassusPinocchio(){};
166 | 
167 |   DelassusPinocchio(const Model &model, const Data &data,
168 |                     const RigidConstraintModelVector &contact_models,
169 |                     const RigidConstraintDataVector &contact_datas);
170 | 
171 |   void evaluateDel();
172 | 
173 |   void evaluateDiagDel();
174 | 
175 |   // template <typename VecIn, typename VecOut>
176 |   void applyOnTheRight(const Eigen::Ref<const VectorXs> &x,
177 |                        Eigen::Ref<VectorXs> x_out) const;
178 | 
179 |   void applyPerContactOnTheRight(int i, const Eigen::Ref<const VectorXs> &x,
180 |                                  Eigen::Ref<VectorXs> x_out_) const;
181 | 
182 |   void applyPerContactNormalOnTheRight(int i,
183 |                                        const Eigen::Ref<const VectorXs> &x,
184 |                                        T &x_out_) const;
185 | 
186 |   void applyPerContactTangentOnTheRight(int i,
187 |                                         const Eigen::Ref<const VectorXs> &x,
188 |                                         Eigen::Ref<VectorXs> x_out_) const;
189 | 
190 |   // T computeLargestEigenValue(const int max_it = 10, const T tol = 1e-8);
191 | 
192 |   void computeChol(T mu);
193 | 
194 |   void computeChol(const Eigen::Ref<const VectorXs> &mus);
195 | 
196 |   void updateChol(T mu);
197 | 
198 |   void updateChol(const Eigen::Ref<const VectorXs> &mus);
199 | 
200 |   // template <typename VecIn, typename VecOut>
201 |   void solve(const Eigen::Ref<const VectorXs> &x,
202 |              Eigen::Ref<VectorXs> x_out) const;
203 | 
204 |   void solveInPlace(Eigen::Ref<VectorXs> x) const;
205 | 
206 |   virtual bool isDense() const{
207 |     return false;
208 |   }
209 | };
210 | 
211 | 
212 | template<typename _Vector>
213 |   struct PowerIterationWrapper: pinocchio::PowerIterationAlgoTpl<_Vector> 
214 |   {
215 |     typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(_Vector) Vector;
216 |     typedef typename Vector::Scalar Scalar;
217 |     typedef pinocchio::PowerIterationAlgoTpl<_Vector> Base;
218 | 
219 |     explicit PowerIterationWrapper(const Eigen::DenseIndex size,
220 |                                    const int max_it = 10,
221 |                                    const Scalar rel_tol = Scalar(1e-8))
222 |     : Base(size, max_it, rel_tol)
223 |     {
224 |     }
225 | 
226 |     void run(const DelassusBase<Scalar>* delassus)
227 |     {
228 |       if(delassus->isDense())
229 |       {
230 |         run(dynamic_cast<const DelassusDense<Scalar>&>(*delassus));
231 |       }
232 |       else
233 |       {
234 |         run(dynamic_cast<const DelassusPinocchio<Scalar>&>(*delassus));
235 |       }
236 |     }
237 | 
238 |     void run(const DelassusPinocchio<Scalar>& delassus)
239 |     { 
240 |       Base::run(delassus.contact_chol_.getDelassusCholeskyExpression());
241 |     }
242 | 
243 |     void run(const DelassusDense<Scalar>& delassus)
244 |     { 
245 |       Base::run(delassus.G_);
246 |     }
247 | 
248 |   }; // struct PowerIterationAlgoTpl
249 | 
250 | } // end namespace contactbench
251 | 
252 | #include "contactbench/delassus-wrapper.hxx"
253 | 
254 | #endif
255 | 
256 | #if CONTACTBENCH_ENABLE_TEMPLATE_INSTANTIATION
257 | #include "contactbench/delassus-wrapper.txx"
258 | #endif // CONTACTBENCH_ENABLE_TEMPLATE_INSTANTIATION
259 | 


--------------------------------------------------------------------------------
/bindings/pycontact/utils/pin_utils.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from pycontact import DelassusPinocchio
  4 | 
  5 | def computeContacts(model, data, geom_model, geom_data, prev_contacts:dict = {}, prev_Rs : list= []):
  6 |     nc = 0
  7 |     J, Del, R, e = None, None, None, None
  8 |     mus = []
  9 |     els = []
 10 |     comps = []
 11 |     contact_models = []
 12 |     contact_datas = []
 13 |     active_col = []
 14 |     contact_points = []
 15 |     contact_joints = []
 16 |     contact_placements = []
 17 |     for i, res in enumerate(geom_data.collisionResults):
 18 |         if res.isCollision():
 19 |             active_col += [i]
 20 |             geom_id1, geom_id2 = (
 21 |                 geom_model.collisionPairs[i].first,
 22 |                 geom_model.collisionPairs[i].second,
 23 |             )
 24 |             mu_i = geom_model.frictions[i]
 25 |             comp_i = geom_model.compliances[i]
 26 |             el_i = geom_model.elasticities[i]
 27 |             mus += [mu_i]
 28 |             comps += [comp_i]
 29 |             els += [el_i]
 30 |             joint_id1 = geom_model.geometryObjects[geom_id1].parentJoint
 31 |             joint_id2 = geom_model.geometryObjects[geom_id2].parentJoint
 32 |             contacts = res.getContacts()
 33 |             joint_placement_1 = data.oMi[joint_id1]
 34 |             joint_placement_2 = data.oMi[joint_id2]
 35 |             for contact in contacts:
 36 |                 pos_i = contact.pos
 37 |                 normal_i = contact.normal
 38 |                 if i in prev_contacts.keys():
 39 |                     # prev_R = prev_contacts[i][1]
 40 |                     prev_R = prev_Rs[i]
 41 |                     warm_start = True
 42 |                 else:
 43 |                     warm_start=False
 44 |                 if warm_start and np.allclose(normal_i,prev_R[:,2]):
 45 |                     R_i = prev_R
 46 |                 else:
 47 |                     ex_i, ey_i = complete_orthonormal_basis(contact.normal, joint_placement_1)
 48 |                     ex_i = np.expand_dims(ex_i, axis=1)
 49 |                     ey_i = np.expand_dims(ey_i, axis=1)
 50 |                     normal_i = np.expand_dims(contact.normal, axis=1)
 51 |                     R_i = np.concatenate((ex_i, ey_i, normal_i), axis=1)
 52 |                 R_i1 = np.dot(joint_placement_1.rotation.T, R_i)
 53 |                 R_i2 = np.dot(joint_placement_2.rotation.T, R_i)
 54 |                 pos_i1 = joint_placement_1.rotation.T @ (
 55 |                     pos_i - joint_placement_1.translation
 56 |                 )
 57 |                 pos_i2 = joint_placement_2.rotation.T @ (
 58 |                     pos_i - joint_placement_2.translation
 59 |                 )
 60 |                 placement_i1 = pin.SE3(R_i1, pos_i1)
 61 |                 placement_i2 = pin.SE3(R_i2, pos_i2)
 62 |                 contact_model_i = pin.RigidConstraintModel(
 63 |                     pin.ContactType.CONTACT_3D,
 64 |                     model,
 65 |                     joint_id2,
 66 |                     placement_i2,
 67 |                     joint_id1,
 68 |                     placement_i1,
 69 |                     pin.ReferenceFrame.LOCAL,
 70 |                 )
 71 |                 contact_models += [contact_model_i]
 72 |                 contact_data_i = contact_model_i.createData()
 73 |                 contact_datas += [contact_data_i]
 74 |                 contact_points += [pos_i]
 75 |                 contact_joints += [(joint_id1, joint_id2)]
 76 |                 contact_placements += [(placement_i1, placement_i2)]
 77 |                 e_i = np.array([0.0, 0.0, contact.penetration_depth])
 78 |                 if R is None:
 79 |                     R = R_i
 80 |                     e = e_i
 81 |                 else:
 82 |                     R = np.concatenate((R, R_i), axis=1)
 83 |                     e = np.concatenate((e, e_i), axis=0)
 84 |             nc += len(contacts)
 85 |     if nc > 0:
 86 |         Del = DelassusPinocchio(model, data, contact_models, contact_datas)
 87 |         J = pin.getConstraintsJacobian(model, data, contact_models, contact_datas)
 88 |     return J, Del, data.M, R, e, mus, comps, els, active_col, contact_points, contact_joints, contact_placements
 89 | 
 90 | def computeAllContacts(model, data, geom_model, geom_data):
 91 |     print("WARNING: going through `computeAllContacts function!!! Should pass through `computeContacts` instead!")
 92 |     # compute the contact quantities as if every body was in contact
 93 |     # this computation is done offline and can be used to set the compliance (as it is done in MuJoCo)
 94 |     nc = 0
 95 |     Del, J, R, e = None, None, None, None
 96 |     mus = []
 97 |     els = []
 98 |     contact_models = []
 99 |     contact_datas = []
100 |     contact_points = []
101 |     nc = len(geom_model.collisionPairs)
102 |     for i, col_pair in enumerate(geom_model.collisionPairs):
103 |         geom_id1, geom_id2 = (
104 |             col_pair.first,
105 |             col_pair.second,
106 |         )
107 |         mu_i = geom_model.frictions[i]
108 |         el_i = geom_model.elasticities[i]
109 |         mus += [mu_i]
110 |         els += [el_i]
111 |         joint_id1 = geom_model.geometryObjects[geom_id1].parentJoint
112 |         joint_id2 = geom_model.geometryObjects[geom_id2].parentJoint
113 |         contact_pen = 0.
114 |         joint_placement_1 = data.oMi[joint_id1]
115 |         joint_placement_2 = data.oMi[joint_id2]
116 |         pos_i = geom_data.oMg[geom_id2].translation # TODO: where to chose the contact point to compute J ?
117 |         normal_i = model.gravity.vector[:3]
118 |         normal_i = normal_i/np.linalg.norm(normal_i)
119 |         ex_i, ey_i = complete_orthonormal_basis(normal_i, joint_placement_1)
120 |         ex_i = np.expand_dims(ex_i, axis=1)
121 |         ey_i = np.expand_dims(ey_i, axis=1)
122 |         normal_i = np.expand_dims(normal_i, axis=1)
123 |         R_i = np.concatenate((ex_i, ey_i, normal_i), axis=1)
124 |         R_i1 = np.dot(joint_placement_1.rotation.T, R_i)
125 |         R_i2 = np.dot(joint_placement_2.rotation.T, R_i)
126 |         pos_i1 = joint_placement_1.rotation.T @ (
127 |             pos_i - joint_placement_1.translation
128 |         )
129 |         pos_i2 = joint_placement_2.rotation.T @ (
130 |             pos_i - joint_placement_2.translation
131 |         )
132 |         placement_i1 = pin.SE3(R_i1, pos_i1)
133 |         placement_i2 = pin.SE3(R_i2, pos_i2)
134 |         contact_model_i = pin.RigidConstraintModel(
135 |             pin.ContactType.CONTACT_3D,
136 |             model,
137 |             joint_id2,
138 |             placement_i2,
139 |             joint_id1,
140 |             placement_i1,
141 |             pin.ReferenceFrame.LOCAL,
142 |         )
143 |         contact_models += [contact_model_i]
144 |         contact_data_i = contact_model_i.createData()
145 |         contact_datas += [contact_data_i]
146 |         contact_points += [pos_i]
147 |         e_i = np.array([0.0, 0.0, contact_pen])
148 |         if R is None:
149 |             R = R_i
150 |             e = e_i
151 |         else:
152 |             R = np.concatenate((R, R_i), axis=1)
153 |             e = np.concatenate((e, e_i), axis=0)
154 |     if nc>0:
155 |         chol = pin.ContactCholeskyDecomposition(model, contact_models)
156 |         chol.compute(model, data, contact_models, contact_datas, 1e-9)
157 |         Del = chol.getInverseOperationalSpaceInertiaMatrix()
158 |         J = chol.matrix()[: 3 * nc, 3 * nc :]
159 |     return J, Del, R, e, mus, els, contact_points
160 | 
161 | 
162 | def complete_orthonormal_basis(ez, joint_placement):
163 |     ex = joint_placement.rotation[:,0]
164 |     ey = np.cross(ez, ex)
165 |     if np.linalg.norm(ey) < 1e-6:
166 |         ex = joint_placement.rotation[:,1]
167 |         ey = np.cross(ez, ex)
168 |     ey /= np.linalg.norm(ey)
169 |     ex = np.cross(ey, ez)
170 |     return ex, ey
171 | 
172 | def complete_orthonormal_basis_random(ez):
173 |     ex = np.random.rand(3)
174 |     ey = np.cross(ez, ex)
175 |     ey /= np.linalg.norm(ey)
176 |     ex = np.cross(ey, ez)
177 |     return ex, ey
178 | 
179 | 
180 | def computeDFreeVelocity(model, data, q, v, tau, fext, dt):
181 |     return dt * pin.aba(model, data, q, v, tau, fext)
182 | 
183 | def inverse_kinematics_trans(model, data, frame_translation, frame_id, eps = 1e-4, max_it = 1000):
184 |     q = pin.neutral(model)
185 |     DT = 1e-1
186 |     damp = 1e-12
187 | 
188 |     i = 0
189 |     while True:
190 |         pin.forwardKinematics(model, data, q)
191 |         pin.updateFramePlacements(model, data)
192 |         err = data.oMf[frame_id].translation - frame_translation
193 |         if np.linalg.norm(err) < eps:
194 |             success = True
195 |             break
196 |         if i >= max_it:
197 |             success = False
198 |             break
199 |         J = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL)
200 |         J_trans = np.dot(data.oMf[frame_id].rotation, J[:3])
201 |         v = -J_trans.T.dot(
202 |             np.linalg.solve(J_trans.dot(J_trans.T) + damp * np.eye(3), err)
203 |         )
204 |         q = pin.integrate(model, q, v * DT)
205 |         i += 1
206 |     return q
207 | 


--------------------------------------------------------------------------------
/examples/hand.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from tap import Tap
  4 | from utils.models import create_hand, addBall, addCube
  5 | from utils.visualize import sub_sample
  6 | from pycontact.simulators import (
  7 |     CCPADMMSimulator,
  8 |     CCPPGSSimulator,
  9 |     RaisimSimulator,
 10 |     RaisimCorrectedSimulator,
 11 |     NCPPGSSimulator,
 12 |     CCPNewtonPrimalSimulator
 13 | )
 14 | import meshcat
 15 | import os
 16 | from pathlib import Path
 17 | from tqdm import trange
 18 | 
 19 | 
 20 | class Args(Tap):
 21 |     waving: bool = False
 22 |     ball: bool = False
 23 |     cube: bool = False
 24 |     display: bool = False
 25 |     timings: bool = False
 26 |     record: bool = False
 27 |     plot: bool = False
 28 |     seed: int = 1234
 29 |     save: bool = False
 30 |     debug: bool = False
 31 | 
 32 |     def process_args(self):
 33 |         if self.record:
 34 |             self.display = True
 35 | 
 36 | 
 37 | args = Args().parse_args()
 38 | 
 39 | np.random.seed(args.seed)
 40 | pin.seed(args.seed)
 41 | 
 42 | mu = 0.9  # friction parameter
 43 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = create_hand(
 44 |     mu,
 45 |     0.,
 46 |     0.,
 47 |     True,
 48 |     True
 49 | )
 50 | if args.ball:
 51 |     ball_mass = 10.0
 52 |     ball_radius = 0.1
 53 |     model, geom_model, visual_model, data, geom_data, visual_data, actuation = addBall(model, geom_model, visual_model, actuation,ball_radius,ball_mass, mu, 0., 0.)
 54 |     model.qinit[-7:-4] = np.array([-0.05,-0.04,0.27])
 55 | elif args.cube:
 56 |     cube_mass = 10.0
 57 |     cube_radius = 0.08
 58 |     model, geom_model, visual_model, data, geom_data, visual_data, actuation = addCube(model, geom_model, visual_model, actuation,cube_radius,cube_mass, mu, 0., 0., geom_with_box=True)
 59 |     model.qinit[-7:-4] = np.array([-0.05,-0.04,0.30])
 60 | 
 61 | # Number of time steps
 62 | T = 1000
 63 | # T = 1
 64 | # time steps
 65 | dt = 1e-3
 66 | 
 67 | # Physical parameters of the contact problem
 68 | Kb = 1e-4  # Baumgarte
 69 | eps = 0.0  # elasticity
 70 | 
 71 | # initial state
 72 | q0 = model.qinit
 73 | v0 = np.zeros(model.nv)
 74 | q, v = q0.copy(), v0.copy()
 75 | 
 76 | 
 77 | # simulator
 78 | warm_start = True
 79 | # simulator = NCPPGSSimulator(warm_start = warm_start,timings = args.timings)
 80 | # simulator = CCPPGSSimulator(warm_start =warm_start, timings = args.timings)
 81 | simulator = CCPADMMSimulator(warm_start=warm_start, timings = args.timings)
 82 | # simulator = CCPNewtonPrimalSimulator(warm_start=warm_start, timings = args.timings)
 83 | # simulator = RaisimSimulator( warm_start=warm_start, timings = args.timings)
 84 | # simulator = RaisimCorrectedSimulator(warm_start=warm_start, timings = args.timings)
 85 | 
 86 | simulator.setSimulation(model, data, geom_model, geom_data)
 87 | 
 88 | # record quantities during trajectory
 89 | xs = [np.concatenate((q0, v0))]
 90 | lams = []
 91 | Js = []
 92 | us = []
 93 | Rs = []
 94 | es = []
 95 | Gs = []
 96 | gs = []
 97 | if args.timings:
 98 |     timings =  []
 99 | def ext_torque(t):
100 |     return 0.
101 | if args.waving:
102 |     def control_torque(t, model, data, q, v, actuation):
103 |         des_acc = np.zeros(model.nv)
104 |         des_acc[2] = 20.*np.cos(10*t)
105 |         b = pin.rnea(model, data, q, v, des_acc)
106 |         tau, _, _, _= np.linalg.lstsq(actuation, b, rcond=None)
107 |         return actuation @tau
108 | else:
109 |     def control_torque(t, model, data, q, v, actuation):
110 |         des_acc = np.zeros(model.nv)
111 |         b = pin.rnea(model, data, q, v, des_acc)
112 |         tau, _, _, _= np.linalg.lstsq(actuation, b, rcond=None)
113 |         return actuation @tau
114 | 
115 | for t in trange(T):
116 |     tau = control_torque(t*dt, model, data, q, v, actuation)
117 |     fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
118 |     # q, v = simulator.step(
119 |     #     model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 10000, 1e-6, alpha_min = 1e-4, gamma = 0.9
120 |     # )
121 |     q, v = simulator.step(
122 |         model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 10000, 1e-6
123 |     )
124 |     if args.debug:
125 |         print("t=", t)
126 |         simulator.plotCallback(block=False)
127 |         input("Press enter for next step!")
128 |     # print( "stop_", simulator.solver.stop_)
129 |     # print( "comp_reg_", simulator.solver.comp_reg_)
130 |     lams += [simulator.lam]
131 |     Js += [simulator.J]
132 |     Rs += [simulator.R]
133 |     es += [simulator.signed_dist]
134 |     # Gs += [simulator.G]
135 |     gs += [simulator.g]
136 |     xs += [np.concatenate((q, v))]
137 |     us += [tau]
138 |     if args.timings:
139 |         timings += [simulator.solve_time]
140 | 
141 | if args.debug:
142 |     simulator.close_plot_callback()
143 | 
144 | 
145 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
146 | os.makedirs(dir_path / "log", exist_ok=True)
147 | os.makedirs(dir_path / "log/hand/", exist_ok=True)
148 | 
149 | if args.save:
150 |     if simulator.Del is not None:
151 |         simulator.Del.evaluateDel()
152 |         np.savez(
153 |             dir_path /"log/hand/contact_problem.npz", G=simulator.Del.G_, g=simulator.g, mus=simulator.mus
154 |         )
155 |     np.save(dir_path/"log/hand/traj.npy", xs)
156 |     if args.timings:
157 |         np.save(dir_path/"log/hand/timings.npy", timings)
158 | 
159 | if args.plot:
160 |     import matplotlib.pyplot as plt
161 |     import matplotlib
162 | 
163 |     matplotlib.use("agg")
164 |     matplotlib.rcParams["pdf.fonttype"] = 42
165 |     matplotlib.rcParams["ps.fonttype"] = 42
166 | 
167 |     plt.figure()
168 |     iters = [t for t in range(len(lams))]
169 |     internal_forces  = np.zeros((len(iters), 4))
170 |     for i in range(len(iters)):
171 |         Ri = Rs[i]
172 |         lami = lams[i]
173 |         nci = int(len(lami)/3)
174 |         for j in range(nci):
175 |             Rij = Ri[:,3*j:3*(j+1)]
176 |             lamij = lami[3*j:3*(j+1),:]
177 |             fij = Rij @lamij
178 |             internal_forces[i,j] = fij[0]
179 |     plt.plot(iters, internal_forces, marker="+")
180 |     plt.xlabel("time step")
181 |     plt.ylabel("Internal forces")
182 |     if args.save:
183 |         plt.savefig(dir_path/"log/hand/internal_forces.pdf")
184 |     plt.close()
185 | 
186 |     plt.figure()
187 |     nc = simulator.G.shape[0]//3
188 |     contact_forces = simulator.lam
189 |     resulting_force = simulator.J.T @ simulator.lam
190 |     forces_name = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"]
191 |     if args.save:
192 |         np.save(dir_path/"log/hand/contact_forces.npy", contact_forces)
193 |         np.save(dir_path/"log/hand/resulting_force.npy", resulting_force)
194 | 
195 |     if nc ==4:
196 |         fig = plt.figure()
197 |         ax1 = fig.add_subplot(111, projection='3d')
198 | 
199 |         x = [1,1,3,3]
200 |         y = [1,3,1,3]
201 |         z = np.zeros(4)
202 | 
203 |         dx = np.ones(4)
204 |         dy = np.ones(4)
205 |         dz = contact_forces[[2,5,8,11]].squeeze(-1)
206 | 
207 |         ax1.bar3d(x, y, z, dx, dy, dz, label="contact forces")
208 | 
209 |         x = [5]
210 |         y = [2]
211 |         z = np.zeros(1)
212 | 
213 |         dx = np.ones(1)
214 |         dy = np.ones(1)
215 |         dz = resulting_force[2]
216 | 
217 |         ax1.bar3d(x, y, z, dx, dy, dz, label="resulting force", color = "red")
218 | 
219 |         ax1.set_zlabel('Forces (N)')
220 | 
221 |         ax1.view_init(elev=10., azim=-110.)
222 | 
223 |         if args.save:
224 |             plt.savefig(dir_path/"log/hand/contact_forces.pdf")
225 |         plt.close()
226 | 
227 | 
228 | 
229 | if args.display:
230 |     from pinocchio.visualize import MeshcatVisualizer
231 | 
232 |     # visualize the trajectory
233 |     vizer = MeshcatVisualizer(model, geom_model, visual_model)
234 |     vizer.initViewer(open=True, loadModel=True)
235 | 
236 |     vizer.viewer["plane"].set_object(meshcat.geometry.Box(np.array([20, 20, 0.1])))
237 |     placement = np.eye(4)
238 |     placement[:3, 3] = np.array([0, 0, -0.05])
239 |     vizer.viewer["plane"].set_transform(placement)
240 |     # vizer.displayCollisions(True)
241 |     vizer.display(q0)
242 |     pin.SE3ToXYZQUAT( pin.SE3.Random())
243 |     numrep = 2
244 |     cp = [-0.3, -0.3, 0.5]
245 |     # camera positions for visualization
246 |     cps_ = [cp.copy() for _ in range(numrep)]
247 |     rps_ = [np.zeros(3)]*numrep
248 |     qs = [x[:model.nq] for x in xs]
249 |     vs = [x[model.nq:] for x in xs]
250 | 
251 |     max_fps = 30.
252 |     fps = min([max_fps,1./dt])
253 |     qs = sub_sample(qs,dt*T, fps)
254 |     vs = sub_sample(vs,dt*T, fps)
255 | 
256 |     def get_callback(i: int):
257 |         def _callback(t):
258 |             vizer.setCameraPosition(cps_[i])
259 |             vizer.setCameraTarget(rps_[i])
260 |             pin.forwardKinematics(model, vizer.data, qs[t], vs[t])
261 |             # vizer.drawFrameVelocities(base_link_id)
262 | 
263 |         return _callback
264 | 
265 |     input("[Press enter]")
266 | 
267 |     if args.record:
268 |         ctx = vizer.create_video_ctx(dir_path/"log/solo/simulation.mp4", fps=fps)
269 |     else:
270 |         import contextlib
271 | 
272 |         ctx = contextlib.nullcontext()
273 |     with ctx:
274 |         for i in range(numrep):
275 |             vizer.play(
276 |                 qs,
277 |                 1./fps,
278 |                 get_callback(i)
279 |             )
280 | 


--------------------------------------------------------------------------------
/tests/autodiff/friction-constraints-cppad.cpp:
--------------------------------------------------------------------------------
  1 | // #define BOOST_TEST_MODULE FRICTION_CONSTRAINTS
  2 | #include "contactbench/friction-constraint.hpp"
  3 | #include "vector"
  4 | 
  5 | #include <iostream>
  6 | #include <boost/test/unit_test.hpp>
  7 | #include <boost/utility/binary.hpp>
  8 | #include <pinocchio/autodiff/cppad.hpp>
  9 | 
 10 | BOOST_AUTO_TEST_SUITE(FRICTION_CONSTRAINTS_CPPAD)
 11 | 
 12 | namespace cb = contactbench;
 13 | using CppAD::AD;
 14 | using CppAD::NearEqual;
 15 | 
 16 | typedef double Scalar;
 17 | typedef AD<Scalar> ADScalar;
 18 | using T = ADScalar;
 19 | CONTACTBENCH_EIGEN_TYPEDEFS(ADScalar);
 20 | 
 21 | BOOST_AUTO_TEST_CASE(ice_cream_cone_init_cppad) {
 22 |   cb::IceCreamCone<T> cone(.84);
 23 |   BOOST_CHECK(!CppAD::isnan(cone.mu_));
 24 | }
 25 | 
 26 | BOOST_AUTO_TEST_CASE(pyramid_cone_cast) {
 27 |   cb::PyramidCone<Scalar> originalCone(.84);
 28 |   cb::PyramidCone<ADScalar> castedCone = originalCone.cast<ADScalar>();
 29 |   BOOST_CHECK(!CppAD::isnan(castedCone.mu_));
 30 | }
 31 | 
 32 | BOOST_AUTO_TEST_CASE(ice_cream_cone_cast) {
 33 |   cb::IceCreamCone<Scalar> originalCone(.84);
 34 |   cb::IceCreamCone<ADScalar> castedCone = originalCone.cast<ADScalar>();
 35 |   BOOST_CHECK(!CppAD::isnan(castedCone.mu_));
 36 | }
 37 | 
 38 | BOOST_AUTO_TEST_CASE(ice_cream_projection_cppad) {
 39 |   cb::IceCreamCone<T> cone(.84);
 40 |   Vector3s x = Vector3s::Zero();
 41 |   Vector3s x_out;
 42 |   cone.project(x, x_out);
 43 |   BOOST_CHECK(x_out.isApprox(x));
 44 |   x(2) = 1.;
 45 |   x(0) = 1.08;
 46 |   cone.project(x, x_out);
 47 |   BOOST_CHECK(fabs(x_out(0) - (x_out(2) * cone.mu_)) < 1e-3);
 48 |   for (int i = 0; i < 10; i++) {
 49 |     x = Vector3s::Random();
 50 |     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
 51 |     Vector3s x_out2;
 52 |     cone.project(x, x_out);
 53 |     cone.project(x_out, x_out2);
 54 |     BOOST_CHECK(x_out.isApprox(x_out2));
 55 |     BOOST_CHECK(fabs(x_out.dot(x_out - x)) < 1e-6);
 56 |   }
 57 | }
 58 | 
 59 | BOOST_AUTO_TEST_CASE(ice_cream_dual_projection_cppad) {
 60 |   cb::IceCreamCone<T> cone(.84);
 61 |   Vector3s x = Vector3s::Zero();
 62 |   Vector3s x_out;
 63 |   cone.projectDual(x, x_out);
 64 |   BOOST_CHECK(x_out.isApprox(x));
 65 |   x(2) = 1.;
 66 |   x(0) = 1e3;
 67 |   cone.projectDual(x, x_out);
 68 |   BOOST_CHECK(fabs(x_out(0) - (x_out(2) * 1. / cone.mu_)) < 1e-3);
 69 |   for (int i = 0; i < 10; i++) {
 70 |     x = Vector3s::Random();
 71 |     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
 72 |     Vector3s x_out2;
 73 |     cone.projectDual(x, x_out);
 74 |     cone.projectDual(x_out, x_out2);
 75 |     BOOST_CHECK(x_out.isApprox(x_out2));
 76 |     BOOST_CHECK(fabs(x_out.dot(x_out - x)) < 1e-6);
 77 |   }
 78 | }
 79 | 
 80 | BOOST_AUTO_TEST_CASE(ice_cream_circle_projection_cppad) {
 81 |   cb::IceCreamCone<T> cone(.37);
 82 |   Vector3s x = Vector3s::Zero();
 83 |   Vector3s x_out;
 84 |   cone.projectHorizontal(x, x_out);
 85 |   BOOST_CHECK(x_out.isApprox(x));
 86 |   x(2) = 1.59;
 87 |   x(0) = 3.46;
 88 |   cone.projectHorizontal(x, x_out);
 89 |   BOOST_CHECK(x_out(0) == x(2) * cone.mu_);
 90 |   for (int i = 0; i < 10; i++) {
 91 |     x = Vector3s::Random();
 92 |     x[2] = fabs(x[2]);
 93 |     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
 94 |     Vector3s x_out2;
 95 |     cone.projectHorizontal(x, x_out);
 96 |     cone.projectHorizontal(x_out, x_out2);
 97 |     BOOST_CHECK(x_out.isApprox(x_out2));
 98 |     x_out2[0] = 0.;
 99 |     x_out2[1] = 0.;
100 |     BOOST_CHECK(fabs(x_out2.dot(x_out - x)) < 1e-6);
101 |   }
102 | }
103 | 
104 | BOOST_AUTO_TEST_CASE(ice_cream_circle_projection_backward_xout_wrt_x_cppad) {
105 |   cb::IceCreamCone<T> cone(.37);
106 |   VectorXs x = VectorXs::Zero(3);
107 |   VectorXs x_out = VectorXs::Zero(3);
108 |   x(2) = 1.59;
109 |   x(1) = 0.2;
110 |   x(0) = 3.5;
111 |   CppAD::Independent(x);
112 |   cone.projectHorizontal(x, x_out);
113 |   CppAD::ADFun<Scalar> f(x, x_out);
114 |   CPPAD_TESTVECTOR(Scalar) x_(static_cast<size_t>(x.size()));
115 |   for (size_t i = 0; i < x.size(); ++i) {
116 |     x_[i] = CppAD::Value(x[i]);
117 |   }
118 | 
119 |   // CPPAD_TESTVECTOR(Scalar) y = f.Forward(0, x_);  // not needed
120 |   CPPAD_TESTVECTOR(Scalar) J = f.Jacobian(x_);
121 | 
122 |   // compare with finite differences using x_out, could also use y
123 |   double eps = 1e-6;
124 |   Vector3s x_out2;
125 |   Vector3s x2 = x;
126 |   for (int i = 0; i < 3; ++i) {
127 |     x2(i) += eps;
128 |     cone.projectHorizontal(x2, x_out2);
129 |     BOOST_CHECK_CLOSE(x_out2(0), (x_out[0] + eps * J[0]), 1e-3);
130 |     BOOST_CHECK_CLOSE(x_out2(1), (x_out[1] + eps * J[1]), 1e-3);
131 |     BOOST_CHECK_CLOSE(x_out2(2), (x_out[2] + eps * J[2]), 1e-3);
132 |     x2(i) = x(i);
133 |   }
134 | 
135 |   // check at different point in the same if branch of projectHorizontal
136 |   x_[0] = -x_[0];
137 |   x(0) = -x(0);
138 |   CPPAD_TESTVECTOR(Scalar) y = f.Forward(0, x_); // for FD
139 |   J = f.Jacobian(x_);
140 |   x2 = x;
141 |   for (int i = 0; i < 3; ++i) {
142 |     x2(i) += eps;
143 |     cone.projectHorizontal(x2, x_out2);
144 |     BOOST_CHECK_CLOSE(x_out2(0), (y[0] + eps * J[0]), 1e-3);
145 |     BOOST_CHECK_CLOSE(x_out2(1), (y[1] + eps * J[1]), 1e-3);
146 |     BOOST_CHECK_CLOSE(x_out2(2), (y[2] + eps * J[2]), 1e-3);
147 |     x2(i) = x(i);
148 |   }
149 | 
150 |   // check at different point in the else statement of projectHorizontal
151 |   // NOTE: This here is failing with our current implementation of
152 |   // projectHorizontal. We can fix it by using the static if_then_else
153 |   // from pinocchio (using CppAD::CondExpOp).
154 |   // x_[0] = 0.3;
155 |   // x(0) = 0.3;
156 |   // cone.projectHorizontal(x, x_out);
157 |   // y = f.Forward(0, x_);
158 |   // J = f.Jacobian(x_);
159 | 
160 |   // // compare with finite differences
161 |   // x2 = x;
162 |   // for (int i = 0; i < 3; ++i) {
163 |   //   x2(i) += eps;
164 |   //   cone.projectHorizontal(x2, x_out2);
165 |   //   std::cout << "x2: " << x2.transpose() << std::endl;
166 |   //   BOOST_CHECK_CLOSE(x_out2(0), (y[0] + eps * J[0]), 1e-3);
167 |   //   BOOST_CHECK_CLOSE(x_out2(1), (y[1] + eps * J[1]), 1e-3);
168 |   //   BOOST_CHECK_CLOSE(x_out2(2), (y[2] + eps * J[2]), 1e-3);
169 |   //   x2(i) = x(i);
170 |   // }
171 | }
172 | 
173 | BOOST_AUTO_TEST_CASE(ice_cream_circle_projection2_cppad) {
174 |   cb::IceCreamCone<T> cone(.37);
175 |   Vector2s x = Vector2s::Zero();
176 |   double xn = 0.;
177 |   Vector2s x_out;
178 |   cone.projectHorizontal(x, xn, x_out);
179 |   BOOST_CHECK(x_out.isApprox(x));
180 |   xn = 1.59;
181 |   x(0) = 3.46;
182 |   cone.projectHorizontal(x, xn, x_out);
183 |   BOOST_CHECK(x_out(0) == xn * cone.mu_);
184 |   for (int i = 0; i < 10; i++) {
185 |     x = Vector2s::Random();
186 |     xn = (1. / (1. + std::rand())) * 10.;
187 |     cone = cb::IceCreamCone<T>(1. / (1 + std::rand()));
188 |     Vector2s x_out2;
189 |     cone.projectHorizontal(x, xn, x_out);
190 |     cone.projectHorizontal(x_out, xn, x_out2);
191 |     BOOST_CHECK(x_out.isApprox(x_out2));
192 |   }
193 | }
194 | 
195 | BOOST_AUTO_TEST_CASE(ice_cream_circle_projection2_backward_cppad) {
196 |   cb::IceCreamCone<T> cone(.37);
197 |   VectorXs x = VectorXs::Zero(2);
198 |   CppAD::Independent(x);
199 |   double xn = 0.;
200 |   VectorXs x_out = VectorXs::Zero(2);
201 |   cone.projectHorizontal(x, xn, x_out);
202 |   BOOST_CHECK(x_out.isApprox(x));
203 | 
204 |   CppAD::ADFun<Scalar> f(x, x_out);
205 |   CPPAD_TESTVECTOR(Scalar) x_(2);
206 |   x_[0] = 0;
207 |   x_[1] = 0;
208 | 
209 |   // Perform Forward and Jacobian calculations using CPPAD_TESTVECTOR
210 |   CPPAD_TESTVECTOR(Scalar) y = f.Forward(0, x_);
211 |   CPPAD_TESTVECTOR(Scalar) J = f.Jacobian(x_);
212 | }
213 | 
214 | BOOST_AUTO_TEST_CASE(ice_cream_circle_contact_complementarity_cppad) {
215 |   cb::IceCreamCone<T> cone(.37);
216 |   Vector3s x = Vector3s::Zero();
217 |   Vector3s v = Vector3s::Zero();
218 |   T comp = cone.computeContactComplementarity(x, v);
219 |   BOOST_CHECK(comp == 0.);
220 | }
221 | 
222 | BOOST_AUTO_TEST_CASE(ice_cream_circle_signorini_complementarity_cppad) {
223 |   cb::IceCreamCone<T> cone(.37);
224 |   Vector3s x = Vector3s::Zero();
225 |   Vector3s v = Vector3s::Zero();
226 |   T comp = cone.computeSignoriniComplementarity(x, v);
227 |   BOOST_CHECK(comp == 0.);
228 | }
229 | 
230 | BOOST_AUTO_TEST_CASE(ice_cream_circle_conic_complementarity_cppad) {
231 |   MatrixXs A = MatrixXs::Zero(3, 3);
232 |   VectorXs b = VectorXs::Zero(3);
233 |   cb::IceCreamCone<T> cone(.37);
234 |   Vector3s x = Vector3s::Zero();
235 |   Vector3s v = Vector3s::Zero();
236 |   T comp = cone.computeConicComplementarity(x, v);
237 |   BOOST_CHECK(comp == 0.);
238 | }
239 | 
240 | BOOST_AUTO_TEST_CASE(pyramid_cone_init_cppad) {
241 |   cb::PyramidCone<T> cone(.84);
242 |   BOOST_CHECK(!CppAD::isnan(cone.mu_));
243 | }
244 | 
245 | BOOST_AUTO_TEST_CASE(pyramid_cone_horizontal_projection_cppad) {
246 |   cb::PyramidCone<T> cone(.37);
247 |   Vector3s x = Vector3s::Zero();
248 |   Vector3s x_out;
249 |   cone.projectHorizontal(x, x_out);
250 |   BOOST_CHECK(x_out.isApprox(x));
251 |   x(2) = 1.59;
252 |   x(0) = 3.46;
253 |   cone.projectHorizontal(x, x_out);
254 |   BOOST_CHECK(x_out(0) == x(2) * cone.mu_);
255 | }
256 | 
257 | BOOST_AUTO_TEST_CASE(pyramid_cone_horizontal_projection2_cppad) {
258 |   cb::PyramidCone<T> cone(.37);
259 |   Vector2s x = Vector2s::Zero();
260 |   double xn = 0.;
261 |   Vector2s x_out;
262 |   cone.projectHorizontal(x, xn, x_out);
263 |   BOOST_CHECK(x_out.isApprox(x));
264 |   xn = 1.59;
265 |   x(0) = 3.46;
266 |   cone.projectHorizontal(x, xn, x_out);
267 |   BOOST_CHECK(x_out(0) == xn * cone.mu_);
268 | }
269 | 
270 | BOOST_AUTO_TEST_CASE(pyramid_computeCoordinatesInD_cppad) {
271 |   double mu = .37;
272 |   cb::PyramidCone<T> cone(mu);
273 |   Vector3s x = Vector3s::Zero();
274 |   Vector4s x_out;
275 |   cone.computeCoordinatesInD(x, x_out);
276 |   BOOST_CHECK(x_out.isZero(0.));
277 |   x(2) = 1.59;
278 |   x(0) = mu * x(2);
279 |   cone.computeCoordinatesInD(x, x_out);
280 |   BOOST_CHECK(x_out(1) == 0.);
281 |   BOOST_CHECK(x_out(2) == 0.);
282 |   BOOST_CHECK(fabs(x_out(0) - x_out(3)) < 1e-5);
283 | }
284 | 
285 | BOOST_AUTO_TEST_SUITE_END()
286 | 


--------------------------------------------------------------------------------
/examples/solo.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from tap import Tap
  4 | from utils.models import create_solo
  5 | from utils.visualize import sub_sample
  6 | from pycontact.simulators import (
  7 |     CCPADMMSimulator,
  8 |     CCPPGSSimulator,
  9 |     RaisimSimulator,
 10 |     RaisimCorrectedSimulator,
 11 |     NCPPGSSimulator,
 12 |     NCPStagProjSimulator,
 13 |     CCPNewtonPrimalSimulator,
 14 |     PinNCPADMMSimulator
 15 | )
 16 | from pycontact.inverse_dynamics import inverse_contact_dynamics
 17 | import meshcat
 18 | import os
 19 | from pathlib import Path
 20 | from tqdm import trange
 21 | import cProfile, pstats
 22 | 
 23 | 
 24 | class Args(Tap):
 25 |     id: bool = False
 26 |     drag: bool = False
 27 |     perturb: bool = False
 28 |     heightfield: bool = False
 29 |     timings: bool = False
 30 |     display: bool = False
 31 |     record: bool = False
 32 |     plot: bool = False
 33 |     seed: int = 1234
 34 |     save: bool = False
 35 |     debug: bool = False
 36 | 
 37 |     def process_args(self):
 38 |         if self.record:
 39 |             self.display = True
 40 | 
 41 | 
 42 | args = Args().parse_args()
 43 | 
 44 | np.random.seed(args.seed)
 45 | pin.seed(args.seed)
 46 | 
 47 | mu = 0.9  # friction parameter
 48 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = create_solo(
 49 |     mu,
 50 |     0.,
 51 |     0.,
 52 |     False,
 53 |     True,
 54 |     args.heightfield,
 55 | 
 56 | )
 57 | 
 58 | # Number of time steps
 59 | T = 100
 60 | # T = 1
 61 | # time steps
 62 | dt = 1e-3
 63 | 
 64 | # Physical parameters of the contact problem
 65 | Kb = 1e-4  # Baumgarte
 66 | eps = 0.0  # elasticity
 67 | 
 68 | # initial state
 69 | q0 = model.qinit
 70 | v0 = np.zeros(model.nv)
 71 | q, v = q0.copy(), v0.copy()
 72 | 
 73 | 
 74 | # simulator
 75 | warm_start = True
 76 | simulator = NCPPGSSimulator(warm_start=warm_start, timings=args.timings)
 77 | # simulator = CCPPGSSimulator(warm_start=warm_start, timings=args.timings)
 78 | # simulator = CCPADMMSimulator(warm_start=warm_start, timings=args.timings)
 79 | # simulator = RaisimSimulator(warm_start=warm_start, timings=args.timings)
 80 | # simulator = CCPNewtonPrimalSimulator(warm_start=warm_start, timings=args.timings)
 81 | # simulator = RaisimCorrectedSimulator(warm_start=warm_start, timings=args.timings)
 82 | # simulator = NCPStagProjSimulator(warm_start=warm_start, timings=args.timings)
 83 | 
 84 | simulator.setSimulation(model, data, geom_model, geom_data)
 85 | 
 86 | # record quantities during trajectory
 87 | xs = [np.concatenate((q0, v0))]
 88 | lams = []
 89 | Js = []
 90 | us = []
 91 | Rs = []
 92 | es = []
 93 | Gs = []
 94 | gs = []
 95 | timings = []
 96 | if args.drag:
 97 |     def ext_torque(t):
 98 |         fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
 99 |         fext[1].linear[1] = np.min([mu*9.81*8, t*10*9.81])
100 |         fext[1] = pin.Force(data.oMi[1].actionInverse @ fext[1])
101 |         return fext
102 | elif args.perturb:
103 |     def ext_torque(t):
104 |         fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
105 |         noise_intensity = 12.
106 |         freq = 2.
107 |         linear_force =(np.array([np.sin(freq*t), np.sin(freq*t+1.), np.sin(freq*t+2)]))*noise_intensity
108 |         fext[1] = pin.Force(np.concatenate((linear_force,np.zeros(3))))
109 |         return fext
110 | else:
111 |     def ext_torque(t):
112 |         fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
113 |         return fext
114 | for t in trange(T):
115 |     if args.id:
116 |         v_ref = np.zeros(model.nv)
117 |         tau = inverse_contact_dynamics(model,data, geom_model, geom_data, q, v, v_ref, dt, 1000, 1e-6)
118 |         print("tau", tau)
119 |     else:
120 |         tau = np.zeros(model.nv)
121 |     # fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
122 |     # fext[1].linear[1] = ext_torque(t*dt)
123 |     # fext[1] = pin.Force(data.oMi[1].actionInverse @ fext[1])
124 |     fext = ext_torque(t*dt)
125 |     # q, v = simulator.step(
126 |     #     model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 10000, 1e-6, rel_th_stop = 1e-6, alpha_min = 1e-4, gamma = 0.9
127 |     # )
128 |     q, v = simulator.step(
129 |         model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 100, 1e-6, rel_th_stop = 1e-6)
130 |     # q2, v2 = simulator2.step(
131 |     #     model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 200, 1e-6
132 |     # )
133 |     if args.debug:
134 |         if t < 3:
135 |             profiler =cProfile.Profile()
136 |             profiler.enable()
137 |             simulator.step(model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 500, 1e-7)
138 |             profiler.disable()
139 |             stats = pstats.Stats(profiler)
140 |             # stats.sort_stats('cumtime').print_stats()
141 |             stats.sort_stats('cumtime')
142 |             stats.print_callees(3)
143 |         print("t=", t)
144 |         simulator.plotCallback(block=False)
145 |         input("Press enter for next step!")
146 |     # print(t, simulator.solver.n_iter_)
147 |     # print("lam", simulator.lam)
148 |     lams += [simulator.lam]
149 |     Js += [simulator.J]
150 |     Rs += [simulator.R]
151 |     es += [simulator.signed_dist]
152 |     # Gs += [simulator.G]
153 |     gs += [simulator.g]
154 |     xs += [np.concatenate((q, v))]
155 |     us += [tau]
156 |     if args.timings:
157 |         timings += [simulator.solve_time]
158 | 
159 | if args.debug:
160 |     simulator.close_plot_callback()
161 | 
162 | 
163 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
164 | os.makedirs(dir_path / "log", exist_ok=True)
165 | os.makedirs(dir_path / "log/solo/", exist_ok=True)
166 | 
167 | if args.save:
168 |     if simulator.Del is not None:
169 |         simulator.Del.evaluateDel()
170 |         np.savez(
171 |             dir_path /"log/solo/contact_problem.npz", G=simulator.Del.G_, g=simulator.g, mus=simulator.mus
172 |         )
173 |     np.save(dir_path/"log/solo/traj.npy", xs)
174 |     if args.timings:
175 |         np.save(dir_path/"log/solo/timings.npy", timings)
176 | 
177 | if args.plot:
178 |     import matplotlib.pyplot as plt
179 |     import matplotlib
180 | 
181 |     matplotlib.use("agg")
182 |     matplotlib.rcParams["pdf.fonttype"] = 42
183 |     matplotlib.rcParams["ps.fonttype"] = 42
184 | 
185 |     plt.figure()
186 |     iters = [t for t in range(len(lams))]
187 |     internal_forces  = np.zeros((len(iters), 4))
188 |     for i in range(len(iters)):
189 |         Ri = Rs[i]
190 |         lami = lams[i]
191 |         nci = int(len(lami)/3)
192 |         for j in range(nci):
193 |             Rij = Ri[:,3*j:3*(j+1)]
194 |             lamij = lami[3*j:3*(j+1),:]
195 |             fij = Rij @lamij
196 |             internal_forces[i,j] = fij[0]
197 |     plt.plot(iters, internal_forces, marker="+")
198 |     plt.xlabel("time step")
199 |     plt.ylabel("Internal forces")
200 |     if args.save:
201 |         plt.savefig(dir_path/"log/solo/internal_forces.pdf")
202 |     plt.close()
203 | 
204 |     plt.figure()
205 |     nc = simulator.G.shape[0]//3
206 |     contact_forces = simulator.lam
207 |     resulting_force = simulator.J.T @ simulator.lam
208 |     forces_name = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"]
209 |     if args.save:
210 |         np.save(dir_path/"log/solo/contact_forces.npy", contact_forces)
211 |         np.save(dir_path/"log/solo/resulting_force.npy", resulting_force)
212 | 
213 |     if nc ==4:
214 |         fig = plt.figure()
215 |         ax1 = fig.add_subplot(111, projection='3d')
216 | 
217 |         x = [1,1,3,3]
218 |         y = [1,3,1,3]
219 |         z = np.zeros(4)
220 | 
221 |         dx = np.ones(4)
222 |         dy = np.ones(4)
223 |         dz = contact_forces[[2,5,8,11]].squeeze(-1)
224 | 
225 |         ax1.bar3d(x, y, z, dx, dy, dz, label="contact forces")
226 | 
227 |         x = [5]
228 |         y = [2]
229 |         z = np.zeros(1)
230 | 
231 |         dx = np.ones(1)
232 |         dy = np.ones(1)
233 |         dz = resulting_force[2]
234 | 
235 |         ax1.bar3d(x, y, z, dx, dy, dz, label="resulting force", color = "red")
236 | 
237 |         ax1.set_zlabel('Forces (N)')
238 | 
239 |         ax1.view_init(elev=10., azim=-110.)
240 | 
241 |         if args.save:
242 |             plt.savefig(dir_path/"log/solo/contact_forces.pdf")
243 |         plt.close()
244 | 
245 | 
246 | 
247 | if args.display:
248 |     from pinocchio.visualize import MeshcatVisualizer
249 | 
250 |     # visualize the trajectory
251 |     vizer = MeshcatVisualizer(model, geom_model, visual_model)
252 |     vizer.initViewer(open=True, loadModel=True)
253 | 
254 |     if not args.heightfield:
255 |         vizer.viewer["plane"].set_object(meshcat.geometry.Box(np.array([20, 20, 0.1])))
256 |         placement = np.eye(4)
257 |         placement[:3, 3] = np.array([0, 0, -0.05])
258 |         vizer.viewer["plane"].set_transform(placement)
259 |     vizer.displayCollisions(True)
260 |     vizer.display(q0)
261 | 
262 |     numrep = 2
263 |     cp = [1.6, 0.0, 0.6]
264 |     # camera positions for visualization
265 |     cps_ = [cp.copy() for _ in range(numrep)]
266 |     rps_ = [np.zeros(3)]*numrep
267 |     qs = [x[:model.nq] for x in xs]
268 |     vs = [x[model.nq:] for x in xs]
269 | 
270 |     max_fps = 30.
271 |     fps = min([max_fps,1./dt])
272 |     qs = sub_sample(qs,dt*T, fps)
273 |     vs = sub_sample(vs,dt*T, fps)
274 | 
275 |     def get_callback(i: int):
276 |         def _callback(t):
277 |             vizer.setCameraPosition(cps_[i])
278 |             vizer.setCameraTarget(rps_[i])
279 |             pin.forwardKinematics(model, vizer.data, qs[t], vs[t])
280 |             # vizer.drawFrameVelocities(base_link_id)
281 | 
282 |         return _callback
283 | 
284 |     input("[Press enter]")
285 | 
286 |     if args.record:
287 |         ctx = vizer.create_video_ctx(dir_path/"log/solo/simulation.mp4", fps=fps)
288 |     else:
289 |         import contextlib
290 | 
291 |         ctx = contextlib.nullcontext()
292 |     with ctx:
293 |         for i in range(numrep):
294 |             vizer.play(
295 |                 qs,
296 |                 1./fps,
297 |                 get_callback(i)
298 |             )
299 | 


--------------------------------------------------------------------------------
/examples/ball.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from tap import Tap
  4 | from utils.models import create_balls, addPlaneToGeomModel
  5 | from utils.visualize import sub_sample
  6 | from pycontact.utils.bench import (
  7 |     compute_contact_torque,
  8 |     compute_contact_vel,
  9 |     compute_mdp,
 10 |     compute_signorini,
 11 |     compute_ncp,
 12 |     compute_dual_feas,
 13 | )
 14 | from pycontact.simulators import (
 15 |     CCPADMMSimulator,
 16 |     PyCCPCVXSimulator,
 17 |     CCPPGSSimulator,
 18 |     RaisimSimulator,
 19 |     NCPPGSSimulator,
 20 |     PyCCPADMMPrimalSimulator
 21 | )
 22 | import meshcat
 23 | import os
 24 | from pathlib import Path
 25 | 
 26 | 
 27 | class Args(Tap):
 28 |     drag: bool = False
 29 |     lift: bool = False
 30 |     display: bool = False
 31 |     record: bool = False
 32 |     plot: bool = False
 33 |     seed: int = 1234
 34 |     save: bool = False
 35 | 
 36 |     def process_args(self):
 37 |         if self.record:
 38 |             self.display = True
 39 | 
 40 | 
 41 | args = Args().parse_args()
 42 | 
 43 | np.random.seed(args.seed)
 44 | pin.seed(args.seed)
 45 | 
 46 | # Physical parameters of the contact problem
 47 | a = 0.2  # size of ball
 48 | m = 1.0  # mass of ball
 49 | eps = 1.0  # elasticity
 50 | mu = 0.9  # friction parameter
 51 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = create_balls(
 52 |     [a], [m], mu, eps
 53 | )
 54 | # if args.drag:
 55 | #     geom_model = addPlaneToGeomModel(geom_model, np.array([0,0,-1]),np.array([0.,0.,a]),mu,eps,0.,False)
 56 | #     geom_data = geom_model.createData()
 57 | #     visual_model = addPlaneToGeomModel(visual_model, np.array([0,0,-1]),np.array([0.,0.,a]),mu,eps,0.,True)
 58 | #     visual_data = visual_model.createData()
 59 | 
 60 | # Number of time steps
 61 | T = 800
 62 | # time steps
 63 | dt = 1e-3
 64 | Kb = 1e-4  # Baumgarte
 65 | 
 66 | # initial state
 67 | q0 = pin.neutral(model)
 68 | q0[2] = a / 2
 69 | # rand_place = pin.SE3.Random()
 70 | # q0[-4:] = pin.pin.SE3ToXYZQUAT(rand_place)[-4:]
 71 | # v0 = np.zeros(model.nv)
 72 | # v0 = np.random.rand(model.nv)
 73 | v0 = np.zeros(model.nv)
 74 | # if not args.drag:
 75 | #     v0[1] = 1.0
 76 | q, v = q0.copy(), v0.copy()
 77 | 
 78 | # simulator
 79 | # simulator = RaisimSimulator()
 80 | # simulator = CCPADMMSimulator()
 81 | # simulator = PyCCPCVXSimulator()
 82 | # simulator = NCPPGSSimulator()
 83 | simulator = PyCCPADMMPrimalSimulator()
 84 | 
 85 | simulator.setSimulation(model, data, geom_model, geom_data)
 86 | 
 87 | 
 88 | # record quantities during trajectory
 89 | xs = [np.concatenate((q0, v0))]
 90 | lams = []
 91 | Js = []
 92 | Rs = []
 93 | es = []
 94 | us = []
 95 | Gs = []
 96 | gs = []
 97 | if args.drag:
 98 |     def ext_torque(t):
 99 |         # return np.array([0.,np.min([mu*9.81*1.2, t*2*9.81]),0.])
100 |         return np.array([0.,mu*9.81*1.5,0.])
101 |         # return np.array([0.,mu*9.81*0.01,0.])
102 | elif args.lift:
103 |     def ext_torque(t):
104 |         return np.array([0.,0.,10.])
105 |         # return np.array([0.,0.,5.])
106 | else:
107 |     def ext_torque(t):
108 |         return np.zeros(3)
109 | for t in range(T):
110 |     tau = np.zeros(model.nv)
111 |     fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
112 |     fext[1].linear = ext_torque(t*dt)
113 |     fext[1] = pin.Force(data.oMi[1].actionInverse @ fext[1])
114 |     q, v = simulator.step(
115 |         model, data, geom_model, geom_data, q, v, tau, fext, dt, Kb, 100
116 |     )
117 |     lams += [simulator.lam]
118 |     Js += [simulator.J]
119 |     Rs += [simulator.R]
120 |     es += [simulator.signed_dist]
121 |     Gs += [simulator.Del]
122 |     gs += [simulator.g]
123 |     xs += [np.concatenate((q, v))]
124 |     us += [tau]
125 | 
126 | 
127 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
128 | os.makedirs(dir_path / "log", exist_ok=True)
129 | os.makedirs(dir_path /"log/ball/", exist_ok=True)
130 | 
131 | if args.save:
132 |     np.savez(
133 |         dir_path /"log/ball/contact_problem.npz", G=simulator.G, g=simulator.g, mus=simulator.mus
134 |     )
135 | 
136 | if args.plot:
137 |     import matplotlib.pyplot as plt
138 |     import matplotlib
139 | 
140 |     matplotlib.use("agg")
141 |     matplotlib.rcParams["pdf.fonttype"] = 42
142 |     matplotlib.rcParams["ps.fonttype"] = 42
143 | 
144 |     # igs, axs = plt.subplots(9, 1, figsize=(20, 15))
145 | 
146 |     plt.figure()
147 |     iters = [t for t in range(len(xs))]
148 |     plt.plot(iters, [x[:3] for x in xs], marker="+")
149 |     plt.xlabel("time steps")
150 |     plt.tight_layout()
151 |     if args.save:
152 |         plt.savefig(dir_path / "log/ball/positions.pdf")
153 | 
154 |     plt.figure()
155 |     plt.plot(iters, [x[model.nq : model.nq + 3] for x in xs], marker="+")
156 |     plt.xlabel("time steps")
157 |     plt.ylabel("Joint velocity")
158 |     plt.tight_layout()
159 |     if args.save:
160 |         plt.savefig(dir_path / "log/ball/velocities.pdf")
161 | 
162 |     plt.figure()
163 |     v_world = []
164 |     for i in range(len(iters)-1):
165 |         # pin.computeJointJacobian(model, data, xs[i][:model.nq], 1)
166 |         # Ji = pin.getJointJacobian(model, data, 1, pin.LOCAL_WORLD_ALIGNED)
167 |         # v_world += [(Ji @xs[i][model.nq:])[:3] ]
168 |         v_world += [(xs[i+1][:3] - xs[i][:3])/dt]
169 |     plt.plot(iters[:-1], v_world, marker="+")
170 |     plt.xlabel("time steps")
171 |     plt.ylabel("Base velocity")
172 |     plt.tight_layout()
173 |     if args.save:
174 |         plt.savefig(dir_path / "log/ball/base_velocities.pdf")
175 | 
176 |     plt.figure()
177 |     iters = [t for t in range(len(Js))]
178 |     vs = [xs[t + 1][model.nq :] for t in range(len(lams))]
179 |     Jv = compute_contact_vel(Js, vs)
180 |     ids = []
181 |     for i in range(len(iters)):
182 |         if Jv[i] is not None:
183 |             ids += [i]
184 |     # plt.plot([iters[i] for i in ids], [Jv[i] for i in ids], marker="+")
185 |     plt.xlabel("time steps")
186 |     plt.ylabel(" Contact velocity")
187 |     plt.tight_layout()
188 |     if args.save:
189 |         plt.savefig(dir_path / "log/ball/contact_vel.pdf")
190 | 
191 |     plt.figure()
192 |     iters = [t for t in range(len(lams))]
193 |     lam_plt = [lam[:, 0] if len(lam) != 0 else np.zeros(3) for lam in lams]
194 |     # axs[3].plot(iters, lam_plt, marker="+")
195 |     plt.xlabel("time steps")
196 |     plt.ylabel("contact forces")
197 |     plt.tight_layout()
198 |     if args.save:
199 |         plt.savefig(dir_path / "log/ball/contact_forces.pdf")
200 | 
201 |     plt.figure()
202 |     iters = [t for t in range(len(lams))]
203 |     Jlam = compute_contact_torque(Js, lams)
204 |     plt.plot(iters, Jlam, marker="+")
205 |     plt.xlabel("time steps")
206 |     plt.ylabel("contact torque")
207 |     plt.tight_layout()
208 |     if args.save:
209 |         plt.savefig(dir_path / "log/ball/contact_torque.pdf")
210 | 
211 |     plt.figure()
212 |     iters = [t for t in range(len(lams))]
213 |     signorini = compute_signorini(Js, lams, vs)
214 |     plt.plot(iters, signorini, marker="+")
215 |     plt.xlabel("time steps")
216 |     plt.ylabel("Signorini constraint")
217 |     plt.tight_layout()
218 |     if args.save:
219 |         plt.savefig(dir_path / "log/ball/sig_comp.pdf")
220 | 
221 |     plt.figure()
222 |     iters = [t for t in range(len(lams))]
223 |     # mdp_gap = compute_mdp(Js, vs, lams, mu)
224 |     # plt.plot(iters, mdp_gap, marker="+")
225 |     plt.xlabel("time steps")
226 |     plt.ylabel("MDP gap")
227 |     plt.tight_layout()
228 |     if args.save:
229 |         plt.savefig(dir_path / "log/ball/mdp_gap.pdf")
230 | 
231 |     plt.figure()
232 |     ncp_crit = compute_ncp(
233 |         Gs,
234 |         gs,
235 |         lams,
236 |         [[mu for i in range(int(len(gs[t]) / 3))] for t in range(len(iters))],
237 |     )
238 |     plt.plot(iters, ncp_crit, marker="+")
239 |     plt.xlabel("time steps")
240 |     plt.ylabel("NCP crit")
241 |     plt.tight_layout()
242 |     if args.save:
243 |         plt.savefig(dir_path / "log/ball/ncp_crit.pdf")
244 | 
245 |     plt.figure()
246 |     dual_crit = compute_dual_feas(
247 |         Gs,
248 |         gs,
249 |         lams,
250 |         [[mu for i in range(int(len(gs[t]) / 3))] for t in range(len(iters))],
251 |     )
252 |     plt.plot(iters, dual_crit, marker="+")
253 |     plt.xlabel("time steps")
254 |     plt.ylabel("Dual feas")
255 |     plt.tight_layout()
256 |     if args.save:
257 |         plt.savefig(dir_path / "log/ball/dual_feas.pdf")
258 | 
259 | 
260 | if args.display:
261 |     from pinocchio.visualize import MeshcatVisualizer
262 | 
263 |     # visualize the trajectory
264 |     vizer = MeshcatVisualizer(model, geom_model, visual_model)
265 |     vizer.initViewer(open=True, loadModel=True)
266 | 
267 |     vizer.viewer["plane"].set_object(meshcat.geometry.Box(np.array([1., 10., 0.1])))
268 |     placement = np.eye(4)
269 |     placement[:3, 3] = np.array([0, 0, -0.05])
270 |     vizer.viewer["plane"].set_transform(placement)
271 |     # if args.drag:
272 |     #     vizer.viewer["plane2"].set_object(meshcat.geometry.Box(np.array([1., 10., 0.1])))
273 |     #     placement = np.eye(4)
274 |     #     placement[:3, 3] = np.array([0, 0, a+0.05])
275 |     #     vizer.viewer["plane2"].set_transform(placement)
276 |     vizer.display(q0)
277 | 
278 |     numrep = 2
279 |     cp = [1.3, 0.0, 0.1]
280 |     # camera positions for visualization
281 |     cps_ = [cp.copy() for _ in range(numrep)]
282 |     rps_ = [np.zeros(3)]*numrep
283 |     qs = [x[:model.nq] for x in xs]
284 |     vs = [x[model.nq:] for x in xs]
285 | 
286 |     max_fps = 30.
287 |     fps = min([max_fps,1./dt])
288 |     qs = sub_sample(qs,dt*T, fps)
289 |     vs = sub_sample(vs,dt*T, fps)
290 | 
291 |     def get_callback(i: int):
292 |         def _callback(t):
293 |             vizer.setCameraPosition(cps_[i])
294 |             vizer.setCameraTarget(rps_[i])
295 |             pin.forwardKinematics(model, vizer.data, qs[t], vs[t])
296 |             # vizer.drawFrameVelocities(base_link_id)
297 | 
298 |         return _callback
299 | 
300 |     input("[Press enter]")
301 | 
302 |     if args.record:
303 |         ctx = vizer.create_video_ctx(dir_path/"log/ball/simulation.mp4", fps=fps)
304 |     else:
305 |         import contextlib
306 | 
307 |         ctx = contextlib.nullcontext()
308 |     with ctx:
309 |         for i in range(numrep):
310 |             vizer.play(
311 |                 qs,
312 |                 1./fps,
313 |                 get_callback(i)
314 |             )
315 | 


--------------------------------------------------------------------------------
/examples/cube.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import pinocchio as pin
  3 | from tap import Tap
  4 | from utils.models import create_cubes
  5 | from utils.visualize import sub_sample
  6 | from pycontact.simulators import (
  7 |     CCPADMMSimulator,
  8 |     CCPADMMPrimalSimulator,
  9 |     CCPNewtonPrimalSimulator,
 10 |     CCPPGSSimulator,
 11 |     RaisimSimulator,
 12 |     RaisimCorrectedSimulator,
 13 |     NCPPGSSimulator,
 14 |     PinNCPPGSSimulator,
 15 |     PinNCPADMMSimulator,
 16 |     PyNCPPGSSimulator,
 17 |     NCPStagProjSimulator,
 18 |     LCPPGSSimulator,
 19 |     LCPQPSimulator,
 20 |     LCPStagProjSimulator,
 21 |     PyCCPADMMPrimalSimulator,
 22 |     PyCCPNSNewtonPrimalSimulator,
 23 |     PyCCPCVXSimulator,
 24 |     PyCCPSCSSimulator
 25 | )
 26 | import meshcat
 27 | import os
 28 | from pathlib import Path
 29 | from tqdm import trange
 30 | 
 31 | 
 32 | class Args(Tap):
 33 |     drag: bool = False
 34 |     slide: bool = False
 35 |     display: bool = False
 36 |     timings: bool = False
 37 |     record: bool = False
 38 |     plot: bool = False
 39 |     seed: int = 1234
 40 |     save: bool = False
 41 |     debug: bool = False
 42 | 
 43 |     def process_args(self):
 44 |         if self.record:
 45 |             self.display = True
 46 | 
 47 | 
 48 | args = Args().parse_args()
 49 | 
 50 | np.random.seed(args.seed)
 51 | pin.seed(args.seed)
 52 | 
 53 | 
 54 | a = 0.2  # size of cube
 55 | m = 1.0  # mass of cube
 56 | mu = 0.4  # friction parameter
 57 | eps = 0.0  # elasticity
 58 | model, geom_model, visual_model, data, geom_data, visual_data, actuation = create_cubes(
 59 |     [a], [m], mu, eps
 60 | )
 61 | 
 62 | # Number of time steps
 63 | duration = 1.098
 64 | # time steps
 65 | dt = 1e-3
 66 | T = int(duration/dt)
 67 | 
 68 | # Physical parameters of the contact problem
 69 | Kb = 1e-4  # Baumgarte
 70 | 
 71 | # numerical precision
 72 | th = 1e-9
 73 | 
 74 | # initial state
 75 | q0 = model.qinit
 76 | v0 = np.zeros(model.nv)
 77 | if  args.drag or args.slide:
 78 |     q0[1] = -5*a
 79 |     if args.slide:
 80 |         v0[1] = 1.
 81 |         # v0[1] = 0.
 82 |     # q0[2] += 2e-3
 83 | else:
 84 |     q0[2] = a
 85 |     rand_place = pin.SE3.Random()
 86 |     q0[-4:] = pin.pin.SE3ToXYZQUAT(rand_place)[-4:]
 87 | 
 88 | q, v = q0.copy(), v0.copy()
 89 | 
 90 | 
 91 | # simulator
 92 | warm_start = False
 93 | statistics = False
 94 | magic_forces = False
 95 | max_dist_mf = 1e-1
 96 | # simulator = NCPPGSSimulator(warm_start=warm_start, statistics=statistics)
 97 | # simulator = PyCCPSCSSimulator(warm_start=warm_start, statistics=statistics)
 98 | simulator = PinNCPADMMSimulator(warm_start=warm_start, timings = args.timings, statistics=statistics,  magic_forces=magic_forces, max_dist_mf = max_dist_mf)
 99 | # simulator = PinNCPPGSSimulator(warm_start=warm_start, statistics=statistics)
100 | # simulator = LCPPGSSimulator(warm_start=warm_start, statistics=statistics)
101 | # simulator = CCPPGSSimulator(model, geom_model, data, geom_data, q0, regularize = False, warm_start=warm_start, statistics=statistics,  magic_forces=magic_forces, max_dist_mf = max_dist_mf)
102 | # simulator = CCPADMMSimulator(warm_start=warm_start, timings = args.timings, statistics=statistics, magic_forces=magic_forces, max_dist_mf = max_dist_mf)
103 | # simulator = NCPStagProjSimulator(warm_start = warm_start, statistics=statistics)
104 | # simulator = RaisimSimulator(warm_start=warm_start, statistics=statistics, magic_forces = magic_forces, max_dist_mf = max_dist_mf)
105 | # simulator = LCPQPSimulator(warm_start=warm_start, statistics=statistics)
106 | # simulator = PyLCPMehrotraSimulator(warm_start=warm_start, statistics=False)
107 | # simulator = PyLCPIPSimulator(warm_start=warm_start, statistics=False)
108 | # simulator = RaisimCorrectedSimulator(warm_start=warm_start, statistics=statistics)
109 | # simulator2 = PyCCPADMMPrimalSimulator(warm_start=True, statistics=statistics)
110 | # simulator = CCPADMMPrimalSimulator(warm_start=warm_start, statistics=statistics, magic_forces=magic_forces, max_dist_mf = max_dist_mf)
111 | # simulator = PyCCPNSNewtonPrimalSimulator(warm_start=True, statistics=statistics, magic_forces=magic_forces, max_dist_mf = max_dist_mf)
112 | # simulator = CCPNewtonPrimalSimulator(warm_start=warm_start, statistics=statistics, magic_forces=magic_forces, max_dist_mf = max_dist_mf)
113 | 
114 | simulator.setSimulation(model, data, geom_model, geom_data)
115 | 
116 | 
117 | # record quantities during trajectory
118 | xs = [np.concatenate((q0, v0))]
119 | pin.computeMechanicalEnergy(model, data, q0,v0)
120 | energies = [data.mechanical_energy]
121 | lams = []
122 | Js = []
123 | us = []
124 | Rs = []
125 | es = []
126 | Gs = []
127 | gs = []
128 | mus = []
129 | signorini_crits =[]
130 | comp_crits = []
131 | ncp_crits = []
132 | if args.timings:
133 |     timings = []
134 | if args.drag:
135 |     def ext_torque(t):
136 |         return np.min([0.9*9.81*2, t*4*9.81])
137 | else:
138 |     def ext_torque(t):
139 |         return 0.
140 | for t in trange(T):
141 |     tau = np.zeros(model.nv)
142 |     fext = [pin.Force(np.zeros(6)) for i in range(model.njoints)]
143 |     fext[1].linear[1] = ext_torque(t*dt)
144 |     fext[1] = pin.Force(data.oMi[1].actionInverse @ fext[1])
145 |     q, v = simulator.step(
146 |         model = model, data = data, geom_model = geom_model, geom_data = geom_data, q = q, v = v, tau = tau,fext = fext, dt = dt, Kb = Kb,maxIter = 10000, th =th, rel_th_stop =1e-12, eps_reg = 1e-8
147 |     )
148 |     if args.debug:
149 |         print("t=", t)
150 |         simulator.plotCallback(block=False)
151 |         input("Press enter for next step!")
152 |     lams += [simulator.lam]
153 |     Js += [simulator.J]
154 |     Rs += [simulator.R]
155 |     es += [simulator.signed_dist]
156 |     mus += [simulator.mus]
157 |     xs += [np.concatenate((q, v))]
158 |     us += [tau]
159 |     pin.computeMechanicalEnergy(model, data, q,v)
160 |     energies += [data.mechanical_energy]
161 |     if args.timings:
162 |         timings += [simulator.solve_time]
163 | 
164 | if args.debug:
165 |     simulator.close_plot_callback()
166 | 
167 | dir_path = Path(os.path.dirname(os.path.realpath(__file__)))
168 | os.makedirs(dir_path / "log", exist_ok=True)
169 | os.makedirs(dir_path / "log/cube/", exist_ok=True)
170 | dir_file = dir_path/"log/cube"
171 | 
172 | if args.drag:
173 |     dir_file = dir_file/"dragged_cube"
174 |     os.makedirs(dir_file,exist_ok=True)
175 | 
176 | if args.save:
177 |     if simulator.Del is not None:
178 |         simulator.Del.evaluateDel()
179 |         np.savez(
180 |             dir_file/"contact_problem.npz", G=simulator.Del.G_, g=simulator.g, mus=simulator.mus
181 |         )
182 |     np.save(dir_file/"traj.npy", xs)
183 |     np.save(dir_file/"energy.npy", energies)
184 |     np.save(dir_file/"sig_comp.npy",signorini_crits)
185 |     np.save(dir_file/"prob_comp.npy",comp_crits)
186 |     np.save(dir_file/"ncp_comp.npy",ncp_crits)
187 |     if args.timings:
188 |         np.save(dir_file/"timings.npy", timings)
189 | 
190 | if args.plot:
191 |     import matplotlib.pyplot as plt
192 |     import matplotlib
193 | 
194 |     matplotlib.use("agg")
195 |     matplotlib.rcParams["pdf.fonttype"] = 42
196 |     matplotlib.rcParams["ps.fonttype"] = 42
197 | 
198 |     figs, axs = plt.subplots(2, 1, figsize=(20, 5))
199 |     iters = [t for t in range(len(xs))]
200 |     axs[0].plot(iters, [x[:3] for x in xs], marker="+")
201 |     axs[0].set_xlabel("time steps")
202 |     axs[0].set_ylabel("q")
203 | 
204 |     iters = [t for t in range(len(lams))]
205 |     Jlam = [
206 |         np.zeros(3) if len(Js[i]) == 0 else np.dot(Js[i].T, lams[i])[:3, 0]
207 |         for i in range(len(Js))
208 |     ]
209 |     axs[1].plot(iters, Jlam, marker="+")
210 |     axs[1].set_xlabel("time steps")
211 |     axs[1].set_ylabel("contact forces")
212 | 
213 |     plt.tight_layout()
214 |     if args.save:
215 |         plt.savefig(dir_path/"log/cube/traj.pdf")
216 |     plt.close()
217 | 
218 |     plt.figure()
219 |     internal_forces  = np.zeros((len(iters), 4))
220 |     internal_forces_cor  = np.zeros((len(iters), 4))
221 |     for i in range(len(iters)):
222 |         Ri = Rs[i]
223 |         lami = lams[i]
224 |         nci = int(len(lami)/3)
225 |         for j in range(nci):
226 |             Rij = Ri[:,3*j:3*(j+1)]
227 |             lamij = lami[3*j:3*(j+1),:]
228 |             fij = Rij @lamij
229 |             fij_cor = fij
230 |             # fij_cor[:2] += Rij[:2] @( (mus[i][j]**2)*lamij[2]/Gs[i][3*j+2, 3*j+2] *Gs[i][3*j+2,3*j:3*j+2])
231 |             internal_forces[i,j] = fij[0]
232 |             # internal_forces_cor[i,j] = fij_cor[0]
233 |     plt.plot(iters, internal_forces, marker="+")
234 |     plt.xlabel("time step")
235 |     plt.ylabel("Internal forces")
236 |     if args.save:
237 |         np.save(dir_file/"internal_forces.npy", internal_forces)
238 |         plt.savefig(dir_path/"log/cube/internal_forces.pdf")
239 |     plt.close()
240 | 
241 |     plt.figure()
242 |     plt.xlabel("time step")
243 |     plt.ylabel("Internal forces corrected")
244 |     if args.save:
245 |         np.save(dir_path/"log/cube/internal_forces_cor.npy", internal_forces_cor)
246 |         plt.savefig(dir_path/"log/cube/internal_forces_cor.pdf")
247 |     plt.close()
248 | 
249 |     plt.figure()
250 |     iters = [t for t in range(len(ncp_crits))]
251 |     plt.plot(iters,ncp_crits)
252 |     plt.xlabel("Time steps")
253 |     plt.ylabel("NCP criterion")
254 |     plt.tight_layout()
255 |     if args.save:
256 |         plt.savefig(dir_path/"log/cube/ncp_criterion.pdf")
257 |     plt.close()
258 | 
259 | 
260 | if args.display:
261 |     from pinocchio.visualize import MeshcatVisualizer
262 | 
263 |     # visualize the trajectory
264 |     vizer = MeshcatVisualizer(model, geom_model, visual_model)
265 |     vizer.initViewer(open=True, loadModel=True)
266 | 
267 |     vizer.viewer["plane"].set_object(meshcat.geometry.Box(np.array([20, 20, 0.1])))
268 |     placement = np.eye(4)
269 |     placement[:3, 3] = np.array([0, 0, -0.05])
270 |     vizer.viewer["plane"].set_transform(placement)
271 |     vizer.display(q0)
272 | 
273 |     if args.drag or args.slide:
274 |         cp1 = [1.8, 0.0, 0.2]
275 |         cp2 = [0., -1.2, 1.5]
276 |     else:
277 |         cp1 = [0.8, 0.0, 0.2]
278 |         cp2=cp1
279 |     cps_ = [cp1, cp2]
280 |     numrep = len(cps_)
281 |     rps_ = [np.zeros(3)]*numrep
282 |     qs = [x[:model.nq] for x in xs]
283 |     vs = [x[model.nq:] for x in xs]
284 | 
285 |     max_fps = 30.
286 |     fps = min([max_fps,1./dt])
287 |     qs = sub_sample(qs,dt*T, fps)
288 |     vs = sub_sample(vs,dt*T, fps)
289 | 
290 |     def get_callback(i: int):
291 |         def _callback(t):
292 |             vizer.setCameraPosition(cps_[i])
293 |             vizer.setCameraTarget(rps_[i])
294 |             pin.forwardKinematics(model, vizer.data, qs[t], vs[t])
295 |             # vizer.drawFrameVelocities(base_link_id)
296 | 
297 |         return _callback
298 | 
299 |     input("[Press enter]")
300 | 
301 |     if args.record:
302 |         ctx = vizer.create_video_ctx(dir_path/"log/cube/simulation.mp4", fps=fps)
303 |     else:
304 |         import contextlib
305 | 
306 |         ctx = contextlib.nullcontext()
307 |     with ctx:
308 |         for i in range(numrep):
309 |             vizer.play(
310 |                 qs,
311 |                 1./fps,
312 |                 get_callback(i)
313 |             )
314 | 


--------------------------------------------------------------------------------
/tests/python/test_pytorch_solvers.py:
--------------------------------------------------------------------------------
  1 | import torch
  2 | 
  3 | import numpy as np
  4 | from pycontact.torch.solvers import lcp_layer, ncp_layer, ccp_layer
  5 | 
  6 | from pathlib import Path
  7 | import os
  8 | 
  9 | test_python_path = Path(os.path.dirname(os.path.realpath(__file__)))
 10 | 
 11 | 
 12 | def test_lcpqp_solver_forward():
 13 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
 14 |     G = npzfile["G"]
 15 |     g = npzfile["g"]
 16 |     mus = npzfile["mus"]
 17 |     G = torch.from_numpy(G).unsqueeze(0)
 18 |     g = torch.from_numpy(g).unsqueeze(0)
 19 |     mus = torch.from_numpy(mus).unsqueeze(0).unsqueeze(-1)
 20 |     max_iter = 1000
 21 |     eps = 1e-7
 22 |     lam = lcp_layer(G,g,mus, max_iter, eps)
 23 |     loss = 0.5*torch.square(lam).sum()
 24 |     assert True
 25 | 
 26 | def test_lcpqp_solver_backward():
 27 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
 28 |     G = npzfile["G"]
 29 |     g = npzfile["g"]
 30 |     mus = npzfile["mus"]
 31 |     nc = len(mus)
 32 |     G = torch.from_numpy(G).unsqueeze(0)
 33 |     g = torch.from_numpy(g).unsqueeze(0)
 34 |     mus = torch.from_numpy(mus).unsqueeze(0).unsqueeze(-1)
 35 |     G.requires_grad_()
 36 |     g.requires_grad_()
 37 |     mus.requires_grad_()
 38 | 
 39 |     def compute_loss(G,g,mus):
 40 |         max_iter = 1000
 41 |         eps = 1e-7
 42 |         # solver = LCPLayer()
 43 |         lam = lcp_layer(G,g,mus, max_iter, eps)
 44 |         loss = 0.5*torch.square(lam).sum()
 45 |         return loss
 46 | 
 47 |     loss = compute_loss(G, g, mus)
 48 |     loss.backward()
 49 | 
 50 |     G2, g2, mus2 = G.clone(), g.clone(), mus.clone()
 51 |     delta = 1e-5
 52 |     for i in range(nc):
 53 |         mus2[0,i,0] += delta
 54 |         loss2 = compute_loss(G,g,mus2)
 55 |         dl_dmusi = (loss2-loss).detach().numpy()/delta
 56 |         assert np.abs(dl_dmusi - mus.grad[0,i,0].detach().numpy())< 1e-3
 57 |         mus2[0,i,0] -= delta
 58 | 
 59 |     for i in range(3*nc):
 60 |         g2[0,i,0] += delta
 61 |         loss2 = compute_loss(G,g2,mus)
 62 |         dl_dgi = (loss2-loss).detach().numpy()/delta
 63 |         assert np.abs(dl_dgi - g.grad[0,i,0].detach().numpy())< 1e-3
 64 |         g2[0,i,0] -= delta
 65 | 
 66 |     for i in range(3*nc):
 67 |         for j in range(3*nc):
 68 |             G2[0,i,j] += delta
 69 |             loss2 = compute_loss(G2,g,mus)
 70 |             dl_dGij = (loss2-loss).detach().numpy()/delta
 71 |             assert np.abs(dl_dGij - G.grad[0,i,j].detach().numpy())< 1e-3
 72 |             G2[0,i,j] -= delta
 73 | 
 74 | def test_ncppgs_solver_forward():
 75 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
 76 |     G = npzfile["G"]
 77 |     g = npzfile["g"]
 78 |     mus = npzfile["mus"]
 79 |     G = torch.from_numpy(G).unsqueeze(0)
 80 |     g = torch.from_numpy(g).unsqueeze(0)
 81 |     mus = torch.from_numpy(mus).unsqueeze(0).unsqueeze(-1)
 82 |     max_iter = 1000
 83 |     eps = 1e-7
 84 |     lam = ncp_layer(G,g,mus, max_iter, eps)
 85 |     loss = 0.5*torch.square(lam).sum()
 86 |     assert True
 87 | 
 88 | def test_ncppgs_solver_backward():
 89 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
 90 |     G = npzfile["G"]
 91 |     g = npzfile["g"]
 92 |     mus = npzfile["mus"]
 93 |     nc = len(mus)
 94 |     G = torch.from_numpy(G).unsqueeze(0)
 95 |     g = torch.from_numpy(g).unsqueeze(0)
 96 |     mus = torch.from_numpy(mus).unsqueeze(0).unsqueeze(-1)
 97 |     G.requires_grad_()
 98 |     g.requires_grad_()
 99 |     mus.requires_grad_()
100 | 
101 |     def compute_loss(G,g,mus):
102 |         max_iter = 1000
103 |         eps = 1e-7
104 |         # solver = LCPLayer()
105 |         lam = ncp_layer(G,g,mus, max_iter, eps)
106 |         loss = 0.5*torch.square(lam).sum()
107 |         return loss
108 | 
109 |     loss = compute_loss(G, g, mus)
110 |     loss.backward()
111 | 
112 |     G2, g2, mus2 = G.clone(), g.clone(), mus.clone()
113 |     delta = 1e-5
114 |     for i in range(nc):
115 |         mus2[0,i,0] += delta
116 |         loss2 = compute_loss(G,g,mus2)
117 |         dl_dmusi = (loss2-loss).detach().numpy()/delta
118 |         assert np.abs(dl_dmusi - mus.grad[0,i,0].detach().numpy())< 1e-3
119 |         mus2[0,i,0] -= delta
120 | 
121 |     for i in range(3*nc):
122 |         g2[0,i,0] += delta
123 |         loss2 = compute_loss(G,g2,mus)
124 |         dl_dgi = (loss2-loss).detach().numpy()/delta
125 |         assert np.abs(dl_dgi - g.grad[0,i,0].detach().numpy())< 1e-3
126 |         g2[0,i,0] -= delta
127 | 
128 |     for i in range(3*nc):
129 |         for j in range(3*nc):
130 |             G2[0,i,j] += delta
131 |             loss2 = compute_loss(G2,g,mus)
132 |             dl_dGij = (loss2-loss).detach().numpy()/delta
133 |             assert np.abs(dl_dGij - G.grad[0,i,j].detach().numpy())< 1e-3
134 |             G2[0,i,j] -= delta
135 | 
136 | def test_ccppgs_solver_forward():
137 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
138 |     G = npzfile["G"]
139 |     g = npzfile["g"]
140 |     mus = npzfile["mus"]
141 |     G = torch.from_numpy(G).unsqueeze(0)
142 |     g = torch.from_numpy(g).unsqueeze(0)
143 |     mus = torch.from_numpy(mus).unsqueeze(0).unsqueeze(-1)
144 |     max_iter = 1000
145 |     eps = 1e-7
146 |     lam = ccp_layer(G,g,mus, max_iter, eps)
147 |     loss = 0.5*torch.square(lam).sum()
148 |     assert True
149 | 
150 | def test_ccppgs_solver_backward():
151 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
152 |     G = npzfile["G"]
153 |     g = npzfile["g"]
154 |     mus = npzfile["mus"]
155 |     nc = len(mus)
156 |     G = torch.from_numpy(G).unsqueeze(0)
157 |     g = torch.from_numpy(g).unsqueeze(0)
158 |     mus = torch.from_numpy(mus).unsqueeze(0).unsqueeze(-1)
159 |     G.requires_grad_()
160 |     g.requires_grad_()
161 |     mus.requires_grad_()
162 | 
163 |     def compute_loss(G,g,mus,lam_tar):
164 |         max_iter = 1000
165 |         eps = 1e-7
166 |         # solver = LCPLayer()
167 |         lam = ccp_layer(G,g,mus, max_iter, eps)
168 |         # print("lam : ", lam)
169 |         # print("lam_tar : ", lam_tar)
170 |         loss = 0.5*torch.square(lam- lam_tar).sum()
171 |         return loss
172 | 
173 | 
174 |     # lam = ccp_layer(G,g,mus, 1000, 1e-7)
175 |     # print("g", g)
176 |     # print("lam", lam)
177 |     # print("c", G @ lam + g)
178 |     with torch.no_grad():
179 |         mus2 = torch.square(mus*0.9)
180 |         lam_tar = ccp_layer(G,g,mus2, 1000, 1e-7).clone()
181 |     # lam_tar = torch.zeros_like(g)
182 |     loss = compute_loss(G, g, mus, lam_tar)
183 |     loss.backward()
184 | 
185 |     G2, g2, mus2 = G.clone(), g.clone(), mus.clone()
186 |     delta = 1e-8
187 |     for i in range(nc):
188 |         mus2[0,i,0] += delta
189 |         loss2 = compute_loss(G,g,mus2, lam_tar)
190 |         dl_dmusi = (loss2-loss).detach().numpy()/delta
191 |         # assert np.abs(dl_dmusi - mus.grad[0,i,0].detach().numpy())< 1e-3
192 |         mus2[0,i,0] -= delta
193 | 
194 |     for i in range(3*nc):
195 |         g2[0,i,0] += delta
196 |         loss2 = compute_loss(G,g2,mus, lam_tar)
197 |         dl_dgi = (loss2-loss).detach().numpy()/delta
198 |         # assert np.abs(dl_dgi - g.grad[0,i,0].detach().numpy())< 1e-3
199 |         g2[0,i,0] -= delta
200 | 
201 |     for i in range(3*nc):
202 |         for j in range(3*nc):
203 |             G2[0,i,j] += delta
204 |             loss2 = compute_loss(G2,g,mus, lam_tar)
205 |             dl_dGij = (loss2-loss).detach().numpy()/delta
206 |             # assert np.abs(dl_dGij - G.grad[0,i,j].detach().numpy())< 1e-3
207 |             G2[0,i,j] -= delta
208 | 
209 |     lr = 5e-5
210 |     # lr = 5e-4
211 |     optimizer = optimizer = torch.optim.Adam([mus], lr=lr)
212 |     print("mus", mus)
213 |     G.requires_grad_(False)
214 |     g.requires_grad_(False)
215 |     lam_tar.requires_grad_(False)
216 |     loss = compute_loss(G, g, mus2, lam_tar)
217 |     print("init loss",loss.detach().numpy())
218 |     for i in range(10000):
219 |         # print(i)
220 |         optimizer.zero_grad()
221 |         mus2 = torch.square(mus)
222 |         loss = compute_loss(G, g, mus2, lam_tar)
223 |         loss.backward()
224 |         optimizer.step()
225 |         # print("mus",mus.detach().numpy())
226 |         # print("grad mus: ", mus.grad.detach().numpy())
227 |         # print("loss",loss.detach().numpy())
228 |     print("mus", mus)
229 |     print("final loss",loss.detach().numpy())
230 |     print("lam : ", ccp_layer(G,g,torch.square(mus), 1000, 1e-7))
231 |     print("lam_tar : ", lam_tar)
232 | 
233 | def test_ncppgs_solver_backward2():
234 |     npzfile = np.load(test_python_path/'data/sliding_cube.npz')
235 |     G = npzfile["G"]
236 |     g = npzfile["g"]
237 |     mus = npzfile["mus"]
238 |     nc = len(mus)
239 |     print("nc", nc)
240 |     G = torch.from_numpy(G).unsqueeze(0)
241 |     g = torch.from_numpy(g).unsqueeze(0)
242 |     mus = torch.from_numpy(mus).unsqueeze(0).unsqueeze(-1)
243 |     G.requires_grad_()
244 |     g.requires_grad_()
245 |     mus.requires_grad_()
246 | 
247 |     def compute_loss(G,g,mus,lam_tar):
248 |         max_iter = 200
249 |         eps = 1e-7
250 |         # solver = LCPLayer()
251 |         lam = ncp_layer(G,g,mus, max_iter, eps)
252 |         # print("lam : ", lam)
253 |         # print("lam_tar : ", lam_tar)
254 |         loss = 0.5*torch.square(lam- lam_tar).sum()
255 |         return loss
256 | 
257 | 
258 |     # lam = ccp_layer(G,g,mus, 1000, 1e-7)
259 |     # print("g", g)
260 |     # print("lam", lam)
261 |     # print("c", G @ lam + g)
262 |     with torch.no_grad():
263 |         mus2 = torch.square(mus*0.9)
264 |         print("mus^2 target", mus2)
265 |         lam_tar = ncp_layer(G,g,mus2, 1000, 1e-7).clone()
266 |     # lam_tar = torch.zeros_like(g)
267 |     loss = compute_loss(G, g, mus, lam_tar)
268 |     loss.backward()
269 | 
270 |     G2, g2, mus2 = G.clone(), g.clone(), mus.clone()
271 |     delta = 1e-8
272 |     for i in range(nc):
273 |         mus2[0,i,0] += delta
274 |         loss2 = compute_loss(G,g,mus2, lam_tar)
275 |         dl_dmusi = (loss2-loss).detach().numpy()/delta
276 |         # assert np.abs(dl_dmusi - mus.grad[0,i,0].detach().numpy())< 1e-3
277 |         mus2[0,i,0] -= delta
278 | 
279 |     for i in range(3*nc):
280 |         g2[0,i,0] += delta
281 |         loss2 = compute_loss(G,g2,mus, lam_tar)
282 |         dl_dgi = (loss2-loss).detach().numpy()/delta
283 |         # assert np.abs(dl_dgi - g.grad[0,i,0].detach().numpy())< 1e-3
284 |         g2[0,i,0] -= delta
285 | 
286 |     for i in range(3*nc):
287 |         for j in range(3*nc):
288 |             G2[0,i,j] += delta
289 |             loss2 = compute_loss(G2,g,mus, lam_tar)
290 |             dl_dGij = (loss2-loss).detach().numpy()/delta
291 |             # assert np.abs(dl_dGij - G.grad[0,i,j].detach().numpy())< 1e-3
292 |             G2[0,i,j] -= delta
293 | 
294 |     lr = 1e-3
295 |     # lr = 5e-4
296 |     optimizer = optimizer = torch.optim.Adam([mus], lr=lr)
297 |     print("init mus", mus)
298 |     G.requires_grad_(False)
299 |     g.requires_grad_(False)
300 |     lam_tar.requires_grad_(False)
301 |     loss = compute_loss(G, g, mus2, lam_tar)
302 |     print("init loss",loss.detach().numpy())
303 |     for i in range(200):
304 |         # print(i)
305 |         optimizer.zero_grad()
306 |         mus2 = torch.square(mus)
307 |         # print("mus2", mus2)
308 |         loss = compute_loss(G, g, mus2, lam_tar)
309 |         loss.backward()
310 |         optimizer.step()
311 |         print("mus",mus.detach().numpy())
312 |         # print("grad mus: ", mus.grad.detach().numpy())
313 |         # print("loss",loss.detach().numpy())
314 |     print("final mus", mus)
315 |     print("final loss",loss.detach().numpy())
316 |     print("lam : ", ncp_layer(G,g,torch.square(mus), 1000, 1e-7))
317 |     print("lam_tar : ", lam_tar)
318 | 
319 | 
320 | 
321 | 
322 | if __name__ == '__main__':
323 |     import sys
324 |     import pytest
325 |     # sys.exit(pytest.main(sys.argv))
326 |     # test_ccppgs_solver_backward()
327 |     test_ncppgs_solver_backward2()
328 | 


--------------------------------------------------------------------------------
/include/contactbench/contact-problem.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef CONTACT_BENCH_PROBLEM_H
  2 | #define CONTACT_BENCH_PROBLEM_H
  3 | 
  4 | #include <vector>
  5 | #include <Eigen/Core>
  6 | #include <contactbench/friction-constraint.hpp>
  7 | #include <contactbench/delassus-wrapper.hpp>
  8 | #include <contactbench/helpers.hpp>
  9 | namespace contactbench {
 10 | 
 11 | namespace {
 12 | using Eigen::Matrix;
 13 | using Eigen::MatrixBase;
 14 | 
 15 | } // namespace
 16 | 
 17 | using isize = Eigen::Index;
 18 | using usize = std::make_unsigned<isize>::type;
 19 | 
 20 | template <typename T, template <typename> class ConstraintTpl>
 21 | struct ContactProblem {
 22 |   using ConstraintType = ConstraintTpl<T>;
 23 |   static_assert(
 24 |       std::is_base_of<FrictionConstraint<T>, ConstraintType>::value,
 25 |       "ConstraintType should be inheritating from FrictionConstraint.");
 26 | 
 27 | public:
 28 |   CONTACTBENCH_EIGEN_TYPEDEFS(T);
 29 |   // physical quantities involved in the dual contact problem
 30 |   std::shared_ptr<DelassusBase<T>> Del_;
 31 |   VectorXs g_;
 32 |   // physical quantities involved in the primal contact problem
 33 |   MatrixXs M_, J_;
 34 |   VectorXs dqf_, vstar_, R_comp_;
 35 |   // friction constraints
 36 |   std::vector<ConstraintType> contact_constraints_;
 37 |   std::vector<PyramidCone<T>> linear_contact_constraints_;
 38 | 
 39 |   isize nc_, n_breaking_;
 40 | 
 41 |   // quantities necesary for the equivalent LCP
 42 |   MatrixXs A_, UD_;
 43 |   VectorXs b_;
 44 |   isize n_in_tan_, n_eq_tan_;
 45 | 
 46 |   ContactProblem(){};
 47 |   ContactProblem(const Ref<const MatrixXs> &G, const Ref<const VectorXs> &g,
 48 |                  const std::vector<T> &mus, const T comp = 0.);
 49 |   ContactProblem(const Ref<const MatrixXs> &G, const Ref<const VectorXs> &g,
 50 |                  const std::vector<T> &mus, const Ref<const VectorXs> &comp);
 51 |   ContactProblem(std::shared_ptr<DelassusBase<T>> Del,
 52 |                  const Ref<const VectorXs> &g, const std::vector<T> &mus,
 53 |                  const T comp = 0.);
 54 |   ContactProblem(std::shared_ptr<DelassusBase<T>> Del,
 55 |                  const Ref<const VectorXs> &g, const std::vector<T> &mus,
 56 |                  const Ref<const VectorXs> &comp);
 57 |   ContactProblem(const Ref<const MatrixXs> &M, const Ref<const MatrixXs> &J,
 58 |                  const Ref<const VectorXs> &dqf,
 59 |                  const Ref<const VectorXs> &vstar, const std::vector<T> &mus,
 60 |                  const T comp = 0.);
 61 |   ContactProblem(const Ref<const MatrixXs> &M, const Ref<const MatrixXs> &J,
 62 |                  const Ref<const VectorXs> &dqf,
 63 |                  const Ref<const VectorXs> &vstar, const std::vector<T> &mus,
 64 |                  const Ref<const VectorXs> &comp);
 65 |   ContactProblem(const Ref<const MatrixXs> &G, const Ref<const VectorXs> &g,
 66 |                  const Ref<const MatrixXs> &M, const Ref<const MatrixXs> &J,
 67 |                  const Ref<const VectorXs> &dqf,
 68 |                  const Ref<const VectorXs> &vstar, const std::vector<T> &mus,
 69 |                  const T comp = 0.);
 70 |   ContactProblem(const Ref<const MatrixXs> &G, const Ref<const VectorXs> &g,
 71 |                  const Ref<const MatrixXs> &M, const Ref<const MatrixXs> &J,
 72 |                  const Ref<const VectorXs> &dqf,
 73 |                  const Ref<const VectorXs> &vstar, const std::vector<T> &mus,
 74 |                  const Ref<const VectorXs> &comp);
 75 |   ContactProblem(std::shared_ptr<DelassusBase<T>> Del,
 76 |                  const Ref<const VectorXs> &g, const Ref<const MatrixXs> &M,
 77 |                  const Ref<const MatrixXs> &J, const Ref<const VectorXs> &dqf,
 78 |                  const Ref<const VectorXs> &vstar, const std::vector<T> &mus,
 79 |                  const T comp = 0.);
 80 |   ContactProblem(std::shared_ptr<DelassusBase<T>> Del,
 81 |                  const Ref<const VectorXs> &g, const Ref<const MatrixXs> &M,
 82 |                  const Ref<const MatrixXs> &J, const Ref<const VectorXs> &dqf,
 83 |                  const Ref<const VectorXs> &vstar, const std::vector<T> &mus,
 84 |                  const Ref<const VectorXs> &comp);
 85 | 
 86 |   void setCompliance(const T comp);
 87 | 
 88 |   void setCompliance(const Ref<const VectorXs> &comp);
 89 | 
 90 |   template <typename VecIn, typename VecOut>
 91 |   static void project(const std::vector<T> &mus,
 92 |                       const Eigen::MatrixBase<VecIn> &lam,
 93 |                       const Eigen::MatrixBase<VecOut> &lam_out);
 94 | 
 95 |   template <typename VecIn, typename VecOut>
 96 |   void project(const Eigen::MatrixBase<VecIn> &lam,
 97 |                const Eigen::MatrixBase<VecOut> &lam_out) const;
 98 | 
 99 |   template <typename VecIn, typename VecOut>
100 |   void projectDual(const Eigen::MatrixBase<VecIn> &v,
101 |                    const Eigen::MatrixBase<VecOut> &v_out) const;
102 | 
103 |   bool isInside(const Ref<const VectorXs> &lam, T thresh) const;
104 | 
105 |   bool isInsideDual(const Ref<const VectorXs> &v, T thresh) const;
106 | 
107 |   template <typename VecIn, typename VecOut>
108 |   void computeVelocity(const Eigen::MatrixBase<VecIn> &lam,
109 |                        const Eigen::MatrixBase<VecOut> &v_out_) const;
110 | 
111 |   void computeDeSaxceCorrection(const Ref<const VectorXs> &v,
112 |                                 VectorRef v_out) const;
113 | 
114 |   static T computeComplementarity(const Ref<const VectorXs> &lam,
115 |                                   const Ref<const VectorXs> &v);
116 | 
117 |   T computeContactComplementarity(const Ref<const VectorXs> &lam);
118 | 
119 |   T computeContactComplementarity(const Ref<const VectorXs> &lam,
120 |                                   const Ref<const VectorXs> &v) const;
121 | 
122 |   T computePerContactContactComplementarity(usize i, const Vector3s &lam,
123 |                                             const Vector3s &v) const;
124 | 
125 |   T computeSignoriniComplementarity(const Ref<const VectorXs> &lam);
126 | 
127 |   T computeSignoriniComplementarity(const Ref<const VectorXs> &lam,
128 |                                     const Ref<const VectorXs> &v) const;
129 | 
130 |   T computePerContactSignoriniComplementarity(usize i, const Vector3s &lam,
131 |                                               const Vector3s &v) const;
132 | 
133 |   T computeConicComplementarity(const Ref<const VectorXs> &lam);
134 | 
135 |   T computeConicComplementarity(const Ref<const VectorXs> &lam,
136 |                                 const Ref<const VectorXs> &v) const;
137 | 
138 |   T computePerContactConicComplementarity(usize i, const Vector3s &lam,
139 |                                           const Vector3s &v) const;
140 | 
141 |   T computeLinearComplementarity(const Ref<const VectorXs> &lam);
142 | 
143 |   T computeLinearComplementarity(const Ref<const VectorXs> &lam,
144 |                                  const Ref<const VectorXs> &v);
145 | 
146 |   void setLCP();
147 | 
148 |   void computeLCP(const T eps_reg = 0.);
149 | 
150 |   void computeLCP(const Ref<const VectorXs> &R_reg);
151 | 
152 |   void computeTangentLCP(const Ref<const VectorXs> &lam,
153 |                          const Ref<const VectorXs> &R_reg);
154 | 
155 |   void computeTangentLCP(const Ref<const VectorXs> &lam, const T esp_reg = 0.);
156 | 
157 |   void computeInscribedLCP(const Ref<const VectorXs> &lam,
158 |                            const Ref<const VectorXs> &R_reg);
159 | 
160 |   void computeInscribedLCP(const Ref<const VectorXs> &lam,
161 |                            const T eps_reg = 0.);
162 | 
163 |   void setLCCP();
164 | 
165 |   void computeLCCP();
166 | 
167 |   void computeTangentLCCP(const Ref<const VectorXs> &lam);
168 | 
169 |   void computeInscribedLCCP(const Ref<const VectorXs> &lam);
170 | 
171 |   void computeLCPSolution(const Ref<const VectorXs> &lam,
172 |                           const Ref<const VectorXs> &c,
173 |                           VectorRef lam_lcp) const;
174 | 
175 |   void computeLCPSolution(const Ref<const VectorXs> &lam, VectorRef lam_lcp);
176 | 
177 |   void computeLCPdLdg(const Ref<const VectorXs> &dL_db, VectorRef dL_dg);
178 |   void computeLCPdLdDel(const Ref<const MatrixXs> &dL_dA, MatrixRef dL_dDel);
179 |   void computeLCPdLdmus(const Ref<const MatrixXs> &dL_dA, VectorRef dL_dmus);
180 | 
181 |   void computeLCPdbdtheta(const Ref<const MatrixXs> &dg_dtheta,
182 |                           MatrixRef db_dtheta);
183 | 
184 |   void computeLCPdAdtheta(const Ref<const MatrixXs> &dmus_dtheta,
185 |                           const Ref<const MatrixXs> &dG_dtheta,
186 |                           const Ref<const VectorXs> &lam, MatrixRef mat_out);
187 | 
188 |   void computeInscribedLCPdLdmus(const Ref<const MatrixXs> &dL_dA,
189 |                                  VectorRef dL_dmus);
190 |   void computeTangentLCPdLdmus(const Ref<const MatrixXs> &dL_dA,
191 |                                VectorRef dL_dmus);
192 | 
193 |   void computeLCCPdLdmus(const Ref<const MatrixXs> &dL_dC, VectorRef dL_dmus);
194 | 
195 |   void computeInscribedLCCPdLdmus(const Ref<const MatrixXs> &dL_dC,
196 |                                   VectorRef dL_dmus);
197 | 
198 |   void computeTangentLCCPdLdmus(const Ref<const MatrixXs> &dL_dC,
199 |                                 VectorRef dL_dmus);
200 | 
201 |   const Ref<const MatrixXs> getLinearInConstraintMatrix() const;
202 | 
203 |   const Ref<const MatrixXs> getLinearEqConstraintMatrix() const;
204 | 
205 |   template <typename VecIn, typename VecOut>
206 |   void computeVelInTangentBasis(const MatrixBase<VecIn> &v,
207 |                                 const MatrixBase<VecOut> &v_out) const;
208 | 
209 |   template <typename VecIn, typename VecOut>
210 |   void computeVelInInscribedBasis(const MatrixBase<VecIn> &v,
211 |                                   const MatrixBase<VecOut> &v_out) const;
212 | 
213 |   template <typename VecIn, typename VecIn2, typename VecOut, typename VecOut2>
214 |   void computeDualSolutionOfTangent(const MatrixBase<VecIn> &lam,
215 |                                     const MatrixBase<VecIn2> &v,
216 |                                     const MatrixBase<VecOut> &y_out,
217 |                                     const MatrixBase<VecOut2> &z_out,
218 |                                     const double eps) const;
219 | 
220 |   template <typename VecIn, typename VecIn2, typename VecOut, typename VecOut2>
221 |   void computeDualSolutionOfInscribed(const MatrixBase<VecIn> &lam,
222 |                                       const MatrixBase<VecIn2> &v,
223 |                                       const MatrixBase<VecOut> &y_out,
224 |                                       const MatrixBase<VecOut2> &z_out,
225 |                                       const double eps) const;
226 | 
227 |   template <typename NewScalar>
228 |   ContactProblem<NewScalar, ConstraintTpl> cast() const {
229 |     using NewContactProblem = ContactProblem<NewScalar, ConstraintTpl>;
230 |     NewContactProblem newProblem;
231 | 
232 |     newProblem.nc_ = nc_;
233 |     newProblem.n_breaking_ = n_breaking_;
234 |     newProblem.n_in_tan_ = n_in_tan_;
235 |     newProblem.n_eq_tan_ = n_eq_tan_;
236 | 
237 |     newProblem.contact_constraints_.reserve(contact_constraints_.size());
238 |     for (const auto &constraint : contact_constraints_) {
239 |       newProblem.contact_constraints_.push_back(
240 |           constraint.template cast<NewScalar>());
241 |     }
242 | 
243 |     newProblem.g_ = g_.template cast<NewScalar>();
244 |     newProblem.dqf_ = dqf_.template cast<NewScalar>();
245 |     newProblem.vstar_ = vstar_.template cast<NewScalar>();
246 |     newProblem.R_comp_ = R_comp_.template cast<NewScalar>();
247 |     newProblem.M_ = M_.template cast<NewScalar>();
248 |     newProblem.J_ = J_.template cast<NewScalar>();
249 |     newProblem.UD_ = UD_.template cast<NewScalar>();
250 |     newProblem.A_ = A_.template cast<NewScalar>();
251 |     newProblem.b_ = b_.template cast<NewScalar>();
252 | 
253 |     return newProblem;
254 |   }
255 | 
256 | protected:
257 |   VectorXs v_, v_comp_, v_tmp_;
258 |   MatrixXs GUD_, G_reg_, E_, F_, H_, C_in_, C_eq_;
259 |   MatrixXs dL_dDel_t_;
260 | 
261 |   void computeAb_(const T eps_reg = 0.);
262 |   void computeAb_(const Ref<const VectorXs> &R_reg);
263 |   void computeC_();
264 |   VectorXs lam_lcp_, v_lcp_;
265 | };
266 | 
267 | } // end namespace contactbench
268 | 
269 | #endif
270 | 
271 | #include "contactbench/contact-problem.hxx"
272 | 
273 | #if CONTACTBENCH_ENABLE_TEMPLATE_INSTANTIATION
274 | #include "contactbench/contact-problem.txx"
275 | #endif // CONTACTBENCH_ENABLE_TEMPLATE_INSTANTIATION
276 | 


--------------------------------------------------------------------------------
/include/contactbench/friction-constraint.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef CONTACT_BENCH_FRICTION_H
  2 | #define CONTACT_BENCH_FRICTION_H
  3 | 
  4 | #include <Eigen/Core>
  5 | #include <cmath>
  6 | #include "contactbench/fwd.hpp"
  7 | namespace contactbench {
  8 | 
  9 | template <typename T> struct FrictionConstraint {
 10 | 
 11 |   int n_cons_;
 12 | 
 13 |   FrictionConstraint(){};
 14 | 
 15 |   template <typename VecIn, typename VecOut>
 16 |   static void
 17 |   project(CONTACTBENCH_MAYBE_UNUSED const T &mu,
 18 |           CONTACTBENCH_MAYBE_UNUSED const Eigen::MatrixBase<VecIn> &x,
 19 |           CONTACTBENCH_MAYBE_UNUSED const Eigen::MatrixBase<VecOut> &x_out_) {
 20 |     CONTACTBENCH_RUNTIME_ERROR("Unimplemented.");
 21 |   }
 22 | 
 23 |   template <typename VecIn, typename VecOut>
 24 |   void project(
 25 |       CONTACTBENCH_MAYBE_UNUSED const Eigen::MatrixBase<VecIn> &x,
 26 |       CONTACTBENCH_MAYBE_UNUSED const Eigen::MatrixBase<VecOut> &x_out_) const {
 27 |     CONTACTBENCH_RUNTIME_ERROR("Unimplemented.");
 28 |   }
 29 | 
 30 |   template <typename NewScalar> FrictionConstraint<NewScalar> cast() const {
 31 |     FrictionConstraint<NewScalar> newConstraint;
 32 |     newConstraint.n_cons_ = n_cons_;
 33 |     return newConstraint;
 34 |   }
 35 | };
 36 | 
 37 | template <typename T> struct PyramidCone : FrictionConstraint<T> {
 38 |   CONTACTBENCH_EIGEN_TYPEDEFS(T);
 39 | 
 40 |   int n_cons_;
 41 |   T mu_;
 42 |   int n_facets_ = 4;
 43 |   Matrix2s Uxy_;
 44 |   Vector4s e_;
 45 |   Matrix24s D_, UD_;
 46 |   Matrix36s D_lcp_, UD_lcp_;
 47 | 
 48 |   PyramidCone() { n_cons_ = 3; };
 49 |   PyramidCone(T mu) {
 50 |     n_cons_ = 3;
 51 |     mu_ = mu;
 52 |     e_.setOnes();
 53 |     D_ << 1., -1., -1., 1., 1., 1., -1., -1.;
 54 |     D_ /= std::sqrt(2);
 55 |     D_lcp_.setZero();
 56 |     D_lcp_(2, 4) = 1.;
 57 |     D_lcp_.template block<2, 4>(0, 0) = D_;
 58 |     Uxy_ = Matrix2s::Identity();
 59 |     UD_ = Uxy_ * D_;
 60 |     UD_lcp_.setZero();
 61 |     UD_lcp_(2, 4) = 1.;
 62 |     UD_lcp_.template block<2, 4>(0, 0) = UD_;
 63 |   }
 64 |   template <typename VecIn>
 65 |   PyramidCone(T mu, const Eigen::MatrixBase<VecIn> &ux) : PyramidCone(mu) {
 66 |     if (ux.norm() <= 0) {
 67 |       Uxy_.setIdentity();
 68 |     } else {
 69 |       Uxy_.col(0) = ux;
 70 |       Uxy_.col(0).normalize();
 71 |       Uxy_(0, 1) = -Uxy_(1, 0);
 72 |       Uxy_(1, 1) = Uxy_(0, 0);
 73 |     }
 74 |     UD_ = Uxy_ * D_;
 75 |     UD_lcp_.setZero();
 76 |     UD_lcp_(2, 4) = 1.;
 77 |     UD_lcp_.template block<2, 4>(0, 0) = UD_;
 78 |   }
 79 | 
 80 |   template <typename VecIn>
 81 |   void setOrientation(const Eigen::MatrixBase<VecIn> &ux);
 82 | 
 83 |   template <typename VecIn, typename VecOut>
 84 |   void computeCoord(const Eigen::MatrixBase<VecIn> &x,
 85 |                     const Eigen::MatrixBase<VecOut> &x_out) const;
 86 | 
 87 |   template <typename VecIn, typename VecOut>
 88 |   void project(const Eigen::MatrixBase<VecIn> &x,
 89 |                const Eigen::MatrixBase<VecOut> &x_out) const;
 90 | 
 91 |   template <typename VecIn, typename VecOut>
 92 |   void projectDual(const Eigen::MatrixBase<VecIn> &x,
 93 |                    const Eigen::MatrixBase<VecOut> &x_out) const;
 94 | 
 95 |   template <typename VecIn>
 96 |   bool isInside(const Eigen::MatrixBase<VecIn> &x, T thresh = 1e-3) const;
 97 | 
 98 |   template <typename VecIn>
 99 |   bool isInsideDual(const Eigen::MatrixBase<VecIn> &x, T thresh = 1e-3) const;
100 | 
101 |   template <typename VecIn, typename VecOut>
102 |   void projectHorizontal(const Eigen::MatrixBase<VecIn> &x,
103 |                          const Eigen::MatrixBase<VecOut> &x_out_) const;
104 | 
105 |   template <typename VecIn, typename VecOut>
106 |   void projectHorizontal(const Eigen::MatrixBase<VecIn> &x, double x_n,
107 |                          const Eigen::MatrixBase<VecOut> &x_out_) const;
108 | 
109 |   template <typename VecIn, typename VecIn2>
110 |   T computeConicComplementarity(const Eigen::MatrixBase<VecIn> &lam,
111 |                                 const Eigen::MatrixBase<VecIn2> &v) const;
112 | 
113 |   template <typename VecIn, typename VecIn2>
114 |   T computeSignoriniComplementarity(const Eigen::MatrixBase<VecIn> &lam,
115 |                                     const Eigen::MatrixBase<VecIn2> &v) const;
116 | 
117 |   template <typename VecIn, typename VecOut>
118 |   void computeDeSaxceCorrection(const Eigen::MatrixBase<VecIn> &v,
119 |                                 const Eigen::MatrixBase<VecOut> &v_out_) const;
120 | 
121 |   template <typename VecIn, typename VecIn2>
122 |   T computeContactComplementarity(const Eigen::MatrixBase<VecIn> &lam,
123 |                                   const Eigen::MatrixBase<VecIn2> &v) const;
124 | 
125 |   template <typename VecIn, typename VecIn2>
126 |   T computeLinearComplementarity(const Eigen::MatrixBase<VecIn> &lam,
127 |                                  const Eigen::MatrixBase<VecIn2> &v) const;
128 | 
129 |   template <typename VecIn, typename VecOut>
130 |   void computeCoordinatesInD(const Eigen::MatrixBase<VecIn> &x,
131 |                              const Eigen::MatrixBase<VecOut> &x_out) const;
132 | 
133 |   template <typename MatOut>
134 |   void computeRotMat2D(const T theta,
135 |                        const Eigen::MatrixBase<MatOut> &R_out) const;
136 | 
137 |   // template <typename VecIn>
138 |   // void computeTangentPyramid(const Eigen::MatrixBase<VecIn> &x) {}
139 | 
140 |   // template <typename VecIn>
141 |   // void computeInscribedPyramid(const Eigen::MatrixBase<VecIn> &x) {}
142 | 
143 |   // const double &getTangentPyramidFriction() const { return mu_; }
144 | 
145 |   // const double &getInscribedPyramidFriction() const { return mu_; }
146 | 
147 |   // const Matrix36s &getLCPTangentBasisMatrix() const { return UD_lcp_; }
148 | 
149 |   // const Matrix36s &getLCPInscribedBasisMatrix() const { return UD_lcp_; }
150 | 
151 |   bool operator==(const PyramidCone &rhs) const { return mu_ == rhs.mu_; }
152 | 
153 |   template <typename NewScalar> PyramidCone<NewScalar> cast() const {
154 |     PyramidCone<NewScalar> newCone;
155 | 
156 |     newCone.n_cons_ = n_cons_;
157 |     newCone.mu_ = static_cast<NewScalar>(mu_);
158 |     newCone.n_facets_ = n_facets_;
159 |     newCone.Uxy_ = Uxy_.template cast<NewScalar>();
160 |     newCone.e_ = e_.template cast<NewScalar>();
161 |     newCone.D_ = D_.template cast<NewScalar>();
162 |     newCone.D_lcp_ = D_lcp_.template cast<NewScalar>();
163 |     newCone.UD_ = UD_.template cast<NewScalar>();
164 |     newCone.Uxy_ = Uxy_.template cast<NewScalar>();
165 |     newCone.UD_lcp_ = UD_lcp_.template cast<NewScalar>();
166 | 
167 |     return newCone;
168 |   }
169 | 
170 | protected:
171 |   Matrix2s D_red_inv_;
172 | };
173 | 
174 | template <typename T> struct IceCreamCone : FrictionConstraint<T> {
175 |   CONTACTBENCH_EIGEN_TYPEDEFS(T);
176 | 
177 |   int n_cons_;
178 |   T mu_;
179 |   // PyramidCone<T> tangentPyramid_;
180 |   // PyramidCone<T> inscribedPyramid_;
181 |   // Vector4s e_;
182 | 
183 |   IceCreamCone() { n_cons_ = 3; };
184 |   IceCreamCone(T mu) {
185 |     mu_ = mu;
186 |     n_cons_ = 3;
187 |     // tangentPyramid_ = PyramidCone<T>(mu);
188 |     // inscribedPyramid_ = PyramidCone<T>(mu / std::sqrt(2.));
189 |     // e_ = tangentPyramid_.e_;
190 |   }
191 | 
192 |   template <typename VecIn, typename VecOut>
193 |   static void project(const T &mu, const Eigen::MatrixBase<VecIn> &x,
194 |                       const Eigen::MatrixBase<VecOut> &x_out);
195 | 
196 |   template <typename VecIn, typename VecOut>
197 |   void project(const Eigen::MatrixBase<VecIn> &x,
198 |                const Eigen::MatrixBase<VecOut> &x_out) const;
199 | 
200 |   template <typename VecIn, typename VecOut>
201 |   void projectDual(const Eigen::MatrixBase<VecIn> &x,
202 |                    const Eigen::MatrixBase<VecOut> &x_out) const;
203 | 
204 |   template <typename VecIn>
205 |   static bool isInside(const T &mu, const Eigen::MatrixBase<VecIn> &x,
206 |                        T thresh = 1e-6);
207 | 
208 |   template <typename VecIn>
209 |   bool isInside(const Eigen::MatrixBase<VecIn> &x, T thresh = 1e-3) const;
210 | 
211 |   template <typename VecIn>
212 |   bool isInsideDual(const Eigen::MatrixBase<VecIn> &x, T thresh = 1e-3) const;
213 | 
214 |   template <typename VecIn>
215 |   static bool isOnBorder(const T &mu, const Eigen::MatrixBase<VecIn> &x,
216 |                          T thresh = 1e-3);
217 | 
218 |   template <typename VecIn>
219 |   bool isOnBorder(const Eigen::MatrixBase<VecIn> &x, T thresh = 1e-3) const;
220 | 
221 |   template <typename VecIn>
222 |   bool isOnBorderDual(const Eigen::MatrixBase<VecIn> &x, T thresh = 1e-3) const;
223 | 
224 |   template <typename VecIn, typename VecOut>
225 |   static void projectHorizontal(const T &mu, const Eigen::MatrixBase<VecIn> &x,
226 |                                 const Eigen::MatrixBase<VecOut> &x_out_);
227 | 
228 |   template <typename VecIn, typename VecOut>
229 |   void projectHorizontal(const Eigen::MatrixBase<VecIn> &x,
230 |                          const Eigen::MatrixBase<VecOut> &x_out_) const;
231 | 
232 |   template <typename VecIn, typename VecOut>
233 |   void projectHorizontal(const Eigen::MatrixBase<VecIn> &x, double x_n,
234 |                          const Eigen::MatrixBase<VecOut> &x_out_) const;
235 | 
236 |   template <typename VecIn, typename VecIn2>
237 |   T computeConicComplementarity(const Eigen::MatrixBase<VecIn> &lam,
238 |                                 const Eigen::MatrixBase<VecIn2> &v) const;
239 | 
240 |   template <typename VecIn, typename VecIn2>
241 |   T computeSignoriniComplementarity(const Eigen::MatrixBase<VecIn> &lam,
242 |                                     const Eigen::MatrixBase<VecIn2> &v) const;
243 | 
244 |   template <typename VecIn, typename VecOut>
245 |   void computeDeSaxceCorrection(const Eigen::MatrixBase<VecIn> &v,
246 |                                 const Eigen::MatrixBase<VecOut> &v_out_) const;
247 | 
248 |   template <typename VecIn, typename VecIn2>
249 |   T computeContactComplementarity(const Eigen::MatrixBase<VecIn> &lam,
250 |                                   const Eigen::MatrixBase<VecIn2> &v) const;
251 | 
252 |   bool operator==(const IceCreamCone &rhs) const { return mu_ == rhs.mu_; }
253 | 
254 |   // template <typename VecIn>
255 |   // void computeTangentPyramid(const Eigen::MatrixBase<VecIn> &lam);
256 | 
257 |   // template <typename VecIn>
258 |   // void computeInscribedPyramid(const Eigen::MatrixBase<VecIn> &lam);
259 | 
260 |   // template <typename VecIn, typename VecOut>
261 |   // void computeVelInTangentBasis(const Eigen::MatrixBase<VecIn> &v,
262 |   //                               const Eigen::MatrixBase<VecOut> &v_out)
263 |   //                               const;
264 | 
265 |   // template <typename VecIn, typename VecOut>
266 |   // void computeVelInInscribedBasis(const Eigen::MatrixBase<VecIn> &v,
267 |   //                                 const Eigen::MatrixBase<VecOut> &v_out)
268 |   //                                 const;
269 | 
270 |   // const double &getTangentPyramidFriction() const {
271 |   //   return tangentPyramid_.mu_;
272 |   // }
273 | 
274 |   // const double &getInscribedPyramidFriction() const {
275 |   //   return inscribedPyramid_.mu_;
276 |   // }
277 | 
278 |   // const Matrix2s &getTangentBasisMatrix() const;
279 | 
280 |   // const Matrix2s &getInscribedBasisMatrix() const;
281 | 
282 |   // const Matrix36s &getLCPTangentBasisMatrix() const;
283 | 
284 |   // const Matrix36s &getLCPInscribedBasisMatrix() const;
285 | 
286 |   template <typename NewScalar> IceCreamCone<NewScalar> cast() const {
287 |     IceCreamCone<NewScalar> newCone;
288 | 
289 |     newCone.n_cons_ = n_cons_;
290 |     newCone.mu_ = static_cast<NewScalar>(mu_);
291 | 
292 |     return newCone;
293 |   }
294 | 
295 |   // protected:
296 |   //   Matrix2s D_tan_;
297 |   //   Matrix2s D_ins_;
298 | };
299 | 
300 | template <typename T> struct Box : FrictionConstraint<T> {
301 |   CONTACTBENCH_EIGEN_TYPEDEFS(T);
302 | 
303 |   int n_cons_;
304 | 
305 |   VectorXs mus_;
306 | 
307 |   Box(){};
308 |   Box(VectorXs mus) {
309 |     mus_ = mus;
310 |     n_cons_ = mus_.size();
311 |   }
312 | 
313 |   template <typename VecIn, typename VecOut>
314 |   void project(const Eigen::MatrixBase<VecIn> &x,
315 |                const Eigen::MatrixBase<VecOut> &x_out) const;
316 | 
317 |   template <typename VecIn>
318 |   bool isInside(const Eigen::MatrixBase<VecIn> &x, T thresh = 1e-3) const;
319 | 
320 |   bool operator==(const Box &rhs) { return mus_ == rhs.mus_; }
321 | 
322 |   template <typename NewScalar> Box<NewScalar> cast() const {
323 |     Box<NewScalar> newBox;
324 | 
325 |     newBox.n_cons_ = n_cons_;
326 |     newBox.mus_ = mus_.template cast<NewScalar>();
327 | 
328 |     return newBox;
329 |   }
330 | };
331 | 
332 | } // end namespace contactbench
333 | 
334 | #include "contactbench/friction-constraint.hxx"
335 | 
336 | #endif
337 | 


--------------------------------------------------------------------------------