diff --git a/Makefile b/Makefile index be09a38d..ab055ed6 100644 --- a/Makefile +++ b/Makefile @@ -1,4 +1,4 @@ -PYSRC = python/rcsss +PYSRC = python CPPSRC = src COMPILE_MODE = Release diff --git a/pyproject.toml b/pyproject.toml index b5087daa..f2fde75e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -101,6 +101,7 @@ ignore = [ "PLR5501", # elif to reduce indentation "PT018", # assertion should be broken down into multiple parts "NPY002", + "G004", # Logging format string does not contain any placeholders ] [tool.pylint.format] diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index cf107da5..ff0ef334 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -1,6 +1,5 @@ import logging -from dotenv import dotenv_values from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager from rcsss.control.utils import load_creds_fr3_desk from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance @@ -45,6 +44,7 @@ def main(): + resource_manger: FCI | DummyResourceManager if ROBOT_INSTANCE == RobotInstance.HARDWARE: user, pw = load_creds_fr3_desk() resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) @@ -60,7 +60,7 @@ def main(): collision_guard="lab", gripper_cfg=default_fr3_hw_gripper_cfg(), max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP + relative_to=RelativeTo.LAST_STEP, ) else: env_rel = fr3_sim_env( @@ -70,12 +70,12 @@ def main(): gripper_cfg=default_fr3_sim_gripper_cfg(), camera_set_cfg=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP + relative_to=RelativeTo.LAST_STEP, ) env_rel.get_wrapper_attr("sim").open_gui() env_rel.reset() - print(env_rel.unwrapped.robot.get_cartesian_position()) + print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore for _ in range(10): for _ in range(10): diff --git a/python/examples/env_joint_control.py b/python/examples/env_joint_control.py index a79dc92e..1bb48538 100644 --- a/python/examples/env_joint_control.py +++ b/python/examples/env_joint_control.py @@ -1,9 +1,6 @@ import logging -import mujoco import numpy as np -import rcsss -from dotenv import dotenv_values from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager from rcsss.control.utils import load_creds_fr3_desk from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance @@ -48,6 +45,7 @@ def main(): + resource_manger: FCI | DummyResourceManager if ROBOT_INSTANCE == RobotInstance.HARDWARE: user, pw = load_creds_fr3_desk() resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) diff --git a/python/examples/fr3.py b/python/examples/fr3.py index f87135ec..5bf3b325 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -1,10 +1,8 @@ import logging import sys -from time import sleep import numpy as np import rcsss -from dotenv import dotenv_values from rcsss import sim from rcsss._core.hw import FR3Config, IKController from rcsss._core.sim import CameraType @@ -57,6 +55,7 @@ def main(): if "lab" not in rcsss.scenes: logger.error("This pip package was not built with the UTN lab models, aborting.") sys.exit() + resource_manger: FCI | DummyResourceManager if ROBOT_INSTANCE == RobotInstance.HARDWARE: user, pw = load_creds_fr3_desk() resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) @@ -64,9 +63,12 @@ def main(): resource_manger = DummyResourceManager() with resource_manger: + robot: rcsss.common.Robot + gripper: rcsss.common.Gripper if ROBOT_INSTANCE == RobotInstance.SIMULATION: simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]) urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) + assert urdf_path is not None ik = rcsss.common.IK(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) cfg = sim.FR3Config() @@ -74,8 +76,8 @@ def main(): cfg.realtime = False robot.set_parameters(cfg) - gripper_cfg = sim.FHConfig() - gripper = sim.FrankaHand(simulation, "0", gripper_cfg) + gripper_cfg_sim = sim.FHConfig() + gripper = sim.FrankaHand(simulation, "0", gripper_cfg_sim) # add camera to have a rendering gui cameras = { @@ -83,23 +85,24 @@ def main(): "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), } cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20) - camera_set = SimCameraSet(simulation, cam_cfg) + camera_set = SimCameraSet(simulation, cam_cfg) # noqa: F841 simulation.open_gui() else: urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) + assert urdf_path is not None ik = rcsss.common.IK(urdf_path) - robot = rcsss.hw.FR3(ROBOT_IP, str(rcsss.scenes["lab"].parent / "fr3.urdf"), ik) + robot = rcsss.hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) robot_cfg.controller = IKController.robotics_library robot.set_parameters(robot_cfg) - gripper_cfg = rcsss.hw.FHConfig() - gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 - gripper_cfg.speed = 0.1 - gripper_cfg.force = 30 - gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg) + gripper_cfg_hw = rcsss.hw.FHConfig() + gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1 + gripper_cfg_hw.speed = 0.1 + gripper_cfg_hw.force = 30 + gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg_hw) input("the robot is going to move, press enter whenever you are ready") # move to home position and open gripper @@ -115,7 +118,7 @@ def main(): ) if ROBOT_INSTANCE == RobotInstance.SIMULATION: simulation.step_until_convergence() - logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"IK success: {robot.get_state().ik_success}") # type: ignore logger.debug(f"sim converged: {simulation.is_converged()}") # 5cm in y direction @@ -124,7 +127,7 @@ def main(): ) if ROBOT_INSTANCE == RobotInstance.SIMULATION: simulation.step_until_convergence() - logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"IK success: {robot.get_state().ik_success}") # type: ignore logger.debug(f"sim converged: {simulation.is_converged()}") # 5cm in z direction @@ -133,7 +136,7 @@ def main(): ) if ROBOT_INSTANCE == RobotInstance.SIMULATION: simulation.step_until_convergence() - logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"IK success: {robot.get_state().ik_success}") # type: ignore logger.debug(f"sim converged: {simulation.is_converged()}") # rotate the arm 90 degrees around the inverted y and z axis @@ -143,7 +146,7 @@ def main(): robot.set_cartesian_position(new_pose) if ROBOT_INSTANCE == RobotInstance.SIMULATION: simulation.step_until_convergence() - logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"IK success: {robot.get_state().ik_success}") # type: ignore logger.debug(f"sim converged: {simulation.is_converged()}") if ROBOT_INSTANCE == RobotInstance.HARDWARE: @@ -157,7 +160,7 @@ def main(): ) if ROBOT_INSTANCE == RobotInstance.SIMULATION: simulation.step_until_convergence() - logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"IK success: {robot.get_state().ik_success}") # type: ignore logger.debug(f"sim converged: {simulation.is_converged()}") # grasp the object @@ -172,7 +175,7 @@ def main(): ) if ROBOT_INSTANCE == RobotInstance.SIMULATION: simulation.step_until_convergence() - logger.debug(f"IK success: {robot.get_state().ik_success}") + logger.debug(f"IK success: {robot.get_state().ik_success}") # type: ignore logger.debug(f"sim converged: {simulation.is_converged()}") if ROBOT_INSTANCE == RobotInstance.HARDWARE: diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 795ad7eb..baef831f 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -1,11 +1,11 @@ import logging +from typing import Any, cast import gymnasium as gym import mujoco import numpy as np from rcsss._core.common import Pose -from rcsss._core.sim import FR3State -from rcsss.envs.base import GripperWrapper +from rcsss.envs.base import FR3Env, GripperWrapper logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -14,9 +14,10 @@ class PickUpDemo: def __init__(self, env: gym.Env): self.env = env - self.home_pose = self.env.unwrapped.robot.get_cartesian_position() + self.unwrapped: FR3Env = cast(FR3Env, self.env.unwrapped) + self.home_pose = self.unwrapped.robot.get_cartesian_position() - def _action(self, pose: Pose, gripper: float) -> np.ndarray: + def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: return {"xyzrpy": pose.xyzrpy(), "gripper": gripper} def get_object_pose(self, geom_name) -> Pose: @@ -33,22 +34,19 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in waypoints.append(start_pose.interpolate(end_pose, t)) return waypoints - def step(self, action: np.ndarray) -> dict: - re = self.env.step(action) - s: FR3State = self.env.unwrapped.robot.get_state() - return re[0] + def step(self, action: dict) -> dict: + return self.env.step(action)[0] def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: - end_eff_pose = self.env.unwrapped.robot.get_cartesian_position() + end_eff_pose = self.unwrapped.robot.get_cartesian_position() goal_pose = self.get_object_pose(geom_name=geom_name) # goal pose is above the object and gripper coordinate must flip z-axis (end effector base rotation is [1, 0, 0, 0]) # be careful we define identity quaternion as as [0, 0, 0, 1] # this does not work if the object is flipped - goal_pose *= Pose(translation=[0, 0, delta_up], quaternion=[1, 0, 0, 0]) + goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) - waypoints = self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) - return waypoints + return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints) def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict: for i in range(1, len(waypoints)): @@ -72,7 +70,7 @@ def grasp(self, geom_name: str): self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) def move_home(self): - end_eff_pose = self.env.unwrapped.robot.get_cartesian_position() + end_eff_pose = self.unwrapped.robot.get_cartesian_position() waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 09617d1e..b5c85388 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -18,8 +18,6 @@ ControlMode, FR3Env, GripperWrapper, - LimitedJointsRelDictType, - ObsArmsGrCam, RelativeActionSpace, RelativeTo, ) @@ -186,7 +184,7 @@ def fr3_sim_env( urdf_path: str | PathLike | None = None, mjcf: str | PathLike = "fr3_empty_world", sim_wrapper: Type[SimWrapper] | None = None, -) -> gym.Env[ObsArmsGrCam, LimitedJointsRelDictType]: +) -> gym.Env: """ Creates a simulation environment for the FR3 robot. diff --git a/python/tests/test_common.py b/python/tests/test_common.py index 2a241278..087a0bed 100644 --- a/python/tests/test_common.py +++ b/python/tests/test_common.py @@ -10,7 +10,7 @@ class TestPose: This class tests the methods of the Pose class and its multiple constructors. """ - @pytest.fixture + @pytest.fixture() def identity_pose(self): """This fixture can be reused wherever if no transformation pose is needed""" return common.Pose() diff --git a/python/tests/test_envs.py b/python/tests/test_envs.py index a65fb808..bacc9200 100644 --- a/python/tests/test_envs.py +++ b/python/tests/test_envs.py @@ -77,7 +77,7 @@ class Composed(AdvancedNestedSpaceWithLambda, SimpleSpace): ... class TestGetSpace: def test_simple_space(self): - get_space(SimpleSpace) == gym.spaces.Dict( + assert get_space(SimpleSpace) == gym.spaces.Dict( { "my_int": gym.spaces.Discrete(1), "my_float": gym.spaces.Box(low=0, high=1, shape=(1,), dtype=np.float32), @@ -196,4 +196,4 @@ def test_composed_space(self): ) def test_get_space_keys(self): - get_space_keys(SimpleSpace) == {"my_int", "my_float"} + assert set(get_space_keys(SimpleSpace)) == {"my_int", "my_float"} diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 36980124..f2a9e253 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -1,29 +1,35 @@ -from collections import OrderedDict +from typing import cast + import numpy as np import pytest import rcsss +from rcsss.envs.base import ( + ControlMode, + FR3Env, + GripperDictType, + JointsDictType, + TQuartDictType, + TRPYDictType, +) from rcsss.envs.factories import ( default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, fr3_sim_env, - get_urdf_path, ) -from rcsss.envs.base import ControlMode, TRPYDictType, GripperDictType, TQuartDictType, JointsDictType - -@pytest.fixture +@pytest.fixture() def cfg(): return default_fr3_sim_robot_cfg() -@pytest.fixture +@pytest.fixture() def gripper_cfg(): return default_fr3_sim_gripper_cfg() -@pytest.fixture +@pytest.fixture() def cam_cfg(): return default_mujoco_cameraset_cfg() @@ -146,14 +152,15 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): max_relative_movement=0.5, ) obs, _ = env.reset() - p1 = env.unwrapped.robot.get_joint_position() + unwrapped = cast(FR3Env, env.unwrapped) + p1 = unwrapped.robot.get_joint_position() # an obvious below ground collision action obs["xyzrpy"][0] = 0.3 obs["xyzrpy"][2] = -0.2 collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action) - p2 = env.unwrapped.robot.get_joint_position() + p2 = unwrapped.robot.get_joint_position() self.assert_collision(info) # assure that the robot did not move assert np.allclose(p1, p2) @@ -203,7 +210,7 @@ def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): ControlMode.CARTESIAN_TQuart, cfg, gripper_cfg=gripper_cfg, - camera_set_cfg=None, + camera_set_cfg=cam_cfg, max_relative_movement=0.5, ) obs_initial, _ = env_rel.reset() @@ -247,14 +254,15 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): max_relative_movement=None, ) obs, _ = env.reset() - p1 = env.unwrapped.robot.get_joint_position() + unwrapped = cast(FR3Env, env.unwrapped) + p1 = unwrapped.robot.get_joint_position() # an obvious below ground collision action obs["tquart"][0] = 0.3 obs["tquart"][2] = -0.2 collision_action = TQuartDictType(tquart=obs["tquart"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action) - p2 = env.unwrapped.robot.get_joint_position() + p2 = unwrapped.robot.get_joint_position() self.assert_collision(info) # assure that the robot did not move assert np.allclose(p1, p2) @@ -321,12 +329,13 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): max_relative_movement=None, ) env.reset() - p1 = env.unwrapped.robot.get_joint_position() + unwrapped = cast(FR3Env, env.unwrapped) + p1 = unwrapped.robot.get_joint_position() # the below action is a test_case where there is an obvious collision regardless of the gripper action collision_act = JointsDictType(joints=np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)) collision_act.update(GripperDictType(gripper=1)) _, _, _, _, info = env.step(collision_act) - p2 = env.unwrapped.robot.get_joint_position() + p2 = unwrapped.robot.get_joint_position() self.assert_collision(info) # assure that the robot did not move