diff --git a/.gitignore b/.gitignore index 362217ad..3ee6fb06 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,4 @@ config.h wheels .env settings.json +*.egg-info diff --git a/README.md b/README.md index 30187621..6fd0d549 100644 --- a/README.md +++ b/README.md @@ -72,7 +72,6 @@ from rcs.envs.utils import ( from rcs.envs.base import ControlMode, RelativeTo env_rel = SimEnvCreator()( control_mode=ControlMode.JOINTS, - collision_guard=False, robot_cfg=default_sim_robot_cfg(), gripper_cfg=default_sim_gripper_cfg(), cameras=default_mujoco_cameraset_cfg(), @@ -93,10 +92,11 @@ for _ in range(100): ### Examples -Checkout the python examples in the [examples](examples) folder: -- [fr3_direct_control.py](examples/fr3_direct_control.py) shows direct robot control with RCS's python bindings -- [fr3_env_joint_control.py](examples/env_joint_control.py) and [fr3_env_cartesian_control.py](examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control -All of these examples work both in the MuJoCo simulation as well as on your hardware FR3. +Checkout the python examples in the [examples](examples) folder. For example +[fr3_direct_control.py](examples/fr3/fr3_direct_control.py) shows direct robot control with RCS's python bindings. +And [fr3_env_joint_control.py](examples/fr3/fr3_env_joint_control.py) and [fr3_env_cartesian_control.py](examples/fr3/fr3_env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control +Checkout the other sub folders for other robot-specific examples. +Most of these examples work both in the MuJoCo simulation as well as on hardware. ### Hardware Extensions diff --git a/assets/scenes/fr3_empty_world/scene.xml b/assets/scenes/fr3_empty_world/scene.xml index 128dc17b..2ed02ba3 100644 --- a/assets/scenes/fr3_empty_world/scene.xml +++ b/assets/scenes/fr3_empty_world/scene.xml @@ -20,9 +20,9 @@ - - + - diff --git a/assets/scenes/fr3_simple_pick_up/scene.xml b/assets/scenes/fr3_simple_pick_up/scene.xml index 716bfce5..88b1f269 100644 --- a/assets/scenes/fr3_simple_pick_up/scene.xml +++ b/assets/scenes/fr3_simple_pick_up/scene.xml @@ -1,4 +1,4 @@ - + @@ -12,19 +12,14 @@ - - - - + + - - - + @@ -32,9 +27,9 @@ - - - + + + \ No newline at end of file diff --git a/examples/__init__.py b/examples/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/examples/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py similarity index 100% rename from examples/fr3_direct_control.py rename to examples/fr3/fr3_direct_control.py diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py new file mode 100644 index 00000000..a34104bf --- /dev/null +++ b/examples/fr3/fr3_env_cartesian_control.py @@ -0,0 +1,73 @@ +import logging + +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import ( + default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, +) + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +""" +This script demonstrates how to control the FR3 robot in Cartesian position control mode +using relative movements. The robot (or its simulation) moves 1cm forward and then 1cm backward +in a loop while opening and closing the gripper. + +To control a real FR3 robot, install the rcs_fr3 extension (`pip install extensions/rcs_fr3`), +and set the FR3_IP variable to the robot's IP address. Make sure to unlock the robot's joints and +put it into FCI mode before running this script. For a scripted way of unlocking and guiding mode see the +fr3_direct_control.py example which uses the FCI context manager. +""" + +ROBOT_INSTANCE = RobotPlatform.SIMULATION +FR3_IP = "192.168.101.1" + + +def main(): + if ROBOT_INSTANCE == RobotPlatform.SIMULATION: + env_rel = SimEnvCreator()( + control_mode=ControlMode.CARTESIAN_TQuat, + robot_cfg=default_sim_robot_cfg(scene="fr3_empty_world"), + gripper_cfg=default_sim_gripper_cfg(), + cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + else: + from rcs_fr3.creators import RCSFR3EnvCreator + from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg + + env_rel = RCSFR3EnvCreator()( + ip=FR3_IP, + control_mode=ControlMode.CARTESIAN_TQuat, + robot_cfg=default_fr3_hw_robot_cfg(), + gripper_cfg=default_fr3_hw_gripper_cfg(), + camera_set=None, + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + input("the robot is going to move, press enter whenever you are ready") + + env_rel.reset() + + # access low level robot api to get current cartesian position + print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore + + for _ in range(100): + for _ in range(10): + # move 1cm in x direction (forward) and close gripper + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + obs, reward, terminated, truncated, info = env_rel.step(act) + for _ in range(10): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + obs, reward, terminated, truncated, info = env_rel.step(act) + + +if __name__ == "__main__": + main() diff --git a/examples/fr3/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py new file mode 100644 index 00000000..3a0887d8 --- /dev/null +++ b/examples/fr3/fr3_env_joint_control.py @@ -0,0 +1,70 @@ +import logging + +import numpy as np +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs.envs.utils import ( + default_mujoco_cameraset_cfg, + default_sim_gripper_cfg, + default_sim_robot_cfg, +) + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +""" +This script demonstrates how to control the FR3 robot in joint position control mode +using relative movements. The robot (or its simulation) samples random relative joint movements +in a loop. + +To control a real FR3 robot, install the rcs_fr3 extension (`pip install extensions/rcs_fr3`), +change the ROBOT_INSTANCE variable to RobotPlatform.HARDWARE +and set the FR3_IP variable to the robot's IP address. Make sure to unlock the robot's joints and +put it into FCI mode before running this script. For a scripted way of unlocking and guiding mode see the +fr3_direct_control.py example which uses the FCI context manager. +""" + +ROBOT_INSTANCE = RobotPlatform.SIMULATION +FR3_IP = "192.168.101.1" + + +def main(): + if ROBOT_INSTANCE == RobotPlatform.SIMULATION: + env_rel = SimEnvCreator()( + control_mode=ControlMode.JOINTS, + robot_cfg=default_sim_robot_cfg("fr3_empty_world"), + gripper_cfg=default_sim_gripper_cfg(), + cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + else: + from rcs_fr3.creators import RCSFR3EnvCreator + from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg + + env_rel = RCSFR3EnvCreator()( + ip=FR3_IP, + control_mode=ControlMode.JOINTS, + robot_cfg=default_fr3_hw_robot_cfg(), + gripper_cfg=default_fr3_hw_gripper_cfg(), + camera_set=None, + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + input("the robot is going to move, press enter whenever you are ready") + + # access low level robot api to get current cartesian position + print(env_rel.unwrapped.robot.get_joint_position()) # type: ignore + + for _ in range(100): + obs, info = env_rel.reset() + for _ in range(10): + # sample random relative action and execute it + act = env_rel.action_space.sample() + obs, reward, terminated, truncated, info = env_rel.step(act) + + +if __name__ == "__main__": + main() diff --git a/examples/grasp_demo.py b/examples/fr3/grasp_demo.py similarity index 77% rename from examples/grasp_demo.py rename to examples/fr3/grasp_demo.py index d741597c..d54efc06 100644 --- a/examples/grasp_demo.py +++ b/examples/fr3/grasp_demo.py @@ -1,4 +1,5 @@ import logging +from time import sleep from typing import Any, cast import gymnasium as gym @@ -6,6 +7,7 @@ import numpy as np from rcs._core.common import Pose from rcs.envs.base import GripperWrapper, RobotEnv +from rcs.envs.creators import FR3SimplePickUpSimEnvCreator logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -25,14 +27,18 @@ def get_object_pose(self, geom_name) -> Pose: data = self.env.get_wrapper_attr("sim").data geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) - return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3)) + obj_pose_world_coordinates = Pose( + translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3) + ) * Pose( + rpy_vector=np.array([0, 0, np.pi]), translation=[0, 0, 0] # type: ignore + ) + return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates) def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]: waypoints = [] for i in range(num_waypoints + 1): - t = i / (num_waypoints + 1) + t = i / (num_waypoints) waypoints.append(start_pose.interpolate(end_pose, t)) - waypoints.append(end_pose) return waypoints def step(self, action: dict) -> dict: @@ -40,39 +46,32 @@ def step(self, action: dict) -> dict: def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]: 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=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0])) # type: ignore - 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(len(waypoints)): - # calculate delta action - # delta_action = waypoints[i] * waypoints[i - 1].inverse() obs = self.step(self._action(waypoints[i], gripper)) return obs def approach(self, geom_name: str): - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=5) + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) def grasp(self, geom_name: str): - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=15) + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.01, num_waypoints=60) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN) self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED)) - waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=20) + waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) def move_home(self): end_eff_pose = self.unwrapped.robot.get_cartesian_position() - waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10) + waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=60) self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED) def pickup(self, geom_name: str): @@ -82,15 +81,17 @@ def pickup(self, geom_name: str): def main(): - env = gym.make( - "rcs/FR3SimplePickUpSim-v0", + env = FR3SimplePickUpSimEnvCreator()( render_mode="human", delta_actions=False, ) - env.reset() - print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore - controller = PickUpDemo(env) - controller.pickup("box_geom") + # wait for gui to open + sleep(3) + for _ in range(100): + env.reset() + print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore + controller = PickUpDemo(env) + controller.pickup("box_geom") if __name__ == "__main__": diff --git a/examples/fr3_env_cartesian_control.py b/examples/fr3_env_cartesian_control.py deleted file mode 100644 index fecb2a16..00000000 --- a/examples/fr3_env_cartesian_control.py +++ /dev/null @@ -1,49 +0,0 @@ -import logging - -from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import SimEnvCreator -from rcs.envs.utils import ( - default_mujoco_cameraset_cfg, - default_sim_gripper_cfg, - default_sim_robot_cfg, -) - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - - -def main(): - - env_rel = SimEnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_sim_robot_cfg(scene="fr3_empty_world"), - collision_guard=False, - gripper_cfg=default_sim_gripper_cfg(), - cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - env_rel.reset() - print(env_rel.unwrapped.robot.get_cartesian_position()) # type: ignore - - for _ in range(100): - for _ in range(10): - # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - for _ in range(10): - # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -if __name__ == "__main__": - main() diff --git a/examples/fr3_env_joint_control.py b/examples/fr3_env_joint_control.py deleted file mode 100644 index ff19708b..00000000 --- a/examples/fr3_env_joint_control.py +++ /dev/null @@ -1,40 +0,0 @@ -import logging - -import numpy as np -from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import SimEnvCreator -from rcs.envs.utils import ( - default_mujoco_cameraset_cfg, - default_sim_gripper_cfg, - default_sim_robot_cfg, -) - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - - -def main(): - env_rel = SimEnvCreator()( - control_mode=ControlMode.JOINTS, - collision_guard=False, - robot_cfg=default_sim_robot_cfg("fr3_empty_world"), - gripper_cfg=default_sim_gripper_cfg(), - cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=np.deg2rad(5), - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - for _ in range(100): - obs, info = env_rel.reset() - for _ in range(10): - # sample random relative action and execute it - act = env_rel.action_space.sample() - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -if __name__ == "__main__": - main() diff --git a/examples/rpc_run_client.py b/examples/rpc_server_client/rpc_run_client.py similarity index 100% rename from examples/rpc_run_client.py rename to examples/rpc_server_client/rpc_run_client.py diff --git a/examples/rpc_run_server.py b/examples/rpc_server_client/rpc_run_server.py similarity index 95% rename from examples/rpc_run_server.py rename to examples/rpc_server_client/rpc_run_server.py index 74a81125..b2ef4915 100644 --- a/examples/rpc_run_server.py +++ b/examples/rpc_server_client/rpc_run_server.py @@ -11,7 +11,6 @@ def run_server(): env = SimEnvCreator()( control_mode=ControlMode.JOINTS, - collision_guard=False, robot_cfg=default_sim_robot_cfg(), gripper_cfg=default_sim_gripper_cfg(), cameras=default_mujoco_cameraset_cfg(), diff --git a/examples/xarm7/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py new file mode 100644 index 00000000..9bf3e50c --- /dev/null +++ b/examples/xarm7/xarm7_env_cartesian_control.py @@ -0,0 +1,89 @@ +import logging +from time import sleep + +from rcs._core.common import RobotPlatform +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import SimEnvCreator +from rcs_xarm7.creators import RCSXArm7EnvCreator + +import rcs +from rcs import sim + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +""" +The example shows how to create a xArm7 environment with Cartesian control +and a relative action space. The example works both with a real robot and in +simulation. + +To test with a real robot, set ROBOT_INSTANCE to RobotPlatform.HARDWARE, +install the rcs_xarm7 extension (`pip install extensions/rcs_xarm7`) +and set the ROBOT_IP variable to the robot's IP address. +""" + +ROBOT_IP = "192.168.1.245" +ROBOT_INSTANCE = RobotPlatform.SIMULATION +# ROBOT_INSTANCE = RobotPlatform.HARDWARE + + +def main(): + + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSXArm7EnvCreator()( + control_mode=ControlMode.CARTESIAN_TQuat, + ip=ROBOT_IP, + relative_to=RelativeTo.LAST_STEP, + max_relative_movement=0.5, + ) + else: + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + gripper_cfg=None, + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + sleep(3) # wait for gui to open + env_rel.get_wrapper_attr("sim").open_gui() + obs, info = env_rel.reset() + + for _ in range(100): + for _ in range(10): + # move 1cm in x direction (forward) and close gripper + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1]} + obs, reward, terminated, truncated, info = env_rel.step(act) + for _ in range(10): + # move 1cm in negative x direction (backward) and open gripper + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1]} + obs, reward, terminated, truncated, info = env_rel.step(act) + + +if __name__ == "__main__": + main() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py similarity index 69% rename from extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py rename to examples/xarm7/xarm7_env_joint_control.py index 9ae8ae5f..5b98abdd 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py +++ b/examples/xarm7/xarm7_env_joint_control.py @@ -13,19 +13,28 @@ logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) +""" +The example shows how to create a xArm7 environment with joint control +and a relative action space. The example works both with a real robot and in +simulation. + +To test with a real robot, set ROBOT_INSTANCE to RobotPlatform.HARDWARE, +install the rcs_xarm7 extension (`pip install extensions/rcs_xarm7`) +and set the ROBOT_IP variable to the robot's IP address. +""" + ROBOT_IP = "192.168.1.245" -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE +ROBOT_INSTANCE = RobotPlatform.SIMULATION +# ROBOT_INSTANCE = RobotPlatform.HARDWARE def main(): - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: env_rel = RCSXArm7EnvCreator()( control_mode=ControlMode.JOINTS, ip=ROBOT_IP, relative_to=RelativeTo.LAST_STEP, - max_relative_movement=np.deg2rad(3), + max_relative_movement=np.deg2rad(5), ) else: robot_cfg = sim.SimRobotConfig() @@ -56,29 +65,20 @@ def main(): env_rel = SimEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - collision_guard=False, gripper_cfg=None, # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=np.deg2rad(5), relative_to=RelativeTo.LAST_STEP, ) env_rel.get_wrapper_attr("sim").open_gui() + sleep(3) # wait for gui to open - with env_rel: + for _ in range(100): + obs, info = env_rel.reset() for _ in range(10): - obs, info = env_rel.reset() - for _ in range(3): - # sample random relative action and execute it - act = env_rel.action_space.sample() - print(act) - # act["gripper"] = 1.0 - obs, reward, terminated, truncated, info = env_rel.step(act) - print(obs) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - # logger.info(act["gripper"]) - sleep(2.0) + # sample random relative action and execute it + act = env_rel.action_space.sample() + obs, reward, terminated, truncated, info = env_rel.step(act) if __name__ == "__main__": diff --git a/examples/xarm7_env_cartesian_control.py b/examples/xarm7_env_cartesian_control.py deleted file mode 100644 index 99b1ff7b..00000000 --- a/examples/xarm7_env_cartesian_control.py +++ /dev/null @@ -1,70 +0,0 @@ -import logging - -from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import SimEnvCreator - -import rcs -from rcs import sim - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - - -def main(): - - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ - "act1", - "act2", - "act3", - "act4", - "act5", - "act6", - "act7", - ] - robot_cfg.joints = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.CARTESIAN_TQuat, - collision_guard=False, - gripper_cfg=None, - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - obs, info = env_rel.reset() - - for _ in range(100): - for _ in range(10): - # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - for _ in range(10): - # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -if __name__ == "__main__": - main() diff --git a/examples/xarm7_env_joint_control.py b/examples/xarm7_env_joint_control.py deleted file mode 100644 index c66e88a5..00000000 --- a/examples/xarm7_env_joint_control.py +++ /dev/null @@ -1,63 +0,0 @@ -import logging - -import numpy as np -from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import SimEnvCreator - -import rcs -from rcs import sim - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - - -def main(): - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ - "act1", - "act2", - "act3", - "act4", - "act5", - "act6", - "act7", - ] - robot_cfg.joints = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.JOINTS, - collision_guard=False, - gripper_cfg=None, - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=np.deg2rad(5), - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - for _ in range(100): - obs, info = env_rel.reset() - for _ in range(10): - # sample random relative action and execute it - act = env_rel.action_space.sample() - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -if __name__ == "__main__": - main() diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 76369ef1..2c05b60e 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -49,8 +49,6 @@ def __call__( # type: ignore robot_cfg (hw.FR3Config): Configuration for the FR3 robot. collision_guard (str | PathLike | None): Key to a built-in scene robot_cfg (hw.FR3Config): Configuration for the FR3 robot. - collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present) - or the path to a mujoco scene for collision guarding. If None, collision guarding is not used. gripper_cfg (hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. camera_set (BaseHardwareCameraSet | None): Camera set to be used. If None, no cameras are used. max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts @@ -84,6 +82,7 @@ def __call__( # type: ignore logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) + # TODO collision guard not working atm # if collision_guard is not None: # assert urdf_path is not None # env = CollisionGuard.env_from_xml_paths( diff --git a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py index d7cbbb80..a5e25f0d 100644 --- a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py +++ b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py @@ -43,7 +43,6 @@ def main(): control_mode=ControlMode.JOINTS, urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", robot_cfg=cfg, - collision_guard=False, mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/SO101/scene.xml", gripper_cfg=grp_cfg, # camera_set_cfg=default_mujoco_cameraset_cfg(), diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index b928ed91..7c59e103 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -19,13 +19,13 @@ RobotEnv, ) from rcs.envs.creators import RCSHardwareEnvCreator -from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper +from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper, SimWrapper from rcs.hand.tilburg_hand import THConfig, TilburgHand from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig from rcs_xarm7.hw import XArm7 import rcs -from rcs import common, sim +from rcs import sim logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -114,18 +114,18 @@ def __call__( # type: ignore env = GripperWrapper(env, gripper, binary=False) env = GripperWrapperSim(env, gripper) - if collision_guard: - env = CollisionGuard.env_from_xml_paths( - env, - str(rcs.scenes.get(str(mjcf), mjcf)), - str(urdf_path), - gripper=gripper_cfg is not None, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=True, - ) + # if collision_guard: + # env = CollisionGuard.env_from_xml_paths( + # env, + # str(rcs.scenes.get(str(mjcf), mjcf)), + # str(urdf_path), + # gripper=gripper_cfg is not None, + # check_home_collision=False, + # control_mode=control_mode, + # tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), + # sim_gui=True, + # truncate_on_collision=True, + # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py deleted file mode 100644 index bf450c1e..00000000 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py +++ /dev/null @@ -1,108 +0,0 @@ -import logging - -import numpy as np -from rcs._core.common import RobotPlatform -from rcs.envs.base import ControlMode, RelativeTo -from rcs.envs.creators import SimEnvCreator -from rcs.envs.utils import default_sim_tilburg_hand_cfg -from rcs.hand.tilburg_hand import THConfig -from rcs_xarm7.creators import RCSXArm7EnvCreator - -import rcs -from rcs import sim - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - -ROBOT_IP = "192.168.1.245" -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE - - -def sim_env(): - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ - "act1", - "act2", - "act3", - "act4", - "act5", - "act6", - "act7", - ] - robot_cfg.joints = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.CARTESIAN_TQuat, - collision_guard=False, - gripper_cfg=None, - hand_cfg=default_sim_tilburg_hand_cfg(), - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - return env_rel - - -def main(): - - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - hand_cfg = THConfig( - calibration_file="/home/ken/tilburg_hand/calibration.json", grasp_percentage=1, hand_orientation="right" - ) - env_rel = RCSXArm7EnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - ip=ROBOT_IP, - hand_cfg=hand_cfg, - relative_to=RelativeTo.LAST_STEP, - max_relative_movement=np.deg2rad(3), - ) - else: - env_rel = sim_env() - - twin_env = sim_env() - twin_robot = twin_env.unwrapped.robot - twin_sim = twin_env.get_wrapper_attr("sim") - - env_rel.reset() - # act = {"tquat": [0.1, 0, 0, 0, 0, 0, 1], "gripper": 0} - # obs, reward, terminated, truncated, info = env_rel.step(act) - - with env_rel: - for _ in range(10): - for _ in range(10): - # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "hand": 1} - obs, reward, terminated, truncated, info = env_rel.step(act) - joints = env_rel.unwrapped.robot.get_joint_position() # type: ignore - twin_robot.set_joints_hard(joints) - twin_sim.step(50) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - for _ in range(10): - # move 1cm in negative x direction (backward) and open gripper - act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "hand": 0} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -if __name__ == "__main__": - main() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py index 5acdcfa8..350b0295 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py @@ -47,7 +47,6 @@ def sim_env(): env_rel = SimEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - collision_guard=False, gripper_cfg=None, hand_cfg=default_sim_tilburg_hand_cfg(), # cameras=default_mujoco_cameraset_cfg(), diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 070a0e0e..07b172c7 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -16,6 +16,7 @@ __all__ = [ "Sim", "SimCameraConfig", "SimCameraSet", + "SimConfig", "SimGripper", "SimGripperConfig", "SimGripperState", @@ -86,8 +87,10 @@ class Sim: def __init__(self, mjmdl: int, mjdata: int) -> None: ... def _start_gui_server(self, id: str) -> None: ... def _stop_gui_server(self) -> None: ... + def get_config(self) -> SimConfig: ... def is_converged(self) -> bool: ... def reset(self) -> None: ... + def set_config(self, cfg: SimConfig) -> bool: ... def step(self, k: int) -> None: ... def step_until_convergence(self) -> None: ... @@ -106,6 +109,13 @@ class SimCameraSet: @property def _sim(self) -> Sim: ... +class SimConfig: + async_control: bool + frequency: int + max_convergence_steps: int + realtime: bool + def __init__(self) -> None: ... + class SimGripper(rcs._core.common.Gripper): def __init__(self, sim: Sim, cfg: SimGripperConfig) -> None: ... def get_parameters(self) -> SimGripperConfig: ... diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 865b604b..b3d0ca50 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -47,6 +47,7 @@ def __call__( # type: ignore robot_cfg: rcs.sim.SimRobotConfig, collision_guard: bool = False, gripper_cfg: rcs.sim.SimGripperConfig | None = None, + sim_cfg: rcs.sim.SimConfig | None = None, hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, cameras: dict[str, SimCameraConfig] | None = None, max_relative_movement: float | tuple[float, float] | None = None, @@ -75,7 +76,7 @@ def __call__( # type: ignore Returns: gym.Env: The configured simulation environment for the FR3 robot. """ - simulation = sim.Sim(robot_cfg.mjcf_scene_path) + simulation = sim.Sim(robot_cfg.mjcf_scene_path, sim_cfg) ik = rcs.common.Pin( robot_cfg.kinematic_model_path, @@ -137,6 +138,7 @@ def __call__( # type: ignore cameras: dict[str, SimCameraConfig] | None = None, hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, gripper_cfg: rcs.sim.SimGripperConfig | None = None, + sim_cfg: rcs.sim.SimConfig | None = None, random_pos_args: dict | None = None, ) -> gym.Env: mode = "gripper" @@ -170,6 +172,7 @@ def __call__( # type: ignore collision_guard=False, gripper_cfg=_gripper_cfg, hand_cfg=_hand_cfg, + sim_cfg=sim_cfg, cameras=cameras, max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, relative_to=RelativeTo.LAST_STEP, @@ -209,8 +212,16 @@ def __call__( # type: ignore for cam in cam_list } robot_cfg = default_sim_robot_cfg(scene="fr3_simple_pick_up") + robot_cfg.tcp_offset = rcs.common.Pose( + translation=np.array([0.0, 0.0, 0.1034]), # type: ignore + rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), # type: ignore + ) + sim_cfg = sim.SimConfig() + sim_cfg.realtime = False + sim_cfg.async_control = True + sim_cfg.frequency = 30 # in Hz - return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras) + return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras, sim_cfg=sim_cfg) class FR3LabDigitGripperPickUpSimEnvCreator(EnvCreator): diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index ee36ab60..80b0c4d9 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -12,6 +12,7 @@ ) from rcs.envs.space_utils import ActObsInfoWrapper from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg +from rcs.utils import SimpleFrameRate import rcs from rcs import sim @@ -42,10 +43,20 @@ def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | Non assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) self.sim = simulation + cfg = self.sim.get_config() + self.frame_rate = SimpleFrameRate(1 / cfg.frequency, "RobotSimWrapper") def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: _, _, _, _, info = super().step(action) - self.sim.step_until_convergence() + cfg = self.sim.get_config() + if cfg.async_control: + self.sim.step(round(1 / cfg.frequency / self.sim.model.opt.timestep)) + if cfg.realtime: + self.frame_rate.frame_rate = 1 / cfg.frequency + self.frame_rate() + + else: + self.sim.step_until_convergence() state = self.sim_robot.get_state() info["collision"] = state.collision info["ik_success"] = state.ik_success @@ -348,7 +359,7 @@ def reset( class RandomCubePos(SimWrapper): """Wrapper to randomly place cube in the lab environments.""" - def __init__(self, env: gym.Env, simulation: sim.Sim, include_rotation: bool = False): + def __init__(self, env: gym.Env, simulation: sim.Sim, include_rotation: bool = True): super().__init__(env, simulation) self.include_rotation = include_rotation @@ -361,7 +372,7 @@ def reset( iso_cube = np.array([0.498, 0.0, 0.226]) iso_cube_pose = rcs.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) # type: ignore iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() - pos_z = 0.826 + pos_z = 0.0288 / 2 pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index 036f5bb5..dab8c835 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -1,5 +1,6 @@ from rcs._core.sim import ( SimCameraConfig, + SimConfig, SimGripper, SimGripperConfig, SimGripperState, @@ -25,4 +26,5 @@ "SimTilburgHandState", "gui_loop", "SimCameraConfig", + "SimConfig", ] diff --git a/python/rcs/sim/sim.py b/python/rcs/sim/sim.py index 9fc5feb2..542e7370 100644 --- a/python/rcs/sim/sim.py +++ b/python/rcs/sim/sim.py @@ -12,7 +12,7 @@ import mujoco.viewer from rcs._core.sim import GuiClient as _GuiClient from rcs._core.sim import Sim as _Sim -from rcs.sim import egl_bootstrap +from rcs.sim import SimConfig, egl_bootstrap from rcs.utils import SimpleFrameRate egl_bootstrap.bootstrap() @@ -42,7 +42,7 @@ def gui_loop(gui_uuid: str, close_event): class Sim(_Sim): - def __init__(self, mjmdl: str | PathLike): + def __init__(self, mjmdl: str | PathLike, cfg: SimConfig | None = None): mjmdl = Path(mjmdl) if mjmdl.suffix == ".xml": self.model = mj.MjModel.from_xml_path(str(mjmdl)) @@ -58,6 +58,8 @@ def __init__(self, mjmdl: str | PathLike): self._gui_client: Optional[_GuiClient] = None self._gui_process: Optional[mp.context.SpawnProcess] = None self._stop_event: Optional[EventClass] = None + if cfg is not None: + self.set_config(cfg) def close_gui(self): if self._stop_event is not None: diff --git a/python/rcs/utils.py b/python/rcs/utils.py index 704fdd48..e6543377 100644 --- a/python/rcs/utils.py +++ b/python/rcs/utils.py @@ -36,7 +36,7 @@ def __call__(self): sleep(sleep_time) if self._last_print is None or perf_counter() - self._last_print > 10: self._last_print = perf_counter() - logger.info(f"FPS {self.loop_name}: {1 / (perf_counter() - self.t)}") + logger.debug(f"FPS {self.loop_name}: {1 / (perf_counter() - self.t)}") self.t = perf_counter() diff --git a/python/tests/test_rpc.py b/python/tests/test_rpc.py index 10a2efab..83f00caf 100644 --- a/python/tests/test_rpc.py +++ b/python/tests/test_rpc.py @@ -70,7 +70,6 @@ def run_server(host: str, port: int, err_q: multiprocessing.Queue) -> None: try: env = SimEnvCreator()( control_mode=ControlMode.JOINTS, - collision_guard=False, robot_cfg=default_sim_robot_cfg(), gripper_cfg=default_sim_gripper_cfg(), # Disabled to avoid rendering problem in python subprocess. diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 63ea9e45..aa26abe7 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -487,6 +487,14 @@ PYBIND11_MODULE(_core, m) { .def_readonly("last_width", &rcs::sim::SimGripperState::last_width) .def_readonly("collision", &rcs::sim::SimGripperState::collision); + py::class_(sim, "SimConfig") + .def(py::init<>()) + .def_readwrite("async_control", &rcs::sim::SimConfig::async_control) + .def_readwrite("realtime", &rcs::sim::SimConfig::realtime) + .def_readwrite("frequency", &rcs::sim::SimConfig::frequency) + .def_readwrite("max_convergence_steps", + &rcs::sim::SimConfig::max_convergence_steps); + py::class_>(sim, "Sim") .def(py::init([](long m, long d) { return std::make_shared((mjModel *)m, (mjData *)d); @@ -495,10 +503,13 @@ PYBIND11_MODULE(_core, m) { .def("step_until_convergence", &rcs::sim::Sim::step_until_convergence, py::call_guard()) .def("is_converged", &rcs::sim::Sim::is_converged) + .def("set_config", &rcs::sim::Sim::set_config, py::arg("cfg")) + .def("get_config", &rcs::sim::Sim::get_config) .def("step", &rcs::sim::Sim::step, py::arg("k")) .def("reset", &rcs::sim::Sim::reset) .def("_start_gui_server", &rcs::sim::Sim::start_gui_server, py::arg("id")) .def("_stop_gui_server", &rcs::sim::Sim::stop_gui_server); + py::class_>(sim, "SimGripper") .def(py::init, diff --git a/src/sim/sim.cpp b/src/sim/sim.cpp index b33caefd..8059921e 100644 --- a/src/sim/sim.cpp +++ b/src/sim/sim.cpp @@ -28,16 +28,12 @@ bool get_last_return_value(ConditionCallback cb) { Sim::Sim(mjModel* m, mjData* d) : m(m), d(d), renderer(m){}; -bool Sim::set_config(const Config& cfg) { - if (cfg.async) { - /* Not implemented :) */ - return false; - } +bool Sim::set_config(const SimConfig& cfg) { this->cfg = cfg; return true; } -Config Sim::get_config() { return this->cfg; } +SimConfig Sim::get_config() { return this->cfg; } void Sim::invoke_callbacks() { for (int i = 0; i < std::size(this->callbacks); ++i) { @@ -115,9 +111,6 @@ void Sim::step(size_t k) { this->invoke_callbacks(); mj_step2(this->m, this->d); this->invoke_rendering_callbacks(); - if (this->cfg.realtime) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } } } diff --git a/src/sim/sim.h b/src/sim/sim.h index ac48d603..62a2fbbf 100644 --- a/src/sim/sim.h +++ b/src/sim/sim.h @@ -26,10 +26,11 @@ class Renderer { std::unordered_map ctxs; }; -struct Config { - bool async = false; +struct SimConfig { + bool async_control = false; bool realtime = false; - int max_convergence_steps = 5000; + int frequency = 30; // in Hz + int max_convergence_steps = 500; }; struct Callback { @@ -56,7 +57,7 @@ struct RenderingCallback { class Sim { private: - Config cfg; + SimConfig cfg; std::vector callbacks; std::vector any_callbacks; std::vector all_callbacks; @@ -74,8 +75,8 @@ class Sim { mjModel* m; mjData* d; Sim(mjModel* m, mjData* d); - bool set_config(const Config& cfg); - Config get_config(); + bool set_config(const SimConfig& cfg); + SimConfig get_config(); bool is_converged(); void step_until_convergence(); void step(size_t k); diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 602ba8cd..41cf2916 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -123,7 +123,7 @@ int test_sim() { auto sim = std::make_shared(m, d); auto cfg = sim->get_config(); cfg.realtime = true; - cfg.async = false; + cfg.async_control = false; sim->set_config(cfg); std::string id = "0";