diff --git a/CMakeLists.txt b/CMakeLists.txt index e99caeb8..15a91fcc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,7 @@ include(FetchContent) find_package(Eigen3 REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) find_package(MuJoCo REQUIRED) - +find_package(pinocchio REQUIRED) FetchContent_Declare( libfranka GIT_REPOSITORY https://github.com/frankaemika/libfranka.git diff --git a/Makefile b/Makefile index b2f6c9a7..49fc50cc 100644 --- a/Makefile +++ b/Makefile @@ -27,8 +27,8 @@ stubgen: pybind11-stubgen -o python --numpy-array-use-type-var rcs find ./python -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete - find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' - find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' + find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g' + find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g' ruff check --fix python/rcs/_core isort python/rcs/_core black python/rcs/_core diff --git a/cmake/Findpinocchio.cmake b/cmake/Findpinocchio.cmake index d1da5ba9..3cf7d3f1 100644 --- a/cmake/Findpinocchio.cmake +++ b/cmake/Findpinocchio.cmake @@ -18,7 +18,7 @@ if (NOT pinocchio_FOUND) endif() # Check if the library file exists - cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio.so OUTPUT_VARIABLE pinocchio_library_path) + cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_default.so OUTPUT_VARIABLE pinocchio_library_path) if (NOT EXISTS ${pinocchio_library_path}) set(pinocchio_FOUND FALSE) if (pinocchio_FIND_REQUIRED) @@ -27,6 +27,16 @@ if (NOT pinocchio_FOUND) return() endif() + # Check if the library file exists + cmake_path(APPEND Python3_SITELIB cmeel.prefix lib libpinocchio_parsers.so OUTPUT_VARIABLE pinocchio_parsers_path) + if (NOT EXISTS ${pinocchio_parsers_path}) + set(pinocchio_FOUND FALSE) + if (pinocchio_FIND_REQUIRED) + message(FATAL_ERROR "Could not find pinocchio parsers path. Please install pinocchio using pip.") + endif() + return() + endif() + # Extract version from the library filename file(GLOB pinocchio_dist_info "${Python3_SITELIB}/pin-*.dist-info") cmake_path(GET pinocchio_dist_info FILENAME pinocchio_library_filename) @@ -38,8 +48,17 @@ if (NOT pinocchio_FOUND) target_include_directories(pinocchio::pinocchio INTERFACE ${pinocchio_INCLUDE_DIRS}) set_target_properties( pinocchio::pinocchio - PROPERTIES + PROPERTIES IMPORTED_LOCATION "${pinocchio_library_path}" ) + + add_library(pinocchio_parsers SHARED IMPORTED) + target_include_directories(pinocchio_parsers INTERFACE ${pinocchio_INCLUDE_DIRS}) + set_target_properties( + pinocchio_parsers + PROPERTIES + IMPORTED_LOCATION "${pinocchio_parsers_path}" + ) set(pinocchio_FOUND TRUE) + endif() diff --git a/pyproject.toml b/pyproject.toml index f41a8a55..d106907c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,7 +24,7 @@ dependencies = ["websockets>=11.0", # NOTE: when changing the mujoco version, also change it in requirements_dev.txt "mujoco==3.2.6", # NOTE: Same for pinocchio (pin) - "pin==2.7.0", + "pin==3.7.0", "tilburg-hand", "digit-interface", ] diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 6652c95f..981a5c88 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -60,7 +60,7 @@ def main(): if ROBOT_INSTANCE == RobotPlatform.SIMULATION: simulation = sim.Sim(rcs.scenes["fr3_empty_world"]["mjb"]) urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) + ik = rcs.common.RL(str(urdf_path)) cfg = sim.SimRobotConfig() cfg.add_id("0") cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) @@ -92,7 +92,7 @@ def main(): else: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) + ik = rcs.common.RL(str(urdf_path)) robot = rcs.hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 7216bc9f..3238eb55 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -21,7 +21,9 @@ __all__ = [ "IdentityRotMatrix", "IdentityRotQuatVec", "IdentityTranslation", + "Pin", "Pose", + "RL", "RPY", "Robot", "RobotConfig", @@ -62,11 +64,13 @@ class GripperState: pass class IK: - def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... - def forward(self, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... + def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ... def ik( - self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ... - ) -> numpy.ndarray[M, numpy.dtype[numpy.float64]] | None: ... + self, pose: Pose, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ... + ) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] | None: ... + +class Pin(IK): + def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = True) -> None: ... class Pose: def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ... @@ -80,26 +84,28 @@ class Pose: def __init__( self, rotation: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]], - translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], + translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], ) -> None: ... @typing.overload def __init__( self, - quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]], - translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], + quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]], + translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], ) -> None: ... @typing.overload - def __init__(self, rpy: RPY, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ... + def __init__( + self, rpy: RPY, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] + ) -> None: ... @typing.overload def __init__( self, - rpy_vector: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], - translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]], + rpy_vector: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], + translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]], ) -> None: ... @typing.overload - def __init__(self, translation: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ... + def __init__(self, translation: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ... @typing.overload - def __init__(self, quaternion: numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]) -> None: ... + def __init__(self, quaternion: numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]) -> None: ... @typing.overload def __init__(self, rpy: RPY) -> None: ... @typing.overload @@ -118,11 +124,14 @@ class Pose: def limit_translation_length(self, max_length: float) -> Pose: ... def pose_matrix(self) -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ... def rotation_m(self) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... - def rotation_q(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ... + def rotation_q(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ... def rotation_rpy(self) -> RPY: ... def total_angle(self) -> float: ... - def translation(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... - def xyzrpy(self) -> numpy.ndarray[typing.Literal[6], numpy.dtype[numpy.float64]]: ... + def translation(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ... + def xyzrpy(self) -> numpy.ndarray[tuple[typing.Literal[6]], numpy.dtype[numpy.float64]]: ... + +class RL(IK): + def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ... class RPY: pitch: float @@ -133,11 +142,11 @@ class RPY: @typing.overload def __init__(self, roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0) -> None: ... @typing.overload - def __init__(self, rpy: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]) -> None: ... + def __init__(self, rpy: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]) -> None: ... def __setstate__(self, arg0: tuple) -> None: ... def __str__(self) -> str: ... - def as_quaternion_vector(self) -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ... - def as_vector(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... + def as_quaternion_vector(self) -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ... + def as_vector(self) -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ... def is_close(self, other: RPY, eps: float = 1e-08) -> bool: ... def rotation_matrix( self, @@ -147,13 +156,13 @@ class Robot: def get_base_pose_in_world_coordinates(self) -> Pose: ... def get_cartesian_position(self) -> Pose: ... def get_ik(self) -> IK | None: ... - def get_joint_position(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def get_joint_position(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... def get_parameters(self) -> RobotConfig: ... def get_state(self) -> RobotState: ... def move_home(self) -> None: ... def reset(self) -> None: ... def set_cartesian_position(self, pose: Pose) -> None: ... - def set_joint_position(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ... + def set_joint_position(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ... def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ... def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ... @@ -168,7 +177,7 @@ class RobotMetaConfig: @property def joint_limits(self) -> numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]: ... @property - def q_home(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def q_home(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... class RobotPlatform: """ @@ -236,8 +245,8 @@ class RobotType: def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ... def IdentityRotMatrix() -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... -def IdentityRotQuatVec() -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ... -def IdentityTranslation() -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... +def IdentityRotQuatVec() -> numpy.ndarray[tuple[typing.Literal[4]], numpy.dtype[numpy.float64]]: ... +def IdentityTranslation() -> numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]: ... def _bootstrap_egl(fn_addr: int, display: int, context: int) -> None: ... def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ... diff --git a/python/rcs/_core/hw/__init__.pyi b/python/rcs/_core/hw/__init__.pyi index 995f33e4..19d7fea6 100644 --- a/python/rcs/_core/hw/__init__.pyi +++ b/python/rcs/_core/hw/__init__.pyi @@ -55,7 +55,7 @@ class FR3(rcs._core.common.Robot): def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ... def automatic_error_recovery(self) -> None: ... def controller_set_joint_position( - self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] + self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] ) -> None: ... def double_tap_robot_to_continue(self) -> None: ... def get_parameters(self) -> FR3Config: ... @@ -91,7 +91,7 @@ class FR3Config(rcs._core.common.RobotConfig): def __init__(self) -> None: ... class FR3Load: - f_x_cload: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]] | None + f_x_cload: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] | None load_inertia: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]] | None load_mass: float def __init__(self) -> None: ... diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 6cb64111..6c6e34fe 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -67,9 +67,9 @@ class CameraType: class FrameSet: def __init__(self) -> None: ... @property - def color_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.uint8]]]: ... + def color_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.uint8]]]: ... @property - def depth_frames(self) -> dict[str, numpy.ndarray[M, numpy.dtype[numpy.float32]]]: ... + def depth_frames(self) -> dict[str, numpy.ndarray[tuple[M], numpy.dtype[numpy.float32]]]: ... @property def timestamp(self) -> float: ... @@ -142,7 +142,7 @@ class SimRobot(rcs._core.common.Robot): ) -> None: ... def get_parameters(self) -> SimRobotConfig: ... def get_state(self) -> SimRobotState: ... - def set_joints_hard(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ... + def set_joints_hard(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ... def set_parameters(self, cfg: SimRobotConfig) -> bool: ... class SimRobotConfig(rcs._core.common.RobotConfig): @@ -173,9 +173,9 @@ class SimRobotState(rcs._core.common.RobotState): @property def is_moving(self) -> bool: ... @property - def previous_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def previous_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... @property - def target_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def target_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ... default_free: CameraType # value = fixed: CameraType # value = diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index e1c984f4..ab48b788 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -84,7 +84,7 @@ def __call__( # type: ignore """ if urdf_path is None: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robot = rcs.hw.FR3(ip, ik) robot.set_parameters(robot_cfg) @@ -136,7 +136,7 @@ def __call__( # type: ignore ) -> gym.Env: urdf_path = rcs.scenes["fr3_empty_world"]["urdf"] - ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + ik = rcs.common.RL(str(urdf_path)) if urdf_path is not None else None robots: dict[str, rcs.hw.FR3] = {} for ip in ips: robots[ip] = rcs.hw.FR3(ip, ik) @@ -232,7 +232,7 @@ def __call__( # type: ignore mjcf = rcs.scenes[mjcf]["mjb"] simulation = sim.Sim(mjcf) - ik = rcs.common.IK(str(urdf_path)) + ik = rcs.common.RL(str(urdf_path)) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) env: gym.Env = RobotEnv(robot, control_mode) env = RobotSimWrapper(env, simulation, sim_wrapper) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index c1698e1c..e3359608 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -204,7 +204,7 @@ def env_from_xml_paths( # TODO: this needs to support non FR3 robots assert isinstance(env.unwrapped, RobotEnv) simulation = sim.Sim(mjmld) - ik = rcs.common.IK(urdf, max_duration_ms=300) + ik = rcs.common.RL(urdf, max_duration_ms=300) cfg = default_fr3_sim_robot_cfg(mjmld, id) cfg.realtime = False if tcp_offset is not None: diff --git a/python/rcs/envs/space_utils.py b/python/rcs/envs/space_utils.py index 86eb39ab..537bdf88 100644 --- a/python/rcs/envs/space_utils.py +++ b/python/rcs/envs/space_utils.py @@ -17,10 +17,10 @@ M = TypeVar("M", bound=int) VecType: TypeAlias = np.ndarray[M, np.dtype[np.float64]] -Vec7Type: TypeAlias = np.ndarray[Literal[7], np.dtype[np.float64]] -Vec3Type: TypeAlias = np.ndarray[Literal[3], np.dtype[np.float64]] -Vec6Type: TypeAlias = np.ndarray[Literal[6], np.dtype[np.float64]] -Vec18Type: TypeAlias = np.ndarray[Literal[18], np.dtype[np.float64]] +Vec7Type: TypeAlias = np.ndarray[tuple[Literal[7]], np.dtype[np.float64]] +Vec3Type: TypeAlias = np.ndarray[tuple[Literal[3]], np.dtype[np.float64]] +Vec6Type: TypeAlias = np.ndarray[tuple[Literal[6]], np.dtype[np.float64]] +Vec18Type: TypeAlias = np.ndarray[tuple[Literal[18]], np.dtype[np.float64]] class RCSpaceType(TypedDict): ... diff --git a/python/rcs_so101/creators.py b/python/rcs_so101/creators.py index 32d1132c..5d4b79cb 100644 --- a/python/rcs_so101/creators.py +++ b/python/rcs_so101/creators.py @@ -118,7 +118,7 @@ def __call__( # type: ignore """ simulation = sim.Sim(mjcf) - ik = rcs.common.IK(str(urdf_path)) + ik = rcs.common.RL(str(urdf_path)) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) env: gym.Env = RobotEnv(robot, control_mode) env = RobotSimWrapper(env, simulation, sim_wrapper) diff --git a/python/rcs_so101/hw.py b/python/rcs_so101/hw.py index ee4e9ec7..54367003 100644 --- a/python/rcs_so101/hw.py +++ b/python/rcs_so101/hw.py @@ -7,7 +7,7 @@ class SO101(common.Robot): def __init__(self, hf_robot: SO101Follower, urdf_path: str): - self.ik = common.IK(urdf_path=urdf_path) + self.ik = common.RL(urdf_path=urdf_path) self._hf_robot = hf_robot self._hf_robot.connect() @@ -18,7 +18,7 @@ def get_cartesian_position(self) -> common.Pose: def get_ik(self) -> common.IK | None: return self.ik - def get_joint_position(self) -> np.ndarray[typing.Literal[5], np.dtype[np.float64]]: # type: ignore + def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore obs = self._hf_robot.get_observation() return np.array( [ @@ -43,7 +43,7 @@ def get_state(self) -> common.RobotState: def move_home(self) -> None: home = typing.cast( - np.ndarray[typing.Literal[5], np.dtype[np.float64]], + np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]], common.robots_meta_config(common.RobotType.SO101).q_home, ) self.set_joint_position(home) @@ -56,7 +56,7 @@ def set_cartesian_position(self, pose: common.Pose) -> None: if joints is not None: self.set_joint_position(joints) - def set_joint_position(self, q: np.ndarray[typing.Literal[5], np.dtype[np.float64]]) -> None: # type: ignore + def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore self._hf_robot.send_action( { "shoulder_pan.pos": q[0], diff --git a/requirements_dev.txt b/requirements_dev.txt index b1ff0539..25a38953 100644 --- a/requirements_dev.txt +++ b/requirements_dev.txt @@ -11,5 +11,5 @@ scikit-build-core>=0.3.3 poetry-core>=2.1.3 pybind11 mujoco==3.2.6 -pin==2.7.0 +pin==3.7.0 wheel diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index 72e7733d..abccdf63 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,3 +1,3 @@ add_library(common) target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp utils.cpp) -target_link_libraries(common PUBLIC Eigen3::Eigen mdl) +target_link_libraries(common PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio pinocchio_parsers) diff --git a/src/common/IK.cpp b/src/common/IK.cpp index e100024d..a413e5f9 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -12,45 +12,116 @@ #include #include +#include +#include +#include +#include +#include +#include + namespace rcs { namespace common { -IK::IK(const std::string& urdf_path, size_t max_duration_ms) : rl() { - this->rl = RL(); - - this->rl.mdl = rl::mdl::UrdfFactory().create(urdf_path); - this->rl.kin = std::dynamic_pointer_cast(this->rl.mdl); - this->rl.ik = - std::make_shared(this->rl.kin.get()); - this->rl.ik->setRandomRestarts(0); - this->rl.ik->setEpsilon(1e-3); - this->rl.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); +RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() { + this->rl_data.mdl = rl::mdl::UrdfFactory().create(urdf_path); + this->rl_data.kin = + std::dynamic_pointer_cast(this->rl_data.mdl); + this->rl_data.ik = std::make_shared( + this->rl_data.kin.get()); + this->rl_data.ik->setRandomRestarts(this->random_restarts); + this->rl_data.ik->setEpsilon(this->eps); + this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); } -std::optional IK::ik(const Pose& pose, const VectorXd& q0, +std::optional RL::ik(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset) { // pose is assumed to be in the robots coordinate frame - this->rl.kin->setPosition(q0); - this->rl.kin->forwardPosition(); + this->rl_data.kin->setPosition(q0); + this->rl_data.kin->forwardPosition(); rcs::common::Pose new_pose = pose * tcp_offset.inverse(); - this->rl.ik->addGoal(new_pose.affine_matrix(), 0); - bool success = this->rl.ik->solve(); + this->rl_data.ik->addGoal(new_pose.affine_matrix(), 0); + bool success = this->rl_data.ik->solve(); if (success) { // is this forward needed and is it mabye possible to call // this on the model? - this->rl.kin->forwardPosition(); - return this->rl.kin->getPosition(); + this->rl_data.kin->forwardPosition(); + return this->rl_data.kin->getPosition(); + } else { + return std::nullopt; + } +} +Pose RL::forward(const VectorXd& q0, const Pose& tcp_offset) { + // pose is assumed to be in the robots coordinate frame + this->rl_data.kin->setPosition(q0); + this->rl_data.kin->forwardPosition(); + rcs::common::Pose pose = this->rl_data.kin->getOperationalPosition(0); + // apply the tcp offset + return pose * tcp_offset.inverse(); +} + +Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf = true) + : model() { + if (urdf) { + pinocchio::urdf::buildModel(path, this->model); + } else { + pinocchio::mjcf::buildModel(path, this->model); + } + this->data = pinocchio::Data(this->model); + this->FRAME_ID = model.getFrameId(frame_id); + if (FRAME_ID == -1) { + throw std::runtime_error( + frame_id + " frame id could not be found in the provided URDF"); + } +} + +std::optional Pin::ik(const Pose& pose, const VectorXd& q0, + const Pose& tcp_offset) { + rcs::common::Pose new_pose = pose * tcp_offset.inverse(); + VectorXd q(q0); + const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); + pinocchio::Data::Matrix6x J(6, model.nv); + J.setZero(); + bool success = false; + Vector6d err; + Eigen::VectorXd v(model.nv); + for (int i = 0;; i++) { + pinocchio::forwardKinematics(model, data, q); + pinocchio::updateFramePlacements(model, data); + const pinocchio::SE3 iMd = data.oMf[this->FRAME_ID].actInv(oMdes); + err = pinocchio::log6(iMd).toVector(); + if (err.norm() < this->eps) { + success = true; + break; + } + if (i >= this->IT_MAX) { + success = false; + break; + } + pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J); + pinocchio::Data::Matrix6 Jlog; + pinocchio::Jlog6(iMd.inverse(), Jlog); + J = -Jlog * J; + pinocchio::Data::Matrix6 JJt; + JJt.noalias() = J * J.transpose(); + JJt.diagonal().array() += this->damp; + v.noalias() = -J.transpose() * JJt.ldlt().solve(err); + q = pinocchio::integrate(model, q, v * this->DT); + } + if (success) { + return q; } else { return std::nullopt; } } -Pose IK::forward(const VectorXd& q0, const Pose& tcp_offset) { +Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) { // pose is assumed to be in the robots coordinate frame - this->rl.kin->setPosition(q0); - this->rl.kin->forwardPosition(); - rcs::common::Pose pose = this->rl.kin->getOperationalPosition(0); + pinocchio::forwardKinematics(model, data, q0); + pinocchio::updateFramePlacements(model, data); + rcs::common::Pose pose(data.oMf[this->FRAME_ID].rotation(), + data.oMf[this->FRAME_ID].translation()); + // apply the tcp offset return pose * tcp_offset.inverse(); } diff --git a/src/common/IK.h b/src/common/IK.h index 4912c2da..5c13529a 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -8,30 +8,62 @@ #include #include #include +#include +#include #include "Pose.h" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/algorithm/kinematics.hpp" #include "utils.h" namespace rcs { namespace common { -struct RL { - std::shared_ptr mdl; - std::shared_ptr kin; - std::shared_ptr ik; +class IK { + public: + virtual ~IK(){}; + virtual std::optional ik( + const Pose& pose, const VectorXd& q0, + const Pose& tcp_offset = Pose::Identity()) = 0; + virtual Pose forward(const VectorXd& q0, const Pose& tcp_offset) = 0; }; -class IK { +class RL : public IK { private: - RL rl; + const int random_restarts = 0; + const double eps = 1e-3; + struct { + std::shared_ptr mdl; + std::shared_ptr kin; + std::shared_ptr ik; + } rl_data; public: - IK(const std::string& urdf_path, size_t max_duration_ms = 300); - std::optional ik(const Pose& pose, const VectorXd& q0, - const Pose& tcp_offset = Pose::Identity()); - Pose forward(const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()); + RL(const std::string& path, size_t max_duration_ms = 300); + std::optional ik( + const Pose& pose, const VectorXd& q0, + const Pose& tcp_offset = Pose::Identity()) override; + Pose forward(const VectorXd& q0, const Pose& tcp_offset) override; +}; - // TODO: set max time +class Pin : public IK { + private: + const double eps = 1e-4; + const int IT_MAX = 1000; + const double DT = 1e-1; + const double damp = 1e-6; + int FRAME_ID; + + pinocchio::Model model; + pinocchio::Data data; + + public: + Pin(const std::string& path, const std::string& frame_id, bool urdf); + std::optional ik( + const Pose& pose, const VectorXd& q0, + const Pose& tcp_offset = Pose::Identity()) override; + Pose forward(const VectorXd& q0, const Pose& tcp_offset) override; }; } // namespace common } // namespace rcs diff --git a/src/hw/CMakeLists.txt b/src/hw/CMakeLists.txt index a8669235..a0edd471 100644 --- a/src/hw/CMakeLists.txt +++ b/src/hw/CMakeLists.txt @@ -6,5 +6,5 @@ target_link_libraries( PRIVATE math ) -add_executable(test_hw main.cpp) -target_link_libraries(test_hw PRIVATE hw) +# add_executable(test_hw main.cpp) +# target_link_libraries(test_hw PRIVATE hw) diff --git a/src/hw/main.cpp b/src/hw/main.cpp index 6c815142..2e2c89b5 100644 --- a/src/hw/main.cpp +++ b/src/hw/main.cpp @@ -17,7 +17,7 @@ const string urdf_path = "models/urdf/fr3.urdf"; int main() { try { - auto ik = make_shared(urdf_path); + auto ik = make_shared(urdf_path); rcs::hw::FR3 robot(ip, ik); robot.automatic_error_recovery(); std::cout << "WARNING: This example will move the robot! " diff --git a/src/pybind/CMakeLists.txt b/src/pybind/CMakeLists.txt index ff76e118..fc3e987a 100644 --- a/src/pybind/CMakeLists.txt +++ b/src/pybind/CMakeLists.txt @@ -3,7 +3,7 @@ target_link_libraries(_core PRIVATE hw sim common) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) set_target_properties(_core PROPERTIES - INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco" + INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco;$ORIGIN/../cmeel.prefix/lib" INTERPROCEDURAL_OPTIMIZATION TRUE ) set_target_properties(franka PROPERTIES diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 1aa6387f..a473dd59 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -224,13 +224,22 @@ PYBIND11_MODULE(_core, m) { })); py::class_>(common, "IK") - .def(py::init(), py::arg("urdf_path"), - py::arg("max_duration_ms") = 300) .def("ik", &rcs::common::IK::ik, py::arg("pose"), py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity()) .def("forward", &rcs::common::IK::forward, py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity()); + py::class_>(common, "RL") + .def(py::init(), py::arg("urdf_path"), + py::arg("max_duration_ms") = 300); + + py::class_>(common, "Pin") + .def(py::init(), + py::arg("path"), py::arg("frame_id") = "fr3_link8", + py::arg("urdf") = true); + py::enum_(common, "RobotType") .value("FR3", rcs::common::RobotType::FR3) .value("UR5e", rcs::common::RobotType::UR5e) diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index f04d5639..91f4bebc 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -5,4 +5,4 @@ target_sources(sim target_link_libraries(sim PUBLIC common MuJoCo::MuJoCo) #add_executable(test_sim test.cpp sim.cpp) -#target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common egl) +#target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common egl pinocchio::pinocchio) diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 5e7995af..174e0d0e 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -15,7 +15,8 @@ // const std::string mjcf = MODEL_DIR "/mjcf/fr3_modular/scene.xml"; // const std::string urdf = MODEL_DIR "/fr3/urdf/fr3_from_panda.urdf"; const std::string mjcf = "build/_deps/scenes-src/scenes/lab/scene.xml"; -const std::string urdf = "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf"; +const std::string urdf_path = + "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf"; static const Eigen::Matrix iso_cube_center( 0.498, 0.0, 0.226); static const float iso_cube_size = 0.4; @@ -126,7 +127,7 @@ int test_sim() { sim->set_config(cfg); std::string id = "0"; - auto ik = std::make_shared(urdf); + auto ik = std::make_shared(urdf_path); auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset()); rcs::sim::SimRobotConfig fr3_config; fr3_config.tcp_offset = tcp_offset;