diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 16f2ba14..886954c3 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -35,7 +35,7 @@ jobs: - name: Install wheel run: python -m pip install wheel - name: Install the package - run: python -m pip install -v --no-build-isolation '.[dev]' + run: python -m pip install -ve '.[dev]' --no-build-isolation - name: Check formatting run: make pycheckformat - name: Code linting @@ -95,10 +95,10 @@ jobs: run: pip install -r requirements.txt - name: Install rcs - run: pip install -v --no-build-isolation '.[dev]' + run: pip install -ve '.[dev]' --no-build-isolation - name: Install extension - run: pip install -v --no-build-isolation extensions/${{ matrix.extension }} + run: pip install -ve extensions/${{ matrix.extension }} --no-build-isolation build_cpp_extensions: needs: [pythonpackage, check-paths] @@ -138,9 +138,9 @@ jobs: - name: Install Python dependencies run: pip install -r requirements.txt - name: Install RCS - run: pip install -v --no-build-isolation '.[dev]' + run: pip install -ve '.[dev]' --no-build-isolation - name: Install extension - run: pip install -v --no-build-isolation extensions/${{ matrix.extension }} + run: pip install -ve extensions/${{ matrix.extension }} --no-build-isolation - name: Check that stub files are up-to-date run: make -C extensions/${{ matrix.extension }} stubgen && git diff --exit-code - name: Check clang format diff --git a/extensions/rcs_fr3/pyproject.toml b/extensions/rcs_fr3/pyproject.toml index 977531cb..ed0ce96b 100644 --- a/extensions/rcs_fr3/pyproject.toml +++ b/extensions/rcs_fr3/pyproject.toml @@ -7,7 +7,8 @@ name = "rcs_fr3" version = "0.5.2" description="RCS libfranka integration" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", + "frankik @ git+https://github.com/juelg/frankik", ] readme = "README.md" maintainers = [ @@ -29,7 +30,7 @@ install.components = ["python_package"] [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" \ No newline at end of file diff --git a/extensions/rcs_fr3/src/rcs_fr3/__init__.py b/extensions/rcs_fr3/src/rcs_fr3/__init__.py index c4b5606c..309eb288 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/__init__.py +++ b/extensions/rcs_fr3/src/rcs_fr3/__init__.py @@ -1,8 +1,9 @@ from rcs_fr3 import desk, envs -from rcs_fr3._core import hw +from rcs_fr3._core import __version__, hw __all__ = [ "desk", "hw", "envs", + "__version__", ] diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 0683256c..2dedb09b 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -1,9 +1,12 @@ import logging +import typing from os import PathLike import gymnasium as gym import numpy as np import rcs.hand.tilburg_hand +from frankik import FrankaKinematics +from rcs._core.common import Kinematics, Pose from rcs.camera.hw import HardwareCameraSet from rcs.envs.base import ( CameraSetWrapper, @@ -27,6 +30,30 @@ logger.setLevel(logging.INFO) +class FrankIK(Kinematics): + def __init__(self, allow_elbow_flips: bool = False): + Kinematics.__init__(self) + self.allow_elbow_flips = allow_elbow_flips + self.kin = FrankaKinematics(robot_type="fr3") + + def forward(self, q0: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]], tcp_offset: Pose) -> Pose: # type: ignore + print("forward called") + return Pose(pose_matrix=self.kin.forward(q0, tcp_offset.pose_matrix())) + + def inverse( # type: ignore + self, pose: Pose, q0: np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]], tcp_offset: Pose + ) -> np.ndarray[tuple[typing.Literal[7]], np.dtype[np.float64]] | None: + tcp_offset = self.kin.FrankaHandTCPOffset + return self.kin.inverse( + pose.pose_matrix(), q0, tcp_offset.pose_matrix(), allow_elbow_flips=self.allow_elbow_flips + ) + + +# FYI: this needs to be in global namespace to avoid auto garbage collection issues +# pybind11 3.x would avoid this but with smart_holder but we cannot update due to the subfiles issue yet +FastIK = FrankIK() + + class RCSFR3EnvCreator(RCSHardwareEnvCreator): def __call__( # type: ignore self, @@ -62,6 +89,7 @@ def __call__( # type: ignore robot_cfg.attachment_site, urdf=robot_cfg.kinematic_model_path.endswith(".urdf"), ) + # ik = FastIK # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path) robot = hw.Franka(ip, ik) robot.set_config(robot_cfg) diff --git a/extensions/rcs_panda/pyproject.toml b/extensions/rcs_panda/pyproject.toml index 8d741532..6d030056 100644 --- a/extensions/rcs_panda/pyproject.toml +++ b/extensions/rcs_panda/pyproject.toml @@ -7,7 +7,7 @@ name = "rcs_panda" version = "0.5.2" description="RCS libfranka integration" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", ] readme = "README.md" maintainers = [ @@ -29,7 +29,7 @@ install.components = ["python_package"] [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" \ No newline at end of file diff --git a/extensions/rcs_panda/src/rcs_panda/__init__.py b/extensions/rcs_panda/src/rcs_panda/__init__.py index f22ba0da..7730a114 100644 --- a/extensions/rcs_panda/src/rcs_panda/__init__.py +++ b/extensions/rcs_panda/src/rcs_panda/__init__.py @@ -1,7 +1,8 @@ from rcs_panda import envs -from rcs_panda._core import hw +from rcs_panda._core import __version__, hw __all__ = [ "hw", "envs", + "__version__", ] diff --git a/extensions/rcs_realsense/pyproject.toml b/extensions/rcs_realsense/pyproject.toml index e6413409..3c80c5ea 100644 --- a/extensions/rcs_realsense/pyproject.toml +++ b/extensions/rcs_realsense/pyproject.toml @@ -7,7 +7,7 @@ name = "rcs_realsense" version = "0.5.2" description="RCS realsense module" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", "pyrealsense2~=2.55.1", "pupil_apriltags", "diskcache", @@ -22,7 +22,7 @@ requires-python = ">=3.11" [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" diff --git a/extensions/rcs_realsense/src/rcs_realsense/__init__.py b/extensions/rcs_realsense/src/rcs_realsense/__init__.py index e69de29b..72251527 100644 --- a/extensions/rcs_realsense/src/rcs_realsense/__init__.py +++ b/extensions/rcs_realsense/src/rcs_realsense/__init__.py @@ -0,0 +1 @@ +__version__ = "0.5.2" diff --git a/extensions/rcs_robotics_library/pyproject.toml b/extensions/rcs_robotics_library/pyproject.toml index 051757a2..64804f65 100644 --- a/extensions/rcs_robotics_library/pyproject.toml +++ b/extensions/rcs_robotics_library/pyproject.toml @@ -8,7 +8,7 @@ version = "0.5.2" description="RCS robotics library integration" readme = "README.md" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", ] maintainers = [ { name = "Tobias Jülg", email = "tobias.juelg@utn.de" }, @@ -30,7 +30,7 @@ install.components = ["python_package"] [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" \ No newline at end of file diff --git a/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py b/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py index 684b6b35..ff5f354a 100644 --- a/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py +++ b/extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.py @@ -1,5 +1,6 @@ -from rcs_robotics_library._core import rl +from rcs_robotics_library._core import __version__, rl __all__ = [ "rl", + "__version__", ] diff --git a/extensions/rcs_so101/pyproject.toml b/extensions/rcs_so101/pyproject.toml index 78bb8c35..d76a1625 100644 --- a/extensions/rcs_so101/pyproject.toml +++ b/extensions/rcs_so101/pyproject.toml @@ -7,7 +7,7 @@ name = "rcs_so101" version = "0.5.2" description="RCS SO101 module" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", "lerobot==0.3.3", ] readme = "README.md" @@ -29,7 +29,7 @@ install.components = ["python_package"] [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" \ No newline at end of file diff --git a/extensions/rcs_so101/src/rcs_so101/__init__.py b/extensions/rcs_so101/src/rcs_so101/__init__.py index f710b730..210e5e0b 100644 --- a/extensions/rcs_so101/src/rcs_so101/__init__.py +++ b/extensions/rcs_so101/src/rcs_so101/__init__.py @@ -1,6 +1,6 @@ +from rcs_so101._core import __version__ from rcs_so101._core.so101_ik import SO101IK +from rcs_so101.creators import RCSSO101EnvCreator +from rcs_so101.hw import SO101, SO101Config, SO101Gripper -from .creators import RCSSO101EnvCreator -from .hw import SO101, SO101Config, SO101Gripper - -__all__ = ["SO101IK", "RCSSO101EnvCreator", "SO101", "SO101Config", "SO101Gripper"] +__all__ = ["SO101IK", "RCSSO101EnvCreator", "SO101", "SO101Config", "SO101Gripper", "__version__"] diff --git a/extensions/rcs_tacto/pyproject.toml b/extensions/rcs_tacto/pyproject.toml index 21476cfd..d1ffc97f 100644 --- a/extensions/rcs_tacto/pyproject.toml +++ b/extensions/rcs_tacto/pyproject.toml @@ -7,7 +7,7 @@ name = "rcs_tacto" version = "0.5.0" description="RCS integration of tacto" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", "mujoco-tacto@git+https://github.com/utn-air/mujoco-tacto.git@main" ] readme = "README.md" @@ -22,7 +22,7 @@ requires-python = ">=3.10" [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" diff --git a/extensions/rcs_tacto/src/rcs_tacto/__init__.py b/extensions/rcs_tacto/src/rcs_tacto/__init__.py new file mode 100644 index 00000000..72251527 --- /dev/null +++ b/extensions/rcs_tacto/src/rcs_tacto/__init__.py @@ -0,0 +1 @@ +__version__ = "0.5.2" diff --git a/extensions/rcs_ur5e/pyproject.toml b/extensions/rcs_ur5e/pyproject.toml index 6afefe9f..400a822d 100644 --- a/extensions/rcs_ur5e/pyproject.toml +++ b/extensions/rcs_ur5e/pyproject.toml @@ -7,7 +7,7 @@ name = "rcs_ur5e" version = "0.5.2" description="RCS UR5e module" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", "ur_rtde==1.6.1", ] readme = "README.md" @@ -19,9 +19,11 @@ authors = [ { name = "Johannes Hechtl", email = "johannes.hechtl@siemens.com" }, ] requires-python = ">=3.10" -classifiers = [ - "Development Status :: 3 - Alpha", - "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)", - "Programming Language :: Python :: 3", -] + +[tool.black] +line-length = 120 +target-version = ["py310"] + +[tool.isort] +profile = "black" diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/__init__.py b/extensions/rcs_ur5e/src/rcs_ur5e/__init__.py index e69de29b..72251527 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/__init__.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/__init__.py @@ -0,0 +1 @@ +__version__ = "0.5.2" diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py index 5eaa44e2..8aa1aa35 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py @@ -278,7 +278,7 @@ def set_cartesian_position(self, pose: common.Pose) -> None: if q is None: print("IK failed") return - self.set_joint_position(q[0:6]) + self.set_joint_position(q[0:6]) # type: ignore return def set_joint_position(self, q: np.ndarray[tuple[typing.Any], np.dtype[np.float64]]) -> None: diff --git a/extensions/rcs_usb_cam/pyproject.toml b/extensions/rcs_usb_cam/pyproject.toml index f1ae241d..5f5c621a 100644 --- a/extensions/rcs_usb_cam/pyproject.toml +++ b/extensions/rcs_usb_cam/pyproject.toml @@ -7,7 +7,7 @@ name = "rcs_usb_cam" version = "0.5.0" description="RCS USB Camera module" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", "opencv-python~=4.10.0" ] maintainers = [ @@ -21,7 +21,7 @@ requires-python = ">=3.10" [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" \ No newline at end of file diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py index e69de29b..72251527 100644 --- a/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py +++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py @@ -0,0 +1 @@ +__version__ = "0.5.2" diff --git a/extensions/rcs_xarm7/pyproject.toml b/extensions/rcs_xarm7/pyproject.toml index 1acfebe2..124f4559 100644 --- a/extensions/rcs_xarm7/pyproject.toml +++ b/extensions/rcs_xarm7/pyproject.toml @@ -7,7 +7,7 @@ name = "rcs_xarm7" version = "0.5.2" description="RCS xArm7 module" dependencies = [ - "rcs>=0.5.0", + "rcs>=0.5.2", "xarm-python-sdk==1.17.0", ] readme = "README.md" @@ -23,7 +23,7 @@ requires-python = ">=3.10" [tool.black] line-length = 120 -target-version = ["py311"] +target-version = ["py310"] [tool.isort] profile = "black" diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py b/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py index e69de29b..72251527 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/__init__.py @@ -0,0 +1 @@ +__version__ = "0.5.2" diff --git a/pyproject.toml b/pyproject.toml index 84acbd70..a92070a1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -155,20 +155,41 @@ version_files = [ "CMakeLists.txt:VERSION", "python/rcs/_core/__init__.pyi:__version__", + # --- FR3 Extension --- "extensions/rcs_fr3/CMakeLists.txt:VERSION", "extensions/rcs_fr3/src/rcs_fr3/_core/__init__.pyi:__version__", - "extensions/rcs_fr3/pyproject.toml:version", + "extensions/rcs_fr3/pyproject.toml:version", + # The line below updates the dependency string "rcs>=x.y.z" + "extensions/rcs_fr3/pyproject.toml:\"rcs>=(.*)\"", + # --- Panda Extension --- "extensions/rcs_panda/CMakeLists.txt:VERSION", "extensions/rcs_panda/src/rcs_fr3/_core/__init__.pyi:__version__", "extensions/rcs_panda/pyproject.toml:version", + "extensions/rcs_panda/pyproject.toml:\"rcs>=(.*)\"", + # --- Robotics Library --- "extensions/rcs_robotics_library/CMakeLists.txt:VERSION", "extensions/rcs_robotics_library/src/rcs_robotics_library/_core/__init__.pyi:__version__", "extensions/rcs_robotics_library/pyproject.toml:version", + "extensions/rcs_robotics_library/pyproject.toml:\"rcs>=(.*)\"", - "extensions/rcs_realsense/pyproject.toml:version", + # --- SO101 --- + "extensions/rcs_so101/CMakeLists.txt:VERSION", + "extensions/rcs_so101/src/rcs_so101/_core/__init__.pyi:__version__", "extensions/rcs_so101/pyproject.toml:version", + "extensions/rcs_so101/pyproject.toml:\"rcs>=(.*)\"", + + # --- Other Extensions --- + "extensions/rcs_realsense/pyproject.toml:version", + "extensions/rcs_realsense/src/rcs_realsense/__init__.py:__version__", + "extensions/rcs_realsense/pyproject.toml:\"rcs>=(.*)\"", + "extensions/rcs_xarm7/pyproject.toml:version", + "extensions/rcs_xarm7/src/rcs_xarm7/__init__.py:__version__", + "extensions/rcs_xarm7/pyproject.toml:\"rcs>=(.*)\"", + "extensions/rcs_ur5e/pyproject.toml:version", + "extensions/rcs_ur5e/src/rcs_ur5e/__init__.py:__version__", + "extensions/rcs_ur5e/pyproject.toml:\"rcs>=(.*)\"", ] diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 9ac1f819..0e51534d 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -130,6 +130,7 @@ class HandState: def __init__(self) -> None: ... class Kinematics: + def __init__(self) -> None: ... def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... def inverse( self, pose: Pose, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ... diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index be86cd68..6496ee1f 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -183,6 +183,29 @@ class PyHand : public rcs::common::Hand { } }; +// trampoline kinematics +class PyKinematics : public rcs::common::Kinematics { + // TODO: use line below when upgraded to pybind11 3.x + // public py::trampoline_self_life_support { + public: + using rcs::common::Kinematics::Kinematics; // Inherit constructors + + std::optional inverse( + const rcs::common::Pose& pose, const rcs::common::VectorXd& q0, + const rcs::common::Pose& tcp_offset = + rcs::common::Pose::Identity()) override { + PYBIND11_OVERRIDE_PURE(std::optional, + rcs::common::Kinematics, inverse, pose, q0, + tcp_offset); + } + + rcs::common::Pose forward(const rcs::common::VectorXd& q0, + const rcs::common::Pose& tcp_offset) override { + PYBIND11_OVERRIDE_PURE(rcs::common::Pose, rcs::common::Kinematics, forward, + q0, tcp_offset); + } +}; + PYBIND11_MODULE(_core, m) { m.doc() = R"pbdoc( Robot Control Stack Python Bindings @@ -296,8 +319,12 @@ PYBIND11_MODULE(_core, m) { return rcs::common::Pose(t); })); - py::class_>( + py::class_>( + // TODO: use line below when upgraded to pybind11 3.x + // py::class_( common, "Kinematics") + .def(py::init<>()) .def("inverse", &rcs::common::Kinematics::inverse, py::arg("pose"), py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity()) .def("forward", &rcs::common::Kinematics::forward, py::arg("q0"),