Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
1 change: 1 addition & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ if __name__ == "__main__":
ik = rcs.common.Pin(
kinematic_model_path,
attachment_site,
joint_limits=robot_cfg.joint_limits,
)

# base env
Expand Down
2 changes: 1 addition & 1 deletion docs/getting_started/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
2 changes: 2 additions & 0 deletions examples/fr3/fr3_direct_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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)

Expand Down
1 change: 1 addition & 0 deletions examples/fr3/fr3_env_cartesian_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/fr3/fr3_env_joint_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/fr3/fr3_readme.py
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
ik = rcs.common.Pin(
kinematic_model_path,
attachment_site,
joint_limits=robot_cfg.joint_limits,
)

# base env
Expand Down
1 change: 1 addition & 0 deletions examples/so101/so101_env_cartesian_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/so101/so101_env_joint_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/ur5e/ur5e_env_cartesian_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/ur5e/ur5e_env_joint_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/xarm7/xarm7_env_cartesian_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/xarm7/xarm7_env_joint_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions examples/yam/yam_env_cartesian_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
5 changes: 2 additions & 3 deletions extensions/rcs_fr3/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_fr3/src/rcs_fr3/creators.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
5 changes: 2 additions & 3 deletions extensions/rcs_panda/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_panda/src/rcs_panda/configs.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_panda/src/rcs_panda/creators.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
5 changes: 2 additions & 3 deletions extensions/rcs_so101/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_ur5e/src/rcs_ur5e/creators.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()}")
Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_xarm7/src/rcs_xarm7/creators.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion extensions/rcs_yam/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_yam/src/rcs_yam/creators.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
17 changes: 16 additions & 1 deletion include/rcs/Kinematics.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>& base_frame = std::nullopt,
const std::optional<Eigen::Matrix<double, 2, Eigen::Dynamic>>&
joint_limits = std::nullopt);
std::optional<VectorXd> inverse(
const Pose& pose, const VectorXd& q0,
const Pose& tcp_offset = Pose::Identity()) override;
Expand Down
4 changes: 2 additions & 2 deletions python/rcs/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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],
]
),
),
Expand Down
32 changes: 31 additions & 1 deletion python/rcs/_core/common.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -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]]: ...
Expand Down
1 change: 1 addition & 0 deletions python/rcs/envs/scenes.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions python/rcs/lerobot_joint_converter.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading
Loading