style: format py and cpp

Author: juelg

python/rcs/__init__.py

@@ -6,7 +6,10 @@
 
 from gymnasium import register
 from rcs._core import __version__, common
-from rcs.envs.creators import FR3SimplePickUpSimEnvCreator, FR3LabDigitGripperPickUpSimEnvCreator
+from rcs.envs.creators import (
+    FR3LabDigitGripperPickUpSimEnvCreator,
+    FR3SimplePickUpSimEnvCreator,
+)
 
 from rcs import camera, envs, hand, sim
 

python/rcs/envs/creators.py

@@ -1,5 +1,6 @@
 import logging
 import typing
+from functools import partial
 from typing import Type
 
 import gymnasium as gym
@@ -27,7 +28,6 @@
     SimWrapper,
 )
 from rcs.envs.utils import default_sim_gripper_cfg, default_sim_robot_cfg
-from functools import partial
 
 import rcs
 from rcs import sim
@@ -163,7 +163,7 @@ def __call__(  # type: ignore
                 missing_keys = [key for key in required_keys if key not in random_pos_args]
                 logger.warning(f"Missing random position arguments: {missing_keys}; Defaulting to RandomCubePos")
             random_env = partial(RandomObjectPos, **random_pos_args)
-        
+
         env_rel = SimEnvCreator()(
             control_mode=control_mode,
             robot_cfg=robot_cfg,
@@ -192,7 +192,7 @@ def __call__(  # type: ignore
         resolution: tuple[int, int] | None = None,
         frame_rate: int = 0,
         delta_actions: bool = True,
-        cam_list: list[str] = ["wrist", "bird_eye", "side", "right_side", "left_side", "front"]
+        cam_list: list[str] = ["wrist", "bird_eye", "side", "right_side", "left_side", "front"],
     ) -> gym.Env:
         if resolution is None:
             resolution = (256, 256)
@@ -210,6 +210,7 @@ def __call__(  # type: ignore
 
         return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras)
 
+
 class FR3LabDigitGripperPickUpSimEnvCreator(EnvCreator):
     def __call__(  # type: ignore
         self,
@@ -219,7 +220,7 @@ def __call__(  # type: ignore
         frame_rate: int = 0,
         delta_actions: bool = True,
         cam_list: list[str] = [],
-        mjcf_path: str = ''
+        mjcf_path: str = "",
     ) -> gym.Env:
         if resolution is None:
             resolution = (256, 256)
@@ -237,20 +238,24 @@ def __call__(  # type: ignore
             for cam in cam_list
         }
         robot_cfg = rcs.sim.SimRobotConfig()
-        robot_cfg.tcp_offset = rcs.common.Pose(translation=np.array([0.0, 0.0, 0.15]), rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]))
+        robot_cfg.tcp_offset = rcs.common.Pose(
+            translation=np.array([0.0, 0.0, 0.15]),
+            rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]),
+        )
         robot_cfg.robot_type = rcs.common.RobotType.FR3
         robot_cfg.realtime = False
-        robot_cfg.add_id("0") # only required for fr3
+        robot_cfg.add_id("0")  # only required for fr3
         robot_cfg.mjcf_scene_path = mjcf_path
-        robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot # .urdf (in case for urdf)
-        print(f"Creating FR3LabDigitGripperPickUpSim with the following parameters: \n"
-                    f"  render_mode: {render_mode}\n"
-                    f"  control_mode: {control_mode}\n"
-                    f"  resolution: {resolution}\n"
-                    f"  frame_rate: {frame_rate}\n"
-                    f"  delta_actions: {delta_actions}\n"
-                    f"  cameras: {cameras}\n"
-                    f"  mjcf_path: {mjcf_path}\n"
-                    )
+        robot_cfg.kinematic_model_path = rcs.scenes["fr3_empty_world"].mjcf_robot  # .urdf (in case for urdf)
+        print(
+            f"Creating FR3LabDigitGripperPickUpSim with the following parameters: \n"
+            f"  render_mode: {render_mode}\n"
+            f"  control_mode: {control_mode}\n"
+            f"  resolution: {resolution}\n"
+            f"  frame_rate: {frame_rate}\n"
+            f"  delta_actions: {delta_actions}\n"
+            f"  cameras: {cameras}\n"
+            f"  mjcf_path: {mjcf_path}\n"
+        )
 
