diff --git a/.github/workflows/python_package.yml b/.github/workflows/python_package.yml index 4456455..2e51d45 100644 --- a/.github/workflows/python_package.yml +++ b/.github/workflows/python_package.yml @@ -59,11 +59,7 @@ jobs: run: | pip install pybind11-stubgen pip install . - mkdir -p stubs_temp - pybind11-stubgen dqrobotics --output-dir stubs_temp python regenerate_stubs.py - cp -r stubs_temp/dqrobotics/dqrobotics/ dqrobotics-stubs - rm -r stubs_temp pip uninstall dqrobotics -y - name: Compile run: | diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0e33c7b --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +dqrobotics/_dqrobotics/ diff --git a/dqrobotics-stubs/README.md b/dqrobotics-stubs/README.md deleted file mode 100644 index ebb1447..0000000 --- a/dqrobotics-stubs/README.md +++ /dev/null @@ -1 +0,0 @@ -Adding this file as a placeholder otherwise the package cannot build before the stubs are generated. \ No newline at end of file diff --git a/dqrobotics/__init__.py b/dqrobotics/__init__.py index c5647c1..36514c3 100644 --- a/dqrobotics/__init__.py +++ b/dqrobotics/__init__.py @@ -23,5 +23,5 @@ # # ################################################################ """ -from dqrobotics._dqrobotics import * +from ._dqrobotics import * diff --git a/dqrobotics/interfaces/__init__.py b/dqrobotics/interfaces/__init__.py index f400db5..eacbed5 100644 --- a/dqrobotics/interfaces/__init__.py +++ b/dqrobotics/interfaces/__init__.py @@ -23,4 +23,4 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._interfaces import * +from .._dqrobotics._interfaces import * diff --git a/dqrobotics/interfaces/coppeliasim/__init__.py b/dqrobotics/interfaces/coppeliasim/__init__.py deleted file mode 100644 index 213e860..0000000 --- a/dqrobotics/interfaces/coppeliasim/__init__.py +++ /dev/null @@ -1,26 +0,0 @@ -""" -# Copyright (c) 2019-2022 DQ Robotics Developers -# -# This file is part of DQ Robotics. -# -# DQ Robotics is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# DQ Robotics is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with DQ Robotics. If not, see . -# -# ################################################################ -# -# Contributors: -# - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp -# -# ################################################################ -""" -from dqrobotics._dqrobotics._interfaces._coppeliasim import * diff --git a/dqrobotics/interfaces/json11/__init__.py b/dqrobotics/interfaces/json11/__init__.py index 701df31..86f3cfa 100644 --- a/dqrobotics/interfaces/json11/__init__.py +++ b/dqrobotics/interfaces/json11/__init__.py @@ -23,4 +23,4 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._interfaces._json11 import * +from ..._dqrobotics._interfaces._json11 import * diff --git a/dqrobotics/interfaces/vrep/__init__.py b/dqrobotics/interfaces/vrep/__init__.py deleted file mode 100644 index 925e80d..0000000 --- a/dqrobotics/interfaces/vrep/__init__.py +++ /dev/null @@ -1,26 +0,0 @@ -""" -# Copyright (c) 2019-2022 DQ Robotics Developers -# -# This file is part of DQ Robotics. -# -# DQ Robotics is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# DQ Robotics is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with DQ Robotics. If not, see . -# -# ################################################################ -# -# Contributors: -# - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp -# -# ################################################################ -""" -from dqrobotics._dqrobotics._interfaces._vrep import * diff --git a/dqrobotics/interfaces/vrep/robots/__init__.py b/dqrobotics/interfaces/vrep/robots/__init__.py deleted file mode 100644 index 1e2a8c9..0000000 --- a/dqrobotics/interfaces/vrep/robots/__init__.py +++ /dev/null @@ -1,26 +0,0 @@ -""" -# Copyright (c) 2019-2022 DQ Robotics Developers -# -# This file is part of DQ Robotics. -# -# DQ Robotics is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# DQ Robotics is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with DQ Robotics. If not, see . -# -# ################################################################ -# -# Contributors: -# - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp -# -# ################################################################ -""" -from dqrobotics._dqrobotics._interfaces._vrep._robots import * diff --git a/dqrobotics/py.typed b/dqrobotics/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/dqrobotics/robot_control/__init__.py b/dqrobotics/robot_control/__init__.py index 1801af8..dbcaab8 100644 --- a/dqrobotics/robot_control/__init__.py +++ b/dqrobotics/robot_control/__init__.py @@ -23,4 +23,4 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._robot_control import * +from .._dqrobotics._robot_control import * diff --git a/dqrobotics/robot_modeling/__init__.py b/dqrobotics/robot_modeling/__init__.py index 024d8e4..9d16054 100644 --- a/dqrobotics/robot_modeling/__init__.py +++ b/dqrobotics/robot_modeling/__init__.py @@ -23,4 +23,4 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._robot_modeling import * +from .._dqrobotics._robot_modeling import * diff --git a/dqrobotics/robots/__init__.py b/dqrobotics/robots/__init__.py index 7118062..031aea4 100644 --- a/dqrobotics/robots/__init__.py +++ b/dqrobotics/robots/__init__.py @@ -23,4 +23,4 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._robots import * +from .._dqrobotics._robots import * diff --git a/dqrobotics/solvers/__init__.py b/dqrobotics/solvers/__init__.py index 606231d..fae3d6e 100644 --- a/dqrobotics/solvers/__init__.py +++ b/dqrobotics/solvers/__init__.py @@ -23,14 +23,14 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._solvers import * +from .._dqrobotics._solvers import * try: - from dqrobotics.solvers._dq_quadprog_solver import DQ_QuadprogSolver + from dqrobotics.solvers._dq_quadprog_solver import DQ_QuadprogSolver as DQ_QuadprogSolver except: pass try: - from dqrobotics.solvers._dq_cplex_solver import DQ_CPLEXSolver + from dqrobotics.solvers._dq_cplex_solver import DQ_CPLEXSolver as DQ_CPLEXSolver except: pass diff --git a/dqrobotics/solvers/_dq_cplex_solver.py b/dqrobotics/solvers/_dq_cplex_solver.py index 5eedb4b..64e5082 100644 --- a/dqrobotics/solvers/_dq_cplex_solver.py +++ b/dqrobotics/solvers/_dq_cplex_solver.py @@ -23,21 +23,30 @@ # # ################################################################ """ +from __future__ import annotations from dqrobotics._dqrobotics._solvers import DQ_QuadraticProgrammingSolver import numpy as np +from numpy.typing import NDArray import cplex # https://github.com/dqrobotics/python/issues/24 class DQ_CPLEXSolver(DQ_QuadraticProgrammingSolver): - def __init__(self): + def __init__(self) -> None: DQ_QuadraticProgrammingSolver.__init__(self) # Make and set a solver instance - self.P = cplex.Cplex() + self.P: cplex.Cplex = cplex.Cplex() self.P.objective.set_sense(self.P.objective.sense.minimize) self.P.set_problem_type(self.P.problem_type.QP) self.P.set_results_stream(results_file=None) - def solve_quadratic_program(self, H, f, A, b, Aeq, beq): + # Narrower than the base class's ArrayLike: this implementation requires numpy arrays. + def solve_quadratic_program(self, # type: ignore[override] + H: NDArray[np.float64], + f: NDArray[np.float64], + A: NDArray[np.float64], + b: NDArray[np.float64], + Aeq: NDArray[np.float64], + beq: NDArray[np.float64]) -> NDArray[np.float64]: problem_size = H.shape[0] inequality_constraint_size = b.shape[0] equality_constraint_size = beq.shape[0] diff --git a/dqrobotics/solvers/_dq_quadprog_solver.py b/dqrobotics/solvers/_dq_quadprog_solver.py index 06000b1..0401e82 100644 --- a/dqrobotics/solvers/_dq_quadprog_solver.py +++ b/dqrobotics/solvers/_dq_quadprog_solver.py @@ -23,31 +23,41 @@ # # ################################################################ """ +from __future__ import annotations +from typing import Optional from dqrobotics._dqrobotics._solvers import DQ_QuadraticProgrammingSolver import numpy as np +from numpy.typing import NDArray import quadprog class DQ_QuadprogSolver(DQ_QuadraticProgrammingSolver): - def __init__(self): + def __init__(self) -> None: DQ_QuadraticProgrammingSolver.__init__(self) - self.equality_constraints_tolerance = 0 # default of np.finfo(np.float64).eps is already included in the solver + self.equality_constraints_tolerance: float = 0 # default of np.finfo(np.float64).eps is already included in the solver pass - def set_equality_constraints_tolerance(self, tolerance): + def set_equality_constraints_tolerance(self, tolerance: float) -> None: """ Set allowed tolerance for the equality constraints :param tolerance: Tolerance allowed for equality constraints """ self.equality_constraints_tolerance = tolerance - def get_equality_constraints_tolerance(self): + def get_equality_constraints_tolerance(self) -> float: """ Get allowed tolerance for the equality constraints :return: Current tolerance """ return self.equality_constraints_tolerance - def solve_quadratic_program(self, H, f, A, b, Aeq, beq): + # Narrower than the base class's ArrayLike: this implementation requires numpy arrays. + def solve_quadratic_program(self, # type: ignore[override] + H: NDArray[np.float64], + f: NDArray[np.float64], + A: Optional[NDArray[np.float64]], + b: Optional[NDArray[np.float64]], + Aeq: Optional[NDArray[np.float64]], + beq: Optional[NDArray[np.float64]]) -> NDArray[np.float64]: """ Solves the following quadratic program min(x) 0.5*x'Hx + f'x @@ -81,24 +91,29 @@ def solve_quadratic_program(self, H, f, A, b, Aeq, beq): # Turn equality into a bounded inequality ## Aeq.x <= beq + delta ## Aeq.x >= beq - delta ==> -Aeq.x <= -beq + delta - if Aeq is not None: # beq is None already checked by the ValueError + if Aeq is not None: + assert beq is not None # Already checked by the ValueError Aeq = np.vstack([Aeq, -Aeq]) beq = beq.reshape(-1) beq = np.concatenate([beq + self.equality_constraints_tolerance, -beq + self.equality_constraints_tolerance]) - # Use (A,b), (Aeq,beq), or both. - if Aeq is None: + # Use (A,b), (Aeq,beq), or both. The asserts were already checked by the ValueError. + A_internal: NDArray[np.float64] + b_internal: NDArray[np.float64] + if A is not None and Aeq is not None: + assert b is not None and beq is not None + A_internal = np.vstack([A, Aeq]) + b_internal = np.concatenate([b.reshape(-1), beq]) + elif A is not None: + assert b is not None A_internal = A b_internal = b - if A is None: + elif Aeq is not None: + assert beq is not None A_internal = Aeq b_internal = beq - if Aeq is not None and A is not None: - A_internal = np.vstack([A, Aeq]) - b_internal = np.concatenate([b.reshape(-1), beq]) - - # Solve for the unconstrained case. quadprog does not accept None, so we add a dummy constraint. - if A is None and b is None and Aeq is None and beq is None: + else: + # Solve for the unconstrained case. quadprog does not accept None, so we add a dummy constraint. A_internal = np.zeros((1, H.shape[0])) b_internal = np.zeros(1) diff --git a/dqrobotics/utils/DQ_LinearAlgebra/__init__.py b/dqrobotics/utils/DQ_LinearAlgebra/__init__.py index 3a127db..74f8dec 100644 --- a/dqrobotics/utils/DQ_LinearAlgebra/__init__.py +++ b/dqrobotics/utils/DQ_LinearAlgebra/__init__.py @@ -23,4 +23,4 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._utils._DQ_LinearAlgebra import * +from ..._dqrobotics._utils._DQ_LinearAlgebra import * diff --git a/dqrobotics/utils/DQ_Math/__init__.py b/dqrobotics/utils/DQ_Math/__init__.py index da924d0..69ce290 100644 --- a/dqrobotics/utils/DQ_Math/__init__.py +++ b/dqrobotics/utils/DQ_Math/__init__.py @@ -1 +1 @@ -from dqrobotics._dqrobotics._utils._DQ_Math import * +from ..._dqrobotics._utils._DQ_Math import * diff --git a/dqrobotics/utils/__init__.py b/dqrobotics/utils/__init__.py index 273d3d8..ff6b598 100644 --- a/dqrobotics/utils/__init__.py +++ b/dqrobotics/utils/__init__.py @@ -23,4 +23,4 @@ # # ################################################################ """ -from dqrobotics._dqrobotics._utils import * +from .._dqrobotics._utils import * diff --git a/pyproject.toml b/pyproject.toml index 89cf6d6..9973bbb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -35,4 +35,10 @@ Issues = "https://github.com/dqrobotics/python/issues" enabled = true # https://stackoverflow.com/questions/73605607/how-to-use-setuptools-scm dev_template = "{tag}.a{ccount}" -dirty_template = "{tag}.a{ccount}" \ No newline at end of file +dirty_template = "{tag}.a{ccount}" + +[tool.mypy] +# The optional solver backends of dqrobotics.solvers ship no type information. +[[tool.mypy.overrides]] +module = ["quadprog", "cplex"] +ignore_missing_imports = true diff --git a/regenerate_stubs.py b/regenerate_stubs.py index a0fd2f5..14784e7 100644 --- a/regenerate_stubs.py +++ b/regenerate_stubs.py @@ -1,67 +1,122 @@ """ -Fix the stubs created by pybind11-stubgen +Generate the stubs of the compiled `dqrobotics._dqrobotics` module, to be shipped inside the `dqrobotics` package. -The default version of pybind11 stubgen will generate something like +The public modules of dqrobotics (e.g. `dqrobotics.robot_modeling`) are Python files that do +`from .._dqrobotics._robot_modeling import *`, so type checkers read them directly. Only the compiled module +needs stubs, which pybind11-stubgen writes next to it, as below. Together with `dqrobotics/py.typed`, this makes +dqrobotics an inline-typed package (PEP 561). -. -└── dqrobotics - ├── __init__.pyi - └── _dqrobotics - ├── __init__.pyi - ├── _interfaces - │   ├── __init__.pyi - │   └── _json11.pyi - ├── _robot_control.pyi - ├── _robot_modeling.pyi - ├── _robots.pyi - ├── _solvers.pyi - └── _utils - ├── _DQ_LinearAlgebra.pyi - ├── _DQ_Math.pyi - └── __init__.pyi - -which is not compatible with dqrobotics' structure. It's not clear to me why that's the case. -This script will adjust it to something like below, which make the stubs quite useful. - -. -├── README.md -├── __init__.pyi -├── interfaces -│   ├── __init__.pyi -│   └── json11 -│   └── __init__.py -├── robot_control -│   └── __init__.py +dqrobotics +├── py.typed +├── __init__.py ├── robot_modeling -│   └── __init__.py -├── robots -│   └── __init__.py -├── solvers -│   └── __init__.py -└── utils - ├── DQ_LinearAlgebra - │   └── __init__.py - ├── DQ_Math - │   └── __init__.py - └── __init__.pyi +│ └── __init__.py +├── ... +├── _dqrobotics..so (compiled module) +└── _dqrobotics (generated stubs) + ├── __init__.pyi + ├── _robot_modeling.pyi + └── ... + +Usage (with dqrobotics and pybind11-stubgen installed in the current environment) + python regenerate_stubs.py Author: Murilo M. Marinho """ import os +import re +import shutil +import subprocess +import sys +import tempfile + +OUTPUT_DIR: str = os.path.join(os.path.dirname(os.path.abspath(__file__)), "dqrobotics", "_dqrobotics") +MODULE: str = "dqrobotics._dqrobotics" + + +def remove_keyword_identifiers(stub_file: str) -> None: + """ + Remove the attributes named `None` from a stub file. + `ControlObjective.None` is valid at runtime, through getattr(), but `None: ...` is a syntax error in a stub. + :param stub_file: The path to the stub file. + """ + with open(stub_file) as f: + content = f.read() + content = re.sub(r"^[ \t]*None: .*\n", "", content, flags=re.MULTILINE) + content = content.replace("'None', ", "") + with open(stub_file, "w") as f: + f.write(content) + -def main(): - cwd = os.path.join(os.getcwd(),"stubs_temp") - for root, dirs, files in os.walk(cwd, topdown=False): +def replace_once(content: str, old: str, new: str) -> str: + """ + Replace a snippet of generated stub that must appear exactly once, so that changes in pybind11-stubgen's output are + noticed instead of silently ignored. + """ + if content.count(old) != 1: + raise RuntimeError(f"Expected exactly one occurrence of:\n{old}") + return content.replace(old, new) + + +def complete_dq_comparison(stub_file: str) -> None: + """ + Make the stub of `DQ` consistent with `object`, as expected by type checkers. + - `__eq__` and `__ne__` also accept any other object at runtime, for which pybind11 returns NotImplemented and Python + falls back to the default comparison. That last overload cannot be expressed in the bindings with a `bool` result. + - `__hash__` is None because `DQ` defines `__eq__`. This is declared as in typeshed (e.g. `list`), which needs + the ignore because `object.__hash__` is a method. + :param stub_file: The path to the stub file of `dqrobotics._dqrobotics`. + """ + with open(stub_file) as f: + content = f.read() + content = replace_once(content, + " __hash__: typing.ClassVar[None] = None\n", + " __hash__: typing.ClassVar[None] = None # type: ignore[assignment]\n") + for method, result in (("__eq__", "False"), ("__ne__", "True")): + last_overload = re.findall(rf' def {method}\(self, arg0: typing\.SupportsFloat \| typing\.SupportsIndex\) -> bool:\n' + rf' """\n.*?\n """\n', content, flags=re.DOTALL) + if len(last_overload) != 1: + raise RuntimeError(f"Expected exactly one scalar overload of DQ.{method}") + content = replace_once(content, last_overload[0], last_overload[0] + + " @typing.overload\n" + f" def {method}(self, other: object) -> bool:\n" + ' """\n' + f" Returns {result} for any other object, through Python's default comparison.\n" + ' """\n') + with open(stub_file, "w") as f: + f.write(content) + + +def normalize_empty_all(stub_dir: str) -> None: + """ + Write the empty `__all__` of the generated stubs as `[]`. pybind11-stubgen writes `list()`, which type checkers + do not evaluate, so they cannot tell which names the module exports. + :param stub_dir: The directory with the stub files. + """ + for root, _, files in os.walk(stub_dir): for name in files: - if name.startswith('__'): - continue - elif name.startswith('_') and name.endswith('.pyi'): - os.makedirs(os.path.join(root, name[1:-4]), exist_ok=True) - os.rename(os.path.join(root, name), os.path.join(root, name[1:-4], "__init__.py")) - for name in dirs: - if name.startswith('_'): - os.rename(os.path.join(root, name), os.path.join(root, name[1:])) + if name.endswith(".pyi"): + stub_file = os.path.join(root, name) + with open(stub_file) as f: + content = f.read() + with open(stub_file, "w") as f: + f.write(content.replace("__all__: list[str] = list()", "__all__: list[str] = []")) + + +def main() -> None: + with tempfile.TemporaryDirectory() as temp_dir: + # Run from temp_dir so that the source folder `dqrobotics`, which lacks the compiled module, does not shadow + # the installed package. + subprocess.check_call([sys.executable, "-m", "pybind11_stubgen", MODULE, "--output-dir", temp_dir, + "--exit-code"], + cwd=temp_dir) + shutil.rmtree(OUTPUT_DIR, ignore_errors=True) + shutil.copytree(os.path.join(temp_dir, *MODULE.split(".")), OUTPUT_DIR) + normalize_empty_all(OUTPUT_DIR) + remove_keyword_identifiers(os.path.join(OUTPUT_DIR, "_robot_control.pyi")) + complete_dq_comparison(os.path.join(OUTPUT_DIR, "__init__.pyi")) + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/setup.py b/setup.py index 8f9dd9e..3e60827 100644 --- a/setup.py +++ b/setup.py @@ -11,13 +11,13 @@ class CMakeExtension(Extension): - def __init__(self, name, sourcedir=''): + def __init__(self, name: str, sourcedir: str = '') -> None: Extension.__init__(self, name, sources=[]) self.sourcedir = os.path.abspath(sourcedir) class CMakeBuild(build_ext): - def run(self): + def run(self) -> None: try: out = subprocess.check_output(['cmake', '--version']) except OSError: @@ -32,7 +32,7 @@ def run(self): for ext in self.extensions: self.build_extension(ext) - def build_extension(self, ext): + def build_extension(self, ext: CMakeExtension) -> None: extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) cmake_args = ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=' + extdir, '-DPYTHON_EXECUTABLE=' + sys.executable] @@ -71,7 +71,7 @@ def build_extension(self, ext): zip_safe=False, packages=find_namespace_packages(where='.', exclude=['*pybind11*', '*tests*']), package_data={ - 'dqrobotics-stubs': ["**/*.pyi"], + 'dqrobotics': ["py.typed", "_dqrobotics/**/*.pyi"], }, classifiers=[ "Programming Language :: Python :: 3.10", diff --git a/src/DQ_py.cpp b/src/DQ_py.cpp index 5dcddb4..e09319c 100644 --- a/src/DQ_py.cpp +++ b/src/DQ_py.cpp @@ -107,9 +107,9 @@ void init_DQ_py(py::module& m) dq.def(py::self + double(), "Returns the addition between this dual quaternion and a scalar."); dq.def(double() - py::self, "Returns the subtraction between a scalar and this dual quaternion."); dq.def(py::self - double(), "Returns the subtraction between this dual quaternion and a scalar."); - dq.def(double() == py::self, "Returns true if a scalar and this dual quaternion are equal, up to a numerical threshold."); + // No `double() == py::self` or `double() != py::self`: they would bind a second, unreachable `__eq__`/`__ne__`, + // since Python already evaluates `scalar == dq` as the reflected `dq == scalar`. dq.def(py::self == double(), "Returns true if this dual quaternion and a scalar are equal, up to a numerical threshold."); - dq.def(double() != py::self, "Returns true if a scalar and this dual quaternion are different, up to a numerical threshold."); dq.def(py::self != double(), "Returns true if this dual quaternion and a scalar are different, up to a numerical threshold."); ///Namespace Functions diff --git a/src/dqrobotics_module.h b/src/dqrobotics_module.h index 8fceb5b..dd9b65c 100644 --- a/src/dqrobotics_module.h +++ b/src/dqrobotics_module.h @@ -98,10 +98,6 @@ void init_DQ_QuadraticProgrammingController_py(py::module& m); //dqrobotics/solvers void init_DQ_QuadraticProgrammingSolver_py(py::module& m); -//dqrobotics/interfaces/coppeliasim -void init_DQ_CoppeliaSimInterface_py(py::module& m); -void init_DQ_CoppeliaSimInterfaceZMQ_py(py::module& m); - //dqrobotics/interfaces/json11 void init_DQ_JsonReader_py(py::module& m); diff --git a/src/interfaces/vrep/DQ_SerialVrepRobot_py.cpp b/src/interfaces/vrep/DQ_SerialVrepRobot_py.cpp deleted file mode 100644 index 75bd376..0000000 --- a/src/interfaces/vrep/DQ_SerialVrepRobot_py.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/** -(C) Copyright 2023 DQ Robotics Developers - -This file is part of DQ Robotics. - - DQ Robotics is free software: you can redistribute it and/or modify - it under the terms of the GNU Lesser General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - DQ Robotics is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public License - along with DQ Robotics. If not, see . - -Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -*/ - -#include "../../dqrobotics_module.h" - -/** - * @brief Binds `DQ_SerialVrepRobot`, a serial-robot wrapper that exposes joint - * names, target commands, velocities, and torques in CoppeliaSim, to the - * Python module @p m. - */ -void init_DQ_SerialVrepRobot_py(py::module& m) -{ - /***************************************************** - * SerialVrepRobot - * **************************************************/ - py::class_< - DQ_SerialVrepRobot, - std::shared_ptr, - DQ_VrepRobot - > dqsv_robot( - m, - "DQ_SerialVrepRobot", - "Serial robot wrapper for exchanging joint names, target commands, velocities, and torques with CoppeliaSim."); - - - dqsv_robot.def("get_joint_names", - &DQ_SerialVrepRobot::get_joint_names, - "Gets the joint names used in CoppeliaSim."); - - dqsv_robot.def("set_target_configuration_space_positions", - &DQ_SerialVrepRobot::set_target_configuration_space_positions, - py::arg("q"), - "Sets the target configuration-space positions in CoppeliaSim."); - - dqsv_robot.def("get_configuration_space_velocities", - &DQ_SerialVrepRobot::get_configuration_space_velocities, - "Gets the configuration-space velocities from CoppeliaSim."); - dqsv_robot.def("set_target_configuration_space_velocities", - &DQ_SerialVrepRobot::set_target_configuration_space_velocities, - py::arg("q_dot"), - "Sets the target configuration-space velocities in CoppeliaSim."); - - dqsv_robot.def("set_configuration_space_torques", - &DQ_SerialVrepRobot::set_configuration_space_torques, - py::arg("torques"), - "Sets the configuration-space torques in CoppeliaSim."); - dqsv_robot.def("get_configuration_space_torques", - &DQ_SerialVrepRobot::get_configuration_space_torques, - "Gets the configuration-space torques from CoppeliaSim."); - - //Deprecated - dqsv_robot.def("send_q_target_to_vrep", - &DQ_SerialVrepRobot::send_q_target_to_vrep, - py::arg("q"), - "Deprecated alias for setting the target configuration-space positions in CoppeliaSim."); -} diff --git a/src/interfaces/vrep/DQ_VrepInterface_py.cpp b/src/interfaces/vrep/DQ_VrepInterface_py.cpp deleted file mode 100644 index 243639f..0000000 --- a/src/interfaces/vrep/DQ_VrepInterface_py.cpp +++ /dev/null @@ -1,329 +0,0 @@ -/** -(C) Copyright 2019-2023 DQ Robotics Developers - -This file is part of DQ Robotics. - - DQ Robotics is free software: you can redistribute it and/or modify - it under the terms of the GNU Lesser General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - DQ Robotics is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public License - along with DQ Robotics. If not, see . - -Contributors: -1. Murilo M. Marinho (murilomarinho@ieee.org) - - Initial implementation. - -2. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) - -Added the method wait_for_simulation_step_to_end() -*/ - -#include "../../dqrobotics_module.h" - -//Default arguments added with: -//https://pybind11.readthedocs.io/en/stable/basics.html#default-args - -/** - * @brief Binds `DQ_VrepInterface`, an interface for connecting to - * V-REP/CoppeliaSim and exchanging simulation, object, joint, and inertial - * data, to the Python module @p m. - */ -void init_DQ_VrepInterface_py(py::module& m) -{ - /***************************************************** - * VrepInterface - * **************************************************/ - py::class_< - DQ_VrepInterface, - std::shared_ptr - > dqvrepinterface_py( - m, - "DQ_VrepInterface", - "Interface for connecting to V-REP/CoppeliaSim and exchanging simulation, object, joint, and inertial data."); - dqvrepinterface_py.def(py::init<>(), "Constructs a V-REP/CoppeliaSim interface."); - dqvrepinterface_py.def(py::init(), - py::arg("termination_flag"), - "Constructs a V-REP/CoppeliaSim interface that uses an external atomic termination flag."); - - py::enum_(dqvrepinterface_py, "OP_MODES") - .value("OP_BUFFER", DQ_VrepInterface::OP_MODES::OP_BUFFER) - .value("OP_ONESHOT", DQ_VrepInterface::OP_MODES::OP_ONESHOT) - .value("OP_BLOCKING", DQ_VrepInterface::OP_MODES::OP_BLOCKING) - .value("OP_STREAMING", DQ_VrepInterface::OP_MODES::OP_STREAMING) - .value("OP_AUTOMATIC", DQ_VrepInterface::OP_MODES::OP_AUTOMATIC) - .export_values(); - - py::enum_(dqvrepinterface_py, "SCRIPT_TYPES") - .value("ST_CHILD", DQ_VrepInterface::SCRIPT_TYPES::ST_CHILD) - .value("ST_MAIN", DQ_VrepInterface::SCRIPT_TYPES::ST_MAIN) - .value("ST_CUSTOMIZATION", DQ_VrepInterface::SCRIPT_TYPES::ST_CUSTOMIZATION) - .export_values(); - - py::enum_(dqvrepinterface_py, "REFERENCE_FRAMES") - .value("BODY_FRAME", DQ_VrepInterface::REFERENCE_FRAMES::BODY_FRAME) - .value("ABSOLUTE_FRAME", DQ_VrepInterface::REFERENCE_FRAMES::ABSOLUTE_FRAME) - .export_values(); - - dqvrepinterface_py.def("connect", - (bool (DQ_VrepInterface::*) (const int&, const int&, const int&))&DQ_VrepInterface::connect, - py::arg("port"), - py::arg("max_attempts"), - py::arg("attempt_interval_in_ms"), - "Attempts to connect to a V-REP/CoppeliaSim server on the local machine."); - dqvrepinterface_py.def("connect", - (bool (DQ_VrepInterface::*) (const std::string&, const int&, const int&, const int&))&DQ_VrepInterface::connect, - py::arg("ip"), - py::arg("port"), - py::arg("max_attempts"), - py::arg("attempt_interval_in_ms"), - "Attempts to connect to a V-REP/CoppeliaSim server at the given IP address."); - - dqvrepinterface_py.def("disconnect", &DQ_VrepInterface::disconnect, "Disconnects from the V-REP/CoppeliaSim server."); - dqvrepinterface_py.def("disconnect_all", &DQ_VrepInterface::disconnect_all, "Disconnects all active V-REP/CoppeliaSim connections."); - - dqvrepinterface_py.def("start_simulation", &DQ_VrepInterface::start_simulation, "Starts the simulation."); - dqvrepinterface_py.def("stop_simulation", &DQ_VrepInterface::stop_simulation, "Stops the simulation."); - - dqvrepinterface_py.def("is_simulation_running", &DQ_VrepInterface::is_simulation_running, "Returns whether the simulation is currently running."); - - // void set_synchronous(const bool& flag); - dqvrepinterface_py.def("set_synchronous", - (void (DQ_VrepInterface::*) (const bool&))&DQ_VrepInterface::set_synchronous, - py::arg("flag"), - "Enables or disables synchronous simulation mode."); - - //void trigger_next_simulation_step(); - dqvrepinterface_py.def("trigger_next_simulation_step", &DQ_VrepInterface::trigger_next_simulation_step, "Sends the synchronization trigger for the next simulation step."); - - //void wait_for_simulation_step_to_end(); - dqvrepinterface_py.def("wait_for_simulation_step_to_end", &DQ_VrepInterface::wait_for_simulation_step_to_end, "Waits until the current simulation step finishes."); - - //dqvrepinterface_py.def("get_object_handle", &DQ_VrepInterface::get_object_handle,"Gets an object handle"); - //dqvrepinterface_py.def("get_object_handles",&DQ_VrepInterface::get_object_handles,"Get object handles"); - - //dqvrepinterface_py.def("get_object_translation",(DQ (DQ_VrepInterface::*) (const int&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation,"Gets object translation."); - //dqvrepinterface_py.def("get_object_translation",(DQ (DQ_VrepInterface::*) (const std::string&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation,"Gets object translation."); - //dqvrepinterface_py.def("get_object_translation",(DQ (DQ_VrepInterface::*) (const int&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation,"Gets object translation."); - dqvrepinterface_py.def("get_object_translation", - (DQ (DQ_VrepInterface::*) (const std::string&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation, - py::arg("object_name") = std::string(""), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets an object's translation, optionally relative to another object."); - - //dqvrepinterface_py.def("set_object_translation",(void (DQ_VrepInterface::*) (const int&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&) const)&DQ_VrepInterface::set_object_translation,"Sets object translation."); - //dqvrepinterface_py.def("set_object_translation",(void (DQ_VrepInterface::*) (const std::string&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_translation,"Sets object translation."); - //dqvrepinterface_py.def("set_object_translation",(void (DQ_VrepInterface::*) (const int&, const std::string&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_translation,"Sets object translation."); - dqvrepinterface_py.def("set_object_translation", - (void (DQ_VrepInterface::*) (const std::string&, const DQ&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_translation, - py::arg("object_name") = std::string(""), - py::arg("translation") = DQ(0), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets an object's translation, optionally relative to another object."); - - //dqvrepinterface_py.def("get_object_rotation",(DQ (DQ_VrepInterface::*) (const int&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation,"Gets object rotation."); - //dqvrepinterface_py.def("get_object_rotation",(DQ (DQ_VrepInterface::*) (const std::string&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation,"Gets object rotation."); - //dqvrepinterface_py.def("get_object_rotation",(DQ (DQ_VrepInterface::*) (const int&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation,"Gets object rotation."); - dqvrepinterface_py.def("get_object_rotation", - (DQ (DQ_VrepInterface::*) (const std::string&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation, - py::arg("object_name") = std::string(""), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets an object's rotation, optionally relative to another object."); - - //dqvrepinterface_py.def("set_object_rotation",(void (DQ_VrepInterface::*) (const int&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&) const)&DQ_VrepInterface::set_object_rotation,"Sets object rotation."); - //dqvrepinterface_py.def("set_object_rotation",(void (DQ_VrepInterface::*) (const std::string&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_rotation,"Sets object rotation."); - //dqvrepinterface_py.def("set_object_rotation",(void (DQ_VrepInterface::*) (const int&, const std::string&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_rotation,"Sets object rotation."); - dqvrepinterface_py.def("set_object_rotation", - (void (DQ_VrepInterface::*) (const std::string&, const DQ&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_rotation, - py::arg("object_name") = std::string(""), - py::arg("rotation") = DQ(1), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets an object's rotation, optionally relative to another object."); - - //dqvrepinterface_py.def("get_object_pose",(DQ (DQ_VrepInterface::*) (const int&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose,"Gets object pose."); - //dqvrepinterface_py.def("get_object_pose",(DQ (DQ_VrepInterface::*) (const std::string&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose,"Gets object pose."); - //dqvrepinterface_py.def("get_object_pose",(DQ (DQ_VrepInterface::*) (const int&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose,"Gets object pose."); - dqvrepinterface_py.def("get_object_pose", - (DQ (DQ_VrepInterface::*) (const std::string&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose, - py::arg("object_name") = std::string(""), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets an object's pose, optionally relative to another object."); - - //dqvrepinterface_py.def("set_object_pose",(void (DQ_VrepInterface::*) (const int&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&) const)&DQ_VrepInterface::set_object_pose,"Sets object pose."); - //dqvrepinterface_py.def("set_object_pose",(void (DQ_VrepInterface::*) (const std::string&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_pose,"Sets object pose."); - //dqvrepinterface_py.def("set_object_pose",(void (DQ_VrepInterface::*) (const int&, const std::string&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_pose,"Sets object pose."); - dqvrepinterface_py.def("set_object_pose", - (void (DQ_VrepInterface::*) (const std::string&, const DQ&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_pose, - py::arg("object_name") = std::string(""), - py::arg("pose") = DQ(1), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets an object's pose, optionally relative to another object."); - - dqvrepinterface_py.def("get_object_poses", - &DQ_VrepInterface::get_object_poses, - py::arg("object_names"), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets the poses of multiple objects, optionally relative to another object."); - dqvrepinterface_py.def("set_object_poses", - &DQ_VrepInterface::set_object_poses, - py::arg("object_names"), - py::arg("poses"), - py::arg("relative_to_object_name") = VREP_OBJECTNAME_ABSOLUTE, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets the poses of multiple objects, optionally relative to another object."); - - //dqvrepinterface_py.def("set_joint_position",(void (DQ_VrepInterface::*) (const int&, const double&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_position,"Set joint position"); - dqvrepinterface_py.def("set_joint_position", - (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_position, - py::arg("joint_name") = std::string(""), - py::arg("angle_rad") = 0.0, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets one joint position in radians."); - - //dqvrepinterface_py.def("set_joint_target_position",(void (DQ_VrepInterface::*) (const int&, const double&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_target_position,"Set joint position"); - dqvrepinterface_py.def("set_joint_target_position", - (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_position, - py::arg("joint_name") = std::string(""), - py::arg("angle_rad") = 0.0, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets one joint target position in radians."); - - //dqvrepinterface_py.def("get_joint_position",(double (DQ_VrepInterface::*) (const int&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::get_joint_position,"Get joint position"); - dqvrepinterface_py.def("get_joint_position", - (double (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_position, - py::arg("joint_name") = std::string(""), - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets one joint position in radians."); - - //dqvrepinterface_py.def("set_joint_positions",(void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_positions,"Set joint positions"); - dqvrepinterface_py.def("set_joint_positions", - (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_positions, - py::arg("joint_names") = std::vector(), - py::arg("angles_rad") = VectorXd::Zero(1), - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets multiple joint positions in radians."); - - //dqvrepinterface_py.def("set_joint_target_positions",(void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_target_positions,"Set joint positions"); - dqvrepinterface_py.def("set_joint_target_positions", - (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_positions, - py::arg("joint_names") = std::vector(), - py::arg("angles_rad") = VectorXd::Zero(1), - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets multiple joint target positions in radians."); - - //dqvrepinterface_py.def("get_joint_positions",(VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::get_joint_positions,"Get joint positions"); - dqvrepinterface_py.def("get_joint_positions", - (VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_positions, - py::arg("joint_names") = std::vector(), - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets multiple joint positions in radians."); - - - //void set_joint_target_velocity(const std::string& jointname, const double& angle_dot_rad, const OP_MODES& opmode=OP_ONESHOT); - dqvrepinterface_py.def("set_joint_target_velocity", - (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_velocity, - py::arg("joint_name") = std::string(""), - py::arg("angle_dot_rad") = 0.0, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets one joint target velocity in radians per second."); - - //void set_joint_target_velocities(const std::vector& jointnames, const VectorXd& angles_dot_rad, const OP_MODES& opmode=OP_ONESHOT); - dqvrepinterface_py.def("set_joint_target_velocities", - (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_velocities, - py::arg("joint_names") = std::vector(), - py::arg("angles_dot_rad") = VectorXd::Zero(1), - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets multiple joint target velocities in radians per second."); - - //double get_joint_velocity(const std::string& jointname, const OP_MODES& opmode=OP_AUTOMATIC); - dqvrepinterface_py.def("get_joint_velocity", - (double (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_velocity, - py::arg("joint_name") = std::string(""), - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets one joint velocity in radians per second."); - - //VectorXd get_joint_velocities(const std::vector& jointnames, const OP_MODES& opmode=OP_AUTOMATIC); - dqvrepinterface_py.def("get_joint_velocities", - (VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_velocities, - py::arg("joint_names") = std::vector(), - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets multiple joint velocities in radians per second."); - - - //void set_joint_torque(const std::string& jointname, const double& torque, const OP_MODES& opmode=OP_ONESHOT); - dqvrepinterface_py.def("set_joint_torque", - (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_torque, - py::arg("joint_name") = std::string(""), - py::arg("torque") = 0.0, - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets one joint torque."); - - //void set_joint_torques(const std::vector& jointnames, const VectorXd& torques, const OP_MODES& opmode=OP_ONESHOT); - dqvrepinterface_py.def("set_joint_torques", - (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_torques, - py::arg("joint_names") = std::vector(), - py::arg("torques") = VectorXd::Zero(1), - py::arg("op_mode") = DQ_VrepInterface::OP_ONESHOT, - "Sets multiple joint torques."); - - //double get_joint_torque(const std::string& jointname, const OP_MODES& opmode=OP_AUTOMATIC); - dqvrepinterface_py.def("get_joint_torque", - (double (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_torque, - py::arg("joint_name") = std::string(""), - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets one joint torque."); - - //VectorXd get_joint_torques(const std::vector& jointnames, const OP_MODES& opmode=OP_AUTOMATIC); - dqvrepinterface_py.def("get_joint_torques", - (VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_torques, - py::arg("joint_names") = std::vector(), - py::arg("op_mode") = DQ_VrepInterface::OP_AUTOMATIC, - "Gets multiple joint torques."); - - //double get_mass(const std::string& link_name, const std::string& function_name = "get_mass", const std::string& obj_name= "DQRoboticsApiCommandServer"); - dqvrepinterface_py.def("get_mass", - (double (DQ_VrepInterface::*) (const std::string&, const std::string&, const std::string&))&DQ_VrepInterface::get_mass, - py::arg("link_name") = std::string(""), - py::arg("function_name") = std::string("get_mass"), - py::arg("obj_name") = std::string("DQRoboticsApiCommandServer"), - "Gets the mass of an object from CoppeliaSim."); - - //DQ get_center_of_mass(const std::string& link_name, - // const REFERENCE_FRAMES& reference_frame=BODY_FRAME, - // const std::string& function_name = "get_center_of_mass", - // const std::string& obj_name= "DQRoboticsApiCommandServer"); - dqvrepinterface_py.def("get_center_of_mass", - (DQ (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::REFERENCE_FRAMES&, - const std::string&, const std::string&))&DQ_VrepInterface::get_center_of_mass, - py::arg("link_name") = std::string(""), - py::arg("reference_frame") = DQ_VrepInterface::BODY_FRAME, - py::arg("function_name") = std::string("get_center_of_mass"), - py::arg("obj_name") = std::string("DQRoboticsApiCommandServer"), - "Gets the center of mass of an object from CoppeliaSim in the requested reference frame."); - - //MatrixXd get_inertia_matrix(const std::string& link_name, - // const REFERENCE_FRAMES& reference_frame=BODY_FRAME, - // const std::string& function_name = "get_inertia", - // const std::string& obj_name= "DQRoboticsApiCommandServer"); - dqvrepinterface_py.def("get_inertia_matrix", - (MatrixXd (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::REFERENCE_FRAMES&, - const std::string&, const std::string&))&DQ_VrepInterface::get_inertia_matrix, - py::arg("link_name") = std::string(""), - py::arg("reference_frame") = DQ_VrepInterface::BODY_FRAME, - py::arg("function_name") = std::string("get_inertia"), - py::arg("obj_name") = std::string("DQRoboticsApiCommandServer"), - "Gets the inertia matrix of an object from CoppeliaSim in the requested reference frame."); - -} diff --git a/src/interfaces/vrep/DQ_VrepRobot_py.cpp b/src/interfaces/vrep/DQ_VrepRobot_py.cpp deleted file mode 100644 index 481fb8e..0000000 --- a/src/interfaces/vrep/DQ_VrepRobot_py.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/** -(C) Copyright 2019 DQ Robotics Developers - -This file is part of DQ Robotics. - - DQ Robotics is free software: you can redistribute it and/or modify - it under the terms of the GNU Lesser General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - DQ Robotics is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public License - along with DQ Robotics. If not, see . - -Contributors: -- Murilo M. Marinho (murilomarinho@ieee.org) -*/ - -#include "../../dqrobotics_module.h" - -/** - * @brief Binds `DQ_VrepRobot`, a base robot wrapper that exposes - * configuration-space exchanges with CoppeliaSim, to the Python module @p m. - */ -void init_DQ_VrepRobot_py(py::module& m) -{ - /***************************************************** - * VrepRobot - * **************************************************/ - py::class_< - DQ_VrepRobot, - std::shared_ptr - > dqvreprobot_py( - m, - "DQ_VrepRobot", - "Base robot wrapper for reading and writing configuration-space values in CoppeliaSim."); - - dqvreprobot_py.def("set_configuration_space_positions", - &DQ_VrepRobot::set_configuration_space_positions, - py::arg("q"), - "Sets the robot configuration-space positions in CoppeliaSim."); - dqvreprobot_py.def("get_configuration_space_positions", - &DQ_VrepRobot::get_configuration_space_positions, - "Gets the robot configuration-space positions from CoppeliaSim."); - - //Deprecated - dqvreprobot_py.def("send_q_to_vrep", - &DQ_VrepRobot::send_q_to_vrep, - py::arg("q"), - "Deprecated alias for setting the robot configuration-space positions in CoppeliaSim."); - dqvreprobot_py.def("get_q_from_vrep", - &DQ_VrepRobot::get_q_from_vrep, - "Deprecated alias for getting the robot configuration-space positions from CoppeliaSim."); -} diff --git a/src/robot_modeling/DQ_DifferentialDriveRobot_py.cpp b/src/robot_modeling/DQ_DifferentialDriveRobot_py.cpp index 214ba69..1ff4690 100644 --- a/src/robot_modeling/DQ_DifferentialDriveRobot_py.cpp +++ b/src/robot_modeling/DQ_DifferentialDriveRobot_py.cpp @@ -60,15 +60,15 @@ void init_DQ_DifferentialDriveRobot_py(py::module& m) "Computes the full constrained pose Jacobian."); dqdifferentialdriverobot_py.def( "pose_jacobian_derivative", - (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_DifferentialDriveRobot::pose_jacobian_derivative, + (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&, const VectorXd&) const)&DQ_DifferentialDriveRobot::pose_jacobian_derivative, py::arg("q"), py::arg("q_dot"), - py::arg("to_link"), - "Computes the time derivative of the constrained pose Jacobian up to the requested column."); + "Computes the full time derivative of the constrained pose Jacobian."); dqdifferentialdriverobot_py.def( "pose_jacobian_derivative", - (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&, const VectorXd&) const)&DQ_DifferentialDriveRobot::pose_jacobian_derivative, + (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_DifferentialDriveRobot::pose_jacobian_derivative, py::arg("q"), py::arg("q_dot"), - "Computes the full time derivative of the constrained pose Jacobian."); + py::arg("to_link"), + "Computes the time derivative of the constrained pose Jacobian up to the requested column."); } diff --git a/src/robot_modeling/DQ_Kinematics_py.cpp b/src/robot_modeling/DQ_Kinematics_py.cpp index 50a4d7f..b72fa18 100644 --- a/src/robot_modeling/DQ_Kinematics_py.cpp +++ b/src/robot_modeling/DQ_Kinematics_py.cpp @@ -50,7 +50,7 @@ void init_DQ_Kinematics_py(py::module& m) dqkinematics_py.def( "set_reference_frame", &DQ_Kinematics::set_reference_frame, - py::arg("get_reference_frame"), + py::arg("reference_frame"), "Sets the reference frame used by the forward kinematics and Jacobian methods."); dqkinematics_py.def( "get_base_frame", @@ -59,7 +59,7 @@ void init_DQ_Kinematics_py(py::module& m) dqkinematics_py.def( "set_base_frame", &DQ_Kinematics::set_base_frame, - py::arg("get_base_frame"), + py::arg("base_frame"), "Sets the physical base frame of the robot in the workspace."); dqkinematics_py.def_static( diff --git a/src/robot_modeling/DQ_SerialManipulatorDH_py.cpp b/src/robot_modeling/DQ_SerialManipulatorDH_py.cpp index da375f0..0320514 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDH_py.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDH_py.cpp @@ -62,18 +62,34 @@ void init_DQ_SerialManipulatorDH_py(py::module& m) &DQ_SerialManipulatorDH::get_types, "Returns the joint-type row of the stored DH matrix as encoded joint types."); + dqserialmanipulatordh_py.def( + "raw_pose_jacobian", + (MatrixXd (DQ_SerialManipulatorDH::*)(const VectorXd&) const)&DQ_SerialManipulatorDH::raw_pose_jacobian, + py::arg("q_vec"), + "Computes the raw pose Jacobian up to the last link."); dqserialmanipulatordh_py.def( "raw_pose_jacobian", (MatrixXd (DQ_SerialManipulatorDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDH::raw_pose_jacobian, py::arg("q_vec"), py::arg("to_ith_link"), "Computes the raw pose Jacobian under the standard DH convention up to the requested link."); + dqserialmanipulatordh_py.def( + "raw_fkm", + (DQ (DQ_SerialManipulatorDH::*)(const VectorXd&) const)&DQ_SerialManipulatorDH::raw_fkm, + py::arg("q_vec"), + "Computes the raw forward kinematics up to the last link."); dqserialmanipulatordh_py.def( "raw_fkm", (DQ (DQ_SerialManipulatorDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDH::raw_fkm, py::arg("q_vec"), py::arg("to_ith_link"), "Computes the raw forward kinematics under the standard DH convention up to the requested link."); + dqserialmanipulatordh_py.def( + "raw_pose_jacobian_derivative", + (MatrixXd (DQ_SerialManipulatorDH::*)(const VectorXd&, const VectorXd&) const)&DQ_SerialManipulatorDH::raw_pose_jacobian_derivative, + py::arg("q"), + py::arg("q_dot"), + "Computes the time derivative of the raw pose Jacobian up to the last link."); dqserialmanipulatordh_py.def( "raw_pose_jacobian_derivative", (MatrixXd (DQ_SerialManipulatorDH::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_SerialManipulatorDH::raw_pose_jacobian_derivative, diff --git a/src/robot_modeling/DQ_SerialManipulatorDenso_py.cpp b/src/robot_modeling/DQ_SerialManipulatorDenso_py.cpp index 0aaf873..9a86203 100644 --- a/src/robot_modeling/DQ_SerialManipulatorDenso_py.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorDenso_py.cpp @@ -65,18 +65,34 @@ void init_DQ_SerialManipulatorDenso_py(py::module& m) &DQ_SerialManipulatorDenso::get_gammas, "Returns the gamma row of the stored DENSO matrix."); + dqserialmanipulatordh_py.def( + "raw_pose_jacobian", + (MatrixXd (DQ_SerialManipulatorDenso::*)(const VectorXd&) const)&DQ_SerialManipulatorDenso::raw_pose_jacobian, + py::arg("q_vec"), + "Computes the raw pose Jacobian up to the last link."); dqserialmanipulatordh_py.def( "raw_pose_jacobian", (MatrixXd (DQ_SerialManipulatorDenso::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDenso::raw_pose_jacobian, py::arg("q_vec"), py::arg("to_ith_link"), "Computes the raw pose Jacobian under the DENSO convention up to the requested link."); + dqserialmanipulatordh_py.def( + "raw_fkm", + (DQ (DQ_SerialManipulatorDenso::*)(const VectorXd&) const)&DQ_SerialManipulatorDenso::raw_fkm, + py::arg("q_vec"), + "Computes the raw forward kinematics up to the last link."); dqserialmanipulatordh_py.def( "raw_fkm", (DQ (DQ_SerialManipulatorDenso::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDenso::raw_fkm, py::arg("q_vec"), py::arg("to_ith_link"), "Computes the raw forward kinematics under the DENSO convention up to the requested link."); + dqserialmanipulatordh_py.def( + "raw_pose_jacobian_derivative", + (MatrixXd (DQ_SerialManipulatorDenso::*)(const VectorXd&, const VectorXd&) const)&DQ_SerialManipulatorDenso::raw_pose_jacobian_derivative, + py::arg("q"), + py::arg("q_dot"), + "Computes the time derivative of the raw pose Jacobian up to the last link."); dqserialmanipulatordh_py.def( "raw_pose_jacobian_derivative", (MatrixXd (DQ_SerialManipulatorDenso::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_SerialManipulatorDenso::raw_pose_jacobian_derivative, diff --git a/src/robot_modeling/DQ_SerialManipulatorMDH_py.cpp b/src/robot_modeling/DQ_SerialManipulatorMDH_py.cpp index dd05839..d7d8d97 100644 --- a/src/robot_modeling/DQ_SerialManipulatorMDH_py.cpp +++ b/src/robot_modeling/DQ_SerialManipulatorMDH_py.cpp @@ -63,18 +63,34 @@ void init_DQ_SerialManipulatorMDH_py(py::module& m) &DQ_SerialManipulatorMDH::get_types, "Returns the joint-type row of the stored modified DH matrix as encoded joint types."); + dqserialmanipulatormdh_py.def( + "raw_pose_jacobian", + (MatrixXd (DQ_SerialManipulatorMDH::*)(const VectorXd&) const)&DQ_SerialManipulatorMDH::raw_pose_jacobian, + py::arg("q_vec"), + "Computes the raw pose Jacobian up to the last link."); dqserialmanipulatormdh_py.def( "raw_pose_jacobian", (MatrixXd (DQ_SerialManipulatorMDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorMDH::raw_pose_jacobian, py::arg("q_vec"), py::arg("to_ith_link"), "Computes the raw pose Jacobian under the modified DH convention up to the requested link."); + dqserialmanipulatormdh_py.def( + "raw_fkm", + (DQ (DQ_SerialManipulatorMDH::*)(const VectorXd&) const)&DQ_SerialManipulatorMDH::raw_fkm, + py::arg("q_vec"), + "Computes the raw forward kinematics up to the last link."); dqserialmanipulatormdh_py.def( "raw_fkm", (DQ (DQ_SerialManipulatorMDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorMDH::raw_fkm, py::arg("q_vec"), py::arg("to_ith_link"), "Computes the raw forward kinematics under the modified DH convention up to the requested link."); + dqserialmanipulatormdh_py.def( + "raw_pose_jacobian_derivative", + (MatrixXd (DQ_SerialManipulatorMDH::*)(const VectorXd&, const VectorXd&) const)&DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative, + py::arg("q"), + py::arg("q_dot"), + "Computes the time derivative of the raw pose Jacobian up to the last link."); dqserialmanipulatormdh_py.def( "raw_pose_jacobian_derivative", (MatrixXd (DQ_SerialManipulatorMDH::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative, diff --git a/src/robot_modeling/DQ_SerialManipulator_py.cpp b/src/robot_modeling/DQ_SerialManipulator_py.cpp index da15b39..953e7d4 100644 --- a/src/robot_modeling/DQ_SerialManipulator_py.cpp +++ b/src/robot_modeling/DQ_SerialManipulator_py.cpp @@ -89,17 +89,36 @@ void init_DQ_SerialManipulator_py(py::module& m) (DQ (DQ_SerialManipulator::*)(const VectorXd&) const)&DQ_SerialManipulator::raw_fkm, py::arg("q_vec"), "Computes the raw forward kinematics up to the last link and returns the pose before applying the reference frame and the end effector."); + dqserialmanipulator_py.def( + "raw_fkm", + (DQ (DQ_SerialManipulator::*)(const VectorXd&, const int&) const)&DQ_SerialManipulator::raw_fkm, + py::arg("q_vec"), + py::arg("to_ith_link"), + "Computes the raw forward kinematics up to the requested link and returns the pose before applying the reference frame and the end effector."); dqserialmanipulator_py.def( "raw_pose_jacobian", (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&) const)&DQ_SerialManipulator::raw_pose_jacobian, py::arg("q_vec"), "Computes the raw pose Jacobian up to the last link, without reference-frame or end-effector transformations."); + dqserialmanipulator_py.def( + "raw_pose_jacobian", + (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&, const int&) const)&DQ_SerialManipulator::raw_pose_jacobian, + py::arg("q_vec"), + py::arg("to_ith_link"), + "Computes the raw pose Jacobian up to the requested link, without reference-frame or end-effector transformations."); dqserialmanipulator_py.def( "raw_pose_jacobian_derivative", (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&, const VectorXd&) const)&DQ_SerialManipulator::raw_pose_jacobian_derivative, py::arg("q"), py::arg("q_dot"), "Computes the time derivative of the raw pose Jacobian up to the last link."); + dqserialmanipulator_py.def( + "raw_pose_jacobian_derivative", + (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_SerialManipulator::raw_pose_jacobian_derivative, + py::arg("q"), + py::arg("q_dot"), + py::arg("to_ith_link"), + "Computes the time derivative of the raw pose Jacobian up to the requested link."); dqserialmanipulator_py.def( "fkm", diff --git a/src/utils/DQ_Geometry_py.cpp b/src/utils/DQ_Geometry_py.cpp index dfb312a..6bf6b18 100644 --- a/src/utils/DQ_Geometry_py.cpp +++ b/src/utils/DQ_Geometry_py.cpp @@ -97,14 +97,4 @@ void init_DQ_Geometry_py(py::module& m) py::arg("line_point_2"), py::arg("threshold") = DQ_threshold, "Checks whether a line and two endpoints define a valid line segment within the given threshold."); - //Overload with the default threshold - geometry_py.def_static("is_line_segment", - [](const DQ& line, const DQ& line_point_1, const DQ& line_point_2) - { - return DQ_Geometry::is_line_segment(line,line_point_1,line_point_2); - }, - py::arg("line"), - py::arg("line_point_1"), - py::arg("line_point_2"), - "Checks whether a line and two endpoints define a valid line segment using the library default threshold."); }