plainmp 0.0.1__tar.gz

plainmp-0.0.1/.flake8

@@ -0,0 +1,8 @@
+[flake8]
+ignore = 
+    I, H
+    E501
+    E203
+    E741
+    W503
+    A, B, C, CNL, D, Q

plainmp-0.0.1/.gitignore

@@ -0,0 +1,165 @@
+compile_commands.json
+/pybind11
+/build
+/_skbuild
+/dist
+*.egg/
+*.egg-info/
+action.sh
+
+# other stuff
+*.new
+*.bak
+
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/

plainmp-0.0.1/.gitmodules

@@ -0,0 +1,9 @@
+[submodule "tinyfk"]
+	path = tinyfk
+	url = git@github.com:HiroIshida/tinyfk.git
+[submodule "primitive_sdf"]
+	path = primitive_sdf
+	url = git@github.com:HiroIshida/primitive_sdf.git
+[submodule "pybind11"]
+	path = pybind11
+	url = git@github.com:pybind/pybind11.git

plainmp-0.0.1/CMakeLists.txt

@@ -0,0 +1,22 @@
+set(CMAKE_CXX_STANDARD 17)
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif(NOT CMAKE_BUILD_TYPE)
+set(CMAKE_CXX_FLAGS " ${CMAKE_CXX_FLAGS_INIT} -Wall -fPIC")
+
+find_package(Eigen3 REQUIRED)
+ADD_DEFINITIONS(-DEIGEN_NO_DEBUG)
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+option(BUILD_PYTHON_INTERFACE OFF)
+add_subdirectory("tinyfk")
+include_directories("tinyfk/include")
+include_directories("primitive_sdf")
+# add_executable(main main.cpp)
+# target_link_libraries(main tinyfk)
+
+file(GLOB_RECURSE SRC_FILES cpp/*.cpp)
+add_subdirectory(pybind11)
+pybind11_add_module(_plainmp ${SRC_FILES})
+target_link_libraries(_plainmp PRIVATE tinyfk)
+install(TARGETS _plainmp DESTINATION .)

plainmp-0.0.1/PKG-INFO

@@ -0,0 +1,10 @@
+Metadata-Version: 1.0
+Name: plainmp
+Version: 0.0.1
+Summary: experimental
+Home-page: UNKNOWN
+Author: Hirokazu Ishida
+Author-email: UNKNOWN
+License: MIT
+Description: UNKNOWN
+Platform: UNKNOWN

plainmp-0.0.1/cpp/bindings.cpp

@@ -0,0 +1,11 @@
+#include <pybind11/pybind11.h>
+#include "constraint.hpp"
+#include "third/primitive_sdf_binding.hpp"
+#include "third/tinyfk_binding.hpp"
+namespace py = pybind11;
+
+PYBIND11_MODULE(_plainmp, m) {
+  primitive_sdf::bind_primitive_sdf(m);
+  cst::bind_collision_constraints(m);
+  tinyfk::bind_tinyfk(m);
+}

plainmp-0.0.1/cpp/composite_constraint.hpp

@@ -0,0 +1,85 @@
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <algorithm>
+#include <memory>
+#include <tinyfk.hpp>
+#include <utility>
+#include "constraint.hpp"
+#include "primitive_sdf.hpp"
+
+namespace cst {
+template <typename T>
+class CompositeConstraintBase {
+ public:
+  using Ptr = std::shared_ptr<CompositeConstraintBase>;
+  CompositeConstraintBase(std::vector<T> constraints)
+      : constraints_(constraints) {
+    // all constraints must have the same kinematic chain
+    // otherwise, the jacobian will be wrong
+    for (auto cst : constraints_) {
+      if (cst->kin_ != constraints_.front()->kin_) {
+        throw std::runtime_error(
+            "All constraints must have the same kinematic chain");
+      }
+    }
+  }
+
+  void update_kintree(const std::vector<double>& q) {
+    constraints_.front()->update_kintree(q);
+  }
+
+  std::pair<Eigen::VectorXd, Eigen::MatrixXd> evaluate(
+      const std::vector<double>& q) {
+    update_kintree(q);
+
+    size_t dim = this->cst_dim();
+    Eigen::VectorXd vals(dim);
+    Eigen::MatrixXd jac(dim, q_dim());
+    size_t head = 0;
+    for (const auto& cst : constraints_) {
+      size_t dim_local = cst->cst_dim();
+      auto [vals_sub, jac_sub] = cst->evaluate_dirty();
+      vals.segment(head, dim_local) = vals_sub;
+      jac.block(head, 0, dim_local, q_dim()) = jac_sub;
+      head += dim_local;
+    }
+    return {vals, jac};
+  }
+
+  size_t q_dim() const { return constraints_.front()->q_dim(); }
+
+  size_t cst_dim() const {
+    return std::accumulate(
+        constraints_.begin(), constraints_.end(), 0,
+        [](size_t sum, const T& cst) { return sum + cst->cst_dim(); });
+  }
+  std::vector<T> constraints_;
+};
+
+class EqCompositeCst : public CompositeConstraintBase<EqConstraintBase::Ptr> {
+ public:
+  using Ptr = std::shared_ptr<EqCompositeCst>;
+  using CompositeConstraintBase::CompositeConstraintBase;
+  size_t cst_dim() const;
+  bool is_equality() const { return true; }
+};
+
+class IneqCompositeCst
+    : public CompositeConstraintBase<IneqConstraintBase::Ptr> {
+ public:
+  using Ptr = std::shared_ptr<IneqCompositeCst>;
+  using CompositeConstraintBase::CompositeConstraintBase;
+  bool is_valid(const std::vector<double>& q) {
+    update_kintree(q);
+    for (const auto& cst : constraints_) {
+      if (!cst->is_valid_dirty())
+        return false;
+    }
+    return true;
+  }
+  size_t cst_dim() const;
+  bool is_equality() const { return false; }
+};
+}  // namespace cst

plainmp-0.0.1/cpp/constraint.cpp

@@ -0,0 +1,329 @@
+#include "constraint.hpp"
+#include <pybind11/stl.h>
+#include <optional>
+#include <stdexcept>
+#include "composite_constraint.hpp"
+#include "primitive_sdf.hpp"
+#include "tinyfk.hpp"
+
+namespace cst {
+
+namespace py = pybind11;
+
+std::pair<Eigen::VectorXd, Eigen::MatrixXd> LinkPoseCst::evaluate_dirty()
+    const {
+  Eigen::VectorXd vals(cst_dim());
+  Eigen::MatrixXd jac(cst_dim(), q_dim());
+  tinyfk::Transform pose;
+  size_t head = 0;
+  for (size_t i = 0; i < link_ids_.size(); i++) {
+    kin_->get_link_pose(link_ids_[i], pose);
+    if (poses_[i].size() == 3) {
+      vals.segment(head, 3) =
+          Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z) -
+          poses_[i];
+      jac.block(head, 0, 3, q_dim()) =
+          kin_->get_jacobian(link_ids_[i], control_joint_ids_,
+                             tinyfk::RotationType::IGNORE, with_base_);
+      head += 3;
+    } else {
+      vals.segment(head, 3) =
+          Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z) -
+          poses_[i];
+      auto rpy = pose.rotation.getRPY();
+      vals.segment(head + 3, 3) =
+          Eigen::Vector3d(rpy.x, rpy.y, rpy.z) - poses_[i].segment(3, 3);
+      jac.block(head, 0, 6, q_dim()) =
+          kin_->get_jacobian(link_ids_[i], control_joint_ids_,
+                             tinyfk::RotationType::RPY, with_base_);
+      head += 6;
+    }
+  }
+  return {vals, jac};
+}
+
+SphereCollisionCst::SphereCollisionCst(
+    std::shared_ptr<tinyfk::KinematicModel> kin,
+    const std::vector<std::string>& control_joint_names,
+    bool with_base,
+    const std::vector<SphereAttachentSpec>& sphere_specs,
+    const std::vector<std::pair<std::string, std::string>>& selcol_pairs,
+    const std::vector<primitive_sdf::PrimitiveSDFBase::Ptr>& fixed_sdfs)
+    : IneqConstraintBase(kin, control_joint_names, with_base),
+      sphere_specs_(sphere_specs),
+      fixed_sdfs_(fixed_sdfs) {
+  std::vector<std::string> parent_link_names;
+  for (const auto& spec : sphere_specs) {
+    auto parent_id = kin_->get_link_ids({spec.parent_link_name})[0];
+    auto name = "sphere" + std::to_string(sphere_ids_.size());
+    kin_->add_new_link(name, parent_id,
+                       {spec.relative_position.x(), spec.relative_position.y(),
+                        spec.relative_position.z()},
+                       {0.0, 0.0, 0.0});
+    sphere_ids_.push_back(kin_->get_link_ids({name})[0]);
+    parent_link_names.push_back(spec.parent_link_name);
+  }
+  std::vector<std::pair<size_t, size_t>> selcol_pairs_ids;
+  for (const auto& pair : selcol_pairs) {
+    std::vector<size_t> first_ids;
+    std::vector<size_t> second_ids;
+    for (size_t i = 0; i < parent_link_names.size(); i++) {
+      if (parent_link_names[i] == pair.first) {
+        first_ids.push_back(i);
+      }
+      if (parent_link_names[i] == pair.second) {
+        second_ids.push_back(i);
+      }
+    }
+    for (auto& first_id : first_ids) {
+      for (auto& second_id : second_ids) {
+        selcol_pairs_ids.push_back({first_id, second_id});
+      }
+    }
+  }
+  selcol_pairs_ids_ = selcol_pairs_ids;
+}
+
+bool SphereCollisionCst::is_valid_dirty() const {
+  tinyfk::Transform pose;
+  for (size_t i = 0; i < sphere_ids_.size(); i++) {
+    if (sphere_specs_[i].ignore_collision) {
+      continue;
+    }
+    kin_->get_link_pose(sphere_ids_[i], pose);
+    Eigen::Vector3d center(pose.position.x, pose.position.y, pose.position.z);
+    for (auto& sdf : get_all_sdfs()) {
+      if (!sdf->is_outside(center, sphere_specs_[i].radius)) {
+        return false;
+      }
+    }
+  }
+  tinyfk::Transform pose1, pose2;
+  for (const auto& pair : selcol_pairs_ids_) {
+    kin_->get_link_pose(sphere_ids_[pair.first], pose1);
+    kin_->get_link_pose(sphere_ids_[pair.second], pose2);
+    Eigen::Vector3d center1(pose1.position.x, pose1.position.y,
+                            pose1.position.z);
+    Eigen::Vector3d center2(pose2.position.x, pose2.position.y,
+                            pose2.position.z);
+    if ((center1 - center2).norm() <
+        sphere_specs_[pair.first].radius + sphere_specs_[pair.second].radius) {
+      return false;
+    }
+  }
+  return true;
+}
+std::pair<Eigen::VectorXd, Eigen::MatrixXd> SphereCollisionCst::evaluate_dirty()
+    const {
+  auto all_sdfs = get_all_sdfs();
+
+  // collision vs outers
+  tinyfk::Transform pose;
+  Eigen::VectorXd grad_in_cspace_other(q_dim());
+  double min_val_other = std::numeric_limits<double>::max();
+  std::optional<size_t> min_sphere_idx = std::nullopt;
+  std::optional<size_t> min_sdf_idx = std::nullopt;
+  {
+    for (size_t i = 0; i < sphere_ids_.size(); i++) {
+      if (sphere_specs_[i].ignore_collision) {
+        continue;
+      }
+      kin_->get_link_pose(sphere_ids_[i], pose);
+      Eigen::Vector3d center(pose.position.x, pose.position.y, pose.position.z);
+      for (size_t j = 0; j < all_sdfs.size(); j++) {
+        double val = all_sdfs[j]->evaluate(center) - sphere_specs_[i].radius;
+        if (val < min_val_other) {
+          min_val_other = val;
+          min_sphere_idx = i;
+          min_sdf_idx = j;
+        }
+      }
+    }
+
+    Eigen::Vector3d grad;
+    kin_->get_link_pose(sphere_ids_[*min_sphere_idx], pose);
+    Eigen::Vector3d center(pose.position.x, pose.position.y, pose.position.z);
+    for (size_t i = 0; i < 3; i++) {
+      Eigen::Vector3d perturbed_center = center;
+      perturbed_center[i] += 1e-6;
+      double val = all_sdfs[*min_sdf_idx]->evaluate(perturbed_center) -
+                   sphere_specs_[*min_sphere_idx].radius;
+      grad[i] = (val - min_val_other) / 1e-6;
+    }
+    auto sphere_jac =
+        kin_->get_jacobian(sphere_ids_[*min_sphere_idx], control_joint_ids_,
+                           tinyfk::RotationType::IGNORE, with_base_);
+    grad_in_cspace_other = sphere_jac.transpose() * grad;
+  }
+
+  if (selcol_pairs_ids_.size() == 0) {
+    Eigen::MatrixXd jac(1, grad_in_cspace_other.size());
+    jac.row(0) = grad_in_cspace_other;
+    return {Eigen::VectorXd::Constant(1, min_val_other), jac};
+  } else {
+    // collision vs inners (self collision)
+    Eigen::VectorXd grad_in_cspace_self(control_joint_ids_.size());
+    double min_val_self = std::numeric_limits<double>::max();
+    {
+      std::optional<std::pair<size_t, size_t>> min_pair = std::nullopt;
+      for (const auto& pair : selcol_pairs_ids_) {
+        kin_->get_link_pose(sphere_ids_[pair.first], pose);
+        Eigen::Vector3d center1(pose.position.x, pose.position.y,
+                                pose.position.z);
+        kin_->get_link_pose(sphere_ids_[pair.second], pose);
+        Eigen::Vector3d center2(pose.position.x, pose.position.y,
+                                pose.position.z);
+        double val = (center1 - center2).norm() -
+                     sphere_specs_[pair.first].radius -
+                     sphere_specs_[pair.second].radius;
+        if (val < min_val_self) {
+          min_val_self = val;
+          min_pair = pair;
+        }
+      }
+      Eigen::Vector3d center1, center2;
+      kin_->get_link_pose(sphere_ids_[min_pair->first], pose);
+      center1 << pose.position.x, pose.position.y, pose.position.z;
+      kin_->get_link_pose(sphere_ids_[min_pair->second], pose);
+      center2 << pose.position.x, pose.position.y, pose.position.z;
+      Eigen::MatrixXd&& jac1 =
+          kin_->get_jacobian(sphere_ids_[min_pair->first], control_joint_ids_,
+                             tinyfk::RotationType::IGNORE, with_base_);
+      Eigen::MatrixXd&& jac2 =
+          kin_->get_jacobian(sphere_ids_[min_pair->second], control_joint_ids_,
+                             tinyfk::RotationType::IGNORE, with_base_);
+      double norminv = 1.0 / (center1 - center2).norm();
+      grad_in_cspace_self =
+          norminv * (center1 - center2).transpose() * (jac1 - jac2);
+    }
+
+    Eigen::Vector2d vals(min_val_other, min_val_self);
+    Eigen::MatrixXd jac(2, grad_in_cspace_other.size());
+    jac.row(0) = grad_in_cspace_other;
+    jac.row(1) = grad_in_cspace_self;
+    return {vals, jac};
+  }
+}
+
+bool ComInPolytopeCst::is_valid_dirty() const {
+  // COPIED from evaluate() >> START
+  auto com_tmp = kin_->get_com();
+  Eigen::Vector3d com(com_tmp.x, com_tmp.y, com_tmp.z);
+  if (force_link_ids_.size() > 0) {
+    double vertical_force_sum = 1.0;  // 1.0 for normalized self
+    tinyfk::Transform pose;
+    for (size_t j = 0; j < force_link_ids_.size(); ++j) {
+      double force = applied_force_values_[j] / kin_->total_mass_;
+      vertical_force_sum += force;
+      kin_->get_link_pose(force_link_ids_[j], pose);
+      Eigen::Vector3d force_pos(pose.position.x, pose.position.y,
+                                pose.position.z);
+      com += force * force_pos;
+    }
+    com /= vertical_force_sum;
+  }
+  // COPIED from evaluate() >> END
+  return polytope_sdf_->evaluate(com) < 0;
+}
+
+std::pair<Eigen::VectorXd, Eigen::MatrixXd> ComInPolytopeCst::evaluate_dirty()
+    const {
+  Eigen::VectorXd vals(cst_dim());
+  Eigen::MatrixXd jac(cst_dim(), q_dim());
+
+  auto com_tmp = kin_->get_com();
+  Eigen::Vector3d com(com_tmp.x, com_tmp.y, com_tmp.z);
+
+  auto com_jaco = kin_->get_com_jacobian(control_joint_ids_, q_dim());
+  if (force_link_ids_.size() > 0) {
+    double vertical_force_sum = 1.0;  // 1.0 for normalized self
+    tinyfk::Transform pose;
+    for (size_t j = 0; j < force_link_ids_.size(); ++j) {
+      double force = applied_force_values_[j] / kin_->total_mass_;
+      vertical_force_sum += force;
+      kin_->get_link_pose(force_link_ids_[j], pose);
+      Eigen::Vector3d force_pos(pose.position.x, pose.position.y,
+                                pose.position.z);
+      com += force * force_pos;
+
+      com_jaco += kin_->get_jacobian(force_link_ids_[j], control_joint_ids_,
+                                     tinyfk::RotationType::IGNORE, with_base_) *
+                  force;
+    }
+    double inv = 1.0 / vertical_force_sum;
+    com *= inv;
+    com_jaco *= inv;
+  }
+  double val = -polytope_sdf_->evaluate(com);
+  vals[0] = val;
+
+  Eigen::Vector3d grad;
+  for (size_t i = 0; i < 3; i++) {
+    Eigen::Vector3d perturbed_com = com;
+    perturbed_com[i] += 1e-6;
+    double val_perturbed = -polytope_sdf_->evaluate(perturbed_com);
+    grad[i] = (val_perturbed - val) / 1e-6;
+  }
+  jac.row(0) = com_jaco.transpose() * grad;
+
+  return {vals, jac};
+};
+
+void bind_collision_constraints(py::module& m) {
+  auto cst_m = m.def_submodule("constraint");
+  py::class_<ConstraintBase, ConstraintBase::Ptr>(cst_m, "ConstraintBase");
+  py::class_<EqConstraintBase, EqConstraintBase::Ptr, ConstraintBase>(
+      cst_m, "EqConstraintBase");
+  py::class_<IneqConstraintBase, IneqConstraintBase::Ptr, ConstraintBase>(
+      cst_m, "IneqConstraintBase");
+  py::class_<LinkPoseCst, LinkPoseCst::Ptr, EqConstraintBase>(cst_m,
+                                                              "LinkPoseCst")
+      .def(py::init<std::shared_ptr<tinyfk::KinematicModel>,
+                    const std::vector<std::string>&, bool,
+                    const std::vector<std::string>&,
+                    const std::vector<Eigen::VectorXd>&>())
+      .def("update_kintree", &LinkPoseCst::update_kintree)
+      .def("evaluate", &LinkPoseCst::evaluate)
+      .def("cst_dim", &LinkPoseCst::cst_dim);
+  py::class_<SphereAttachentSpec>(cst_m, "SphereAttachentSpec")
+      .def(py::init<const std::string&, const Eigen::Vector3d&, double, bool>())
+      .def_readonly("parent_link_name", &SphereAttachentSpec::parent_link_name);
+
+  py::class_<SphereCollisionCst, SphereCollisionCst::Ptr, IneqConstraintBase>(
+      cst_m, "SphereCollisionCst")
+      .def(py::init<std::shared_ptr<tinyfk::KinematicModel>,
+                    const std::vector<std::string>&, bool,
+                    const std::vector<SphereAttachentSpec>&,
+                    const std::vector<std::pair<std::string, std::string>>&,
+                    const std::vector<primitive_sdf::PrimitiveSDFBase::Ptr>&>())
+      .def("set_sdfs", &SphereCollisionCst::set_sdfs)
+      .def("update_kintree", &SphereCollisionCst::update_kintree)
+      .def("is_valid", &SphereCollisionCst::is_valid)
+      .def("evaluate", &SphereCollisionCst::evaluate);
+
+  py::class_<AppliedForceSpec>(cst_m, "AppliedForceSpec")
+      .def(py::init<const std::string&, double>())
+      .def_readonly("link_name", &AppliedForceSpec::link_name)
+      .def_readonly("force", &AppliedForceSpec::force);
+
+  py::class_<ComInPolytopeCst, ComInPolytopeCst::Ptr, IneqConstraintBase>(
+      cst_m, "ComInPolytopeCst")
+      .def(py::init<std::shared_ptr<tinyfk::KinematicModel>,
+                    const std::vector<std::string>&, bool,
+                    primitive_sdf::BoxSDF::Ptr,
+                    const std::vector<AppliedForceSpec>&>())
+      .def("update_kintree", &ComInPolytopeCst::update_kintree)
+      .def("is_valid", &ComInPolytopeCst::is_valid)
+      .def("evaluate", &ComInPolytopeCst::evaluate);
+  py::class_<EqCompositeCst, EqCompositeCst::Ptr>(cst_m, "EqCompositeCst")
+      .def(py::init<std::vector<EqConstraintBase::Ptr>>())
+      .def("update_kintree", &EqCompositeCst::update_kintree)
+      .def("evaluate", &EqCompositeCst::evaluate);
+  py::class_<IneqCompositeCst, IneqCompositeCst::Ptr>(cst_m, "IneqCompositeCst")
+      .def(py::init<std::vector<IneqConstraintBase::Ptr>>())
+      .def("update_kintree", &IneqCompositeCst::update_kintree)
+      .def("evaluate", &IneqCompositeCst::evaluate)
+      .def("is_valid", &IneqCompositeCst::is_valid);
+}
+
+}  // namespace cst