diff --git a/deformable_gym/envs/__init__.py b/deformable_gym/envs/__init__.py index 98b9745..5935bd5 100644 --- a/deformable_gym/envs/__init__.py +++ b/deformable_gym/envs/__init__.py @@ -5,7 +5,6 @@ from .ur5_mia_grasp_env import UR5MiaGraspEnv from .ur10_shadow_grasp_env import UR10ShadowGraspEnv - __all__ = [ "BaseBulletEnv", "FloatingMiaGraspEnv", diff --git a/deformable_gym/envs/base_env.py b/deformable_gym/envs/base_env.py index 4202c14..cae1b9b 100644 --- a/deformable_gym/envs/base_env.py +++ b/deformable_gym/envs/base_env.py @@ -6,11 +6,12 @@ import numpy.typing as npt import pybullet as pb import pytransform3d.rotations as pr - from gymnasium import spaces -from deformable_gym.robots.bullet_robot import BulletRobot +from pybullet_utils import bullet_client as bc + from deformable_gym.envs.bullet_simulation import BulletSimulation from deformable_gym.helpers.pybullet_helper import MultibodyPose +from deformable_gym.robots.bullet_robot import BulletRobot class BaseBulletEnv(gym.Env, abc.ABC): @@ -53,8 +54,14 @@ def __init__( mode = pb.GUI if gui else pb.DIRECT self.simulation = BulletSimulation( - soft=soft, time_delta=time_delta, real_time=real_time, mode=mode, - verbose_dt=verbose_dt, pybullet_options=pybullet_options) + soft=soft, + time_delta=time_delta, + real_time=real_time, + mode=mode, + verbose_dt=verbose_dt, + pybullet_options=pybullet_options) + + self.pb_client = self.simulation.pb_client # TODO should we make this configurable? this results in 100 Hz self.simulation.timing.add_subsystem("time_step", 100) @@ -67,9 +74,11 @@ def _create_robot(self): def _load_objects(self): """Load objects to PyBullet simulation.""" - self.plane = pb.loadURDF("plane.urdf", - (0, 0, 0), - useFixedBase=1) + self.plane = self.pb_client.loadURDF( + "plane.urdf", + (0, 0, 0), + useFixedBase=1 + ) def _hard_reset(self): """Hard reset the PyBullet simulation and reload all objects. May be @@ -87,7 +96,7 @@ def reset(self, seed=None, options=None) -> npt.ArrayLike: :return: Initial state. """ - super().reset(seed=seed) + super().reset(seed=seed, options=options) if options is not None and "hard_reset" in options: self._hard_reset() @@ -129,18 +138,25 @@ def _get_observation(self) -> npt.ArrayLike: """ return self.robot.get_joint_positions() - def _get_info(self): + def _get_info( + self, + observation: npt.ArrayLike = None, + action: npt.ArrayLike = None, + reward: float = None, + next_observation: npt.ArrayLike = None + ) -> dict: """Returns the current environment state. :return: The observation """ return {} - def _is_terminated(self, - observation: npt.ArrayLike, - action: npt.ArrayLike, - next_observation: npt.ArrayLike - ) -> bool: + def _is_terminated( + self, + observation: npt.ArrayLike, + action: npt.ArrayLike, + next_observation: npt.ArrayLike + ) -> bool: """Checks whether the current episode is terminated. :param observation: observation before action was taken @@ -150,10 +166,12 @@ def _is_terminated(self, """ return self.step_counter >= self.horizon - def _is_truncated(self, - state: npt.ArrayLike, - action: npt.ArrayLike, - next_state: npt.ArrayLike) -> bool: + def _is_truncated( + self, + state: npt.ArrayLike, + action: npt.ArrayLike, + next_state: npt.ArrayLike + ) -> bool: """Checks whether the current episode is truncated. :param state: State @@ -196,22 +214,31 @@ def step(self, action: npt.ArrayLike): truncated = self._is_truncated(observation, action, next_observation) # calculate the reward - reward = self.calculate_reward(observation, action, next_observation, terminated) + reward = self.calculate_reward( + observation, action, next_observation, terminated) + + info = self._get_info(observation, action, reward) if self.verbose: - print(f"Finished environment step: {next_observation=}, {reward=}, {terminated=}, {truncated=}") + print(f"Finished environment step: " + f"{next_observation=}, " + f"{reward=}, " + f"{terminated=}, " + f"{truncated=}") - return next_observation, reward, terminated, truncated, {} + return next_observation, reward, terminated, truncated, info def close(self): self.simulation.disconnect() @abc.abstractmethod - def calculate_reward(self, - state: npt.ArrayLike, - action: npt.ArrayLike, - next_state: npt.ArrayLike, - terminated: bool): + def calculate_reward( + self, + state: npt.ArrayLike, + action: npt.ArrayLike, + next_state: npt.ArrayLike, + terminated: bool + ) -> float: """Calculate reward. :param state: State of the environment. @@ -280,8 +307,13 @@ def _init_hand_pose(self, robot: BulletRobot): :param robot: Floating hand. """ desired_robot2world_pos = self.hand_world_pose[:3] - desired_robot2world_orn = pb.getQuaternionFromEuler(self.hand_world_pose[3:]) - self.multibody_pose = MultibodyPose(robot.get_id(), desired_robot2world_pos, desired_robot2world_orn) + desired_robot2world_orn = pb.getQuaternionFromEuler( + self.hand_world_pose[3:]) + self.multibody_pose = MultibodyPose( + robot.get_id(), + desired_robot2world_pos, + desired_robot2world_orn + ) def set_world_pose(self, world_pose): """Set pose of the hand. @@ -289,10 +321,14 @@ def set_world_pose(self, world_pose): :param world_pose: world pose of hand given as position and quaternion: (x, y, z, qw, qx, qy, qz) """ - self.hand_world_pose = MultibodyPose.external_pose_to_internal_pose(world_pose) + self.hand_world_pose = MultibodyPose.external_pose_to_internal_pose( + world_pose) desired_robot2world_pos = self.hand_world_pose[:3] - desired_robot2world_orn = pb.getQuaternionFromEuler(self.hand_world_pose[3:]) - self.multibody_pose.set_pose(desired_robot2world_pos, desired_robot2world_orn) + desired_robot2world_orn = pb.getQuaternionFromEuler( + self.hand_world_pose[3:]) + self.multibody_pose.set_pose( + desired_robot2world_pos, + desired_robot2world_orn) def get_world_pose(self): """Get pose of the hand. diff --git a/deformable_gym/envs/bullet_simulation.py b/deformable_gym/envs/bullet_simulation.py index cf01952..a425f67 100644 --- a/deformable_gym/envs/bullet_simulation.py +++ b/deformable_gym/envs/bullet_simulation.py @@ -1,5 +1,7 @@ import pybullet as pb import pybullet_data + +from pybullet_utils import bullet_client as bc from deformable_gym.robots.bullet_robot import BulletRobot @@ -16,9 +18,13 @@ class BulletSimulation: connection command. """ def __init__( - self, time_delta: float = 0.001, mode: int = pb.GUI, - gravity: float = -9.81, soft: bool = False, - real_time: bool = False, verbose_dt: float = 0.01, + self, + time_delta: float = 0.001, + mode: int = pb.GUI, + gravity: float = -9.81, + soft: bool = False, + real_time: bool = False, + verbose_dt: float = 0.01, pybullet_options: str = ""): self.time_delta = time_delta @@ -27,33 +33,37 @@ def __init__( self.soft = soft self.real_time = real_time - self._client = pb.connect(self.mode, options=pybullet_options) - self.timing = BulletTiming(dt=time_delta, verbose_dt=verbose_dt, - client_id=self._client) + self.pb_client = bc.BulletClient( + connection_mode=self.mode, + options=pybullet_options + ) + self.pb_client.setAdditionalSearchPath(pybullet_data.getDataPath()) - pb.setAdditionalSearchPath(pybullet_data.getDataPath()) + self.timing = BulletTiming( + pb_client=self.pb_client, + dt=time_delta, + verbose_dt=verbose_dt, + ) self.reset() - self.camera = BulletCamera() + self.camera = BulletCamera(self.pb_client) def reset(self): """Reset and initialize simulation.""" if self.soft: - pb.resetSimulation(pb.RESET_USE_DEFORMABLE_WORLD) + self.pb_client.resetSimulation(pb.RESET_USE_DEFORMABLE_WORLD) else: - pb.resetSimulation() + self.pb_client.resetSimulation() - print(f"resetting client {self._client}") + print(f"resetting client {self.pb_client}") - pb.setGravity(0, 0, self.gravity, physicsClientId=self._client) - pb.setRealTimeSimulation(self.real_time, physicsClientId=self._client) - pb.setTimeStep(self.time_delta, physicsClientId=self._client) + self.pb_client.setGravity(0, 0, self.gravity) + self.pb_client.setRealTimeSimulation(self.real_time) + self.pb_client.setTimeStep(self.time_delta) - pb.configureDebugVisualizer( - pb.COV_ENABLE_RENDERING, 1, physicsClientId=self._client) - pb.configureDebugVisualizer( - pb.COV_ENABLE_GUI, 0, physicsClientId=self._client) + self.pb_client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) + self.pb_client.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) def add_robot(self, robot: BulletRobot): """Add robot to this simulation. @@ -88,34 +98,32 @@ def simulate_time(self, time): for _ in range(int(time / self.time_delta)): self.timing.step() - def get_physics_client_id(self): - """Get physics client ID of PyBullet instance. - - :return: Physics client ID. - """ - return self._client - - def disconnect(self): + def disconnect(self) -> None: """Shut down physics client instance.""" - pb.disconnect(self._client) + self.pb_client.disconnect() class BulletTiming: """This class handles all timing issues for a single BulletSimulation.""" - def __init__(self, dt=0.001, verbose_dt=0.01, client_id=0): + def __init__( + self, + pb_client: bc.BulletClient, + dt: float = 0.001, + verbose_dt: float = 0.01, + ): """ Create new BulletTiming instance. :param dt: The time delta used in the BulletSimulation. :param verbose_dt: Time after we print debug info. - :param client_id: PyBullet instance ID. + :param pb_client: PyBullet instance ID. """ # initialise values self.dt = dt self.verbose_dt = verbose_dt - self._client = client_id + self._pb_client = pb_client self.time_step = 0 self.sim_time = 0.0 @@ -135,8 +143,8 @@ def add_subsystem(self, name, frequency, callback=None): is triggered. """ if name not in self.subsystems.keys(): - self.subsystems[name] = (max(1, round(1.0/frequency/self.dt)), - callback) + self.subsystems[name] = ( + max(1, round(1.0/frequency/self.dt)), callback) def remove_subsystem(self, name): """ @@ -167,7 +175,7 @@ def step(self): """ triggers = self.get_triggered_subsystems() self._run_callbacks(triggers) - pb.stepSimulation(physicsClientId=self._client) + self._pb_client.stepSimulation() self.time_step += 1 self.sim_time += self.dt @@ -182,28 +190,36 @@ def reset(self): class BulletCamera: """This class handles all camera operations for one BulletSimulation.""" - def __init__(self, position=(0, 0, 0), pitch=-52, yaw=30, distance=3): + def __init__( + self, + pb_client: bc.BulletClient, + position: tuple = (0, 0, 0), + pitch: int = -52, + yaw: int = 30, + distance: int = 3, + ): self.position = position self.pitch = pitch self.yaw = yaw self.distance = distance + self.pb_client = pb_client self._active = False self._logging_id = None - pb.resetDebugVisualizerCamera(distance, yaw, pitch, position) + self.pb_client.resetDebugVisualizerCamera(distance, yaw, pitch, position) def start_recording(self, path): if not self._active: - self._logging_id = pb.startStateLogging(pb.STATE_LOGGING_VIDEO_MP4, - path) + self._logging_id = self.pb_client.startStateLogging( + pb.STATE_LOGGING_VIDEO_MP4, path) return self._logging_id else: return None def stop_recording(self): if self._active: - pb.stopStateLogging(self._logging_id) + self.pb_client.stopStateLogging(self._logging_id) def reset(self, position, pitch, yaw, distance): self.position = position @@ -211,4 +227,5 @@ def reset(self, position, pitch, yaw, distance): self.yaw = yaw self.distance = distance - pb.resetDebugVisualizerCamera(distance, yaw, pitch, position) + self.pb_client.resetDebugVisualizerCamera( + distance, yaw, pitch, position) diff --git a/deformable_gym/envs/floating_mia_grasp_env.py b/deformable_gym/envs/floating_mia_grasp_env.py index 80021ff..68af560 100644 --- a/deformable_gym/envs/floating_mia_grasp_env.py +++ b/deformable_gym/envs/floating_mia_grasp_env.py @@ -1,13 +1,13 @@ import numpy as np import pybullet as pb from gymnasium import spaces -from deformable_gym.robots import mia_hand -from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin + +from deformable_gym.envs.grasp_env import GraspEnv from deformable_gym.helpers import pybullet_helper as pbh -from deformable_gym.objects.bullet_object import ObjectFactory +from deformable_gym.robots import mia_hand -class FloatingMiaGraspEnv(GraspDeformableMixin, BaseBulletEnv): +class FloatingMiaGraspEnv(GraspEnv): """Grasp an insole with a floating Mia hand. **State space:** @@ -34,29 +34,35 @@ class FloatingMiaGraspEnv(GraspDeformableMixin, BaseBulletEnv): object should be sampled from the training or the test set. """ - STANDARD_INITIAL_POSE = np.r_[0.03, -0.005, 1.0, - pb.getQuaternionFromEuler([-np.pi/8, np.pi, 0])] - - HARD_INITIAL_POSE = np.r_[0.03, -0.025, 1.0, - pb.getQuaternionFromEuler([-np.pi/8, np.pi, 0])] - - _FINGERS_OPEN = {"j_index_fle": 0.0, - "j_little_fle": 0.0, - "j_mrl_fle": 0.0, - "j_ring_fle": 0.0, - "j_thumb_fle": 0.0} - - _FINGERS_HALFWAY_CLOSED = {"j_index_fle": 0.5, - "j_little_fle": 0.5, - "j_mrl_fle": 0.5, - "j_ring_fle": 0.5, - "j_thumb_fle": 0.3} - - _FINGERS_CLOSED = {"j_index_fle": 0.71, - "j_little_fle": 0.71, - "j_mrl_fle": 0.71, - "j_ring_fle": 0.71, - "j_thumb_fle": 0.3} + STANDARD_INITIAL_POSE = np.r_[ + 0.03, -0.005, 1.0, pb.getQuaternionFromEuler([-np.pi/8, np.pi, 0])] + + HARD_INITIAL_POSE = np.r_[ + 0.03, -0.025, 1.0, pb.getQuaternionFromEuler([-np.pi/8, np.pi, 0])] + + _FINGERS_OPEN = { + "j_index_fle": 0.0, + "j_little_fle": 0.0, + "j_mrl_fle": 0.0, + "j_ring_fle": 0.0, + "j_thumb_fle": 0.0 + } + + _FINGERS_HALFWAY_CLOSED = { + "j_index_fle": 0.5, + "j_little_fle": 0.5, + "j_mrl_fle": 0.5, + "j_ring_fle": 0.5, + "j_thumb_fle": 0.3 + } + + _FINGERS_CLOSED = { + "j_index_fle": 0.71, + "j_little_fle": 0.71, + "j_mrl_fle": 0.71, + "j_ring_fle": 0.71, + "j_thumb_fle": 0.3 + } _MAX_POS_OFFSET = .0005 _MAX_ORN_OFFSET = .000005 @@ -64,21 +70,18 @@ class FloatingMiaGraspEnv(GraspDeformableMixin, BaseBulletEnv): def __init__( self, object_name: str = "insole", - compute_reward: bool = True, object_scale: float = 1.0, observable_object_pos: bool = False, difficulty_mode: str = "hard", **kwargs): - self.insole = None self.velocity_commands = False - self.object_name = object_name - self.randomised = False - self.compute_reward = compute_reward - self.object_scale = object_scale - self._observable_object_pos = observable_object_pos - super().__init__(soft=True, **kwargs) + super().__init__( + object_name=object_name, + object_scale=object_scale, + observable_object_pos=observable_object_pos, + **kwargs) self.hand_world_pose = self.STANDARD_INITIAL_POSE self.robot = self._create_robot() @@ -101,10 +104,10 @@ def __init__( np.full(6, 10.)]) if self._observable_object_pos: - lower_observations = np.append(lower_observations, - -np.full(3, 2.)) - upper_observations = np.append(upper_observations, - np.full(3, 2.)) + lower_observations = np.append( + lower_observations, -np.full(3, 2.)) + upper_observations = np.append( + upper_observations, np.full(3, 2.)) self.observation_space = spaces.Box( low=lower_observations, @@ -113,32 +116,39 @@ def __init__( ) # build the action space - lower = [-np.full(3, self._MAX_POS_OFFSET), # max negative base pos offset - -np.full(4, self._MAX_ORN_OFFSET), # max negative base orn offset - limits[0][self.actuated_finger_ids]] # negative joint limits - - upper = [np.full(3, self._MAX_POS_OFFSET), # max positive base pos offset - np.full(4, self._MAX_ORN_OFFSET), # max positive base orn offset - limits[1][self.actuated_finger_ids]] # positive joint limits - - self.action_space = spaces.Box(low=np.concatenate(lower), - high=np.concatenate(upper), - dtype=np.float64) + lower = [ + -np.full(3, self._MAX_POS_OFFSET), # max negative base pos offset + -np.full(4, self._MAX_ORN_OFFSET), # max negative base orn offset + limits[0][self.actuated_finger_ids] + ] # negative joint limits + + upper = [ + np.full(3, self._MAX_POS_OFFSET), # max positive base pos offset + np.full(4, self._MAX_ORN_OFFSET), # max positive base orn offset + limits[1][self.actuated_finger_ids] + ] # positive joint limits + + self.action_space = spaces.Box( + low=np.concatenate(lower), + high=np.concatenate(upper), + dtype=np.float64) def _create_robot(self): orn_limit = None if self.velocity_commands: robot = mia_hand.MiaHandVelocity( + self.pb_client, world_pos=self.hand_world_pose[:3], - world_orn=pb.getEulerFromQuaternion(self.hand_world_pose[3:]), + world_orn=self.hand_world_pose[3:], verbose=self.verbose, orn_limit=orn_limit, base_commands=True) else: robot = mia_hand.MiaHandPosition( + self.pb_client, world_pos=self.hand_world_pose[:3], - world_orn=pb.getEulerFromQuaternion(self.hand_world_pose[3:]), + world_orn=self.hand_world_pose[3:], verbose=self.verbose, orn_limit=orn_limit, base_commands=True) @@ -147,10 +157,6 @@ def _create_robot(self): return robot - def _load_objects(self): - super()._load_objects() - self.object_to_grasp, self.object_position, self.object_orientation = ObjectFactory().create(self.object_name) - def set_difficulty_mode(self, mode: str): """ Sets the difficulty of the environment by changing the initial hand @@ -171,24 +177,9 @@ def set_difficulty_mode(self, mode: str): else: raise ValueError(f"Received unknown difficulty mode {mode}!") - def reset(self, seed=None, options=None): - - if options is not None and "initial_pose" in options: - self.robot.reset_base(options["initial_pose"]) - else: - self.robot.reset_base(self.hand_world_pose) - - self.object_to_grasp.reset() - self.robot.activate_motors() - - return super().reset(seed, options) - - def _is_truncated(self, state, action, next_state): - # check if insole is exploded - return self._deformable_is_exploded() - def _get_observation(self): - joint_pos = self.robot.get_joint_positions(self.robot.actuated_real_joints) + joint_pos = self.robot.get_joint_positions( + self.robot.actuated_real_joints) ee_pose = self.robot.get_ee_pose() sensor_readings = self.robot.get_sensor_readings() @@ -199,24 +190,3 @@ def _get_observation(self): state = np.append(state, obj_pos) return state - - def calculate_reward(self, state, action, next_state, terminated): - """ - Calculates the reward by counting how many insole vertices are in the - target position. - """ - if terminated: - self.robot.deactivate_motors() - # remove insole anchors and simulate steps - self.object_to_grasp.remove_anchors() - for _ in range(50): - if self._deformable_is_exploded(): - return -1.0 - self.simulation.step_to_trigger("time_step") - height = self.object_to_grasp.get_pose()[2] - if height < 0.9: - return -1.0 - else: - return 1.0 - else: - return 0.0 diff --git a/deformable_gym/envs/floating_shadow_grasp_env.py b/deformable_gym/envs/floating_shadow_grasp_env.py index 9b8edfd..7405369 100644 --- a/deformable_gym/envs/floating_shadow_grasp_env.py +++ b/deformable_gym/envs/floating_shadow_grasp_env.py @@ -1,14 +1,13 @@ -import random - import numpy as np +import pybullet as pb from gymnasium import spaces -from deformable_gym.robots import shadow_hand -from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin + +from deformable_gym.envs.grasp_env import GraspEnv from deformable_gym.helpers import pybullet_helper as pbh -from deformable_gym.objects.bullet_object import ObjectFactory +from deformable_gym.robots import shadow_hand -class FloatingShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv): +class FloatingShadowGraspEnv(GraspEnv): """Grasp an insole with a floating Shadow hand. **State space:** @@ -25,45 +24,51 @@ class FloatingShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv): :param real_time: Simulate environment in real time. :param verbose: Verbose output. :param horizon: Number of steps in simulation. - :param train: Bool flag indicating if the starting position of the grasped - object should be sampled from the training or the test set. """ - def __init__(self, object_name="insole", - horizon=100, train=True, - compute_reward=True, object_scale=1.0, - observable_object_pos: bool = False, - **kwargs): - self.insole = None - self.train = train + STANDARD_INITIAL_POSE = np.r_[ + 0, -0.5, 1, pb.getQuaternionFromEuler([-np.pi/2, np.pi, 0])] + + def __init__( + self, + object_name: str = "insole", + object_scale: float = 1.0, + observable_object_pos: bool = False, + **kwargs): self.velocity_commands = False - self.object_name = object_name - self.randomised = False - self.compute_reward = compute_reward - self.object_scale = object_scale - self._observable_object_pos = observable_object_pos - super().__init__(horizon=horizon, soft=True, **kwargs) + super().__init__(object_name=object_name, + object_scale=object_scale, + observable_object_pos=observable_object_pos, + **kwargs) - self.hand_world_pose = (0, -0.5, 1, -np.pi/2, np.pi, 0) + self.hand_world_pose = self.STANDARD_INITIAL_POSE self.robot = self._create_robot() limits = pbh.get_limit_array(self.robot.motors.values()) lower_observations = np.concatenate([ - np.array([-2, -2, 0]), -np.ones(4), limits[0][6:], + np.array([-2, -2, 0]), + -np.ones(4), + limits[0][6:], np.array([-10, -10, -10])], axis=0) upper_observations = np.concatenate([ - np.array([2, 2, 2]), np.ones(4), limits[1][6:], + np.array([2, 2, 2]), + np.ones(4), + limits[1][6:], np.array([10, 10, 10])], axis=0) if self._observable_object_pos: - lower_observations = np.append(lower_observations, -np.ones(3)*2) - upper_observations = np.append(upper_observations, np.ones(3)*2) + lower_observations = np.append( + lower_observations, -np.full(3, 2.)) + upper_observations = np.append( + upper_observations, np.full(3, 2.)) self.observation_space = spaces.Box( - low=lower_observations, high=upper_observations, dtype=np.float64) + low=lower_observations, + high=upper_observations, + dtype=np.float64) lower_actions = np.concatenate([ -np.ones(7)*0.01, limits[0], [0]], axis=0) @@ -71,7 +76,8 @@ def __init__(self, object_name="insole", upper_actions = np.concatenate([ np.ones(7)*0.01, limits[1], [1]], axis=0) - self.action_space = spaces.Box(low=lower_actions, high=upper_actions, dtype=np.float64) + self.action_space = spaces.Box( + low=lower_actions, high=upper_actions, dtype=np.float64) def _create_robot(self): task_space_limit = None @@ -79,60 +85,27 @@ def _create_robot(self): if self.velocity_commands: robot = shadow_hand.ShadowHandVelocity( + pb_client=self.pb_client, world_pos=self.hand_world_pose[:3], world_orn=self.hand_world_pose[3:], - task_space_limit=task_space_limit, verbose=self.verbose, - orn_limit=orn_limit, base_commands=True) + task_space_limit=task_space_limit, + verbose=self.verbose, + orn_limit=orn_limit, + base_commands=True) else: robot = shadow_hand.ShadowHandPosition( + pb_client=self.pb_client, world_pos=self.hand_world_pose[:3], world_orn=self.hand_world_pose[3:], - task_space_limit=task_space_limit, verbose=self.verbose, - orn_limit=orn_limit, base_commands=True) + task_space_limit=task_space_limit, + verbose=self.verbose, + orn_limit=orn_limit, + base_commands=True) self.simulation.add_robot(robot) return robot - def _load_objects(self): - super()._load_objects() - self.object_to_grasp, self.object_position, self.object_orientation = ObjectFactory().create(self.object_name) - - def reset(self, seed=None, options=None): - self.object_to_grasp.reset() - self.robot.activate_motors() - - return super().reset(seed, options) - - def _is_truncated(self, state, action, next_state): - return self._deformable_is_exploded() - def _get_observation(self): finger_pos = self.robot.get_joint_positions()[6:] return np.concatenate([finger_pos], axis=0) - - def calculate_reward(self, state, action, next_state, done): - """ - Calculates the reward by counting how many insole vertices are in the - target position. - """ - if done: - if not self.compute_reward: - return 0.0 - if not (round(action[-1]) == 1 or self.step_counter >= self.horizon): - return -100 - - self.robot.deactivate_motors() - # remove insole anchors and simulate steps - self.object_to_grasp.remove_anchors() - for _ in range(50): - if self._deformable_is_exploded(): - return -1 - self.simulation.step_to_trigger("time_step") - height = self.object_to_grasp.get_pose()[2] - if height < 0.9: - return -1 - else: - return 1 - else: - return 0.0 diff --git a/deformable_gym/envs/grasp_env.py b/deformable_gym/envs/grasp_env.py index 23a7a79..322c653 100644 --- a/deformable_gym/envs/grasp_env.py +++ b/deformable_gym/envs/grasp_env.py @@ -1,66 +1,71 @@ +from abc import ABC + import numpy as np +import pybullet as pb -from abc import ABC -from base_env import BaseBulletEnv -from ..objects.bullet_object import ObjectFactory +from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin +from ..objects.bullet_object import ObjectFactory -class GraspEnv(BaseBulletEnv, ABC): - def __init__(self, - object_name, - observable_object_pos: bool = False): +class GraspEnv(BaseBulletEnv, GraspDeformableMixin, ABC): + HARD_INITIAL_POSE = np.r_[ + 0.03, -0.025, 1.0, pb.getQuaternionFromEuler([-np.pi / 8, np.pi, 0])] + def __init__( + self, + object_name: str = "insole", + object_scale: float = 1.0, + observable_object_pos: bool = False, + **kwargs + ): self.object_name = object_name + self.object_scale = object_scale self._observable_object_pos = observable_object_pos + self.hand_world_pose = self.HARD_INITIAL_POSE - super().__init__(soft=True) + super().__init__(soft=True, **kwargs) - self.robot = self._create_robot() + # self.robot = self._create_robot() # TODO: adapt if non-robot observable objects in environment - self.action_space = self.robot.action_space - self.observation_space = self.robot.observation_space + # self.action_space = self.robot.action_space + # self.observation_space = self.robot.observation_space def _load_objects(self): super()._load_objects() - self.object_to_grasp, self.object_position, self.object_orientation = ObjectFactory().create(self.object_name) + (self.object_to_grasp, + self.object_position, + self.object_orientation) = ObjectFactory(self.pb_client).create( + self.object_name) def reset(self, seed=None, options=None): - if options is not None and "pos" in options: - pos = options["pos"] + if options is not None and "initial_pose" in options: + self.robot.reset_base(options["initial_pose"]) + else: + self.robot.reset_base(self.hand_world_pose) - self.robot.reset(pos) self.object_to_grasp.reset() self.robot.activate_motors() return super().reset(seed, options) - def _get_observation(self): - joint_pos = self.robot.get_joint_positions(self.robot.actuated_real_joints) - sensor_readings = self.robot.get_sensor_readings() + def _is_truncated(self, state, action, next_state): + return self._deformable_is_exploded() - state = np.concatenate([joint_pos, sensor_readings]) - - if self._observable_object_pos: - obj_pos = self.object_to_grasp.get_pose()[:3] - state = np.append(state, obj_pos) - - return state - - def calculate_reward(self, state, action, next_state, done): + def calculate_reward(self, state, action, next_state, terminated): """ Calculates the reward by counting how many insole vertices are in the target position. """ - if done: - - # self.robot.deactivate_motors() + if terminated: + self.robot.deactivate_motors() # remove insole anchors and simulate steps self.object_to_grasp.remove_anchors() - for _ in range(50): + if self._deformable_is_exploded(): + return -1.0 self.simulation.step_to_trigger("time_step") height = self.object_to_grasp.get_pose()[2] if height < 0.9: diff --git a/deformable_gym/envs/unused/lifting_env.py b/deformable_gym/envs/unused/lifting_env.py index fb0702f..c08df62 100644 --- a/deformable_gym/envs/unused/lifting_env.py +++ b/deformable_gym/envs/unused/lifting_env.py @@ -1,9 +1,10 @@ import numpy as np from gym import spaces -from deformable_gym.robots import ur5_mia + from deformable_gym.envs.base_env import BaseBulletEnv from deformable_gym.helpers import pybullet_helper as pbh from deformable_gym.objects.bullet_object import BoxObject +from deformable_gym.robots import ur5_mia class LiftingEnv(BaseBulletEnv): diff --git a/deformable_gym/envs/unused/stacking_env.py b/deformable_gym/envs/unused/stacking_env.py index 29fb67b..6328675 100644 --- a/deformable_gym/envs/unused/stacking_env.py +++ b/deformable_gym/envs/unused/stacking_env.py @@ -1,9 +1,10 @@ import numpy as np from gym import spaces -from deformable_gym.robots import ur5_mia + from deformable_gym.envs.base_env import BaseBulletEnv from deformable_gym.helpers import pybullet_helper as pbh from deformable_gym.objects.bullet_object import BoxObject +from deformable_gym.robots import ur5_mia class StackingEnv(BaseBulletEnv): diff --git a/deformable_gym/envs/unused/ur5_mia_reach_env.py b/deformable_gym/envs/unused/ur5_mia_reach_env.py index 29592bd..d7ac648 100644 --- a/deformable_gym/envs/unused/ur5_mia_reach_env.py +++ b/deformable_gym/envs/unused/ur5_mia_reach_env.py @@ -1,9 +1,10 @@ import numpy as np from gym import spaces -from deformable_gym.robots import ur5_mia + from deformable_gym.envs.base_env import BaseBulletEnv from deformable_gym.helpers import pybullet_helper as pbh from deformable_gym.objects.bullet_object import UrdfObject +from deformable_gym.robots import ur5_mia VERBOSITY = 0 diff --git a/deformable_gym/envs/ur10_shadow_grasp_env.py b/deformable_gym/envs/ur10_shadow_grasp_env.py index c3949df..5cc3766 100644 --- a/deformable_gym/envs/ur10_shadow_grasp_env.py +++ b/deformable_gym/envs/ur10_shadow_grasp_env.py @@ -1,11 +1,11 @@ -import pytransform3d.transformations as pt import numpy as np +import pytransform3d.transformations as pt +from gymnasium import spaces -from deformable_gym.robots import ur10_shadow from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin from deformable_gym.helpers import pybullet_helper as pbh from deformable_gym.objects.bullet_object import ObjectFactory -from gymnasium import spaces +from deformable_gym.robots import ur10_shadow class UR10ShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv): @@ -29,20 +29,22 @@ class UR10ShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv): object should be sampled from the training or the test set. """ - object2world = pt.transform_from(R=np.eye(3), - p=np.array([-0.7, 0.1, 1.8])) - - def __init__(self, gui=True, real_time=False, object_name="insole", - verbose=False, horizon=100, train=True, - compute_reward=True, object_scale=1.0, verbose_dt=10.0): + object2world = pt.transform_from(R=np.eye(3), p=np.array([-0.7, 0.1, 1.8])) + + def __init__( + self, + gui=True, + real_time=False, + object_name="insole", + verbose=False, + horizon=100, + object_scale=1.0, + verbose_dt=10.0 + ): self.insole = None - self.train = train self.velocity_commands = False self.object_name = object_name - self.randomised = False - self.compute_reward = compute_reward self.object_scale = object_scale - super().__init__(gui=gui, real_time=real_time, horizon=horizon, soft=True, verbose=verbose, verbose_dt=verbose_dt) @@ -77,16 +79,21 @@ def _create_robot(self): if self.velocity_commands: robot = ur10_shadow.UR10ShadowVelocity( + pb_client=self.pb_client, task_space_limit=task_space_limit, end_effector_link="rh_forearm", - verbose=self.verbose, orn_limit=orn_limit) + verbose=self.verbose, + orn_limit=orn_limit) else: robot = ur10_shadow.UR10ShadowPosition( + pb_client=self.pb_client, task_space_limit=task_space_limit, end_effector_link="rh_forearm", - verbose=self.verbose, orn_limit=orn_limit) + verbose=self.verbose, + orn_limit=orn_limit) - robot.set_initial_joint_positions(dict(zip(robot.motors, robot.get_joint_positions()))) + robot.set_initial_joint_positions( + dict(zip(robot.motors, robot.get_joint_positions()))) self.simulation.add_robot(robot) @@ -95,43 +102,31 @@ def _create_robot(self): def _load_objects(self): super()._load_objects() self.object_to_grasp, self.object_position, self.object_orientation = \ - ObjectFactory().create(self.object_name, object2world=self.object2world, scale=self.object_scale) + ObjectFactory(self.pb_client).create( + self.object_name, + object2world=self.object2world, + scale=self.object_scale) def reset(self, seed=None, options=None): - pos = None - - self.object_to_grasp.reset(pos) + self.object_to_grasp.reset() self.robot.activate_motors() return super().reset(seed, options) - def is_done(self, state, action, next_state): - - # check if insole is exploded - if self._deformable_is_exploded(): - print("Exploded insole") - return True - - return super().is_done(state, action, next_state) - def _get_observation(self): finger_pos = self.robot.get_joint_positions()[6:] ee_pose = self.robot.get_ee_pose() + sensor_readings = self.robot.get_sensor_readings() - return np.concatenate([ee_pose, finger_pos], axis=0) + return np.concatenate([ee_pose, finger_pos, sensor_readings], axis=0) - def calculate_reward(self, state, action, next_state, done): + def calculate_reward(self, state, action, next_state, terminated): """ Calculates the reward by counting how many insole vertices are in the target position. """ - if done: - if not self.compute_reward: - return 0.0 - if not (round(action[-1]) == 1 or self.step_counter >= self.horizon): - return -100 - + if terminated: self.robot.deactivate_motors() # remove insole anchors and simulate steps self.object_to_grasp.remove_anchors() diff --git a/deformable_gym/envs/ur5_mia_grasp_env.py b/deformable_gym/envs/ur5_mia_grasp_env.py index 4983f0e..245ff15 100644 --- a/deformable_gym/envs/ur5_mia_grasp_env.py +++ b/deformable_gym/envs/ur5_mia_grasp_env.py @@ -1,11 +1,11 @@ import numpy as np +import pytransform3d.transformations as pt from gymnasium import spaces -from deformable_gym.robots import ur5_mia + from deformable_gym.envs.base_env import BaseBulletEnv, GraspDeformableMixin from deformable_gym.helpers import pybullet_helper as pbh from deformable_gym.objects.bullet_object import ObjectFactory -import pytransform3d.transformations as pt - +from deformable_gym.robots import ur5_mia INITIAL_JOINT_ANGLES = { "ur5_shoulder_pan_joint": 2.44388798, @@ -80,16 +80,7 @@ class UR5MiaGraspEnv(GraspDeformableMixin, BaseBulletEnv): """ robot: ur5_mia.UR5Mia - train_positions = ([-0.7, 0.1, 1.6], - [-0.7, 0.2, 1.6], - [-0.7, 0.0, 1.6]) - - test_positions = ([-0.8, 0.1, 1.6], - [-0.8, 0.2, 1.6], - [-0.8, 0.0, 1.6]) - - object2world = pt.transform_from(R=np.eye(3), - p=np.array([-0.7, 0.1, 1.8])) + object2world = pt.transform_from(R=np.eye(3), p=np.array([-0.7, 0.1, 1.8])) def __init__( self, @@ -152,12 +143,18 @@ def _create_robot(self): if self.velocity_commands: robot = ur5_mia.UR5MiaVelocity( - task_space_limit=task_space_limit, end_effector_link="palm", - verbose=self.verbose, orn_limit=orn_limit) + pb_client=self.pb_client, + task_space_limit=task_space_limit, + end_effector_link="palm", + verbose=self.verbose, + orn_limit=orn_limit) else: robot = ur5_mia.UR5MiaPosition( - task_space_limit=task_space_limit, end_effector_link="palm", - verbose=self.verbose, orn_limit=orn_limit) + pb_client=self.pb_client, + task_space_limit=task_space_limit, + end_effector_link="palm", + verbose=self.verbose, + orn_limit=orn_limit) self.simulation.add_robot(robot) @@ -169,46 +166,42 @@ def _create_robot(self): def _load_objects(self): super()._load_objects() - self.object_to_grasp, self.object_position, self.object_orientation = \ - ObjectFactory().create(self.object_name, object2world=self.object2world, scale=self.object_scale) + (self.object_to_grasp, + self.object_position, + self.object_orientation) = ObjectFactory(self.pb_client).create( + self.object_name, + object2world=self.object2world, + scale=self.object_scale) def reset(self, seed=None, options=None): - pos = None - - self.object_to_grasp.reset(pos=pos) + self.object_to_grasp.reset() self.robot.activate_motors() return super().reset(seed, options) def _get_observation(self): - joint_pos = self.robot.get_joint_positions(self.robot.actuated_real_joints) + joint_pos = self.robot.get_joint_positions( + self.robot.actuated_real_joints) ee_pose = self.robot.get_ee_pose() sensor_readings = self.robot.get_sensor_readings() return np.concatenate([ee_pose, joint_pos, sensor_readings], axis=0) - def calculate_reward(self, state, action, next_state, done): + def calculate_reward(self, state, action, next_state, terminated): """ Calculates the reward by counting how many insole vertices are in the target position. """ - # TODO: integrate contact points when available! - if done: - if not self.compute_reward: - return 0.0 - if not (round(action[-1]) == 1 or self.step_counter >= self.horizon): - return -100 - + if terminated: self.robot.deactivate_motors() # remove insole anchors and simulate steps self.object_to_grasp.remove_anchors() for _ in range(50): if self._deformable_is_exploded(): - return -100 + return -1.0 self.simulation.step_to_trigger("time_step") height = self.object_to_grasp.get_pose()[2] - if height > 0.9: return 1.0 else: diff --git a/deformable_gym/helpers/pybullet_helper.py b/deformable_gym/helpers/pybullet_helper.py index 5d7ee61..7a925fd 100644 --- a/deformable_gym/helpers/pybullet_helper.py +++ b/deformable_gym/helpers/pybullet_helper.py @@ -4,11 +4,14 @@ """ from enum import Enum from typing import Tuple + import numpy as np import numpy.typing as npt import pybullet as pb import pytransform3d.rotations as pr +from pybullet_utils import bullet_client as bc + class JointType(Enum): revolute = pb.JOINT_REVOLUTE @@ -20,16 +23,20 @@ class JointType(Enum): class Joint: - def __init__(self, name: str, - joint_idx: int, - joint_type: JointType, - pos_idx: int, - vel_idx: int, - low: float, - high: float, - max_force: float, - max_vel: float, - body_id: int) -> None: + def __init__( + self, + name: str, + joint_idx: int, + joint_type: JointType, + pos_idx: int, + vel_idx: int, + low: float, + high: float, + max_force: float, + max_vel: float, + body_id: int, + pb_client: bc.BulletClient, + ) -> None: self.name = name self.joint_idx = joint_idx self.joint_type = joint_type @@ -45,6 +52,8 @@ def __init__(self, name: str, self.activated = True self.verbose = False + self.pb_client = pb_client + def __repr__(self): return f"Joint({', '.join([f'{k}={v}' for k,v in vars(self).items()])})" @@ -54,25 +63,35 @@ def reset(self) -> None: def set_position(self, position: float) -> None: """Sets the joint to target position.""" - pb.resetJointState(self.body_id, - self.joint_idx, - targetValue=position, - targetVelocity=0.0) + self.pb_client.resetJointState( + self.body_id, + self.joint_idx, + targetValue=position, + targetVelocity=0.0 + ) def set_limits(self, low: float, high: float) -> None: """Sets the joint limits.""" - pb.changeDynamics(self.body_id, - self.joint_idx, - jointLowerLimit=low, - jointUpperLimit=high) + self.pb_client.changeDynamics( + self.body_id, + self.joint_idx, + jointLowerLimit=low, + jointUpperLimit=high + ) def get_position(self) -> float: """Gets the current position of the joint.""" - return pb.getJointState(self.body_id, self.joint_idx)[0] + return self.pb_client.getJointState( + self.body_id, + self.joint_idx, + )[0] def get_velocity(self) -> float: """Gets the current velocity of the joint.""" - return pb.getJointState(self.body_id, self.joint_idx)[1] + return self.pb_client.getJointState( + self.body_id, + self.joint_idx, + )[1] def set_target_position(self, position: float) -> None: """Sets the target position of the joint.""" @@ -83,12 +102,15 @@ def set_target_position(self, position: float) -> None: f" but is moving with velocity {self.get_velocity()}") if self.activated: - pb.setJointMotorControl2(self.body_id, self.joint_idx, - controlMode=pb.POSITION_CONTROL, - targetPosition=position, - targetVelocity=0.0, - maxVelocity=self.max_vel, - force=self.max_force) + self.pb_client.setJointMotorControl2( + self.body_id, + self.joint_idx, + controlMode=pb.POSITION_CONTROL, + targetPosition=position, + targetVelocity=0.0, + maxVelocity=self.max_vel, + force=self.max_force, + ) else: if self.verbose: @@ -97,11 +119,14 @@ def set_target_position(self, position: float) -> None: def set_target_velocity(self, velocity: float) -> None: """Sets the target position of the joint.""" if self.activated: - pb.setJointMotorControl2(self.body_id, self.joint_idx, - controlMode=pb.VELOCITY_CONTROL, - targetVelocity=velocity, - maxVelocity=self.max_vel, - force=self.max_force) + self.pb_client.setJointMotorControl2( + self.body_id, + self.joint_idx, + controlMode=pb.VELOCITY_CONTROL, + targetVelocity=velocity, + maxVelocity=self.max_vel, + force=self.max_force, + ) else: if self.verbose: print(f"Warning: Trying to control deactivated motor {self.name}.") @@ -116,22 +141,6 @@ def activate(self): self.activated = True -def split_pose(pose): - """ - Transforms a pose from 7D vector into 3D position vector and 4D quaternion vector. Also rearranges the quaternion to - match PyBullet conventions. - - :param pose: 7D numpy array containing position and quaternion of the form [x, y, z, qw, qx, qy, qz] - - :return: a 3D numpy array containing position of the form [x, y, z] and a 4D numpy array containing position and - quaternion of the form [qx, qy, qz, qw] - """ - pos = pose[:3] - rot = pose[3:] - rot = np.hstack((rot[1:], [rot[0]])) # wxyz -> xyzw - return pos, rot - - def merge_pose(pos, rot): """ Transforms a provided 3D position vector and a 4D quaternion into a 7D pose vector. Also rearranges the quaternion @@ -145,7 +154,10 @@ def merge_pose(pos, rot): return np.hstack((pos, [rot[-1]], rot[:-1])) # xyzw -> wxyz -def build_joint_list(robot, verbose=0): +def build_joint_list( + robot, + pb_client: bc.BulletClient, + verbose: bool = False): """ Builds and returns a dictionary of all joints in the provided robot. @@ -154,9 +166,9 @@ def build_joint_list(robot, verbose=0): :return: list of Joint objects """ - n_joints = pb.getNumJoints(robot) + n_joints = pb_client.getNumJoints(robot) - if verbose > 0: + if verbose: print(f"Found {n_joints} joints.") joint_list = [] @@ -164,29 +176,35 @@ def build_joint_list(robot, verbose=0): # iterate over all joints for joint_idx in range(n_joints): _, joint_name, joint_type, pos_idx, vel_idx, _, _, _, lo, hi, max_force, max_vel, *_ = \ - pb.getJointInfo(robot, joint_idx) + pb_client.getJointInfo(robot, joint_idx) joint_name = joint_name.decode('utf-8') - joint_list.append(Joint(joint_name, joint_idx, joint_type, pos_idx, vel_idx, lo, hi, max_force, max_vel, robot)) + joint_list.append( + Joint(joint_name, joint_idx, joint_type, pos_idx, vel_idx, lo, hi, + max_force, max_vel, robot, pb_client)) return joint_list -def analyze_robot(urdf_path=None, robot=None, physics_client_id=0, verbose=0): +def analyze_robot( + urdf_path: str = None, + robot=None, + pb_client: bc.BulletClient = None, + verbose=0): """ Compute mappings between joint and link names and their indices. """ if urdf_path is not None: assert robot is None - physics_client_id = pb.connect(pb.DIRECT) - pb.resetSimulation(physicsClientId=physics_client_id) - robot = pb.loadURDF(urdf_path, physicsClientId=physics_client_id) + pb_client = pb.connect(pb.DIRECT) + pb_client.resetSimulation() + robot = pb_client.loadURDF(urdf_path) assert robot is not None - base_link, robot_name = pb.getBodyInfo(robot, physicsClientId=physics_client_id) + base_link, robot_name = pb_client.getBodyInfo(robot) if verbose: print() @@ -194,7 +212,7 @@ def analyze_robot(urdf_path=None, robot=None, physics_client_id=0, verbose=0): print(f"Robot name: {robot_name}") print(f"Base link: {base_link}") - n_joints = pb.getNumJoints(robot, physicsClientId=physics_client_id) + n_joints = pb_client.getNumJoints(robot) last_link_idx = -1 link_id_to_link_name = {last_link_idx: base_link} @@ -204,9 +222,9 @@ def analyze_robot(urdf_path=None, robot=None, physics_client_id=0, verbose=0): print(f"Number of joints: {n_joints}") for joint_idx in range(n_joints): - _, joint_name, joint_type, q_index, u_index, _, jd, jf, lo, hi,\ - max_force, max_vel, child_link_name, ja, parent_pos,\ - parent_orient, parent_idx = pb.getJointInfo(robot, joint_idx, physicsClientId=physics_client_id) + _, joint_name, joint_type, q_index, u_index, _, jd, jf, lo, hi, \ + max_force, max_vel, child_link_name, ja, parent_pos, \ + parent_orient, parent_idx = pb_client.getJointInfo(robot, joint_idx) child_link_name = child_link_name.decode("utf-8") joint_name = joint_name.decode("utf-8") @@ -249,7 +267,11 @@ def get_joint_by_name(joint_list, name): return j -def link_pose(robot: int, link: int) -> Tuple[Tuple[float], Tuple[float]]: +def link_pose( + robot: int, + link: int, + pb_client: bc.BulletClient +) -> Tuple[Tuple[float], Tuple[float]]: """Compute link pose from link state. A link state contains the world pose of a link's inertial frame and @@ -264,24 +286,30 @@ def link_pose(robot: int, link: int) -> Tuple[Tuple[float], Tuple[float]]: :return: Tuple of position and scalar-last quaternion. """ link_inertial2world_pos, link_inertial2world_orn, \ - link_inertial2link_pos, link_inertial2link_orn = pb.getLinkState( + link_inertial2link_pos, link_inertial2link_orn = pb_client.getLinkState( robot, link)[:4] - link2link_inertial_pos, link2link_inertial_orn = pb.invertTransform( + link2link_inertial_pos, link2link_inertial_orn = pb_client.invertTransform( link_inertial2link_pos, link_inertial2link_orn) - link2world_pos, link2world_orn = pb.multiplyTransforms( + link2world_pos, link2world_orn = pb_client.multiplyTransforms( link_inertial2world_pos, link_inertial2world_orn, link2link_inertial_pos, link2link_inertial_orn,) return link2world_pos, link2world_orn -def start_recording(path): +def start_recording( + path: str, + pb_client: bc.BulletClient): # TODO: double check that recording is not already in progress - logging_id = pb.startStateLogging(pb.STATE_LOGGING_VIDEO_MP4, path) + logging_id = pb_client.startStateLogging( + pb.STATE_LOGGING_VIDEO_MP4, path) return logging_id -def stop_recording(logging_id): - pb.stopStateLogging(logging_id) +def stop_recording( + logging_id: int, + pb_client: bc.BulletClient +): + pb_client.stopStateLogging(logging_id) class MultibodyPose: @@ -304,18 +332,23 @@ class MultibodyPose: Initial orientation when the URDF was loaded. Provided as scalar-last quaternion. - pcid : int, optional (default: 0) + pb_client_id : int, optional (default: 0) Physics client ID. """ - def __init__(self, uuid, initial_pos, initial_orn, pcid=0): + def __init__( + self, + uuid, + initial_pos, + initial_orn, + pb_client: bc.BulletClient): self.uuid = uuid - self.pcid = pcid + self.pb_client = pb_client - inertia_pos, inertia_orn = pb.getBasePositionAndOrientation( - self.uuid, physicsClientId=self.pcid) + inertia_pos, inertia_orn = self.pb_client.getBasePositionAndOrientation( + self.uuid) inv_pos, inv_orn = pb.invertTransform(inertia_pos, inertia_orn) - self.inertia_offset_pos, self.inertia_offset_orn = pb.invertTransform( - *pb.multiplyTransforms(inv_pos, inv_orn, initial_pos, initial_orn)) + self.inertia_offset_pos, self.inertia_offset_orn = self.pb_client.invertTransform( + *self.pb_client.multiplyTransforms(inv_pos, inv_orn, initial_pos, initial_orn)) def set_pose(self, pos, orn): """Set pose of the root link frame with respect to world frame. @@ -337,8 +370,8 @@ def set_pose(self, pos, orn): Inertial frame orientation. Provided as scalar-last quaternion. """ new_pos, new_orn = self.translate_pose(pos, orn) - pb.resetBasePositionAndOrientation( - self.uuid, new_pos, new_orn, physicsClientId=self.pcid) + self.pb_client.resetBasePositionAndOrientation( + self.uuid, new_pos, new_orn) return new_pos, new_orn def translate_pose(self, pos, orn): @@ -360,7 +393,7 @@ def translate_pose(self, pos, orn): orn : array-like, shape (4,) Inertial frame orientation. Provided as scalar-last quaternion. """ - return pb.multiplyTransforms( + return self.pb_client.multiplyTransforms( pos, orn, self.inertia_offset_pos, self.inertia_offset_orn) def get_pose(self): @@ -374,10 +407,10 @@ def get_pose(self): orn : array-like, shape (4,) Orientation. Provided as scalar-last quaternion. """ - measured_pos, measured_orn = pb.getBasePositionAndOrientation( - self.uuid, physicsClientId=self.pcid) - return pb.multiplyTransforms( - measured_pos, measured_orn, *pb.invertTransform( + measured_pos, measured_orn = self.pb_client.getBasePositionAndOrientation( + self.uuid) + return self.pb_client.multiplyTransforms( + measured_pos, measured_orn, *self.pb_client.invertTransform( self.inertia_offset_pos, self.inertia_offset_orn)) @staticmethod diff --git a/deformable_gym/objects/__init__.py b/deformable_gym/objects/__init__.py index 3d2cfe6..c510706 100644 --- a/deformable_gym/objects/__init__.py +++ b/deformable_gym/objects/__init__.py @@ -1,11 +1,11 @@ -from .bullet_object import ( - BulletObjectBase, SoftObjectBase, BoxObject, MeshObject, UrdfObject, - SphereObject, CylinderObject, ObjectFactory, CapsuleObject, SoftObject, - MocapObjectMixin, RigidPrimitiveObject, Insole, InsoleOnConveyorBelt, - PillowSmall, PositionEulerAngleMixin) +from .bullet_object import (BoxObject, BulletObjectBase, CapsuleObject, + CylinderObject, Insole, InsoleOnConveyorBelt, + MeshObject, MocapObjectMixin, ObjectFactory, + PillowSmall, PositionEulerAngleMixin, + RigidPrimitiveObject, SoftObject, SoftObjectBase, + SphereObject, UrdfObject) from .conveyor import Conveyor - __all__ = [ "BulletObjectBase", "SoftObjectBase", "BoxObject", "MeshObject", "UrdfObject", "SphereObject", "CylinderObject", "ObjectFactory", diff --git a/deformable_gym/objects/bullet_object.py b/deformable_gym/objects/bullet_object.py index e6f4fc1..abbbb24 100644 --- a/deformable_gym/objects/bullet_object.py +++ b/deformable_gym/objects/bullet_object.py @@ -1,15 +1,19 @@ import abc -from typing import Union, Tuple, Sequence, List +import os import warnings +from pathlib import Path +from typing import List, Sequence, Tuple, Union + import numpy as np import numpy.typing as npt -import os import pybullet as pb import pytransform3d.rotations as pr import pytransform3d.transformations as pt + from deformable_gym.helpers import pybullet_helper as pbh from deformable_gym.robots.bullet_utils import draw_pose -from pathlib import Path + +from pybullet_utils import bullet_client as bc base_path = Path(os.path.dirname(__file__)).parent.parent.absolute() @@ -20,10 +24,13 @@ class BulletObjectBase(abc.ABC): :param client_id: Physics client ID for PyBullet interface calls. """ object_id: int - client_id: int + pb_client: bc.BulletClient - def __init__(self, client_id): - self.client_id = client_id + def __init__( + self, + pb_client: bc.BulletClient + ): + self.pb_client = pb_client def get_pose(self): """Returns the object's current pose. @@ -31,8 +38,7 @@ def get_pose(self): :return: 7D numpy array containing position and quaternion of the form [x, y, z, qw, qx, qy, qz] """ - pos, rot = pb.getBasePositionAndOrientation( - self.object_id, physicsClientId=self.client_id) + pos, rot = self.pb_client.getBasePositionAndOrientation(self.object_id) return pbh.merge_pose(pos, rot) def get_vertices(self): @@ -40,7 +46,7 @@ def get_vertices(self): :return: list of vertices """ - return pb.getMeshData(self.object_id, physicsClientId=self.client_id)[1] + return self.pb_client.getMeshData(self.object_id)[1] def get_id(self): """Get UUID of object.""" @@ -75,19 +81,26 @@ def _set_init_pose(self, world_pos=None, world_orn=None): def reset(self): """Resets the object to its initial pose.""" - pb.resetBasePositionAndOrientation( - self.object_id, self.init_pos, self.init_orn, - physicsClientId=self.client_id) + self.pb_client.resetBasePositionAndOrientation( + self.object_id, self.init_pos, self.init_orn) class RigidPrimitiveObject(PositionEulerAngleMixin, BulletObjectBase): """Simple primitive object base.""" def __init__( - self, mass=1.0, world_pos=None, - world_orn=None, fixed=False, lateralFriction=None, - rollingFriction=None, restitution=None, contactStiffness=None, - contactDamping=None, client_id=0): - super().__init__(client_id) + self, + pb_client: bc.BulletClient, + mass: float = 1.0, + world_pos=None, + world_orn=None, + fixed=False, + lateralFriction=None, + rollingFriction=None, + restitution=None, + contactStiffness=None, + contactDamping=None + ): + super().__init__(pb_client) self.mass = mass self.lateralFriction = lateralFriction self.rollingFriction = rollingFriction @@ -107,7 +120,7 @@ def _create_primitive(self): def _load_object(self): primitive = self._create_primitive() - self.object = pb.createMultiBody( + self.object = self.pb_client.createMultiBody( baseMass=self.mass, baseCollisionShapeIndex=primitive, baseVisualShapeIndex=-1, @@ -116,7 +129,7 @@ def _load_object(self): ) if self.fixed: - self.anchor = pb.createConstraint( + self.anchor = self.pb_client.createConstraint( self.object, -1, -1, -1, pb.JOINT_FIXED, [0, 0, 1], [0, 0, 0], self.init_pos) self.anchored = True @@ -133,123 +146,205 @@ def _load_object(self): if self.contactDamping is not None: dynamics_config["contactDamping"] = self.contactDamping if dynamics_config: - pb.changeDynamics(self.object, -1, **dynamics_config) + self.pb_client.changeDynamics( + self.object, -1, **dynamics_config) return self.object def remove_anchors(self): if self.fixed and self.anchored: - pb.removeConstraint(self.anchor) + self.pb_client.removeConstraint(self.anchor) self.anchored = False def reset(self): """Resets the object to its initial position by respawning it.""" self.remove_anchors() assert not self.anchored - pb.removeBody(self.object) + self.pb_client.removeBody(self.object) self._load_object() class MeshObject(RigidPrimitiveObject): """Simple mesh object.""" def __init__( - self, filename, world_pos=None, world_orn=None, mass=1.0, scale=(1, 1, 1), - fixed=False, lateralFriction=None, - rollingFriction=None, restitution=None, contactStiffness=None, - contactDamping=None, client_id=0): + self, + filename, + pb_client: bc.BulletClient, + world_pos=None, + world_orn=None, + mass=1.0, + scale=(1, 1, 1), + fixed=False, + lateralFriction=None, + rollingFriction=None, + restitution=None, + contactStiffness=None, + contactDamping=None + ): self.filename = filename self.scale = scale super().__init__( - mass=mass, world_pos=world_pos, world_orn=world_orn, fixed=fixed, - lateralFriction=lateralFriction, rollingFriction=rollingFriction, - restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping, client_id=client_id) + pb_client=pb_client, + mass=mass, + world_pos=world_pos, + world_orn=world_orn, + fixed=fixed, + lateralFriction=lateralFriction, + rollingFriction=rollingFriction, + restitution=restitution, + contactStiffness=contactStiffness, + contactDamping=contactDamping + ) def _create_primitive(self): - return pb.createCollisionShape( - pb.GEOM_MESH, fileName=self.filename, meshScale=self.scale, - physicsClientId=self.client_id) + return self.pb_client.createCollisionShape( + pb.GEOM_MESH, fileName=self.filename, meshScale=self.scale) class BoxObject(RigidPrimitiveObject): """Simple box object.""" def __init__( - self, half_extents=(1, 1, 1), mass=1.0, world_pos=None, - world_orn=None, fixed=False, lateralFriction=None, - rollingFriction=None, restitution=None, contactStiffness=None, - contactDamping=None, client_id=0): + self, + pb_client: bc.BulletClient, + half_extents=(1, 1, 1), + mass=1.0, + world_pos=None, + world_orn=None, + fixed=False, + lateralFriction=None, + rollingFriction=None, + restitution=None, + contactStiffness=None, + contactDamping=None, + client_id=0): self.half_extents = half_extents super().__init__( - mass=mass, world_pos=world_pos, world_orn=world_orn, fixed=fixed, - lateralFriction=lateralFriction, rollingFriction=rollingFriction, - restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping, client_id=client_id) + pb_client=pb_client, + mass=mass, + world_pos=world_pos, + world_orn=world_orn, + fixed=fixed, + lateralFriction=lateralFriction, + rollingFriction=rollingFriction, + restitution=restitution, + contactStiffness=contactStiffness, + contactDamping=contactDamping + ) def _create_primitive(self): - return pb.createCollisionShape( - pb.GEOM_BOX, halfExtents=self.half_extents, - physicsClientId=self.client_id) + return self.pb_client.createCollisionShape( + pb.GEOM_BOX, halfExtents=self.half_extents) class SphereObject(RigidPrimitiveObject): """Simple sphere object.""" def __init__( - self, radius=1.0, mass=1.0, world_pos=None, - world_orn=None, fixed=False, lateralFriction=None, - rollingFriction=None, restitution=None, contactStiffness=None, - contactDamping=None, client_id=0): + self, + pb_client: bc.BulletClient, + radius=1.0, + mass=1.0, + world_pos=None, + world_orn=None, + fixed=False, + lateralFriction=None, + rollingFriction=None, + restitution=None, + contactStiffness=None, + contactDamping=None, + client_id=0): self.radius = radius super().__init__( - mass=mass, world_pos=world_pos, world_orn=world_orn, fixed=fixed, - lateralFriction=lateralFriction, rollingFriction=rollingFriction, - restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping, client_id=client_id) + pb_client=pb_client, + mass=mass, + world_pos=world_pos, + world_orn=world_orn, + fixed=fixed, + lateralFriction=lateralFriction, + rollingFriction=rollingFriction, + restitution=restitution, + contactStiffness=contactStiffness, + contactDamping=contactDamping + ) def _create_primitive(self): - return pb.createCollisionShape( - pb.GEOM_SPHERE, radius=self.radius, physicsClientId=self.client_id) + return self.pb_client.createCollisionShape( + pb.GEOM_SPHERE, radius=self.radius) class CylinderObject(RigidPrimitiveObject): """Simple cylinder object.""" def __init__( - self, radius=1.0, height=1.0, mass=1.0, world_pos=None, - world_orn=None, fixed=False, lateralFriction=None, - rollingFriction=None, restitution=None, contactStiffness=None, - contactDamping=None, client_id=0): + self, + pb_client: bc.BulletClient, + radius=1.0, + height=1.0, + mass=1.0, + world_pos=None, + world_orn=None, + fixed=False, + lateralFriction=None, + rollingFriction=None, + restitution=None, + contactStiffness=None, + contactDamping=None + ): self.radius = radius self.height = height super().__init__( - mass=mass, world_pos=world_pos, world_orn=world_orn, fixed=fixed, - lateralFriction=lateralFriction, rollingFriction=rollingFriction, - restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping, client_id=client_id) + pb_client=pb_client, + mass=mass, + world_pos=world_pos, + world_orn=world_orn, + fixed=fixed, + lateralFriction=lateralFriction, + rollingFriction=rollingFriction, + restitution=restitution, + contactStiffness=contactStiffness, + contactDamping=contactDamping + ) def _create_primitive(self): - return pb.createCollisionShape( - pb.GEOM_CYLINDER, radius=self.radius, height=self.height, - physicsClientId=self.client_id) + return self.pb_client.createCollisionShape( + pb.GEOM_CYLINDER, radius=self.radius, height=self.height) class CapsuleObject(RigidPrimitiveObject): """Simple capsule object.""" def __init__( - self, radius=1.0, height=1.0, mass=1.0, world_pos=None, - world_orn=None, fixed=False, lateralFriction=None, - rollingFriction=None, restitution=None, contactStiffness=None, - contactDamping=None, client_id=0): + self, + pb_client: bc.BulletClient, + radius=1.0, + height=1.0, + mass=1.0, + world_pos=None, + world_orn=None, + fixed=False, + lateralFriction=None, + rollingFriction=None, + restitution=None, + contactStiffness=None, + contactDamping=None + ): self.radius = radius self.height = height super().__init__( - mass=mass, world_pos=world_pos, world_orn=world_orn, fixed=fixed, - lateralFriction=lateralFriction, rollingFriction=rollingFriction, - restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping, client_id=client_id) + pb_client=pb_client, + mass=mass, + world_pos=world_pos, + world_orn=world_orn, + fixed=fixed, + lateralFriction=lateralFriction, + rollingFriction=rollingFriction, + restitution=restitution, + contactStiffness=contactStiffness, + contactDamping=contactDamping + ) def _create_primitive(self): - return pb.createCollisionShape( - pb.GEOM_CAPSULE, radius=self.radius, height=self.height, - physicsClientId=self.client_id) + return self.pb_client.createCollisionShape( + pb.GEOM_CAPSULE, + radius=self.radius, + height=self.height) class UrdfObject(PositionEulerAngleMixin, BulletObjectBase): @@ -257,9 +352,14 @@ class UrdfObject(PositionEulerAngleMixin, BulletObjectBase): Provides some basic meta functionality for URDF-based objects. """ - def __init__(self, filename, verbose=0, world_pos=None, world_orn=None, - fixed=False, client_id=0): - super().__init__(client_id) + def __init__(self, + filename, + pb_client: bc.BulletClient, + verbose=0, + world_pos=None, + world_orn=None, + fixed=False): + super().__init__(pb_client=pb_client) self.filename = filename self.verbose = verbose self.fixed = fixed @@ -268,11 +368,13 @@ def __init__(self, filename, verbose=0, world_pos=None, world_orn=None, self.object_id = self._load_object() def _load_object(self): - return pb.loadURDF( - self.filename, self.init_pos, self.init_orn, useFixedBase=self.fixed, + return self.pb_client.loadURDF( + self.filename, + self.init_pos, + self.init_orn, + useFixedBase=self.fixed, flags=pb.URDF_USE_SELF_COLLISION | - pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT, - physicsClientId=self.client_id) + pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) def remove_anchors(self): if self.fixed: @@ -296,11 +398,21 @@ class SoftObjectBase(BulletObjectBase): constraints: Sequence[int] def __init__( - self, filename, fixed=False, fixed_nodes=None, scale=1.0, nu=0.2, - E=100000.0, damping=0.005, collision_margin=0.0005, - repulsion_stiffness=8000.0, mass=0.1, friction_coefficient=0.5, - client_id=0): - super().__init__(client_id) + self, + filename, + pb_client: bc.BulletClient, + fixed=False, + fixed_nodes=None, + scale=1.0, + nu=0.2, + E=100000.0, + damping=0.005, + collision_margin=0.0005, + repulsion_stiffness=8000.0, + mass=0.1, + friction_coefficient=0.5 + ): + super().__init__(pb_client=pb_client) self.filename = filename self.fixed = fixed self.fixed_nodes = fixed_nodes @@ -316,17 +428,22 @@ def __init__( def _load_object(self): mu, lmbda = self.__lame_parameters(self.nu, self.E) - object_id = pb.loadSoftBody( + object_id = self.pb_client.loadSoftBody( self.filename, - self.init_pos, self.init_orn, scale=self.scale, + self.init_pos, + self.init_orn, + scale=self.scale, simFileName=self.filename, - useNeoHookean=True, NeoHookeanMu=mu, - NeoHookeanLambda=lmbda, NeoHookeanDamping=self.damping, + useNeoHookean=True, + NeoHookeanMu=mu, + NeoHookeanLambda=lmbda, + NeoHookeanDamping=self.damping, collisionMargin=self.collision_margin, - useSelfCollision=True, useFaceContact=True, + useSelfCollision=True, + useFaceContact=True, repulsionStiffness=self.repulsion_stiffness, - mass=self.mass, frictionCoeff=self.friction_coefficient, - physicsClientId=self.client_id) + mass=self.mass, + frictionCoeff=self.friction_coefficient) if self.fixed: self.__make_anchors(object_id) @@ -348,31 +465,50 @@ def __make_anchors(self, object_id): self.constraints = [] else: self.constraints = [ - pb.createSoftBodyAnchor( + self.pb_client.createSoftBodyAnchor( softBodyBodyUniqueId=object_id, - nodeIndex=i, bodyUniqueId=-1, linkIndex=-1, - physicsClientId=self.client_id) + nodeIndex=i, + bodyUniqueId=-1, + linkIndex=-1) for i in self.fixed_nodes] for constraint in self.constraints: - pb.changeConstraint( - constraint, maxForce=1, physicsClientId=self.client_id) + self.pb_client.changeConstraint( + constraint, + maxForce=1) def remove_anchors(self): for anchor in self.constraints: - pb.removeConstraint(anchor, physicsClientId=self.client_id) + self.pb_client.removeConstraint(anchor) class SoftObject(PositionEulerAngleMixin, SoftObjectBase): """Soft Bullet Object.""" - def __init__(self, filename, world_pos=None, world_orn=None, fixed=False, - scale=1.0, nu=0.2, E=100000.0, damping=0.005, - repulsion_stiffness=8000.0, mass=0.1, fixed_nodes=None, - client_id=0): + def __init__(self, + filename, + pb_client: bc.BulletClient, + world_pos=None, + world_orn=None, + fixed=False, + scale=1.0, + nu=0.2, + E=100000.0, + damping=0.005, + repulsion_stiffness=8000.0, + mass=0.1, + fixed_nodes=None + ): super().__init__( - filename=filename, fixed=fixed, fixed_nodes=fixed_nodes, - scale=scale, nu=nu, E=E, damping=damping, - repulsion_stiffness=repulsion_stiffness, mass=mass, - client_id=client_id) + filename=filename, + pb_client=pb_client, + fixed=fixed, + fixed_nodes=fixed_nodes, + scale=scale, + nu=nu, + E=E, + damping=damping, + repulsion_stiffness=repulsion_stiffness, + mass=mass + ) self._set_init_pose(world_pos, world_orn) self.object_id = self._load_object() @@ -384,10 +520,10 @@ def reset(self, pos=None, orn=None): # remove constraints if self.fixed: for constraint in self.constraints: - pb.removeConstraint(constraint, physicsClientId=self.client_id) + self.pb_client.removeConstraint(constraint) # remove the body - pb.removeBody(self.object_id, physicsClientId=self.client_id) + self.pb_client.removeBody(self.object_id) # if necessary, set new position and orientation if pos is not None: @@ -400,7 +536,10 @@ def reset(self, pos=None, orn=None): class MocapObjectMixin: - def reset(self, object_markers2world=None, center_camera=False, pos=None): + def reset(self, + object_markers2world=None, + center_camera=False, + pos=None): """ Resets the object to its initial position by removing and respawning it. This way the initial state of the soft-body is obtained as well. @@ -411,25 +550,36 @@ def reset(self, object_markers2world=None, center_camera=False, pos=None): self.object_markers2world[:3, 3] = pos if self.fixed: self.remove_anchors() - pb.removeBody(self.object_id, physicsClientId=self.client_id) - self.init_pos, self.init_orn = self.mesh_pose( - self.object_markers2world) + self.pb_client.removeBody(self.object_id) + self.init_pos, self.init_orn = self.mesh_pose(self.object_markers2world) self.object_id = self._load_object() if center_camera: - pb.resetDebugVisualizerCamera( - 0.35, -60, -30, self.object_markers2world[:3, 3], - physicsClientId=self.client_id) + self.pb_client.resetDebugVisualizerCamera( + 0.35, -60, -30, self.object_markers2world[:3, 3]) class Insole(MocapObjectMixin, SoftObjectBase): def __init__( - self, insole_markers2world, scale=1.0, E=100000.0, fixed=False, - client_id=0): + self, + insole_markers2world, + pb_client: bc.BulletClient, + scale=1.0, + E=100000.0, + fixed=False + ): super().__init__( - os.path.join(base_path, "object_data/insole.vtk"), fixed=fixed, fixed_nodes=[0, 40, 45], - scale=scale, nu=0.2, E=E, damping=0.005, - collision_margin=0.0005, repulsion_stiffness=8000.0, mass=0.1, - friction_coefficient=1.5, client_id=client_id) + os.path.join(base_path, "object_data/insole.vtk"), + pb_client=pb_client, + fixed=fixed, + fixed_nodes=[0, 40, 45], + scale=scale, + nu=0.2, + E=E, + damping=0.005, + collision_margin=0.0005, + repulsion_stiffness=8000.0, + mass=0.1, + friction_coefficient=1.5) self.object_markers2world = insole_markers2world self.init_pos, self.init_orn = self.mesh_pose( self.object_markers2world) @@ -452,13 +602,25 @@ def mesh_pose(object_markers2world): class PillowSmall(MocapObjectMixin, SoftObjectBase): - def __init__(self, pillow_markers2world, scale=1.0, fixed=False, client_id=0): + def __init__(self, + pillow_markers2world, + pb_client: bc.BulletClient, + scale=1.0, + fixed=False): super().__init__( - os.path.join(base_path, "object_data/insole.vtk"), fixed=fixed, + os.path.join(base_path, "object_data/insole.vtk"), + pb_client=pb_client, + fixed=fixed, fixed_nodes=[0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120], - scale=scale, nu=0.2, E=10000.0, damping=0.005, - collision_margin=0.0001, repulsion_stiffness=1000.0, mass=0.1, - friction_coefficient=1.5, client_id=client_id) + scale=scale, + nu=0.2, + E=10000.0, + damping=0.005, + collision_margin=0.0001, + repulsion_stiffness=1000.0, + mass=0.1, + friction_coefficient=1.5 + ) self.object_markers2world = pillow_markers2world self.init_pos, self.init_orn = self.mesh_pose( self.object_markers2world) @@ -468,7 +630,8 @@ def __init__(self, pillow_markers2world, scale=1.0, fixed=False, client_id=0): def mesh_pose(insole_markers2world): markers2mesh = pt.transform_from( - R=pr.active_matrix_from_extrinsic_roll_pitch_yaw(np.deg2rad([0, 0, 90])), + R=pr.active_matrix_from_extrinsic_roll_pitch_yaw( + np.deg2rad([0, 0, 90])), p=np.array([0.0, -0.02, 0.095])) pq = pt.pq_from_transform(pt.concat(pt.invert_transform(markers2mesh), insole_markers2world)) @@ -478,14 +641,22 @@ def mesh_pose(insole_markers2world): class InsoleOnConveyorBelt(Insole): def __init__( - self, insole_markers2world, grasp_point_name="back", scale=1.0, - E=100000.0, client_id=0): + self, + insole_markers2world, + pb_client: bc.BulletClient, + grasp_point_name="back", + scale=1.0, + E=100000.0): self.conveyor = None assert grasp_point_name in ["front", "middle", "back"] self.grasp_point_name = grasp_point_name super().__init__( - insole_markers2world=insole_markers2world, scale=scale, E=E, - fixed=False, client_id=client_id) + insole_markers2world=insole_markers2world, + pb_client=pb_client, + scale=scale, + E=E, + fixed=False + ) def _load_object(self): assert not self.fixed @@ -501,15 +672,18 @@ def _load_object(self): else: raise NotImplementedError("We would need two boxes for this.") self.conveyor = BoxObject( + pb_client=self.pb_client, half_extents=0.5 * conveyor_extents, - world_pos=conveyor_pos, world_orn=(0, 0, 0), fixed=True, - lateralFriction=0.3, rollingFriction=0.3) + world_pos=conveyor_pos, + world_orn=(0, 0, 0), + fixed=True, + lateralFriction=0.3, + rollingFriction=0.3) return object_id def remove_anchors(self): if self.conveyor is not None: - pb.removeBody( - self.conveyor.get_id(), physicsClientId=self.client_id) + self.pb_client.removeBody(self.conveyor.get_id()) self.conveyor = None def reset(self, object_markers2world=None, center_camera=False, pos=None): @@ -524,14 +698,12 @@ def reset(self, object_markers2world=None, center_camera=False, pos=None): self.remove_anchors() - pb.removeBody(self.object_id, physicsClientId=self.client_id) - self.init_pos, self.init_orn = self.mesh_pose( - self.object_markers2world) + self.pb_client.removeBody(self.object_id) + self.init_pos, self.init_orn = self.mesh_pose(self.object_markers2world) self.object_id = self._load_object() if center_camera: - pb.resetDebugVisualizerCamera( - 0.35, -60, -30, self.object_markers2world[:3, 3], - physicsClientId=self.client_id) + self.pb_client.resetDebugVisualizerCamera( + 0.35, -60, -30, self.object_markers2world[:3, 3]) class ObjectFactory: @@ -559,8 +731,12 @@ class ObjectFactory: "capsule": [0, 0, 0] } + def __init__(self, pb_client: bc.BulletClient): + self.pb_client = pb_client + def create( - self, object_name: str, + self, + object_name: str, object_position: Union[npt.ArrayLike, None] = None, object_orientation: Union[npt.ArrayLike, None] = None, object2world: Union[npt.ArrayLike, None] = None, @@ -591,49 +767,76 @@ def create( fixed_nodes=[0, 40, 45]) args.update(additional_args) object_to_grasp = SoftObject( - os.path.join(base_path, "object_data/insole.vtk"), world_pos=object_position, - world_orn=object_orientation, **args) + os.path.join(base_path, "object_data/insole.vtk"), + self.pb_client, + world_pos=object_position, + world_orn=object_orientation, + **args) elif object_name == "pillow_small": args = dict(fixed=True, mass=0.5, nu=0.1, E=20000.0, fixed_nodes=[0, 40, 45]) args.update(additional_args) object_to_grasp = SoftObject( - os.path.join(base_path, "object_data/pillow_small.vtk"), world_pos=object_position, - world_orn=object_orientation, **args) + os.path.join(base_path, "object_data/pillow_small.vtk"), + self.pb_client, + world_pos=object_position, + world_orn=object_orientation, + **args) elif object_name == "insole2": args = dict(scale=1.0, fixed=True) args.update(additional_args) - object_to_grasp = Insole(object2world, **args) + object_to_grasp = Insole( + object2world, + self.pb_client, + **args) elif object_name == "pillow_small2": args = dict(scale=1.0, fixed=True) args.update(additional_args) - object_to_grasp = PillowSmall(object2world, **args) + object_to_grasp = PillowSmall( + object2world, + self.pb_client, + **args) elif object_name.startswith("insole_on_conveyor_belt"): # TODO hacked special case, refactor this args = dict(scale=1.0) args.update(additional_args) args["grasp_point_name"] = object_name.split("/")[-1] - object_to_grasp = InsoleOnConveyorBelt(object2world, **args) + object_to_grasp = InsoleOnConveyorBelt( + object2world, + self.pb_client, + **args) elif object_name == "box": args = dict(half_extents=(0.1, 0.02, 0.02), mass=0.1, fixed=True) args.update(additional_args) object_to_grasp = BoxObject( - world_pos=object_position, world_orn=object_orientation, **args) + self.pb_client, + world_pos=object_position, + world_orn=object_orientation, + **args) elif object_name == "sphere": args = dict(radius=0.05, mass=0.1, fixed=True) args.update(additional_args) object_to_grasp = SphereObject( - world_pos=object_position, world_orn=object_orientation, **args) + self.pb_client, + world_pos=object_position, + world_orn=object_orientation, + **args) elif object_name == "cylinder": args = dict(radius=0.025, height=0.05, mass=0.1, fixed=True) args.update(additional_args) object_to_grasp = CylinderObject( - world_pos=object_position, world_orn=object_orientation, **args) + self.pb_client, + world_pos=object_position, + world_orn=object_orientation, + **args) elif object_name == "capsule": args = dict(radius=0.025, height=0.05, mass=0.1, fixed=True) args.update(additional_args) object_to_grasp = CapsuleObject( - world_pos=object_position, world_orn=object_orientation, **args) + self.pb_client, + world_pos=object_position, + world_orn=object_orientation, + **args) else: raise KeyError(f"Object '{object_name}' not available.") @@ -663,9 +866,11 @@ def get_pose( if object2world is None: if object_position is None: - object_position = np.copy(self.OBJECT_POSITIONS[object_name]) + object_position = np.copy( + self.OBJECT_POSITIONS[object_name]) if object_orientation is None: - object_orientation = np.copy(self.OBJECT_ORIENTATIONS[object_name]) + object_orientation = np.copy( + self.OBJECT_ORIENTATIONS[object_name]) object2world = pt.transform_from( R=pr.active_matrix_from_extrinsic_euler_xyz( @@ -679,21 +884,33 @@ def get_pose( class Pose(object): - def __init__(self, position, orientation, scale=0.1, line_width=10): + def __init__( + self, + position, + orientation, + pb_client: bc.BulletClient, + scale=0.1, + line_width=10,): self.position = position self.orientation = orientation self.scale = scale self.line_width = line_width + self.pb_client = pb_client self.ids = draw_pose(self.position, self.orientation, s=self.scale, - lw=self.line_width) + lw=self.line_width, pb_client=self.pb_client) def update(self, position, orientation): self.position = position self.orientation = orientation - draw_pose(self.position, self.orientation, s=self.scale, - lw=self.line_width, replace_item_unique_ids=self.ids) + draw_pose( + self.position, + self.orientation, + s=self.scale, + lw=self.line_width, + replace_item_unique_ids=self.ids, + pb_client=self.pb_client) def remove(self): for item in self.ids: - pb.removeUserDebugItem(item) + self.pb_client.removeUserDebugItem(item) del self diff --git a/deformable_gym/objects/conveyor.py b/deformable_gym/objects/conveyor.py index acc864c..4df6c1d 100644 --- a/deformable_gym/objects/conveyor.py +++ b/deformable_gym/objects/conveyor.py @@ -1,27 +1,42 @@ from deformable_gym.objects.bullet_object import BoxObject, ObjectFactory +from pybullet_utils import bullet_client as bc class Conveyor: - def __init__(self, height_m=1, + def __init__(self, + pb_client: bc.BulletClient, + height_m=1, conveyor_pos=(0, 1.5, 0.5), conveyor_rot=(0, 0, 0), insole_pos=(-0.1, 0.9, 1.02), insole_rot=(0.0, 0.0, -1.57), - E=100000.0): + E=100000.0, + ): self.height_m = height_m self.width_m = 0.5 self.length_m = 1.5 + self.pb_client = pb_client self.conveyor = BoxObject( - half_extents=[self.width_m / 2, self.length_m / 2, self.height_m / 2], - world_pos=conveyor_pos, world_orn=conveyor_rot, fixed=True, - lateralFriction=0.3, rollingFriction=0.3) + self.pb_client, + half_extents=[ + self.width_m/2, self.length_m/2, self.height_m/2 + ], + world_pos=conveyor_pos, + world_orn=conveyor_rot, + fixed=True, + lateralFriction=0.3, + rollingFriction=0.3 + ) - self.object_factory = ObjectFactory() + self.object_factory = ObjectFactory(self.pb_client) self.insole, self.insole_pos, self.insole_orn = \ self.object_factory.create( - "insole2", object_position=insole_pos, - object_orientation=insole_rot, E=E) + "insole2", + object_position=insole_pos, + object_orientation=insole_rot, + E=E + ) self.insole.remove_anchors() def reset_insole(self, new_pos=None, new_orn=None): diff --git a/deformable_gym/robots/__init__.py b/deformable_gym/robots/__init__.py index f5f61ab..a78cd10 100644 --- a/deformable_gym/robots/__init__.py +++ b/deformable_gym/robots/__init__.py @@ -4,7 +4,6 @@ from .ur5_mia import UR5Mia, UR5MiaPosition, UR5MiaVelocity from .ur10_shadow import UR10Shadow, UR10ShadowPosition, UR10ShadowVelocity - __all__ = [ "BulletRobot", "MiaHand", "MiaHandPosition", "MiaHandVelocity", diff --git a/deformable_gym/robots/bullet_robot.py b/deformable_gym/robots/bullet_robot.py index b5fffdf..57c826f 100644 --- a/deformable_gym/robots/bullet_robot.py +++ b/deformable_gym/robots/bullet_robot.py @@ -1,13 +1,16 @@ import abc -from typing import Tuple, List, Union, Dict, Iterable, Any +from typing import Any, Dict, Iterable, List, Tuple, Union + import numpy as np import numpy.typing as npt import pybullet as pb +from pybullet_utils import bullet_client as bc +from gymnasium.spaces import Box + from deformable_gym.helpers import pybullet_helper as pbh +from deformable_gym.objects.bullet_object import Pose from deformable_gym.robots.bullet_utils import draw_limits from deformable_gym.robots.inverse_kinematics import PyBulletSolver -from deformable_gym.objects.bullet_object import Pose -from gymnasium.spaces import Box class BulletRobot(abc.ABC): @@ -27,12 +30,17 @@ class BulletRobot(abc.ABC): space). """ def __init__( - self, urdf_path: str, verbose: int = 0, - world_pos: npt.ArrayLike = None, world_orn: npt.ArrayLike = None, + self, + urdf_path: str, + pb_client: bc.BulletClient, + verbose: bool = False, + world_pos: npt.ArrayLike = None, + world_orn: npt.ArrayLike = None, control_mode: int = pb.POSITION_CONTROL, task_space_limit: Union[npt.ArrayLike, None] = None, orn_limit: Union[npt.ArrayLike, None] = None, - base_commands: bool = False): + base_commands: bool = False, + ): self.path = urdf_path self.verbose = verbose self.control_mode = control_mode @@ -42,6 +50,7 @@ def __init__( self.orn_limit = orn_limit self.subsystems = {} self.base_commands = base_commands + self.pb_client = pb_client if self.task_space_limit is not None and verbose: draw_limits(self.task_space_limit) @@ -55,15 +64,20 @@ def __init__( if world_orn is None: self.init_rot = pb.getQuaternionFromEuler((0, 0, 0)) else: - assert len(world_orn) == 3 - self.init_rot = pb.getQuaternionFromEuler(world_orn) + assert len(world_orn) == 4 + self.init_rot = world_orn self._id = self.initialise() self._joint_name_to_joint_id, self._link_name_to_link_id = \ - pbh.analyze_robot(robot=self._id, verbose=verbose) + pbh.analyze_robot( + robot=self._id, + pb_client=self.pb_client, + verbose=verbose, + ) - self.all_joints = pbh.build_joint_list(self._id, verbose=1) + self.all_joints = pbh.build_joint_list( + self._id, self.pb_client, verbose=self.verbose) # separate fixed joints from controllable ones self.fixed_joints = { @@ -82,49 +96,63 @@ def __init__( for name, joint in self.fixed_joints.items(): if "sensor" in name: self.sensors[name] = joint - pb.enableJointForceTorqueSensor(self._id, joint.joint_idx, 1) + self.pb_client.enableJointForceTorqueSensor( + self._id, + joint.joint_idx, + enableSensor=True + ) # set the joints initial positions for j in self.all_joints: - j.init_pos = pb.getJointState(self._id, j.joint_idx)[0] + j.init_pos = self.pb_client.getJointState( + self._id, + j.joint_idx, + )[0] # set initial command self.current_command = {k: None for k in self.motors.keys()} # create fixed constraint for base if robot base is controllable if self.base_commands: - self.base_constraint = \ - pb.createConstraint(self._id, # parent body id - -1, # parent link id, -1 for base - -1, # child body id, -1 for static frame in world coords - -1, # child link id, -1 for base - pb.JOINT_FIXED, # joint type - [0, 0, 0], # joint axis in child frame - [0, 0, 0], # parent frame position - [0, 0, 1]) # child frame position + self.base_constraint = self.pb_client.createConstraint( + self._id, + -1, # parent link id, -1 for base + -1, # child body id, -1 for static frame + -1, # child link id, -1 for base + pb.JOINT_FIXED, # joint type + [0, 0, 0], # joint axis in child frame + [0, 0, 0], # parent frame position + [0, 0, 1], # child frame position + ) if verbose: print(self.all_joints) self.multibody_pose = pbh.MultibodyPose( - self.get_id(), self.init_pos, self.init_rot) + self.get_id(), self.init_pos, self.init_rot, + pb_client=self.pb_client) # create observation and action spaces # TODO: fix to include base actions and sensor observations - self.action_space = Box(np.full(len(self.motors), -1.0), - np.full(len(self.motors), 1.0)) - self.observation_space = Box(np.full(len(self.motors), -1.0), - np.full(len(self.motors), 1.0)) + self.action_space = Box( + np.full(len(self.motors), -1.0), + np.full(len(self.motors), 1.0)) + self.observation_space = Box( + np.full(len(self.motors), -1.0), + np.full(len(self.motors), 1.0)) def initialise(self) -> int: """Initialize robot in simulation. :return: PyBullet UUID for loaded multi-body. """ - return pb.loadURDF( - self.path, self.init_pos, self.init_rot, + return self.pb_client.loadURDF( + self.path, + self.init_pos, + self.init_rot, useFixedBase=not self.base_commands, - flags=pb.URDF_USE_SELF_COLLISION | pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) + flags=pb.URDF_USE_SELF_COLLISION + | pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) def get_id(self) -> int: """Get PyBullet UUID for the robot multi-body. @@ -175,7 +203,9 @@ def get_joint_positions( return np.array([self.motors[k].get_position() for k in keys]) def get_joint_velocities( - self, keys: Union[Iterable[str], None] = None) -> np.ndarray: + self, + keys: Union[Iterable[str], None] = None + ) -> np.ndarray: """Returns the robot's current joint velocities. :param keys: Names of joints from which velocities are requested. @@ -192,7 +222,7 @@ def get_sensor_readings(self) -> np.ndarray: :return: Array of sensor readings. """ return np.array( - [el[2][-1] for el in pb.getJointStates( + [el[2][-1] for el in self.pb_client.getJointStates( self._id, [j.joint_idx for j in self.sensors.values()])]) def update_current_command(self, command: Dict[str, float]): @@ -209,9 +239,10 @@ def update_current_command(self, command: Dict[str, float]): for key in command.keys(): self.current_command[key] = command[key] - def send_current_command(self, - keys: Union[Iterable[str], None] = None - ) -> None: + def send_current_command( + self, + keys: Union[Iterable[str], None] = None + ) -> None: """Sends the currently stored commands to the selected motors. :param keys: The names of the motors to send commands to. @@ -235,9 +266,12 @@ def actuate_motors(self, keys: Union[Iterable[str], None] = None) -> None: """ def compute_ik( - self, joint_positions: np.ndarray, pos_command: np.ndarray, + self, + joint_positions: np.ndarray, + pos_command: np.ndarray, orn_command: Union[np.ndarray, None] = None, - velocities: bool = True) -> np.ndarray: + velocities: bool = True + ) -> np.ndarray: """Translates task space command to motor commands. :param joint_positions: Current joint angles @@ -263,7 +297,12 @@ def compute_ik( if self.task_space_limit is not None: if self.verbose: print(f"Original {target_pos=}") - target_pos = np.clip(target_pos, self.task_space_limit[0], self.task_space_limit[1]) + + target_pos = np.clip( + target_pos, + self.task_space_limit[0], + self.task_space_limit[1]) + if self.verbose: print(f"Clipped {target_pos=}") @@ -276,7 +315,8 @@ def compute_ik( target_orn_euler = pb.getEulerFromQuaternion(target_orn) target_orn_euler_clipped = np.clip( target_orn_euler, self.orn_limit[0], self.orn_limit[1]) - target_orn = pb.getQuaternionFromEuler(target_orn_euler_clipped) + target_orn = pb.getQuaternionFromEuler( + target_orn_euler_clipped) target_orn /= np.maximum(np.linalg.norm(target_orn), 1e-10) @@ -300,15 +340,18 @@ def get_ee_pose(self) -> np.ndarray: quaternion: (x, y, z, qx, qy, qz, qw). """ if self.end_effector is not None: - current_pos, current_orn = pbh.link_pose(self._id, self.end_effector) + current_pos, current_orn = pbh.link_pose( + self._id, self.end_effector) else: - current_pos, current_orn = pb.getBasePositionAndOrientation(self._id) + current_pos, current_orn = ( + self.pb_client.getBasePositionAndOrientation(self._id)) return np.concatenate((current_pos, current_orn), axis=0) - def set_ee_pose(self, - target_pos: np.ndarray, - target_orn: np.ndarray - ) -> None: + def set_ee_pose( + self, + target_pos: np.ndarray, + target_orn: np.ndarray + ) -> None: """Sets the pose of the end-effector instantly. No physics simulation steps required. @@ -317,15 +360,18 @@ def set_ee_pose(self, :param target_orn: Target orientation as (qx, qy, qz, qw). """ joint_pos = self.compute_ik( - self.get_joint_positions()[:6], target_pos, target_orn, + self.get_joint_positions()[:6], + target_pos, + target_orn, velocities=False) position_dict = {joint: pos for joint, pos in zip(self.motors.keys(), joint_pos)} self.set_joint_positions(position_dict) - def reset(self, - keys: Union[Iterable[str], None] = None - ) -> None: + def reset( + self, + keys: Union[Iterable[str], None] = None + ) -> None: """Resets actuators and sensors. :param keys: Names of joints to reset. Default is all. @@ -340,9 +386,10 @@ def reset(self, self.motors[key].reset() self.current_command[key] = self.motors[key].get_position() - def set_joint_positions(self, - position_dict: Dict[str, float] - ) -> None: + def set_joint_positions( + self, + position_dict: Dict[str, float] + ) -> None: """Sets the joint positions of the robot. :param position_dict: Maps joint names to angles. @@ -372,9 +419,11 @@ def move_base(self, offset: npt.ArrayLike) -> None: target_pos, target_orn = self.multibody_pose.translate_pose( target_pos, target_orn) - pb.changeConstraint(self.base_constraint, target_pos, - jointChildFrameOrientation=target_orn, - maxForce=100000) + self.pb_client.changeConstraint( + self.base_constraint, + target_pos, + jointChildFrameOrientation=target_orn, + maxForce=100000) def _enforce_limits(self, pose) -> Tuple[Any, Any]: assert self.base_commands, "tried to move base, but base commands " \ @@ -398,9 +447,12 @@ def reset_base(self, pose: npt.ArrayLike) -> None: target_pos, target_orn = self.multibody_pose.set_pose( target_pos, target_orn) - pb.changeConstraint(self.base_constraint, target_pos, - jointChildFrameOrientation=target_orn, - maxForce=100000) + self.pb_client.changeConstraint( + self.base_constraint, + target_pos, + jointChildFrameOrientation=target_orn, + maxForce=100000 + ) def _get_link_id(self, link_name: str) -> int: """Get PyBullet link ID for link name. @@ -410,7 +462,10 @@ def _get_link_id(self, link_name: str) -> int: """ return self._link_name_to_link_id[link_name] - def get_joint_limits(self, joints: List[str]) -> Tuple[np.ndarray, np.ndarray]: + def get_joint_limits( + self, + joints: List[str] + ) -> Tuple[np.ndarray, np.ndarray]: """Get array of lower limits and array of upper limits of joints. :param joints: Names of joints for which we request limits. @@ -448,11 +503,14 @@ def _init_debug_visualizations(self): self.robot_pose = Pose( np.zeros(3), np.array([0.0, 0.0, 0.0, 1.0]), + pb_client=self.pb_client, scale=0.1, line_width=1) + self.object_pose = Pose( np.zeros(3), np.array([0.0, 0.0, 0.0, 1.0]), + pb_client=self.pb_client, scale=0.1, line_width=1) self.contact_normals = [] @@ -463,28 +521,33 @@ def get_contact_points(self, object_id: int): :param object_id: UUID of object. :return: List of contact point information from PyBullet. """ - contact_points = pb.getContactPoints(self.get_id(), object_id) + contact_points = self.pb_client.getContactPoints( + self.get_id(), object_id) + if self.debug_visualization: for contact_point_id in self.contact_normals: pb.removeUserDebugItem(contact_point_id) - robot_position, robot_orientation = \ - pb.getBasePositionAndOrientation(self.get_id()) + robot_position, robot_orientation = ( + self.pb_client.getBasePositionAndOrientation( + self.get_id())) self.robot_pose.update(robot_position, robot_orientation) - object_position, object_orientation = \ - pb.getBasePositionAndOrientation(object_id) + object_position, object_orientation = ( + self.pb_client.getBasePositionAndOrientation(object_id)) self.object_pose.update(object_position, object_orientation) self.contact_normals = [] for contact_point in contact_points: - _, _, _, _, _, _, position_on_object, \ - contact_normal_on_object, _, normal_force, _, _, _, _ = \ - contact_point + position_on_object = contact_point[6] + contact_normal_on_object = contact_point[7] + normal_force = contact_point[9] object_normal_end = ( np.array(position_on_object) + normal_force * np.array(contact_normal_on_object)) - contact_normal_line = pb.addUserDebugLine( - position_on_object, object_normal_end, [1, 1, 1]) + contact_normal_line = self.pb_client.addUserDebugLine( + position_on_object, + object_normal_end, + [1, 1, 1]) self.contact_normals.append(contact_normal_line) return contact_points diff --git a/deformable_gym/robots/bullet_utils.py b/deformable_gym/robots/bullet_utils.py index 1e2d8ca..638dd84 100644 --- a/deformable_gym/robots/bullet_utils.py +++ b/deformable_gym/robots/bullet_utils.py @@ -3,13 +3,16 @@ import pytransform3d.rotations as pr import pytransform3d.transformations as pt +from pybullet_utils import bullet_client as bc -def q_conj(q_xyzw): - return np.r_[-q_xyzw[:3], q_xyzw[3]] - -def draw_transform(pose2origin, s, client_id=0, lw=1, - replace_item_unique_ids=(-1, -1, -1)): +def draw_transform( + pose2origin, + s, + pb_client: bc.BulletClient, + lw=1, + replace_item_unique_ids=(-1, -1, -1) +): """Draw transformation matrix. Parameters @@ -35,23 +38,35 @@ def draw_transform(pose2origin, s, client_id=0, lw=1, userDataIds : list User data ids that identify the created lines. """ - line_x = pb.addUserDebugLine( - pose2origin[:3, 3], pose2origin[:3, 3] + s * pose2origin[:3, 0], - [1, 0, 0], lw, replaceItemUniqueId=replace_item_unique_ids[0], - physicsClientId=client_id) - line_y = pb.addUserDebugLine( - pose2origin[:3, 3], pose2origin[:3, 3] + s * pose2origin[:3, 1], - [0, 1, 0], lw, replaceItemUniqueId=replace_item_unique_ids[1], - physicsClientId=client_id) - line_z = pb.addUserDebugLine( - pose2origin[:3, 3], pose2origin[:3, 3] + s * pose2origin[:3, 2], - [0, 0, 1], lw, replaceItemUniqueId=replace_item_unique_ids[2], - physicsClientId=client_id) + line_x = pb_client.addUserDebugLine( + pose2origin[:3, 3], + pose2origin[:3, 3] + s * pose2origin[:3, 0], + [1, 0, 0], + lw, + replaceItemUniqueId=replace_item_unique_ids[0]) + line_y = pb_client.addUserDebugLine( + pose2origin[:3, 3], + pose2origin[:3, 3] + s * pose2origin[:3, 1], + [0, 1, 0], + lw, + replaceItemUniqueId=replace_item_unique_ids[1]) + line_z = pb_client.addUserDebugLine( + pose2origin[:3, 3], + pose2origin[:3, 3] + s * pose2origin[:3, 2], + [0, 0, 1], + lw, + replaceItemUniqueId=replace_item_unique_ids[2]) return [line_x, line_y, line_z] -def draw_pose(pos, rot, s, client_id=0, lw=1, - replace_item_unique_ids=(-1, -1, -1)): +def draw_pose( + pos, + rot, + s, + pb_client: bc.BulletClient, + lw=1, + replace_item_unique_ids=(-1, -1, -1), +): """Draw transformation matrix. Parameters @@ -83,10 +98,14 @@ def draw_pose(pos, rot, s, client_id=0, lw=1, q_scalar_first = pr.quaternion_wxyz_from_xyzw(rot) pose2origin = pt.transform_from( R=pr.matrix_from_quaternion(q_scalar_first), p=pos) - return draw_transform(pose2origin, s, client_id, lw, replace_item_unique_ids) + return draw_transform( + pose2origin, s, pb_client, lw, replace_item_unique_ids) -def draw_box(corners_in_world, replace_item_unique_ids=None): +def draw_box( + corners_in_world, + pb_client: bc.BulletClient, + replace_item_unique_ids=None): """Draw box. Parameters @@ -103,13 +122,15 @@ def draw_box(corners_in_world, replace_item_unique_ids=None): [(0, 1), (0, 2), (1, 3), (2, 3), (4, 5), (4, 6), (5, 7), (6, 7), (0, 4), (1, 5), (2, 6), (3, 7)]): - replace_item_unique_ids[line_idx] = pb.addUserDebugLine( - corners_in_world[i], corners_in_world[j], - (0, 0, 0), replaceItemUniqueId=replace_item_unique_ids[line_idx]) + replace_item_unique_ids[line_idx] = pb_client.addUserDebugLine( + corners_in_world[i], + corners_in_world[j], + (0, 0, 0), + replaceItemUniqueId=replace_item_unique_ids[line_idx]) return replace_item_unique_ids -def draw_limits(limits): +def draw_limits(limits, pb_client): x_lo = limits[0][0] y_lo = limits[0][1] z_lo = limits[0][2] @@ -126,4 +147,4 @@ def draw_limits(limits): [x_hi, y_lo, z_lo], [x_hi, y_hi, z_lo], ]) - draw_box(task_space_corners) + draw_box(task_space_corners, pb_client) diff --git a/deformable_gym/robots/control_mixins.py b/deformable_gym/robots/control_mixins.py index 7bbba33..2e42fcd 100644 --- a/deformable_gym/robots/control_mixins.py +++ b/deformable_gym/robots/control_mixins.py @@ -1,4 +1,4 @@ -from typing import Union, List +from typing import List, Union class PositionControlMixin: diff --git a/deformable_gym/robots/inverse_kinematics.py b/deformable_gym/robots/inverse_kinematics.py index 660be2a..cba7106 100644 --- a/deformable_gym/robots/inverse_kinematics.py +++ b/deformable_gym/robots/inverse_kinematics.py @@ -1,11 +1,20 @@ import numpy as np import pybullet as pb -from deformable_gym.robots.ur_kinematics import ( - analytical_ik, urdf_base2kin_base, ee_kin2ee_options, robot_params) + +from deformable_gym.robots.ur_kinematics import (analytical_ik, + ee_kin2ee_options, + robot_params, + urdf_base2kin_base) class PyBulletSolver: - def __init__(self, robot, end_effector, n_iter=200, threshold=1e-6): + def __init__( + self, + robot, + end_effector, + n_iter: int = 200, + threshold: float = 1e-6, + ): self.robot = robot self.end_effector = end_effector self.n_iter = n_iter @@ -16,14 +25,25 @@ def __call__(self, target_pos, target_orn, last_joint_angles): if last_joint_angles is None: # HACK last_joint_angles = np.zeros(6) - return np.array(pb.calculateInverseKinematics( - self.robot, self.end_effector, targetPosition=target_pos, - targetOrientation=target_orn, maxNumIterations=self.n_iter, - residualThreshold=self.threshold))[:6] + + out = np.array(pb.calculateInverseKinematics( + self.robot, + self.end_effector, + targetPosition=target_pos, + targetOrientation=target_orn, + maxNumIterations=self.n_iter, + residualThreshold=self.threshold, + physicsClientId=self.robot.physics_client_id)) + return out[:6] class UniversalRobotAnalyticalInverseKinematics: - def __init__(self, robot_type="ur5", end_effector_name="ur5_tool0", fallback=None): + def __init__( + self, + robot_type="ur5", + end_effector_name="ur5_tool0", + fallback=None + ): self.params = robot_params[robot_type] self.ee_kin2ee = ee_kin2ee_options[end_effector_name] self.fallback = fallback @@ -33,8 +53,10 @@ def __call__(self, target_pos, target_orn, last_joint_angles): last_joint_angles = np.zeros(6) joint_angles = analytical_ik( - np.hstack((target_pos, target_orn)), last_joint_angles, - params=self.params, urdf_base2kin_base=urdf_base2kin_base, + np.hstack((target_pos, target_orn)), + last_joint_angles, + params=self.params, + urdf_base2kin_base=urdf_base2kin_base, ee_kin2ee=self.ee_kin2ee) if joint_angles is None: diff --git a/deformable_gym/robots/mia_hand.py b/deformable_gym/robots/mia_hand.py index 155af09..97742cc 100644 --- a/deformable_gym/robots/mia_hand.py +++ b/deformable_gym/robots/mia_hand.py @@ -1,16 +1,23 @@ import abc -from typing import Union, Tuple, List, Dict -import numpy as np -import numpy.typing as npt import os from pathlib import Path +from typing import Dict, List, Tuple, Union + +import numpy as np +import numpy.typing as npt from deformable_gym.helpers.pybullet_helper import Joint -from deformable_gym.robots.bullet_robot import BulletRobot, RobotCommandWrapper, HandMixin -from deformable_gym.robots.control_mixins import PositionControlMixin, VelocityControlMixin +from deformable_gym.robots.bullet_robot import (BulletRobot, HandMixin, + RobotCommandWrapper) +from deformable_gym.robots.control_mixins import (PositionControlMixin, + VelocityControlMixin) from deformable_gym.robots.sensors import MiaHandForceSensors -URDF_PATH = os.path.join(Path(os.path.dirname(__file__)).parent.parent.absolute(), "robots/urdf/mia_hand.urdf") +from pybullet_utils import bullet_client as bc + +URDF_PATH = os.path.join( + Path(os.path.dirname(__file__)).parent.parent.absolute(), + "robots/urdf/mia_hand.urdf") class MiaHandMixin(HandMixin): @@ -20,7 +27,8 @@ class MiaHandMixin(HandMixin): "j_index_fle", "j_little_fle", "j_mrl_fle", "j_ring_fle", "j_thumb_fle" ] actuated_real_joints: List[str] = [ - "j_index_fle", "j_mrl_fle", "j_thumb_fle"] + "j_index_fle", "j_mrl_fle", "j_thumb_fle" + ] mia_hand_force_sensors: MiaHandForceSensors @@ -69,8 +77,8 @@ def convert_action_to_pybullet(self, command): mrl_command = command[1] hand_joint_target = np.insert(command, [1, 2], mrl_command) # fill j_thumb_opp with target value - hand_joint_target = np.insert(hand_joint_target, 4, - self.motors["j_thumb_opp"].init_pos) + hand_joint_target = np.insert( + hand_joint_target, 4, self.motors["j_thumb_opp"].init_pos) return hand_joint_target def get_joint_limits(self) -> Tuple[np.ndarray, np.ndarray]: @@ -91,7 +99,7 @@ def get_sensor_readings(self) -> np.ndarray: - (4) normal force at thumb, - (5) normal force at middle finger. """ - return self.mia_hand_force_sensors.measure() + return self.mia_hand_force_sensors.measure().copy() def get_sensor_limits(self) -> Tuple[np.ndarray, np.ndarray]: return self.mia_hand_force_sensors.get_limits() @@ -117,7 +125,9 @@ class MiaHand(MiaHandMixin, BulletRobot, abc.ABC): space). """ def __init__( - self, verbose: int = 0, + self, + pb_client: bc.BulletClient, + verbose: bool = False, task_space_limit: Union[npt.ArrayLike, None] = None, orn_limit: Union[npt.ArrayLike, None] = None, world_pos: npt.ArrayLike = (0, 0, 1), @@ -125,9 +135,9 @@ def __init__( debug_visualization: bool = True, **kwargs): super().__init__( - urdf_path=URDF_PATH, verbose=verbose, world_pos=world_pos, - world_orn=world_orn, task_space_limit=task_space_limit, - orn_limit=orn_limit, **kwargs) + pb_client=pb_client, urdf_path=URDF_PATH, verbose=verbose, + world_pos=world_pos, world_orn=world_orn, + task_space_limit=task_space_limit, orn_limit=orn_limit, **kwargs) self.debug_visualization = debug_visualization rcw = RobotCommandWrapper(self, self.actuated_simulated_joints) @@ -136,7 +146,7 @@ def __init__( self._correct_index_limit() self.mia_hand_force_sensors = MiaHandForceSensors( - self._id, self._joint_name_to_joint_id, debug=False) + self._id, self._joint_name_to_joint_id, self.pb_client) self.thumb_adducted = True diff --git a/deformable_gym/robots/sensors.py b/deformable_gym/robots/sensors.py index 7a2e538..2c943ba 100644 --- a/deformable_gym/robots/sensors.py +++ b/deformable_gym/robots/sensors.py @@ -1,6 +1,8 @@ +import matplotlib.pyplot as plt import numpy as np import pybullet as pb -import matplotlib.pyplot as plt + +from pybullet_utils import bullet_client as bc INDEX_SENSOR_NAME = "j_index_sensor" MIDDLE_SENSOR_NAME = "j_middle_sensor" @@ -71,16 +73,27 @@ class MiaHandForceSensors: SENSOR_NAMES = ["middle tangential", "index normal", "index tangential", "thumb tangential", "thumb normal", "middle normal"] - def __init__(self, robot_id, joint_name_to_joint_id, debug=False): + def __init__( + self, + robot_id: int, + joint_name_to_joint_id: dict, + pb_client: bc.BulletClient, + debug: bool = False, + ): self.robot_id = robot_id self.index_sensor_id = joint_name_to_joint_id[INDEX_SENSOR_NAME] self.middle_sensor_id = joint_name_to_joint_id[MIDDLE_SENSOR_NAME] self.thumb_sensor_id = joint_name_to_joint_id[THUMB_SENSOR_NAME] self.debug = debug + self.pb_client = pb_client - for sensor_id in [self.index_sensor_id, self.middle_sensor_id, - self.thumb_sensor_id]: - pb.enableJointForceTorqueSensor(self.robot_id, sensor_id) + for sensor_id in [ + self.index_sensor_id, + self.middle_sensor_id, + self.thumb_sensor_id + ]: + self.pb_client.enableJointForceTorqueSensor( + self.robot_id, sensor_id) if self.debug: self.history = [] @@ -128,17 +141,26 @@ def measure(self, out=None): out = np.empty(6) _, _, _, T_ind_x, _, T_ind_z = -np.array( - pb.getJointState(self.robot_id, self.index_sensor_id)[2]) + self.pb_client.getJointState( + self.robot_id, + self.index_sensor_id, + )[2]) out[1] = T_ind_z * MM_PER_M / 2.006 out[2] = T_ind_x * MM_PER_M / 2.854 _, _, _, T_mrl_x, _, T_mrl_z = -np.array( - pb.getJointState(self.robot_id, self.middle_sensor_id)[2]) + self.pb_client.getJointState( + self.robot_id, + self.middle_sensor_id, + )[2]) out[5] = T_mrl_z * MM_PER_M / 2.006 out[0] = T_mrl_x * MM_PER_M / 2.854 _, F_th_y, _, _, T_th_y, _ = -np.array( - pb.getJointState(self.robot_id, self.thumb_sensor_id)[2]) + self.pb_client.getJointState( + self.robot_id, + self.thumb_sensor_id, + )[2]) out[3] = T_th_y * MM_PER_M / 3.673 out[4] = F_th_y / 0.056 diff --git a/deformable_gym/robots/shadow_hand.py b/deformable_gym/robots/shadow_hand.py index 666ae4f..b1958a8 100644 --- a/deformable_gym/robots/shadow_hand.py +++ b/deformable_gym/robots/shadow_hand.py @@ -1,17 +1,20 @@ import abc +import os +from pathlib import Path from typing import Union import numpy as np import numpy.typing as npt -import os -from pathlib import Path - -from deformable_gym.robots.bullet_robot import BulletRobot, RobotCommandWrapper, HandMixin -from deformable_gym.robots.control_mixins import PositionControlMixin, VelocityControlMixin - +from deformable_gym.robots.bullet_robot import (BulletRobot, HandMixin, + RobotCommandWrapper) +from deformable_gym.robots.control_mixins import (PositionControlMixin, + VelocityControlMixin) +from pybullet_utils import bullet_client as bc -URDF_PATH = os.path.join(Path(os.path.dirname(__file__)).parent.parent.absolute(), "robots/urdf/shadow_hand.urdf") +URDF_PATH = os.path.join( + Path(os.path.dirname(__file__)).parent.parent.absolute(), + "robots/urdf/shadow_hand.urdf") class ShadowHand(HandMixin, BulletRobot, abc.ABC): @@ -37,16 +40,23 @@ class ShadowHand(HandMixin, BulletRobot, abc.ABC): space). """ def __init__( - self, verbose: int = 0, + self, + pb_client: bc.BulletClient, + verbose: bool = False, task_space_limit: Union[npt.ArrayLike, None] = None, orn_limit: Union[npt.ArrayLike, None] = None, world_pos: npt.ArrayLike = (0, 0, 1), world_orn: npt.ArrayLike = (-np.pi / 8, np.pi, 0), debug_visualization: bool = True, **kwargs): super().__init__( - urdf_path=URDF_PATH, verbose=verbose, world_pos=world_pos, - world_orn=world_orn, task_space_limit=task_space_limit, - orn_limit=orn_limit, **kwargs) + urdf_path=URDF_PATH, + pb_client=pb_client, + verbose=verbose, + world_pos=world_pos, + world_orn=world_orn, + task_space_limit=task_space_limit, + orn_limit=orn_limit, + **kwargs) self.debug_visualization = debug_visualization hand_command_wrapper = RobotCommandWrapper(self, self.motors) diff --git a/deformable_gym/robots/ur10_shadow.py b/deformable_gym/robots/ur10_shadow.py index d3f939a..a9f536b 100644 --- a/deformable_gym/robots/ur10_shadow.py +++ b/deformable_gym/robots/ur10_shadow.py @@ -1,14 +1,18 @@ import abc - -import numpy as np -import pybullet as pb import os from pathlib import Path +from typing import Any + +import numpy as np + +from deformable_gym.robots.bullet_robot import (BulletRobot, HandMixin, + RobotCommandWrapper) +from deformable_gym.robots.control_mixins import (PositionControlMixin, + VelocityControlMixin) +from deformable_gym.robots.inverse_kinematics import \ + UniversalRobotAnalyticalInverseKinematics -from deformable_gym.objects.bullet_object import Pose -from deformable_gym.robots.bullet_robot import BulletRobot, RobotCommandWrapper, HandMixin -from deformable_gym.robots.control_mixins import PositionControlMixin, VelocityControlMixin -from deformable_gym.robots.inverse_kinematics import UniversalRobotAnalyticalInverseKinematics +from pybullet_utils import bullet_client as bc # Shadow freq = 500 Hz # UR5 freq = 125 Hz @@ -25,11 +29,20 @@ class UR10Shadow(HandMixin, BulletRobot, abc.ABC): TODO Can (and should) be extended eventually to allow other control methods, e.g. velocity or torque control. """ - def __init__(self, verbose=0, task_space_limit=None, - end_effector_link="rh_forearm", orn_limit=None, - debug_visualization=True): - super().__init__(urdf_path=URDF_PATH, verbose=verbose, - task_space_limit=task_space_limit, orn_limit=orn_limit) + def __init__( + self, + pb_client: bc.BulletClient, + verbose: bool = False, + task_space_limit: Any = None, + end_effector_link: str = "rh_forearm", + orn_limit=None, + debug_visualization: bool = True): + super().__init__( + urdf_path=URDF_PATH, + pb_client=pb_client, + verbose=verbose, + task_space_limit=task_space_limit, + orn_limit=orn_limit) self.command_counter = 0 self.debug_visualization = debug_visualization diff --git a/deformable_gym/robots/ur5_mia.py b/deformable_gym/robots/ur5_mia.py index b55c922..189e0a8 100644 --- a/deformable_gym/robots/ur5_mia.py +++ b/deformable_gym/robots/ur5_mia.py @@ -1,13 +1,18 @@ import abc - -import numpy.typing as npt import os from pathlib import Path +import numpy.typing as npt + from deformable_gym.robots.bullet_robot import BulletRobot, RobotCommandWrapper -from deformable_gym.robots.control_mixins import PositionControlMixin, VelocityControlMixin -from deformable_gym.robots.inverse_kinematics import UniversalRobotAnalyticalInverseKinematics +from deformable_gym.robots.control_mixins import (PositionControlMixin, + VelocityControlMixin) +from deformable_gym.robots.inverse_kinematics import \ + UniversalRobotAnalyticalInverseKinematics from deformable_gym.robots.sensors import MiaHandForceSensors + +from pybullet_utils import bullet_client as bc + from .mia_hand import MiaHandMixin # Mia freq = 20 Hz @@ -16,7 +21,6 @@ URDF_PATH = os.path.join(Path(os.path.dirname(__file__)).parent.parent.absolute(), "robots/urdf/mia_hand_on_ur5.urdf") - class UR5Mia(MiaHandMixin, BulletRobot, abc.ABC): """Controller for the robotic MIA hand. @@ -26,11 +30,17 @@ class UR5Mia(MiaHandMixin, BulletRobot, abc.ABC): TODO Can (and should) be extended eventually to allow other control methods, e.g. velocity or torque control. """ - def __init__(self, verbose=0, task_space_limit=None, - end_effector_link="ur5_tool0", orn_limit=None, - debug_visualization=True): - super().__init__(urdf_path=URDF_PATH, verbose=verbose, - task_space_limit=task_space_limit, orn_limit=orn_limit) + def __init__( + self, + pb_client: bc.BulletClient, + verbose: bool = False, + task_space_limit=None, + end_effector_link="ur5_tool0", + orn_limit=None, + debug_visualization=True): + super().__init__( + urdf_path=URDF_PATH, pb_client=pb_client, verbose=verbose, + task_space_limit=task_space_limit, orn_limit=orn_limit) self.debug_visualization = debug_visualization self._arm_motors = {k: v for k, v in self.motors.items() if "ur5_" in k} @@ -43,7 +53,7 @@ def __init__(self, verbose=0, task_space_limit=None, self.subsystems["arm"] = (125, arm_command_wrapper) self.mia_hand_force_sensors = MiaHandForceSensors( - self._id, self._joint_name_to_joint_id, debug=False) + self._id, self._joint_name_to_joint_id, self.pb_client) if self.debug_visualization: self._init_debug_visualizations() diff --git a/deformable_gym/robots/ur_kinematics.py b/deformable_gym/robots/ur_kinematics.py index 4a9fcd7..986fc3e 100644 --- a/deformable_gym/robots/ur_kinematics.py +++ b/deformable_gym/robots/ur_kinematics.py @@ -2,6 +2,7 @@ # https://smartech.gatech.edu/bitstream/handle/1853/50782/ur_kin_tech_report_1.pdf # http://rasmusan.blog.aau.dk/files/ur5_kinematics.pdf import math + import numpy as np import pytransform3d.rotations as pr import pytransform3d.transformations as pt @@ -42,20 +43,20 @@ class UR10Params: ee_kin2ee_options = { # see experimental/test_ur5_mia_cartesian.py "palm": np.array([ - [ 5.87586868e-05, -9.99999998e-01, -1.83652792e-16, -2.47027865e-05], - [ 9.68930791e-01, 5.69331010e-05, 2.47332003e-01, 1.72392460e-02], + [5.87586868e-05, -9.99999998e-01, -1.83652792e-16, -2.47027865e-05], + [9.68930791e-01, 5.69331010e-05, 2.47332003e-01, 1.72392460e-02], [-2.47332002e-01, -1.45329037e-05, 9.68930792e-01, -4.44860194e-03], - [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]), + [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]), "ur5_tool0": np.array([ [5.87586868e-05, -9.99999998e-01, -1.83652792e-16, -2.47027865e-05], - [ 7.22061953e-04, 4.24274124e-08, -9.99999739e-01, 5.94256999e-05], - [ 9.99999738e-01, 5.87586715e-05, 7.22061954e-04, -1.96120385e-04], - [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]), + [7.22061953e-04, 4.24274124e-08, -9.99999739e-01, 5.94256999e-05], + [9.99999738e-01, 5.87586715e-05, 7.22061954e-04, -1.96120385e-04], + [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]), "rh_forearm": np.array([ # TODO: configure this? [6.34136230e-07, -9.99999683e-01, -7.96326458e-04, 0.00000000e+00], [7.96326458e-04, 7.96326711e-04, -9.99999366e-01, 0.00000000e+00], [9.99999683e-01, 0.00000000e+00, 7.96326711e-04, 0.00000000e+00], - [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) + [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) } urdf_base2kin_base = np.array([ @@ -81,9 +82,12 @@ class UR10Params: def analytical_ik( - pose, last_joint_angles, params=UR5Params, + pose, + last_joint_angles, + params=UR5Params, urdf_base2kin_base=urdf_base2kin_base, - ee_kin2ee=ee_kin2ee_options["ur5_tool0"]): + ee_kin2ee=ee_kin2ee_options["ur5_tool0"] +): """Analytical inverse kinematics for universal robot arms.""" ee_link2urdf_base = pt.transform_from( R=pr.matrix_from_quaternion([pose[-1], pose[3], pose[4], pose[5]]), @@ -94,7 +98,9 @@ def analytical_ik( def analytical_ik_homogeneous( - ee_link2urdf_base, last_joint_angles, params=UR5Params, + ee_link2urdf_base, + last_joint_angles, + params=UR5Params, urdf_base2kin_base=urdf_base2kin_base, ee_kin2ee=ee_kin2ee_options["ur5_tool0"]): """Analytical inverse kinematics for universal robot arms.""" diff --git a/examples/floating_mia_example.py b/examples/floating_mia_example.py index fc3c03b..a25b15e 100644 --- a/examples/floating_mia_example.py +++ b/examples/floating_mia_example.py @@ -1,13 +1,14 @@ import gymnasium from deformable_gym.envs.floating_mia_grasp_env import FloatingMiaGraspEnv + """ ========= Floating Mia Example ========= -This is an example of how to use the FloatingMiaGraspEnv. A random policy is then -used to generate ten episodes. +This is an example of how to use the FloatingMiaGraspEnv. A random policy is +then used to generate ten episodes. """ @@ -17,7 +18,7 @@ episode_return = 0 num_episodes = 0 -while num_episodes <= 10: +while num_episodes < 10: action = env.action_space.sample() @@ -29,6 +30,6 @@ num_episodes += 1 episode_return = 0 - env.reset() + obs, _ = env.reset() env.close() diff --git a/examples/parallel_floating_mia_example.py b/examples/parallel_floating_mia_example.py new file mode 100644 index 0000000..3490612 --- /dev/null +++ b/examples/parallel_floating_mia_example.py @@ -0,0 +1,46 @@ +import gymnasium + +from deformable_gym.envs.floating_mia_grasp_env import FloatingMiaGraspEnv + +SEED = 0 + +env = gymnasium.make("FloatingMiaGraspInsole-v0", gui=False) +env2 = gymnasium.make("FloatingMiaGraspInsole-v0", gui=False) + +obs, info = env.reset(seed=SEED) +num_steps = 0 +episode_return = 0 +num_episodes = 0 + +obs2, info2 = env2.reset(seed=SEED) +num_steps2 = 0 +episode_return2 = 0 +num_episodes2 = 0 + +print(f"{obs=}") + +while num_episodes < 3 and num_episodes2 < 3: + + action = env.action_space.sample() + action2 = env2.action_space.sample() + + obs, reward, terminated, truncated, _ = env.step(action) + num_steps += 1 + obs2, reward2, terminated2, truncated2, _ = env2.step(action2) + num_steps2 += 1 + episode_return += reward + episode_return2 += reward2 + + if terminated or truncated: + num_episodes += 1 + episode_return = 0 + env.reset() + + if terminated2 or truncated2: + num_episodes2 += 1 + episode_return2 = 0 + env2.reset() + +env.close() +env2.close() + diff --git a/setup.py b/setup.py index 1bd6a2f..619932a 100644 --- a/setup.py +++ b/setup.py @@ -6,7 +6,7 @@ long_description = f.read() setup(name='deformable_gym', - version="0.3.0", + version="0.3.1", maintainer='Melvin Laux', maintainer_email='melvin.laux@uni-bremen.de', description='Gym environments for grasping deformable objects', diff --git a/tests/conftest.py b/tests/conftest.py index a08ca31..22b3ea2 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -1,7 +1,35 @@ -import pytest import pybullet as pb +import pytest + from deformable_gym.envs.bullet_simulation import BulletSimulation +MIA_SENSORS = ["j_index_sensor", "j_middle_sensor", "j_thumb_sensor"] +MIA_MOTORS = [ + "j_thumb_fle", "j_thumb_opp", + "j_index_fle", + "j_mrl_fle", "j_ring_fle", "j_little_fle"] + +SHADOW_MOTORS = [ + "rh_WRJ2", "rh_WRJ1", + "rh_THJ5", "rh_THJ4", "rh_THJ3", "rh_THJ2", "rh_THJ1", + "rh_LFJ5", "rh_LFJ4", "rh_LFJ3", "rh_LFJ2", "rh_LFJ1", + "rh_RFJ1", "rh_RFJ4", "rh_RFJ3", "rh_RFJ2", "rh_RFJ1", + "rh_MFJ4", "rh_MFJ3", "rh_MFJ2", "rh_MFJ1", + "rh_FFJ4", "rh_FFJ3", "rh_FFJ2", "rh_FFJ1" +] + +UR5_MOTORS = [ + "ur5_shoulder_pan_joint", "ur5_shoulder_lift_joint", + "ur5_elbow_joint", + "ur5_wrist_1_joint", "ur5_wrist_2_joint", "ur5_wrist_3_joint" +] + +UR10_MOTORS = [ + "ur10_shoulder_pan_joint", "ur10_shoulder_lift_joint", + "ur10_elbow_joint", + "ur10_wrist_1_joint", "ur10_wrist_2_joint", "ur10_wrist_3_joint" +] + @pytest.fixture def simulation(): @@ -12,37 +40,33 @@ def simulation(): @pytest.fixture def mia_motors(): - return ["j_thumb_fle", "j_thumb_opp", "j_index_fle", "j_mrl_fle", "j_ring_fle", "j_little_fle"] + return MIA_MOTORS @pytest.fixture def mia_sensors(): - return ["j_index_sensor", "j_middle_sensor", "j_thumb_sensor"] + return MIA_SENSORS @pytest.fixture def shadow_motors(): - return ["rh_WRJ2", "rh_WRJ1", "rh_THJ5", "rh_THJ4", "rh_THJ3", "rh_THJ2", "rh_THJ1", "rh_LFJ5", "rh_LFJ4", - "rh_LFJ3", "rh_LFJ2", "rh_LFJ1", "rh_RFJ1", "rh_RFJ4", "rh_RFJ3", "rh_RFJ2", "rh_RFJ1", "rh_MFJ4", - "rh_MFJ3", "rh_MFJ2", "rh_MFJ1", "rh_FFJ4", "rh_FFJ3", "rh_FFJ2", "rh_FFJ1"] + return SHADOW_MOTORS @pytest.fixture def ur5_mia_motors(): - return ["ur5_shoulder_pan_joint", "ur5_shoulder_lift_joint", "ur5_elbow_joint", "ur5_wrist_1_joint", - "ur5_wrist_2_joint", "ur5_wrist_3_joint", "j_thumb_fle", "j_thumb_opp", "j_index_fle", "j_mrl_fle", - "j_ring_fle", "j_little_fle"] + all_motors = UR5_MOTORS.copy() + all_motors.extend(MIA_MOTORS.copy()) + return all_motors @pytest.fixture def ur5_mia_sensors(): - return ["j_index_sensor", "j_middle_sensor", "j_thumb_sensor"] + return MIA_SENSORS @pytest.fixture def ur10_shadow_motors(): - return ["ur10_shoulder_pan_joint", "ur10_shoulder_lift_joint", "ur10_elbow_joint", "ur10_wrist_1_joint", - "ur10_wrist_2_joint", "ur10_wrist_3_joint", "rh_WRJ2", "rh_WRJ1", "rh_THJ5", "rh_THJ4", "rh_THJ3", - "rh_THJ2", "rh_THJ1", "rh_LFJ5", "rh_LFJ4", "rh_LFJ3", "rh_LFJ2", "rh_LFJ1", "rh_RFJ1", "rh_RFJ4", - "rh_RFJ3", "rh_RFJ2", "rh_RFJ1", "rh_MFJ4", "rh_MFJ3", "rh_MFJ2", "rh_MFJ1", "rh_FFJ4", "rh_FFJ3", - "rh_FFJ2", "rh_FFJ1"] + all_motors = UR10_MOTORS.copy() + all_motors.extend(SHADOW_MOTORS.copy()) + return all_motors diff --git a/tests/envs/test_floating_mia_grasp_env.py b/tests/envs/test_floating_mia_grasp_env.py index e6fd63f..663c303 100644 --- a/tests/envs/test_floating_mia_grasp_env.py +++ b/tests/envs/test_floating_mia_grasp_env.py @@ -1,7 +1,8 @@ -from numpy.testing import assert_allclose import pytest -from deformable_gym.envs.floating_mia_grasp_env import FloatingMiaGraspEnv from gymnasium.wrappers import RescaleAction +from numpy.testing import assert_allclose + +from deformable_gym.envs.floating_mia_grasp_env import FloatingMiaGraspEnv @pytest.fixture @@ -60,29 +61,32 @@ def test_initial_sensor_info(env: FloatingMiaGraspEnv): def test_episode_reproducibility(): observations = [] termination_flags = [] + actions = [] - env = FloatingMiaGraspEnv(verbose=False, - horizon=10, - gui=False, - observable_object_pos=True, - object_name="insole_on_conveyor_belt/back", - difficulty_mode="hard", - ) - env = RescaleAction(env, 0., 1.) - - env.action_space.seed(SEED) + env = FloatingMiaGraspEnv( + verbose=False, + horizon=3, + gui=False, + object_name="insole_on_conveyor_belt/back", + ) for _ in range(2): observation, _ = env.reset(seed=SEED) - observations.append(observation) + env.action_space.seed(SEED) + + observations.append([observation]) terminated = False + termination_flags.append([terminated]) + actions.append([]) while not terminated: action = env.action_space.sample() + actions[-1].append(action) observation, reward, terminated, truncated, info = env.step(action) - observations.append(observation) - termination_flags.append(terminated) + observations[-1].append(observation) + termination_flags[-1].append(terminated) + assert_allclose(actions[0], actions[1]) assert_allclose(observations[0], observations[1]) assert_allclose(termination_flags[0], termination_flags[1]) diff --git a/tests/envs/test_floating_shadow_grasp_env.py b/tests/envs/test_floating_shadow_grasp_env.py index 658c91f..01ec622 100644 --- a/tests/envs/test_floating_shadow_grasp_env.py +++ b/tests/envs/test_floating_shadow_grasp_env.py @@ -1,7 +1,9 @@ import pytest -from deformable_gym.envs.floating_shadow_grasp_env import FloatingShadowGraspEnv from numpy.testing import assert_allclose +from deformable_gym.envs.floating_shadow_grasp_env import \ + FloatingShadowGraspEnv + SEED = 42 diff --git a/tests/envs/test_parallel_envs.py b/tests/envs/test_parallel_envs.py new file mode 100644 index 0000000..76837b6 --- /dev/null +++ b/tests/envs/test_parallel_envs.py @@ -0,0 +1,57 @@ +import gymnasium + +SEED = 0 + + +def test_parallel_envs(): + env = gymnasium.make( + "FloatingMiaGraspInsole-v0", + gui=False, + horizon=10 + ) + env2 = gymnasium.make( + "FloatingMiaGraspInsole-v0", + gui=False, + horizon=10 + ) + + obs, info = env.reset(seed=SEED) + num_steps = 0 + episode_return = 0 + num_episodes = 0 + + obs2, info2 = env2.reset(seed=SEED) + num_steps2 = 0 + episode_return2 = 0 + num_episodes2 = 0 + + print(f"{obs=}") + + while num_episodes < 3 and num_episodes2 < 3: + + action = env.action_space.sample() + action2 = env2.action_space.sample() + + obs, reward, terminated, truncated, _ = env.step(action) + num_steps += 1 + obs2, reward2, terminated2, truncated2, _ = env2.step(action2) + num_steps2 += 1 + episode_return += reward + episode_return2 += reward2 + + if terminated or truncated: + assert episode_return == -1 + num_episodes += 1 + episode_return = 0 + env.reset() + + if terminated2 or truncated2: + assert episode_return2 == -1 + num_episodes2 += 1 + episode_return2 = 0 + env2.reset() + + env.close() + env2.close() + + assert num_steps == num_steps2 diff --git a/tests/envs/test_ur10_shadow_grasp_env.py b/tests/envs/test_ur10_shadow_grasp_env.py index 79249bd..874fb91 100644 --- a/tests/envs/test_ur10_shadow_grasp_env.py +++ b/tests/envs/test_ur10_shadow_grasp_env.py @@ -1,4 +1,5 @@ import pytest + from deformable_gym.envs.ur10_shadow_grasp_env import UR10ShadowGraspEnv diff --git a/tests/envs/test_ur5_mia_grasp_env.py b/tests/envs/test_ur5_mia_grasp_env.py index 9d9d1a8..1f1ac51 100644 --- a/tests/envs/test_ur5_mia_grasp_env.py +++ b/tests/envs/test_ur5_mia_grasp_env.py @@ -1,4 +1,5 @@ import pytest + from deformable_gym.envs.ur5_mia_grasp_env import UR5MiaGraspEnv diff --git a/tests/objects/test_box.py b/tests/objects/test_box.py index 641d146..c628ca5 100644 --- a/tests/objects/test_box.py +++ b/tests/objects/test_box.py @@ -1,17 +1,18 @@ import numpy as np import pybullet as pb import pytest -from deformable_gym.envs.bullet_simulation import BulletSimulation -from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject from numpy.testing import assert_array_almost_equal +from deformable_gym.envs.bullet_simulation import BulletSimulation +from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) def test_box(simulation): - obj, _, _ = ObjectFactory().create("box", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "box", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_capsule.py b/tests/objects/test_capsule.py index 2e38d3c..062d87d 100644 --- a/tests/objects/test_capsule.py +++ b/tests/objects/test_capsule.py @@ -1,14 +1,15 @@ import numpy as np -from deformable_gym.objects.bullet_object import ObjectFactory from numpy.testing import assert_array_almost_equal +from deformable_gym.objects.bullet_object import ObjectFactory TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) def test_capsule_creation(simulation): - obj, _, _ = ObjectFactory().create("capsule", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "capsule", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_cylinder.py b/tests/objects/test_cylinder.py index 08aae3f..f7a4a7c 100644 --- a/tests/objects/test_cylinder.py +++ b/tests/objects/test_cylinder.py @@ -1,14 +1,15 @@ import numpy as np -from deformable_gym.objects.bullet_object import ObjectFactory from numpy.testing import assert_array_almost_equal +from deformable_gym.objects.bullet_object import ObjectFactory TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) def test_cylinder_creation(simulation): - obj, _, _ = ObjectFactory().create("cylinder", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "cylinder", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_insole.py b/tests/objects/test_insole.py index db731ce..aa4c56a 100644 --- a/tests/objects/test_insole.py +++ b/tests/objects/test_insole.py @@ -1,17 +1,18 @@ import numpy as np import pybullet as pb import pytest -from deformable_gym.envs.bullet_simulation import BulletSimulation -from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject from numpy.testing import assert_array_almost_equal +from deformable_gym.envs.bullet_simulation import BulletSimulation +from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) def test_insole_creation(simulation): - obj, _, _ = ObjectFactory().create("insole", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "insole", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0]), decimal=1) diff --git a/tests/objects/test_insole_on_conveyor.py b/tests/objects/test_insole_on_conveyor.py index b0e7a36..7f82884 100644 --- a/tests/objects/test_insole_on_conveyor.py +++ b/tests/objects/test_insole_on_conveyor.py @@ -1,10 +1,8 @@ import numpy as np -import pybullet as pb import pytest -from deformable_gym.envs.bullet_simulation import BulletSimulation -from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject from numpy.testing import assert_array_almost_equal +from deformable_gym.objects.bullet_object import ObjectFactory TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) @@ -12,9 +10,10 @@ @pytest.mark.skip("TODO") def test_insole_ob_conveyor_creation(simulation): - obj, _, _ = ObjectFactory().create("insole_on_conveyor_belt/back", - object_position=TEST_POS, - object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "insole_on_conveyor_belt/back", + object_position=TEST_POS, + object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_pillow.py b/tests/objects/test_pillow.py index f37e8ab..542fbf3 100644 --- a/tests/objects/test_pillow.py +++ b/tests/objects/test_pillow.py @@ -1,17 +1,17 @@ import numpy as np import pybullet as pb import pytest -from deformable_gym.envs.bullet_simulation import BulletSimulation -from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject from numpy.testing import assert_array_almost_equal +from deformable_gym.envs.bullet_simulation import BulletSimulation +from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) def test_pillow(simulation): - obj, _, _ = ObjectFactory().create("pillow_small", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create("pillow_small", object_position=TEST_POS, object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0]), decimal=1) diff --git a/tests/objects/test_sphere.py b/tests/objects/test_sphere.py index 512c0e6..e4c6233 100644 --- a/tests/objects/test_sphere.py +++ b/tests/objects/test_sphere.py @@ -1,17 +1,20 @@ import numpy as np import pybullet as pb import pytest -from deformable_gym.envs.bullet_simulation import BulletSimulation -from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject from numpy.testing import assert_array_almost_equal +from deformable_gym.envs.bullet_simulation import BulletSimulation +from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) def test_sphere(simulation): - obj, _, _ = ObjectFactory().create("sphere", object_position=TEST_POS, object_orientation=TEST_ORN) + obj, _, _ = ObjectFactory(simulation.pb_client).create( + "sphere", + object_position=TEST_POS, + object_orientation=TEST_ORN) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/objects/test_urdf_object.py b/tests/objects/test_urdf_object.py index 6cbe752..216c130 100644 --- a/tests/objects/test_urdf_object.py +++ b/tests/objects/test_urdf_object.py @@ -1,18 +1,22 @@ import numpy as np import pybullet as pb import pytest -from deformable_gym.envs.bullet_simulation import BulletSimulation -from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject from numpy.testing import assert_array_almost_equal +from deformable_gym.envs.bullet_simulation import BulletSimulation +from deformable_gym.objects.bullet_object import ObjectFactory, UrdfObject TEST_POS = np.array([0, 0, 1]) TEST_ORN = np.array([0, 0, 0]) def test_urdf_object_creation(simulation): - obj = UrdfObject("plane.urdf", world_pos=TEST_POS, world_orn=TEST_ORN, fixed=True, - client_id=simulation.get_physics_client_id()) + obj = UrdfObject( + "plane.urdf", + simulation.pb_client, + world_pos=TEST_POS, + world_orn=TEST_ORN, + fixed=True) pose = obj.get_pose() assert_array_almost_equal(pose, np.array([0, 0, 1, 1, 0, 0, 0])) diff --git a/tests/robots/test_mia_hand_position.py b/tests/robots/test_mia_hand_position.py index 53322dc..c3beffa 100644 --- a/tests/robots/test_mia_hand_position.py +++ b/tests/robots/test_mia_hand_position.py @@ -1,17 +1,21 @@ import numpy as np -from deformable_gym.robots.mia_hand import MiaHandPosition -from numpy.testing import assert_almost_equal - +import pybullet as pb import pytest +from numpy.testing import assert_almost_equal +from deformable_gym.robots.mia_hand import MiaHandPosition TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +TEST_ORN = pb.getQuaternionFromEuler(np.array([0, 0, 0])) @pytest.fixture -def robot(): - robot = MiaHandPosition(world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) +def robot(simulation): + robot = MiaHandPosition( + pb_client=simulation.pb_client, + world_pos=TEST_POS, + world_orn=TEST_ORN, + base_commands=True) robot.set_thumb_opp(thumb_adducted=True) return robot diff --git a/tests/robots/test_mia_hand_velocity.py b/tests/robots/test_mia_hand_velocity.py index f5c8a82..6b6672d 100644 --- a/tests/robots/test_mia_hand_velocity.py +++ b/tests/robots/test_mia_hand_velocity.py @@ -1,17 +1,21 @@ import numpy as np -from deformable_gym.robots.mia_hand import MiaHandVelocity -from numpy.testing import assert_almost_equal - +import pybullet as pb import pytest +from numpy.testing import assert_almost_equal +from deformable_gym.robots.mia_hand import MiaHandVelocity TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +TEST_ORN = pb.getQuaternionFromEuler(np.array([0, 0, 0])) @pytest.fixture -def robot(): - robot = MiaHandVelocity(world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) +def robot(simulation): + robot = MiaHandVelocity( + pb_client=simulation.pb_client, + world_pos=TEST_POS, + world_orn=TEST_ORN, + base_commands=True) robot.set_thumb_opp(thumb_adducted=True) return robot diff --git a/tests/robots/test_shadow_hand_position.py b/tests/robots/test_shadow_hand_position.py index 0fd2fb2..9fe7edc 100644 --- a/tests/robots/test_shadow_hand_position.py +++ b/tests/robots/test_shadow_hand_position.py @@ -1,17 +1,21 @@ import numpy as np -from deformable_gym.robots.shadow_hand import ShadowHandPosition -from numpy.testing import assert_almost_equal - +import pybullet as pb import pytest +from numpy.testing import assert_almost_equal +from deformable_gym.robots.shadow_hand import ShadowHandPosition TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +TEST_ORN = pb.getQuaternionFromEuler(np.array([0, 0, 0])) @pytest.fixture -def robot(): - robot = ShadowHandPosition(world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) +def robot(simulation): + robot = ShadowHandPosition( + pb_client=simulation.pb_client, + world_pos=TEST_POS, + world_orn=TEST_ORN, + base_commands=True) return robot diff --git a/tests/robots/test_shadow_hand_velocity.py b/tests/robots/test_shadow_hand_velocity.py index 3f259d9..bed9aff 100644 --- a/tests/robots/test_shadow_hand_velocity.py +++ b/tests/robots/test_shadow_hand_velocity.py @@ -1,19 +1,21 @@ import numpy as np import pybullet as pb -from deformable_gym.envs.bullet_simulation import BulletSimulation -from deformable_gym.robots.shadow_hand import ShadowHandVelocity -from numpy.testing import assert_almost_equal - import pytest +from numpy.testing import assert_almost_equal +from deformable_gym.robots.shadow_hand import ShadowHandVelocity TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +TEST_ORN = pb.getQuaternionFromEuler(np.array([0, 0, 0])) @pytest.fixture -def robot(): - robot = ShadowHandVelocity(world_pos=TEST_POS, world_orn=TEST_ORN, base_commands=True) +def robot(simulation): + robot = ShadowHandVelocity( + pb_client=simulation.pb_client, + world_pos=TEST_POS, + world_orn=TEST_ORN, + base_commands=True) return robot diff --git a/tests/robots/test_ur10_shadow_position.py b/tests/robots/test_ur10_shadow_position.py index b5926a1..d493156 100644 --- a/tests/robots/test_ur10_shadow_position.py +++ b/tests/robots/test_ur10_shadow_position.py @@ -1,19 +1,13 @@ import numpy as np -import pybullet as pb -from deformable_gym.robots.ur10_shadow import UR10ShadowPosition -from numpy.testing import assert_almost_equal - import pytest +from numpy.testing import assert_almost_equal - -TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +from deformable_gym.robots.ur10_shadow import UR10ShadowPosition @pytest.fixture -def robot(): - robot = UR10ShadowPosition() - +def robot(simulation): + robot = UR10ShadowPosition(pb_client=simulation.pb_client) return robot diff --git a/tests/robots/test_ur10_shadow_velocity.py b/tests/robots/test_ur10_shadow_velocity.py index 8fc1527..ad8cdac 100644 --- a/tests/robots/test_ur10_shadow_velocity.py +++ b/tests/robots/test_ur10_shadow_velocity.py @@ -1,17 +1,14 @@ import numpy as np -from deformable_gym.robots.ur10_shadow import UR10ShadowVelocity -from numpy.testing import assert_almost_equal - import pytest +from numpy.testing import assert_almost_equal - -TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +from deformable_gym.robots.ur10_shadow import UR10ShadowVelocity @pytest.fixture -def robot(): - robot = UR10ShadowVelocity() +def robot(simulation): + robot = UR10ShadowVelocity(pb_client=simulation.pb_client) + simulation.add_robot(robot) return robot @@ -19,13 +16,12 @@ def robot(): @pytest.mark.skip("TODO") def test_ur10_shadow_velocity_creation(simulation, robot): - simulation.add_robot(robot) - actual_pose = np.concatenate(robot.multibody_pose.get_pose()) expected_pose = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]) assert_almost_equal(actual_pose, expected_pose) -def test_ur10_shadow_velocity_motor_creation(simulation, robot, ur10_shadow_motors): +def test_ur10_shadow_velocity_motor_creation( + simulation, robot, ur10_shadow_motors): # check motor creation assert set(robot.motors.keys()) == set(ur10_shadow_motors) diff --git a/tests/robots/test_ur5_mia_position.py b/tests/robots/test_ur5_mia_position.py index f802091..d8b7eba 100644 --- a/tests/robots/test_ur5_mia_position.py +++ b/tests/robots/test_ur5_mia_position.py @@ -1,17 +1,13 @@ import numpy as np -from deformable_gym.robots.ur5_mia import UR5MiaPosition -from numpy.testing import assert_almost_equal - import pytest +from numpy.testing import assert_almost_equal - -TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +from deformable_gym.robots.ur5_mia import UR5MiaPosition @pytest.fixture -def robot(): - robot = UR5MiaPosition() +def robot(simulation): + robot = UR5MiaPosition(pb_client=simulation.pb_client) return robot diff --git a/tests/robots/test_ur5_mia_velocity.py b/tests/robots/test_ur5_mia_velocity.py index fe41c5f..b40b356 100644 --- a/tests/robots/test_ur5_mia_velocity.py +++ b/tests/robots/test_ur5_mia_velocity.py @@ -1,18 +1,13 @@ import numpy as np -import pybullet as pb -from deformable_gym.robots.ur5_mia import UR5MiaVelocity -from numpy.testing import assert_almost_equal - import pytest +from numpy.testing import assert_almost_equal - -TEST_POS = np.array([0, 0, 1]) -TEST_ORN = np.array([0, 0, 0]) +from deformable_gym.robots.ur5_mia import UR5MiaVelocity @pytest.fixture -def robot(): - robot = UR5MiaVelocity() +def robot(simulation): + robot = UR5MiaVelocity(simulation.pb_client) return robot @@ -27,7 +22,8 @@ def test_ur5_mia_velocity_creation(simulation, robot): robot.perform_command(np.array([0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) -def test_ur5_mia_velocity_motor_creation(simulation, robot, ur5_mia_motors, ur5_mia_sensors): +def test_ur5_mia_velocity_motor_creation( + simulation, robot, ur5_mia_motors, ur5_mia_sensors): # check motor creation assert set(robot.motors.keys()) == set(ur5_mia_motors) # check sensor creation