-        return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras)
+        return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras)

python/rcs/envs/sim.py

@@ -276,6 +276,7 @@ def env_from_xml_paths(
             truncate_on_collision=truncate_on_collision,
         )
 
+
 class RandomObjectPos(SimWrapper):
     """
     Wrapper to randomly re-place an object in the lab environments.
@@ -290,12 +291,15 @@ class RandomObjectPos(SimWrapper):
         include_rotation (bool): Whether to include rotation in the randomization.
     """
 
-    def __init__(self, env: gym.Env, 
-                 simulation: sim.Sim, 
-                 joint_name: str, 
-                 init_object_pose: rcs.common.Pose, 
-                 include_position: bool = True, 
-                 include_rotation: bool = False):
+    def __init__(
+        self,
+        env: gym.Env,
+        simulation: sim.Sim,
+        joint_name: str,
+        init_object_pose: rcs.common.Pose,
+        include_position: bool = True,
+        include_rotation: bool = False,
+    ):
         super().__init__(env, simulation)
         self.joint_name = joint_name
         self.init_object_pose = init_object_pose
@@ -305,33 +309,42 @@ def __init__(self, env: gym.Env,
     def reset(
         self, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:
-        if(options is not None and "RandomObjectPos.init_object_pose" in options.keys()):
-            assert isinstance(options["RandomObjectPos.init_object_pose"], rcs.common.Pose), \
-            "RandomObjectPos.init_object_pose must be a rcs.common.Pose"
+        if options is not None and "RandomObjectPos.init_object_pose" in options.keys():
+            assert isinstance(
+                options["RandomObjectPos.init_object_pose"], rcs.common.Pose
+            ), "RandomObjectPos.init_object_pose must be a rcs.common.Pose"
 
             self.init_object_pose = options["RandomObjectPos.init_object_pose"]
             print("Got random object pos!\n", self.init_object_pose)
             del options["RandomObjectPos.init_object_pose"]
         obs, info = super().reset(seed=seed, options=options)
         self.sim.step(1)
-        
 
         pos_z = self.init_object_pose.translation()[2]
-        if(self.include_position):
+        if self.include_position:
             pos_x = self.init_object_pose.translation()[0] + np.random.random() * 0.2 - 0.1
             pos_y = self.init_object_pose.translation()[1] + np.random.random() * 0.2 - 0.1
         else:
             pos_x = self.init_object_pose.translation()[0]
             pos_y = self.init_object_pose.translation()[1]
 
-        quat = self.init_object_pose.rotation_q() # xyzw format
+        quat = self.init_object_pose.rotation_q()  # xyzw format
         if self.include_rotation:
-            self.sim.data.joint(self.joint_name).qpos = [pos_x, pos_y, pos_z, 2 * np.random.random() - quat[3], quat[0], quat[1], quat[2]]
+            self.sim.data.joint(self.joint_name).qpos = [
+                pos_x,
+                pos_y,
+                pos_z,
+                2 * np.random.random() - quat[3],
+                quat[0],
+                quat[1],
+                quat[2],
+            ]
         else:
             self.sim.data.joint(self.joint_name).qpos = [pos_x, pos_y, pos_z, quat[3], quat[0], quat[1], quat[2]]
 
         return obs, info
-    
+
+
 class RandomCubePos(SimWrapper):
     """Wrapper to randomly place cube in the lab environments."""
 

src/sim/SimRobot.cpp

@@ -75,7 +75,8 @@ void SimRobot::init_ids() {
   // Joints
   for (size_t i = 0; i < std::size(this->cfg.joints); ++i) {
     name = this->cfg.joints[i];
-    this->ids.joints.push_back(mj_name2id(this->sim->m, mjOBJ_JOINT, name.c_str()));
+    this->ids.joints.push_back(
+        mj_name2id(this->sim->m, mjOBJ_JOINT, name.c_str()));
     if (this->ids.joints[i] == -1) {
       throw std::runtime_error(std::string("No joint named " + name));
     }