diff --git a/CMakeLists.txt b/CMakeLists.txt index bc18ee7e1..9ccfe3cdb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,9 +45,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL ) FetchContent_Declare( diff --git a/README.md b/README.md index 864c7673d..1ac6724ab 100644 --- a/README.md +++ b/README.md @@ -82,6 +82,7 @@ if __name__ == "__main__": ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) # base env diff --git a/docs/getting_started/index.md b/docs/getting_started/index.md index d7e60720a..39e431cda 100644 --- a/docs/getting_started/index.md +++ b/docs/getting_started/index.md @@ -71,7 +71,7 @@ import numpy as np # Load simulation scene robot_meta = rcs.ROBOTS[rcs.common.RobotType.FR3] simulation = sim.Sim(rcs.SCENE_PATHS["empty_world"]) -ik = rcs.common.Pin(robot_meta.mjcf_model_path, robot_meta.attachment_site) +ik = rcs.common.Pin(robot_meta.mjcf_model_path, robot_meta.attachment_site, joint_limits=robot_meta.joint_limits) # Configure robot cfg = sim.SimRobotConfig() diff --git a/examples/fr3/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py index 7edb1743f..342f1dec9 100644 --- a/examples/fr3/fr3_direct_control.py +++ b/examples/fr3/fr3_direct_control.py @@ -79,6 +79,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) @@ -115,6 +116,7 @@ def main(): fr3_cfg.kinematic_model_path, fr3_cfg.attachment_site, urdf=fr3_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=fr3_cfg.joint_limits, ) robot = hw.Franka(fr3_cfg, ik) diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py index ab0b5fea2..982292fc5 100644 --- a/examples/fr3/fr3_env_cartesian_control.py +++ b/examples/fr3/fr3_env_cartesian_control.py @@ -59,6 +59,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/fr3/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py index 094358d23..50ba5e990 100644 --- a/examples/fr3/fr3_env_joint_control.py +++ b/examples/fr3/fr3_env_joint_control.py @@ -59,6 +59,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(cfg) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index 95d08d2cd..195955b32 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -41,6 +41,7 @@ ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) # base env diff --git a/examples/so101/so101_env_cartesian_control.py b/examples/so101/so101_env_cartesian_control.py index fe8facd0d..62f29618d 100644 --- a/examples/so101/so101_env_cartesian_control.py +++ b/examples/so101/so101_env_cartesian_control.py @@ -40,6 +40,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/so101/so101_env_joint_control.py b/examples/so101/so101_env_joint_control.py index 470dea926..d117a37cc 100644 --- a/examples/so101/so101_env_joint_control.py +++ b/examples/so101/so101_env_joint_control.py @@ -38,6 +38,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(cfg) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/ur5e/ur5e_env_cartesian_control.py b/examples/ur5e/ur5e_env_cartesian_control.py index c323df624..6b1a261ac 100644 --- a/examples/ur5e/ur5e_env_cartesian_control.py +++ b/examples/ur5e/ur5e_env_cartesian_control.py @@ -59,6 +59,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/ur5e/ur5e_env_joint_control.py b/examples/ur5e/ur5e_env_joint_control.py index 8e32dd497..635920381 100644 --- a/examples/ur5e/ur5e_env_joint_control.py +++ b/examples/ur5e/ur5e_env_joint_control.py @@ -58,6 +58,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(sim_cfg_data) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/xarm7/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py index a9dc0ef73..95104b71b 100644 --- a/examples/xarm7/xarm7_env_cartesian_control.py +++ b/examples/xarm7/xarm7_env_cartesian_control.py @@ -64,6 +64,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/xarm7/xarm7_env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py index 7f77575aa..c27edbebc 100644 --- a/examples/xarm7/xarm7_env_joint_control.py +++ b/examples/xarm7/xarm7_env_joint_control.py @@ -61,6 +61,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(sim_cfg_data) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/yam/yam_env_cartesian_control.py b/examples/yam/yam_env_cartesian_control.py index ec99726ee..c2d1d3845 100644 --- a/examples/yam/yam_env_cartesian_control.py +++ b/examples/yam/yam_env_cartesian_control.py @@ -80,6 +80,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/extensions/rcs_fr3/CMakeLists.txt b/extensions/rcs_fr3/CMakeLists.txt index b5365fcb9..7cf1ecdde 100644 --- a/extensions/rcs_fr3/CMakeLists.txt +++ b/extensions/rcs_fr3/CMakeLists.txt @@ -62,9 +62,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL OVERRIDE_FIND_PACKAGE ) diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 36e4f3f9c..57a1f7b05 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -188,6 +188,7 @@ def create_env(self, cfg: FR3HardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = hw.Franka(cfg.robot_cfg, ik) diff --git a/extensions/rcs_panda/CMakeLists.txt b/extensions/rcs_panda/CMakeLists.txt index 63731d07f..49b4e6e12 100644 --- a/extensions/rcs_panda/CMakeLists.txt +++ b/extensions/rcs_panda/CMakeLists.txt @@ -68,9 +68,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL OVERRIDE_FIND_PACKAGE ) diff --git a/extensions/rcs_panda/src/rcs_panda/configs.py b/extensions/rcs_panda/src/rcs_panda/configs.py index d90453e59..0c6ff8835 100644 --- a/extensions/rcs_panda/src/rcs_panda/configs.py +++ b/extensions/rcs_panda/src/rcs_panda/configs.py @@ -24,6 +24,7 @@ def config(self) -> PandaHardwareEnvCreatorConfig: robot_cfg.kinematic_model_path = rcs.ROBOTS[RobotType.Panda].mjcf_model_path robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) robot_cfg.attachment_site = rcs.ROBOTS[RobotType.Panda].attachment_site + robot_cfg.joint_limits = rcs.ROBOTS[RobotType.Panda].joint_limits robot_cfg.speed_factor = 0.1 robot_cfg.ik_solver = hw.IKSolver.rcs_ik robot_cfg.async_control = False diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index 28bace5a0..9ac11494a 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -148,6 +148,7 @@ def create_env(self, cfg: PandaHardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = hw.Franka(cfg.robot_cfg, ik) diff --git a/extensions/rcs_so101/CMakeLists.txt b/extensions/rcs_so101/CMakeLists.txt index 40bae4003..0c9b13b56 100644 --- a/extensions/rcs_so101/CMakeLists.txt +++ b/extensions/rcs_so101/CMakeLists.txt @@ -51,9 +51,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL ) FetchContent_MakeAvailable(pybind11 Eigen3) diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index f89f3880b..aa943714c 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -109,6 +109,7 @@ def create_env(self, cfg: UR5eHardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = UR5e(cfg.robot_cfg, ik) env: gym.Env = HardwareEnv(frequency=cfg.frequency) diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py index 6c58b37b1..55cfa59d1 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py @@ -12,6 +12,7 @@ robot_config.kinematic_model_path, robot_config.attachment_site, urdf=robot_config.kinematic_model_path.endswith(".urdf"), + joint_limits=robot_config.joint_limits, ) robot = UR5e(robot_config, ik) print(f"Robot joint positions: {robot.get_joint_position()}") diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index a4edb2beb..36c198154 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -104,6 +104,7 @@ def create_env(self, cfg: XArm7HardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = XArm7(cfg=cfg.robot_cfg, ik=ik) env: gym.Env = HardwareEnv(frequency=cfg.frequency) diff --git a/extensions/rcs_yam/README.md b/extensions/rcs_yam/README.md index 6be612dc2..12539ec7b 100644 --- a/extensions/rcs_yam/README.md +++ b/extensions/rcs_yam/README.md @@ -91,7 +91,7 @@ from rcs import common from rcs_yam.hw import Yam, YamConfig, YamGripper cfg = YamConfig(channel="can0", async_control=False, dof=6, ...) -ik = common.Pin(cfg.kinematic_model_path, cfg.attachment_site) +ik = common.Pin(cfg.kinematic_model_path, cfg.attachment_site, joint_limits=cfg.joint_limits) robot = Yam(cfg, ik) gripper = YamGripper(common.GripperConfig(gripper_type=common.GripperType("Yam")), robot) diff --git a/extensions/rcs_yam/src/rcs_yam/creators.py b/extensions/rcs_yam/src/rcs_yam/creators.py index aa42554a2..3d90ede3b 100644 --- a/extensions/rcs_yam/src/rcs_yam/creators.py +++ b/extensions/rcs_yam/src/rcs_yam/creators.py @@ -103,6 +103,7 @@ def create_env(self, cfg: YamHardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = Yam(cfg.robot_cfg, ik) env: gym.Env = HardwareEnv(frequency=cfg.frequency) diff --git a/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py index 75a9908fb..d745ecb7e 100644 --- a/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py +++ b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py @@ -30,6 +30,7 @@ robot_config.kinematic_model_path, robot_config.attachment_site, urdf=robot_config.kinematic_model_path.endswith(".urdf"), + joint_limits=robot_config.joint_limits, ) robot = Yam(robot_config, ik) gripper = YamGripper(common.GripperConfig(gripper_type=gripper_type), robot) diff --git a/include/rcs/Kinematics.h b/include/rcs/Kinematics.h index ce6b73cbe..41311565c 100644 --- a/include/rcs/Kinematics.h +++ b/include/rcs/Kinematics.h @@ -34,12 +34,27 @@ class Pin : public Kinematics { const double DT = 1e-1; const double damp = 1e-6; int FRAME_ID; + Pose base_pose; pinocchio::Model model; pinocchio::Data data; + VectorXd q_lower; + VectorXd q_upper; + bool enforce_limits; + + VectorXd nullspace_q; + double nullspace_gain; + + bool solve(const pinocchio::SE3& oMdes, VectorXd& q, double nullspace_gain); + public: - Pin(const std::string& path, const std::string& frame_id, bool urdf); + Pin(const std::string& path, const std::string& frame_id, bool urdf, + const VectorXd& nullspace_q = VectorXd(), double nullspace_gain = 0.0, + bool enforce_limits = true, + const std::optional& base_frame = std::nullopt, + const std::optional>& + joint_limits = std::nullopt); std::optional inverse( const Pose& pose, const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()) override; diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index d8ed06a9f..7a4776a39 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -110,8 +110,8 @@ class RobotMetaConfig: q_home=np.array([0.0, -np.pi / 4, 0.0, -3 * np.pi / 4, 0.0, np.pi / 2, 0.0]), joint_limits=np.array( [ - [-2.3476, -1.5454, -2.4937, -2.7714, -2.5100, 0.7773, -2.7045], - [2.3476, 1.5454, 2.4937, -0.4226, 2.5100, 4.2841, 2.7045], + [-2.9007, -1.8361, -2.9007, -3.0770, -2.8763, 0.4398, -3.0508], + [2.9007, 1.8361, 2.9007, -0.1169, 2.8763, 4.6216, 3.0508], ] ), ), diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 05a073b6e..a9c2380ef 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -315,7 +315,37 @@ class RotVec: ) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... class Pin(Kinematics): - def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = False) -> None: ... + def __init__( + self, + path: str, + frame_id: str = "fr3_link8", + urdf: bool = False, + nullspace_q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] = ..., + nullspace_gain: float = 0.0, + enforce_limits: bool = True, + base_frame: str | None = None, + joint_limits: numpy.ndarray[tuple[typing.Literal[2], M], numpy.dtype[numpy.float64]] | None = None, + ) -> None: + """ + Numerical inverse kinematics (damped least squares) on a Pinocchio model. + + Args: + path: MJCF or URDF file of the robot. + frame_id: Frame whose pose forward and inverse refer to, usually the attachment site. + urdf: Load ``path`` as URDF instead of MJCF. + nullspace_q: Preferred posture of the joints, used as a secondary task in the null space of + the pose task. Missing entries are filled with zeros. + nullspace_gain: Gain of the posture task, 0 disables it. Gains above 1/dt are capped to 1/dt. + If the posture task keeps the solver from reaching the pose, the solve is repeated without it. + enforce_limits: Clamp every solver step to the joint limits. + base_frame: Frame fixed to the world in which poses are expressed. None uses the root body of + the model, whose own pose Pinocchio ignores. + joint_limits: Joint limits for the clamping as a 2 x n matrix, lower limits in the first row and + upper limits in the second, usually the ``joint_limits`` of the robot config. They replace + the limits of the first n joints and must not cover more joints than the model has. None + uses the joint ranges of the MJCF or URDF. Joints with an infinite limit or an empty range + (lower >= upper) are treated as unlimited. Only used if ``enforce_limits`` is True. + """ 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]]: ... diff --git a/python/rcs/envs/scenes.py b/python/rcs/envs/scenes.py index 16ca88f9d..8b7d6e4de 100644 --- a/python/rcs/envs/scenes.py +++ b/python/rcs/envs/scenes.py @@ -353,6 +353,7 @@ def create_env_from_model(self, cfg: SimEnvCreatorConfig, mjmodel: MjModel) -> g ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=prefixed_cfg.robot_cfgs[robot_name].joint_limits, ) env = self.add_robot_env(prefixed_cfg, robot_name, env, simulation, ik) diff --git a/python/rcs/lerobot_joint_converter.py b/python/rcs/lerobot_joint_converter.py index 2e57a06d3..f6015539e 100644 --- a/python/rcs/lerobot_joint_converter.py +++ b/python/rcs/lerobot_joint_converter.py @@ -124,6 +124,7 @@ def __init__( self.ik = rcs.common.Pin( rcs.ROBOTS[robot_type].mjcf_model_path, rcs.ROBOTS[robot_type].attachment_site, + joint_limits=rcs.ROBOTS[robot_type].joint_limits, ) self.camera_resizers = { camera.name: v2.Resize(camera.resolution) # type: ignore[name-defined] # noqa: F821 diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index b530303a8..a0662ff23 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -1,3 +1,4 @@ +import mujoco import numpy as np import pytest @@ -5,7 +6,7 @@ from rcs import common # Restrict this test to robot models that are expected to work through the generic Pin binding. -# Panda currently segfaults in the native binding here, and SO101 uses a dedicated IK implementation. +# Pinocchio drops the Panda attachment site as it sits on a body without a joint. PIN_SUPPORTED_ROBOTS = [ common.RobotType.FR3, common.RobotType("XArm7"), @@ -14,57 +15,241 @@ common.RobotType("Yam"), ] +# Robots with a genuine redundant DOF (7-DoF arms), where null-space biasing has room to act. +REDUNDANT_ROBOTS = [ + common.RobotType.FR3, + common.RobotType("XArm7"), +] + +# Identity pose / no TCP offset, reused across tests. +NO_TCP_OFFSET = common.Pose() + @pytest.mark.parametrize("robot_name", PIN_SUPPORTED_ROBOTS) def test_kinematics_identity(robot_name): robot = rcs.ROBOTS[robot_name] - - # Determine model path and type model_path = robot.mjcf_model_path - frame_id = robot.attachment_site + q_home = robot.q_home - # Initialize Pinocchio interface + # Default Pin: limit-clamping on, no null-space bias. try: pin = common.Pin(model_path, frame_id, False) except Exception as e: pytest.fail(f"Failed to initialize Pin for {robot_name}: {e}") - q_home = robot.q_home - - # Test 1: FK at home - # Identity pose (no TCP offset) - tcp_offset = common.Pose() - - pose_home = pin.forward(q_home, tcp_offset) + # Test 1: FK at home. + pose_home = pin.forward(q_home, NO_TCP_OFFSET) assert isinstance(pose_home, common.Pose) - # Test 2: IK at home pose should return a solution (ideally close to q_home, but IK is redundant) - # We use q_home as initial guess - q_sol: np.ndarray | None = pin.inverse(pose_home, q_home, tcp_offset) - + # Test 2: IK at the home pose returns a solution reaching that pose. The home + # configuration is within the joint limits, so clamping does not interfere. + q_sol: np.ndarray | None = pin.inverse(pose_home, q_home, NO_TCP_OFFSET) assert q_sol is not None, "IK failed for home pose" - # Verify the solution with FK - pose_sol = pin.forward(q_sol, tcp_offset) - - # Check if pose_sol is close to pose_home + pose_sol = pin.forward(q_sol, NO_TCP_OFFSET) assert pose_sol.is_close( pose_home, eps_r=1e-4, eps_t=1e-4 ), f"FK(IK(pose)) does not match pose.\nOriginal: {pose_home}\nResult: {pose_sol}" - # Test 3: Perturbed configuration - # Add small noise to q_home to test non-trivial pose - # Ensure we stay within limits if possible, but for small noise it should be fine + # Test 3: Perturbed configuration. We disable limit clamping here so that + # reachability of FK(q_perturbed) does not depend on how close q_home sits to + # a joint limit (e.g. SO101), keeping this a pure IK-convergence check. + pin_free = common.Pin(model_path, frame_id, False, np.array([]), 0.0, False) + np.random.seed(42) q_perturbed = q_home + np.random.uniform(-0.1, 0.1, size=q_home.shape) - pose_perturbed = pin.forward(q_perturbed, tcp_offset) # type: ignore - q_sol_perturbed: np.ndarray | None = pin.inverse(pose_perturbed, q_home, tcp_offset) # Use q_home as seed - + pose_perturbed = pin_free.forward(q_perturbed, NO_TCP_OFFSET) + q_sol_perturbed: np.ndarray | None = pin_free.inverse(pose_perturbed, q_home, NO_TCP_OFFSET) assert q_sol_perturbed is not None, "IK failed for perturbed pose" - pose_sol_perturbed = pin.forward(q_sol_perturbed, tcp_offset) + pose_sol_perturbed = pin_free.forward(q_sol_perturbed, NO_TCP_OFFSET) assert pose_sol_perturbed.is_close( pose_perturbed, eps_r=1e-3, eps_t=1e-3 ), f"FK(IK(perturbed_pose)) does not match.\nOriginal: {pose_perturbed}\nResult: {pose_sol_perturbed}" + + +@pytest.mark.parametrize("robot_name", REDUNDANT_ROBOTS) +def test_kinematics_nullspace_bias(robot_name): + """A null-space target biases the redundant DOF toward the preferred posture + without changing the achieved end-effector pose.""" + robot = rcs.ROBOTS[robot_name] + model_path = robot.mjcf_model_path + frame_id = robot.attachment_site + q_home = robot.q_home + + # Clamping off on both so the comparison isolates the null-space term. + pin_plain = common.Pin(model_path, frame_id, False, np.array([]), 0.0, False) + pin_ns = common.Pin(model_path, frame_id, False, q_home, 2.0, False) # bias toward home + + # Target the home pose; a seed away from home exercises the redundancy so the + # two solvers can settle on different configurations for the same pose. + pose_home = pin_plain.forward(q_home, NO_TCP_OFFSET) + q_seed = q_home.copy() + q_seed[0] += 0.5 + q_seed[2] += 0.5 + q_seed[3] += 0.3 + + q_plain: np.ndarray | None = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET) + q_ns: np.ndarray | None = pin_ns.inverse(pose_home, q_seed, NO_TCP_OFFSET) + + assert q_plain is not None, "plain IK failed" + assert q_ns is not None, "null-space IK failed" + + # Both solutions must reach the same end-effector pose. + assert pin_plain.forward(q_plain, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) + assert pin_ns.forward(q_ns, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) + + # The null-space-biased solution sits closer to the preferred (home) posture. + d_plain = float(np.linalg.norm(q_plain - q_home)) + d_ns = float(np.linalg.norm(q_ns - q_home)) + assert d_ns < d_plain, f"null-space bias did not pull toward home: d_ns={d_ns} vs d_plain={d_plain}" + + +@pytest.mark.parametrize("gain", [1.0, 10.0, 100.0]) +def test_kinematics_nullspace_large_gain_converges(gain): + robot = rcs.ROBOTS[common.RobotType.FR3] + pin = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, robot.q_home, gain) + pose_home = pin.forward(robot.q_home, NO_TCP_OFFSET) + q_seed = robot.q_home + np.array([0.5, 0.0, 0.5, 0.3, 0.0, 0.0, 0.0]) + + q_sol = pin.inverse(pose_home, q_seed, NO_TCP_OFFSET) + + assert q_sol is not None, f"IK failed with null-space gain {gain}" + assert pin.forward(q_sol, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) + + +@pytest.mark.parametrize("robot_name", REDUNDANT_ROBOTS) +def test_kinematics_nullspace_applies_when_seed_reaches_pose(robot_name): + """A seed that already reaches the target still moves toward the posture.""" + robot = rcs.ROBOTS[robot_name] + pin_plain = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, np.array([]), 0.0, False) + pin_ns = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, robot.q_home, 10.0, False) + pose_home = pin_plain.forward(robot.q_home, NO_TCP_OFFSET) + q_seed = robot.q_home.copy() + q_seed[0] += 0.5 + q_seed[2] += 0.5 + q_reached: np.ndarray | None = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET) + assert q_reached is not None + + q_ns: np.ndarray | None = pin_ns.inverse(pose_home, q_reached, NO_TCP_OFFSET) + + assert q_ns is not None + assert pin_ns.forward(q_ns, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) + assert np.linalg.norm(q_ns - robot.q_home) < np.linalg.norm(q_reached - robot.q_home) + + +@pytest.mark.parametrize("robot_name", REDUNDANT_ROBOTS) +def test_kinematics_nullspace_never_loses_solutions(robot_name): + """Targets the plain solver reaches stay reachable with the posture task.""" + robot = rcs.ROBOTS[robot_name] + pin_plain = common.Pin(robot.mjcf_model_path, robot.attachment_site, False) + pin_ns = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, robot.q_home, 10.0) + lower, upper = robot.joint_limits + rng = np.random.default_rng(1) + for _ in range(50): + q_target = rng.uniform(lower, upper) + q_seed = np.clip(q_target + rng.uniform(-0.3, 0.3, q_target.shape), lower, upper) + pose = pin_plain.forward(q_target, NO_TCP_OFFSET) + if pin_plain.inverse(pose, q_seed, NO_TCP_OFFSET) is not None: + assert pin_ns.inverse(pose, q_seed, NO_TCP_OFFSET) is not None + + +def test_kinematics_missing_frame_raises(): + robot = rcs.ROBOTS[common.RobotType.FR3] + with pytest.raises(RuntimeError, match="not_a_frame"): + common.Pin(robot.mjcf_model_path, "not_a_frame", False) + + +MOUNTED_ARM_MJCF = """ + + + + + + + + + + + + + + + + + + + + + + +""" + + +def test_kinematics_base_frame(tmp_path): + """Poses are expressed in base_frame, matching MuJoCo's site pose relative to that body.""" + path = tmp_path / "mounted_arm.xml" + path.write_text(MOUNTED_ARM_MJCF) + pin = common.Pin(str(path), "tip", False, base_frame="robot_base") + q = np.array([0.4, -0.3, 0.5]) + + model = mujoco.MjModel.from_xml_path(str(path)) + data = mujoco.MjData(model) + data.qpos[:] = q + mujoco.mj_forward(model, data) + base = data.body("robot_base") + base_pose = common.Pose(base.xmat.reshape(3, 3), base.xpos) + site_pose = common.Pose(data.site("tip").xmat.reshape(3, 3), data.site("tip").xpos) + expected = base_pose.inverse() * site_pose + + pose = pin.forward(q, NO_TCP_OFFSET) + assert pose.is_close(expected, eps_r=1e-9, eps_t=1e-9) + + tcp_offset = common.Pose(translation=np.array([0.0, 0.0, 0.1])) + pose_tcp = pin.forward(q, tcp_offset) + q_sol: np.ndarray | None = pin.inverse(pose_tcp, q + 0.1, tcp_offset) + assert q_sol is not None + assert pin.forward(q_sol, tcp_offset).is_close(pose_tcp, eps_r=1e-3, eps_t=1e-3) + + +def test_kinematics_base_frame_moved_by_joint_raises(tmp_path): + path = tmp_path / "mounted_arm.xml" + path.write_text(MOUNTED_ARM_MJCF) + with pytest.raises(RuntimeError, match="link2"): + common.Pin(str(path), "tip", False, base_frame="link2") + + +def test_kinematics_joint_limits_override_model_limits(): + """Given joint limits replace the model limits, so solutions stay inside a range narrower than the MJCF.""" + robot = rcs.ROBOTS[common.RobotType.FR3] + narrow_limits = np.array( + [ + [-2.3476, -1.5454, -2.4937, -2.7714, -2.5100, 0.7773, -2.7045], + [2.3476, 1.5454, 2.4937, -0.4226, 2.5100, 4.2841, 2.7045], + ] + ) + pin_model_limits = common.Pin(robot.mjcf_model_path, robot.attachment_site, False) + pin_narrow_limits = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, joint_limits=narrow_limits) + lower, upper = narrow_limits + q_target = robot.q_home.copy() + q_target[0] = upper[0] + 0.3 + pose = pin_model_limits.forward(q_target, NO_TCP_OFFSET) + q_seed = robot.q_home.copy() + q_seed[0] = upper[0] + + q_model: np.ndarray | None = pin_model_limits.inverse(pose, q_seed, NO_TCP_OFFSET) + q_narrow: np.ndarray | None = pin_narrow_limits.inverse(pose, q_seed, NO_TCP_OFFSET) + + assert q_model is not None and q_model[0] > upper[0] + assert q_narrow is not None + assert np.all((q_narrow >= lower) & (q_narrow <= upper)) + assert pin_narrow_limits.forward(q_narrow, NO_TCP_OFFSET).is_close(pose, eps_r=1e-3, eps_t=1e-3) + + +def test_kinematics_joint_limits_more_joints_than_model_raises(): + robot = rcs.ROBOTS[common.RobotType("UR5e")] + too_many = np.vstack([-np.ones(robot.dof + 1), np.ones(robot.dof + 1)]) + with pytest.raises(ValueError, match="joint_limits"): + common.Pin(robot.mjcf_model_path, robot.attachment_site, False, joint_limits=too_many) diff --git a/python/tests/test_rpc.py b/python/tests/test_rpc.py index b45713f52..dbbc9c379 100644 --- a/python/tests/test_rpc.py +++ b/python/tests/test_rpc.py @@ -50,6 +50,7 @@ def build_rpc_env() -> gym.Env: ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) env: gym.Env = SimEnv(simulation) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 325d1ef4b..4a8428107 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -55,6 +55,7 @@ def build_single_robot_env( ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) env: gym.Env = SimEnv(simulation) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 307137ff5..14616a41d 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -379,9 +379,37 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Pin") - .def(py::init(), - py::arg("path"), py::arg("frame_id") = "fr3_link8", - py::arg("urdf") = false); + .def( + py::init< + const std::string&, const std::string&, bool, + const rcs::common::VectorXd&, double, bool, + const std::optional&, + const std::optional>&>(), + R"doc(Numerical inverse kinematics (damped least squares) on a Pinocchio model. + +Args: + path: MJCF or URDF file of the robot. + frame_id: Frame whose pose forward and inverse refer to, usually the attachment site. + urdf: Load ``path`` as URDF instead of MJCF. + nullspace_q: Preferred posture of the joints, used as a secondary task in the null space of + the pose task. Missing entries are filled with zeros. + nullspace_gain: Gain of the posture task, 0 disables it. Gains above 1/dt are capped to 1/dt. + If the posture task keeps the solver from reaching the pose, the solve is repeated without it. + enforce_limits: Clamp every solver step to the joint limits. + base_frame: Frame fixed to the world in which poses are expressed. None uses the root body of + the model, whose own pose Pinocchio ignores. + joint_limits: Joint limits for the clamping as a 2 x n matrix, lower limits in the first row and + upper limits in the second, usually the ``joint_limits`` of the robot config. They replace + the limits of the first n joints and must not cover more joints than the model has. None + uses the joint ranges of the MJCF or URDF. Joints with an infinite limit or an empty range + (lower >= upper) are treated as unlimited. Only used if ``enforce_limits`` is True. +)doc", + py::arg("path"), py::arg("frame_id") = "fr3_link8", + py::arg("urdf") = false, + py::arg("nullspace_q") = rcs::common::VectorXd(), + py::arg("nullspace_gain") = 0.0, py::arg("enforce_limits") = true, + py::arg("base_frame") = std::nullopt, + py::arg("joint_limits") = std::nullopt); bind_type_class(common, "RobotType") .def_readonly_static("FR3", &rcs::common::RobotType::FR3) diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp index 847c7aa89..7b9cb329c 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -1,17 +1,25 @@ #include "rcs/Kinematics.h" +#include +#include +#include #include #include #include #include #include #include +#include +#include namespace rcs { namespace common { -Pin::Pin(const std::string& path, const std::string& frame_id, - bool urdf = false) +Pin::Pin( + const std::string& path, const std::string& frame_id, bool urdf, + const VectorXd& nullspace_q, double nullspace_gain, bool enforce_limits, + const std::optional& base_frame, + const std::optional>& joint_limits) : model() { if (urdf) { pinocchio::urdf::buildModel(path, this->model); @@ -19,23 +27,80 @@ Pin::Pin(const std::string& path, const std::string& frame_id, pinocchio::mjcf::buildModel(path, this->model); } this->data = pinocchio::Data(this->model); + if (!model.existFrame(frame_id)) { + throw std::runtime_error(frame_id + + " frame id could not be found in the provided " + + (urdf ? "URDF" : "MJCF")); + } 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"); + if (base_frame) { + if (!model.existFrame(*base_frame)) { + throw std::runtime_error(*base_frame + + " base frame could not be found in the model"); + } + const pinocchio::Frame& base = model.frames[model.getFrameId(*base_frame)]; + if (base.parentJoint != 0) { + throw std::runtime_error(*base_frame + + " base frame must not be moved by a joint"); + } + this->base_pose = + Pose(base.placement.rotation(), base.placement.translation()); + } + + this->q_lower = this->model.lowerPositionLimit; + this->q_upper = this->model.upperPositionLimit; + if (joint_limits) { + const Eigen::Index n = joint_limits->cols(); + if (n > this->model.nq) { + throw std::invalid_argument("joint_limits has " + std::to_string(n) + + " columns but the model only has " + + std::to_string(this->model.nq) + " joints"); + } + this->q_lower.head(n) = joint_limits->row(0).transpose(); + this->q_upper.head(n) = joint_limits->row(1).transpose(); } + const double inf = std::numeric_limits::infinity(); + for (Eigen::Index i = 0; i < this->q_lower.size(); i++) { + if (!std::isfinite(this->q_lower[i]) || !std::isfinite(this->q_upper[i]) || + this->q_lower[i] >= this->q_upper[i]) { + this->q_lower[i] = -inf; + this->q_upper[i] = inf; + } + } + this->enforce_limits = enforce_limits; + + this->nullspace_gain = nullspace_gain; + this->nullspace_q = VectorXd::Zero(this->model.nq); + const Eigen::Index n = + std::min(nullspace_q.size(), this->nullspace_q.size()); + this->nullspace_q.head(n) = nullspace_q.head(n); } std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset) { - rcs::common::Pose new_pose = pose * tcp_offset.inverse(); - VectorXd q(model.nq); - q.setZero(); - q.head(q0.size()) = q0; + rcs::common::Pose new_pose = this->base_pose * pose * tcp_offset.inverse(); const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); + VectorXd q_seed(model.nq); + q_seed.setZero(); + q_seed.head(q0.size()) = q0; + // a posture step larger than the remaining distance overshoots + const double nullspace_gain = std::min(this->nullspace_gain, 1.0 / this->DT); + if (nullspace_gain > 0.0) { + VectorXd q = q_seed; + if (this->solve(oMdes, q, nullspace_gain)) { + return q; + } + } + if (this->solve(oMdes, q_seed, 0.0)) { + return q_seed; + } + return std::nullopt; +} + +bool Pin::solve(const pinocchio::SE3& oMdes, VectorXd& q, + double nullspace_gain) { pinocchio::Data::Matrix6x J(6, model.nv); J.setZero(); - bool success = false; Vector6d err; Eigen::VectorXd v(model.nv); for (int i = 0;; i++) { @@ -43,13 +108,12 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, 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; + const bool posture_pending = nullspace_gain > 0.0 && i == 0; + if (err.norm() < this->eps && !posture_pending) { + return true; } if (i >= this->IT_MAX) { - success = false; - break; + return false; } pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J); pinocchio::Data::Matrix6 Jlog; @@ -58,13 +122,26 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, pinocchio::Data::Matrix6 JJt; JJt.noalias() = J * J.transpose(); JJt.diagonal().array() += this->damp; - v.noalias() = -J.transpose() * JJt.ldlt().solve(err); + + if (nullspace_gain > 0.0) { + Eigen::MatrixXd Jpinv = + J.transpose() * + JJt.ldlt().solve(pinocchio::Data::Matrix6::Identity()); + v.noalias() = -Jpinv * err; + + VectorXd dq_ns(model.nv); + pinocchio::difference(model, q, this->nullspace_q, dq_ns); + const Eigen::MatrixXd N = + Eigen::MatrixXd::Identity(model.nv, model.nv) - Jpinv * J; + v.noalias() += N * (nullspace_gain * dq_ns); + } else { + v.noalias() = -J.transpose() * JJt.ldlt().solve(err); + } + q = pinocchio::integrate(model, q, v * this->DT); - } - if (success) { - return q; - } else { - return std::nullopt; + if (this->enforce_limits) { + q = q.cwiseMax(this->q_lower).cwiseMin(this->q_upper); + } } } @@ -78,7 +155,7 @@ Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) { data.oMf[this->FRAME_ID].translation()); // apply the tcp offset - return pose * tcp_offset; + return this->base_pose.inverse() * pose * tcp_offset; } } // namespace common