diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..5ab1c75 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,16 @@ +exclude: ^(object_data/|robots/|doc/) +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-yaml + - id: end-of-file-fixer + - id: trailing-whitespace +- repo: https://github.com/psf/black + rev: 24.4.2 + hooks: + - id: black +- repo: https://github.com/PyCQA/isort + rev: 5.13.2 + hooks: + - id: isort diff --git a/README.md b/README.md index 6d44391..888be3f 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ -# DeformableGym - [![Tests](https://github.com/dfki-ric/deformable_gym/actions/workflows/test.yaml/badge.svg)](https://github.com/dfki-ric/deformable_gym/actions/workflows/test.yaml) +[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) +[![pre-commit](https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit)](https://github.com/pre-commit/pre-commit) +# DeformableGym This repository contains a collection of [gymnasium](https://github.com/Farama-Foundation/Gymnasium) environments built with [PyBullet](https://pybullet.org/). In these environments, the agent needs to learn to grasp deformable object such as shoe insoles or pillows from sparse reward signals. diff --git a/deformable_gym/__init__.py b/deformable_gym/__init__.py index 219c544..6b6b848 100644 --- a/deformable_gym/__init__.py +++ b/deformable_gym/__init__.py @@ -3,13 +3,17 @@ register( id="FloatingMiaGraspInsole-v0", entry_point="deformable_gym.envs.floating_mia_grasp_env:FloatingMiaGraspEnv", - kwargs={"object_name": "insole_on_conveyor_belt/back", - "observable_object_pos": True} + kwargs={ + "object_name": "insole_on_conveyor_belt/back", + "observable_object_pos": True, + }, ) register( id="FloatingShadowGraspInsole-v0", entry_point="deformable_gym.envs.floating_shadow_grasp_env:FloatingShadowGraspEnv", - kwargs={"object_name": "insole_on_conveyor_belt/back", - "observable_object_pos": True} + kwargs={ + "object_name": "insole_on_conveyor_belt/back", + "observable_object_pos": True, + }, ) diff --git a/deformable_gym/envs/__init__.py b/deformable_gym/envs/__init__.py index 5935bd5..ecdef63 100644 --- a/deformable_gym/envs/__init__.py +++ b/deformable_gym/envs/__init__.py @@ -1,4 +1,5 @@ """Deformable Gym environments.""" + from .base_env import BaseBulletEnv from .floating_mia_grasp_env import FloatingMiaGraspEnv from .floating_shadow_grasp_env import FloatingShadowGraspEnv @@ -10,5 +11,5 @@ "FloatingMiaGraspEnv", "FloatingShadowGraspEnv", "UR5MiaGraspEnv", - "UR10ShadowGraspEnv" + "UR10ShadowGraspEnv", ] diff --git a/deformable_gym/envs/base_env.py b/deformable_gym/envs/base_env.py index 45d5a49..16ee30a 100644 --- a/deformable_gym/envs/base_env.py +++ b/deformable_gym/envs/base_env.py @@ -26,6 +26,7 @@ class BaseBulletEnv(gym.Env, abc.ABC): :param verbose_dt: Time after which simulation info should be printed. :param pybullet_options: Options to pass to pybullet.connect(). """ + gui: bool verbose: bool horizon: int @@ -37,15 +38,16 @@ class BaseBulletEnv(gym.Env, abc.ABC): action_space: spaces.Box def __init__( - self, - gui: bool = True, - real_time: bool = False, - horizon: int = 100, - soft: bool = False, - verbose: bool = False, - time_delta: float = 0.001, - verbose_dt: float = 10.00, - pybullet_options: str = ""): + self, + gui: bool = True, + real_time: bool = False, + horizon: int = 100, + soft: bool = False, + verbose: bool = False, + time_delta: float = 0.001, + verbose_dt: float = 10.00, + pybullet_options: str = "", + ): self.gui = gui self.verbose = verbose @@ -59,7 +61,8 @@ def __init__( real_time=real_time, mode=mode, verbose_dt=verbose_dt, - pybullet_options=pybullet_options) + pybullet_options=pybullet_options, + ) self.pb_client = self.simulation.pb_client @@ -75,9 +78,7 @@ def _create_robot(self): def _load_objects(self): """Load objects to PyBullet simulation.""" self.plane = self.pb_client.loadURDF( - "plane.urdf", - (0, 0, 0), - useFixedBase=1 + "plane.urdf", (0, 0, 0), useFixedBase=1 ) def _hard_reset(self): @@ -140,11 +141,11 @@ def _get_observation(self) -> npt.ArrayLike: return self.robot.get_joint_positions() def _get_info( - self, - observation: npt.ArrayLike = None, - action: npt.ArrayLike = None, - reward: float = None, - next_observation: npt.ArrayLike = None + self, + observation: npt.ArrayLike = None, + action: npt.ArrayLike = None, + reward: float = None, + next_observation: npt.ArrayLike = None, ) -> dict: """Returns the current environment state. @@ -153,10 +154,10 @@ def _get_info( return {} def _is_terminated( - self, - observation: npt.ArrayLike, - action: npt.ArrayLike, - next_observation: npt.ArrayLike + self, + observation: npt.ArrayLike, + action: npt.ArrayLike, + next_observation: npt.ArrayLike, ) -> bool: """Checks whether the current episode is terminated. @@ -168,10 +169,10 @@ def _is_terminated( return self.step_counter >= self.horizon def _is_truncated( - self, - state: npt.ArrayLike, - action: npt.ArrayLike, - next_state: npt.ArrayLike + self, + state: npt.ArrayLike, + action: npt.ArrayLike, + next_state: npt.ArrayLike, ) -> bool: """Checks whether the current episode is truncated. @@ -216,16 +217,19 @@ def step(self, action: npt.ArrayLike): # calculate the reward reward = self.calculate_reward( - observation, action, next_observation, terminated) + observation, action, next_observation, terminated + ) info = self._get_info(observation, action, reward) if self.verbose: - print(f"Finished environment step: " - f"{next_observation=}, " - f"{reward=}, " - f"{terminated=}, " - f"{truncated=}") + print( + f"Finished environment step: " + f"{next_observation=}, " + f"{reward=}, " + f"{terminated=}, " + f"{truncated=}" + ) return next_observation, reward, terminated, truncated, info @@ -234,11 +238,11 @@ def close(self): @abc.abstractmethod def calculate_reward( - self, - state: npt.ArrayLike, - action: npt.ArrayLike, - next_state: npt.ArrayLike, - terminated: bool + self, + state: npt.ArrayLike, + action: npt.ArrayLike, + next_state: npt.ArrayLike, + terminated: bool, ) -> float: """Calculate reward. @@ -295,7 +299,8 @@ def get_object_pose(self): quaternion: (x, y, z, qw, qx, qy, qz) """ return MultibodyPose.internal_pose_to_external_pose( - np.hstack((self.object_position, self.object_orientation))) + np.hstack((self.object_position, self.object_orientation)) + ) class FloatingHandMixin: @@ -309,11 +314,10 @@ def _init_hand_pose(self, robot: BulletRobot): """ desired_robot2world_pos = self.hand_world_pose[:3] desired_robot2world_orn = pb.getQuaternionFromEuler( - self.hand_world_pose[3:]) + self.hand_world_pose[3:] + ) self.multibody_pose = MultibodyPose( - robot.get_id(), - desired_robot2world_pos, - desired_robot2world_orn + robot.get_id(), desired_robot2world_pos, desired_robot2world_orn ) def set_world_pose(self, world_pose): @@ -323,13 +327,15 @@ def set_world_pose(self, world_pose): quaternion: (x, y, z, qw, qx, qy, qz) """ self.hand_world_pose = MultibodyPose.external_pose_to_internal_pose( - world_pose) + world_pose + ) desired_robot2world_pos = self.hand_world_pose[:3] desired_robot2world_orn = pb.getQuaternionFromEuler( - self.hand_world_pose[3:]) + self.hand_world_pose[3:] + ) self.multibody_pose.set_pose( - desired_robot2world_pos, - desired_robot2world_orn) + 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 94a3965..48b5941 100644 --- a/deformable_gym/envs/bullet_simulation.py +++ b/deformable_gym/envs/bullet_simulation.py @@ -1,9 +1,9 @@ import pybullet as pb import pybullet_data - from pybullet_utils import bullet_client as bc -from ..robots.bullet_robot import BulletRobot + from ..helpers import pybullet_helper as pbh +from ..robots.bullet_robot import BulletRobot class BulletSimulation: @@ -18,15 +18,17 @@ class BulletSimulation: :param pybullet_options: Options that should be passed to PyBullet 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, - pybullet_options: str = ""): + 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 self.mode = mode @@ -36,8 +38,7 @@ def __init__( with pbh.stdout_redirected(): self.pb_client = bc.BulletClient( - connection_mode=self.mode, - options=pybullet_options + connection_mode=self.mode, options=pybullet_options ) self.pb_client.setAdditionalSearchPath(pybullet_data.getDataPath()) @@ -107,13 +108,13 @@ def disconnect(self) -> None: class BulletTiming: """This class handles all timing issues for a single BulletSimulation.""" + def __init__( - self, - pb_client: bc.BulletClient, - dt: float = 0.001, - verbose_dt: float = 0.01, + self, + pb_client: bc.BulletClient, + dt: float = 0.001, + verbose_dt: float = 0.01, ): - """ Create new BulletTiming instance. @@ -146,7 +147,9 @@ def add_subsystem(self, name, frequency, callback=None): """ if name not in self.subsystems.keys(): self.subsystems[name] = ( - max(1, round(1.0/frequency/self.dt)), callback) + max(1, round(1.0 / frequency / self.dt)), + callback, + ) def remove_subsystem(self, name): """ @@ -182,8 +185,10 @@ def step(self): self.sim_time += self.dt if (self.sim_time % self.verbose_dt) < self.dt: - print(f"Step: {self.time_step}, Time: {self.sim_time}, " - f"Triggers: {triggers}") + print( + f"Step: {self.time_step}, Time: {self.sim_time}, " + f"Triggers: {triggers}" + ) def reset(self): self.time_step = 0 @@ -192,13 +197,14 @@ def reset(self): class BulletCamera: """This class handles all camera operations for one BulletSimulation.""" + def __init__( - self, - pb_client: bc.BulletClient, - position: tuple = (0, 0, 0), - pitch: int = -52, - yaw: int = 30, - distance: int = 3, + 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 @@ -209,12 +215,15 @@ def __init__( self._active = False self._logging_id = None - self.pb_client.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 = self.pb_client.startStateLogging( - pb.STATE_LOGGING_VIDEO_MP4, path) + pb.STATE_LOGGING_VIDEO_MP4, path + ) return self._logging_id else: return None @@ -230,4 +239,5 @@ def reset(self, position, pitch, yaw, distance): self.distance = distance self.pb_client.resetDebugVisualizerCamera( - distance, yaw, pitch, position) + 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 c68857b..779fada 100644 --- a/deformable_gym/envs/floating_mia_grasp_env.py +++ b/deformable_gym/envs/floating_mia_grasp_env.py @@ -35,14 +35,15 @@ class FloatingMiaGraspEnv(GraspEnv): """ INITIAL_POSE = np.r_[ - 0.03, -0.025, 1.0, pb.getQuaternionFromEuler([-np.pi/8, np.pi, 0])] + 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 + "j_thumb_fle": 0.0, } _FINGERS_HALFWAY_CLOSED = { @@ -50,7 +51,7 @@ class FloatingMiaGraspEnv(GraspEnv): "j_little_fle": 0.5, "j_mrl_fle": 0.5, "j_ring_fle": 0.5, - "j_thumb_fle": 0.3 + "j_thumb_fle": 0.3, } _FINGERS_CLOSED = { @@ -58,18 +59,19 @@ class FloatingMiaGraspEnv(GraspEnv): "j_little_fle": 0.71, "j_mrl_fle": 0.71, "j_ring_fle": 0.71, - "j_thumb_fle": 0.3 + "j_thumb_fle": 0.3, } - _MAX_POS_OFFSET = .0005 - _MAX_ORN_OFFSET = .000005 + _MAX_POS_OFFSET = 0.0005 + _MAX_ORN_OFFSET = 0.000005 def __init__( - self, - object_name: str = "insole", - object_scale: float = 1.0, - observable_object_pos: bool = False, - **kwargs): + self, + object_name: str = "insole", + object_scale: float = 1.0, + observable_object_pos: bool = False, + **kwargs, + ): self.velocity_commands = False @@ -77,7 +79,8 @@ def __init__( object_name=object_name, object_scale=object_scale, observable_object_pos=observable_object_pos, - **kwargs) + **kwargs, + ) self.hand_world_pose = self.INITIAL_POSE self.robot = self._create_robot() @@ -85,47 +88,50 @@ def __init__( limits = pbh.get_limit_array(self.robot.motors.values()) self.actuated_finger_ids = np.array([0, 1, 5], dtype=int) - lower_observations = np.concatenate([ - np.array([-2, -2, 0]), - -np.ones(4), - limits[0][self.actuated_finger_ids], - -np.full(6, 10)]) + lower_observations = np.concatenate( + [ + np.array([-2, -2, 0]), + -np.ones(4), + limits[0][self.actuated_finger_ids], + -np.full(6, 10), + ] + ) - upper_observations = np.concatenate([ - np.array([2, 2, 2]), - np.ones(4), - limits[1][self.actuated_finger_ids], - np.full(6, 10.)]) + upper_observations = np.concatenate( + [ + np.array([2, 2, 2]), + np.ones(4), + limits[1][self.actuated_finger_ids], + np.full(6, 10.0), + ] + ) 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.0)) + upper_observations = np.append(upper_observations, np.full(3, 2.0)) self.observation_space = spaces.Box( - low=lower_observations, - high=upper_observations, - dtype=np.float64 + low=lower_observations, high=upper_observations, dtype=np.float64 ) # 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] + 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] + 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) + dtype=np.float64, + ) def _create_robot(self): orn_limit = None @@ -137,7 +143,8 @@ def _create_robot(self): world_orn=self.hand_world_pose[3:], verbose=self.verbose, orn_limit=orn_limit, - base_commands=True) + base_commands=True, + ) else: robot = mia_hand.MiaHandPosition( self.pb_client, @@ -145,7 +152,8 @@ def _create_robot(self): world_orn=self.hand_world_pose[3:], verbose=self.verbose, orn_limit=orn_limit, - base_commands=True) + base_commands=True, + ) self.simulation.add_robot(robot) @@ -153,7 +161,8 @@ def _create_robot(self): def _get_observation(self): joint_pos = self.robot.get_joint_positions( - self.robot.actuated_real_joints) + self.robot.actuated_real_joints + ) ee_pose = self.robot.get_ee_pose() sensor_readings = self.robot.get_sensor_readings() diff --git a/deformable_gym/envs/floating_shadow_grasp_env.py b/deformable_gym/envs/floating_shadow_grasp_env.py index 890d8d3..41e4ed1 100644 --- a/deformable_gym/envs/floating_shadow_grasp_env.py +++ b/deformable_gym/envs/floating_shadow_grasp_env.py @@ -27,58 +27,65 @@ class FloatingShadowGraspEnv(GraspEnv): """ INITIAL_POSE = np.r_[ - 0, -0.5, 1, pb.getQuaternionFromEuler([-np.pi/2, np.pi, 0])] + 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, + object_name: str = "insole", + object_scale: float = 1.0, + observable_object_pos: bool = False, + **kwargs, + ): self.velocity_commands = False super().__init__( object_name=object_name, object_scale=object_scale, observable_object_pos=observable_object_pos, - **kwargs) + **kwargs, + ) self.hand_world_pose = self.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([-10, -10, -10])], axis=0) - - upper_observations = np.concatenate([ - np.array([2, 2, 2]), - np.ones(4), - limits[1][6:], - np.array([10, 10, 10])], axis=0) + lower_observations = np.concatenate( + [ + 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([10, 10, 10]), + ], + axis=0, + ) 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.0)) + upper_observations = np.append(upper_observations, np.full(3, 2.0)) 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]], axis=0) + lower_actions = np.concatenate([-np.ones(7) * 0.01, limits[0]], axis=0) - upper_actions = np.concatenate([ - np.ones(7)*0.01, limits[1]], axis=0) + upper_actions = np.concatenate([np.ones(7) * 0.01, limits[1]], axis=0) self.action_space = spaces.Box( - low=lower_actions, high=upper_actions, dtype=np.float64) + low=lower_actions, high=upper_actions, dtype=np.float64 + ) def _create_robot(self): task_space_limit = None @@ -92,7 +99,8 @@ def _create_robot(self): task_space_limit=task_space_limit, verbose=self.verbose, orn_limit=orn_limit, - base_commands=True) + base_commands=True, + ) else: robot = shadow_hand.ShadowHandPosition( pb_client=self.pb_client, @@ -101,7 +109,8 @@ def _create_robot(self): task_space_limit=task_space_limit, verbose=self.verbose, orn_limit=orn_limit, - base_commands=True) + base_commands=True, + ) self.simulation.add_robot(robot) diff --git a/deformable_gym/envs/grasp_env.py b/deformable_gym/envs/grasp_env.py index 9e6c54d..5d10872 100644 --- a/deformable_gym/envs/grasp_env.py +++ b/deformable_gym/envs/grasp_env.py @@ -1,26 +1,26 @@ from abc import ABC +from typing import Optional import numpy as np import pybullet as pb from ..envs.base_env import BaseBulletEnv, GraspDeformableMixin -from ..envs.sampler import Sampler, FixedSampler -from typing import Optional - +from ..envs.sampler import FixedSampler, Sampler from ..objects.bullet_object import ObjectFactory class GraspEnv(BaseBulletEnv, GraspDeformableMixin, ABC): INITIAL_POSE = np.r_[ - 0.03, -0.025, 1.0, pb.getQuaternionFromEuler([-np.pi / 8, np.pi, 0])] + 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, - initial_state_sampler: Optional[Sampler] = None, - **kwargs + self, + object_name: str = "insole", + object_scale: float = 1.0, + observable_object_pos: bool = False, + initial_state_sampler: Optional[Sampler] = None, + **kwargs, ): self.object_name = object_name self.object_scale = object_scale @@ -38,10 +38,11 @@ def __init__( def _load_objects(self): super()._load_objects() - (self.object_to_grasp, - self.object_position, - self.object_orientation) = ObjectFactory(self.pb_client).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): diff --git a/deformable_gym/envs/sampler.py b/deformable_gym/envs/sampler.py index d33ac89..44f21ca 100644 --- a/deformable_gym/envs/sampler.py +++ b/deformable_gym/envs/sampler.py @@ -1,11 +1,11 @@ -from typing import Protocol, Union, Optional -import numpy.typing as npt +from typing import Optional, Protocol, Union + import numpy as np +import numpy.typing as npt class Sampler(Protocol): - def sample_initial_pose(self) -> npt.NDArray: - ... + def sample_initial_pose(self) -> npt.NDArray: ... class FixedSampler(Sampler): @@ -18,10 +18,10 @@ def sample_initial_pose(self) -> npt.NDArray: class GaussianSampler(Sampler): def __init__( - self, - mu: npt.ArrayLike, - sigma: npt.ArrayLike, - seed: Optional[int] = None + self, + mu: npt.ArrayLike, + sigma: npt.ArrayLike, + seed: Optional[int] = None, ): self.mu = np.array(mu) self.sigma = np.array(sigma) @@ -33,10 +33,10 @@ def sample_initial_pose(self) -> npt.NDArray: class UniformSampler(Sampler): def __init__( - self, - low: npt.ArrayLike, - high: npt.ArrayLike, - seed: Optional[int] = None + self, + low: npt.ArrayLike, + high: npt.ArrayLike, + seed: Optional[int] = None, ): self.low = np.array(low) self.high = np.array(high) @@ -48,11 +48,11 @@ def sample_initial_pose(self) -> npt.NDArray: class GaussianCurriculumSampler(Sampler): def __init__( - self, - mu: npt.ArrayLike, - sigma: npt.ArrayLike, - step_size: Union[float, npt.ArrayLike] = 1e-3, - seed: Optional[int] = None + self, + mu: npt.ArrayLike, + sigma: npt.ArrayLike, + step_size: Union[float, npt.ArrayLike] = 1e-3, + seed: Optional[int] = None, ): self.mu = np.array(mu) self.sigma = np.array(sigma) @@ -66,11 +66,11 @@ def sample_initial_pose(self) -> npt.NDArray: class UniformCurriculumSampler(Sampler): def __init__( - self, - low: npt.ArrayLike, - high: npt.ArrayLike, - step_size: Union[float, npt.ArrayLike] = 1e-3, - seed: Optional[int] = None + self, + low: npt.ArrayLike, + high: npt.ArrayLike, + step_size: Union[float, npt.ArrayLike] = 1e-3, + seed: Optional[int] = None, ): self.low = np.array(low) self.high = np.array(high) @@ -85,16 +85,20 @@ def sample_initial_pose(self) -> npt.NDArray: class GridSampler(Sampler): def __init__( - self, - low: npt.ArrayLike, - high: npt.ArrayLike, - n_points_per_axis: npt.ArrayLike, + self, + low: npt.ArrayLike, + high: npt.ArrayLike, + n_points_per_axis: npt.ArrayLike, ): self.n_dims = len(low) - points_per_axis = [np.linspace( - low[i], high[i], n_points_per_axis[i]) for i in range(self.n_dims)] - - self.grid = np.array(np.meshgrid(*points_per_axis)).T.reshape(-1, self.n_dims) + points_per_axis = [ + np.linspace(low[i], high[i], n_points_per_axis[i]) + for i in range(self.n_dims) + ] + + self.grid = np.array(np.meshgrid(*points_per_axis)).T.reshape( + -1, self.n_dims + ) self.n_samples = len(self.grid) self.n_calls = 0 diff --git a/deformable_gym/envs/unused/lifting_env.py b/deformable_gym/envs/unused/lifting_env.py index c08df62..34e062a 100644 --- a/deformable_gym/envs/unused/lifting_env.py +++ b/deformable_gym/envs/unused/lifting_env.py @@ -16,35 +16,76 @@ class LiftingEnv(BaseBulletEnv): HORIZON = 10000 def __init__( - self, gui=True, real_time=False, - initial_joint_positions=( - 0.0, -1.344, 1.786, -2.646, -1.587, 3.14, - 0, 0, 0, 0, 0, 0), verbose=False): + self, + gui=True, + real_time=False, + initial_joint_positions=( + 0.0, + -1.344, + 1.786, + -2.646, + -1.587, + 3.14, + 0, + 0, + 0, + 0, + 0, + 0, + ), + verbose=False, + ): super().__init__( - gui=gui, real_time=real_time, horizon=self.HORIZON, soft=True, - load_plane=True, verbose=verbose) - - self.robot = ur5_mia.UR5Mia(task_space_limit=(np.array([-0.8, -0.2, 1]), - np.array([-0.3, 0.2, 1.4])), - verbose=verbose) - - self.robot.set_initial_joint_positions(np.array(initial_joint_positions)) + gui=gui, + real_time=real_time, + horizon=self.HORIZON, + soft=True, + load_plane=True, + verbose=verbose, + ) + + self.robot = ur5_mia.UR5Mia( + task_space_limit=( + np.array([-0.8, -0.2, 1]), + np.array([-0.3, 0.2, 1.4]), + ), + verbose=verbose, + ) + + self.robot.set_initial_joint_positions( + np.array(initial_joint_positions) + ) self.step_counter = 0 limits = pbh.get_limit_array(self.robot.motors) - lower = np.concatenate([np.array([-1.25, -0.25, 0.75]), np.zeros(4), - limits[0][6:], np.array([-2, -2, -2]), - np.array([2, 2, 2])], axis=0) - upper = np.concatenate([np.array([-0.75, 0.25, 1.25]), np.ones(4), - limits[1][6:], np.array([2, 2, 2]), - np.array([2, 2, 2])], axis=0) + lower = np.concatenate( + [ + np.array([-1.25, -0.25, 0.75]), + np.zeros(4), + limits[0][6:], + np.array([-2, -2, -2]), + np.array([2, 2, 2]), + ], + axis=0, + ) + upper = np.concatenate( + [ + np.array([-0.75, 0.25, 1.25]), + np.ones(4), + limits[1][6:], + np.array([2, 2, 2]), + np.array([2, 2, 2]), + ], + axis=0, + ) self.observation_space = spaces.Box(low=lower, high=upper) - self.action_space = spaces.Box(low=-np.ones(11)*0.05, - high=np.ones(11)*0.05) + self.action_space = spaces.Box( + low=-np.ones(11) * 0.05, high=np.ones(11) * 0.05 + ) self.goal_space = spaces.Box(np.zeros(3), np.ones(3)) @@ -54,13 +95,19 @@ def _load_objects(self): self.table = BoxObject( half_extents=np.array([0.5, 0.5, 0.5]), - world_pos=[-0.5, 0.0, 0.5], world_orn=np.deg2rad([0, 0, 0]), - mass=100, fixed=True) + world_pos=[-0.5, 0.0, 0.5], + world_orn=np.deg2rad([0, 0, 0]), + mass=100, + fixed=True, + ) self.cube = BoxObject( half_extents=np.array([0.05, 0.05, 0.05]), - world_pos=[-0.5, 0.0, 1.05], world_orn=np.deg2rad([0, 0, 0]), - mass=1, fixed=False) + world_pos=[-0.5, 0.0, 1.05], + world_orn=np.deg2rad([0, 0, 0]), + mass=1, + fixed=False, + ) def _hard_reset(self): super()._hard_reset() @@ -74,7 +121,11 @@ def is_done(self, state, action, next_state): """ Checks whether the current episode is over or not. """ - return next_state[-1] < 0.85 or next_state[-1] > 1.15 or super().is_done(state, action, next_state) + return ( + next_state[-1] < 0.85 + or next_state[-1] > 1.15 + or super().is_done(state, action, next_state) + ) def observe_state(self): joint_pos = self.robot.get_joint_positions() @@ -83,8 +134,9 @@ def observe_state(self): cube_pos = self.cube.get_pose()[:3] - return np.concatenate([endeffector_pose, joint_pos[6:], sensor_readings, - cube_pos], axis=0).astype('float64') + return np.concatenate( + [endeffector_pose, joint_pos[6:], sensor_readings, cube_pos], axis=0 + ).astype("float64") def calculate_reward(self, state, action, next_state, done): """ @@ -98,4 +150,4 @@ def calculate_reward(self, state, action, next_state, done): elif cube_pos[2] > 1.15: return 100 else: - return np.linalg.norm(next_state[2]-1.05) * 0.01 + return np.linalg.norm(next_state[2] - 1.05) * 0.01 diff --git a/deformable_gym/envs/unused/stacking_env.py b/deformable_gym/envs/unused/stacking_env.py index 6328675..4a4b618 100644 --- a/deformable_gym/envs/unused/stacking_env.py +++ b/deformable_gym/envs/unused/stacking_env.py @@ -16,33 +16,76 @@ class StackingEnv(BaseBulletEnv): HORIZON = 2000 def __init__( - self, gui=True, real_time=False, - initial_joint_positions=( - 0.0, -1.344, 1.786, -2.646, -1.587, 3.14, - 0, 0, 0, 0, 0, 0), verbose=False): + self, + gui=True, + real_time=False, + initial_joint_positions=( + 0.0, + -1.344, + 1.786, + -2.646, + -1.587, + 3.14, + 0, + 0, + 0, + 0, + 0, + 0, + ), + verbose=False, + ): super().__init__( - gui=gui, real_time=real_time, horizon=self.HORIZON, soft=True, - load_plane=True, verbose=verbose) - - self.robot = ur5_mia.UR5Mia(task_space_limit=(np.array([-0.8, -0.2, 1]), - np.array([-0.3, 0.2, 1.4])), - verbose=verbose) - - self.robot.set_initial_joint_positions(np.array(initial_joint_positions)) + gui=gui, + real_time=real_time, + horizon=self.HORIZON, + soft=True, + load_plane=True, + verbose=verbose, + ) + + self.robot = ur5_mia.UR5Mia( + task_space_limit=( + np.array([-0.8, -0.2, 1]), + np.array([-0.3, 0.2, 1.4]), + ), + verbose=verbose, + ) + + self.robot.set_initial_joint_positions( + np.array(initial_joint_positions) + ) self.step_counter = 0 limits = pbh.get_limit_array(self.robot.motors) - lower = np.concatenate([np.array([-1.25, -0.25, 0.75]), np.zeros(4), - limits[0][6:], np.array([-5, -5, -5]), np.array([5, 5, 5, 5, 5, 5])], axis=0) - upper = np.concatenate([np.array([-0.75, 0.25, 1.25]), np.ones(4), - limits[1][6:], np.array([5, 5, 5]), np.array([5, 5, 5, 5, 5, 5])], axis=0) + lower = np.concatenate( + [ + np.array([-1.25, -0.25, 0.75]), + np.zeros(4), + limits[0][6:], + np.array([-5, -5, -5]), + np.array([5, 5, 5, 5, 5, 5]), + ], + axis=0, + ) + upper = np.concatenate( + [ + np.array([-0.75, 0.25, 1.25]), + np.ones(4), + limits[1][6:], + np.array([5, 5, 5]), + np.array([5, 5, 5, 5, 5, 5]), + ], + axis=0, + ) self.observation_space = spaces.Box(low=lower, high=upper) # initially: actions are joint targets - self.action_space = spaces.Box(low=-np.ones(11)*0.05, - high=np.ones(11)*0.05) + self.action_space = spaces.Box( + low=-np.ones(11) * 0.05, high=np.ones(11) * 0.05 + ) self.goal_space = spaces.Box(np.zeros(3), np.ones(3)) @@ -50,18 +93,27 @@ def _load_objects(self): super()._load_objects() self.table = BoxObject( half_extents=np.array([0.5, 0.5, 0.5]), - world_pos=[-0.5, 0.0, 0.5], world_orn=np.deg2rad([0, 0, 0]), mass=100, - fixed=True) + world_pos=[-0.5, 0.0, 0.5], + world_orn=np.deg2rad([0, 0, 0]), + mass=100, + fixed=True, + ) self.cube1 = BoxObject( half_extents=np.array([0.05, 0.05, 0.05]), - world_pos=[-0.5, -0.1, 1.05], world_orn=np.deg2rad([0, 0, 0]), mass=1, - fixed=False) + world_pos=[-0.5, -0.1, 1.05], + world_orn=np.deg2rad([0, 0, 0]), + mass=1, + fixed=False, + ) self.cube2 = BoxObject( half_extents=np.array([0.05, 0.05, 0.05]), - world_pos=[-0.5, 0.1, 1.05], world_orn=np.deg2rad([0, 0, 0]), mass=1, - fixed=False) + world_pos=[-0.5, 0.1, 1.05], + world_orn=np.deg2rad([0, 0, 0]), + mass=1, + fixed=False, + ) def _hard_reset(self): super()._hard_reset() @@ -80,8 +132,16 @@ def observe_state(self): cube1_pos = self.cube1.get_pose()[:3] cube2_pos = self.cube2.get_pose()[:3] - return np.concatenate([endeffector_pose, joint_pos[6:], sensor_readings, cube1_pos, cube2_pos], axis=0)\ - .astype('float64') + return np.concatenate( + [ + endeffector_pose, + joint_pos[6:], + sensor_readings, + cube1_pos, + cube2_pos, + ], + axis=0, + ).astype("float64") def calculate_reward(self, state, action, next_state, done): """ @@ -93,4 +153,9 @@ def calculate_reward(self, state, action, next_state, done): if cube1_pos[2] < 0.5 or cube2_pos[2] < 0.5: return -100 else: - return -np.linalg.norm(self.cube1.get_pose()[:2] - self.cube2.get_pose()[:2]) * 0.1 + return ( + -np.linalg.norm( + self.cube1.get_pose()[:2] - self.cube2.get_pose()[:2] + ) + * 0.1 + ) diff --git a/deformable_gym/envs/unused/ur5_mia_reach_env.py b/deformable_gym/envs/unused/ur5_mia_reach_env.py index d7ac648..76577d1 100644 --- a/deformable_gym/envs/unused/ur5_mia_reach_env.py +++ b/deformable_gym/envs/unused/ur5_mia_reach_env.py @@ -27,26 +27,39 @@ def __init__(self, gui=True, real_time=False): limits = pbh.get_limit_array(self.robot.motors.values()) - lower = np.concatenate([np.array([-1.25, -0.25, 0.75]), np.zeros(4), - np.array([-1.5, -0.5, 0.5]), - limits[0][6:], np.array([-5, -5, -5])], axis=0) - upper = np.concatenate([np.array([-0.75, 0.25, 1.25]), np.ones(4), - np.array([-0.5, 0.5, 1.5]), - limits[1][6:], np.array([5, 5, 5])], axis=0) + lower = np.concatenate( + [ + np.array([-1.25, -0.25, 0.75]), + np.zeros(4), + np.array([-1.5, -0.5, 0.5]), + limits[0][6:], + np.array([-5, -5, -5]), + ], + axis=0, + ) + upper = np.concatenate( + [ + np.array([-0.75, 0.25, 1.25]), + np.ones(4), + np.array([-0.5, 0.5, 1.5]), + limits[1][6:], + np.array([5, 5, 5]), + ], + axis=0, + ) self.observation_space = spaces.Box(low=lower, high=upper) # initially: actions are joint targets - self.action_space = spaces.Box(low=-np.ones(11), - high=np.ones(11)) + self.action_space = spaces.Box(low=-np.ones(11), high=np.ones(11)) # define goal - self.table_cube = UrdfObject("cube_small.urdf", - world_pos=[-0.75, 0.0, 0.25], - fixed=True) - self.goal_cube = UrdfObject("cube_small.urdf", - world_pos=[-0.75, 0.0, 0.751], - fixed=False) + self.table_cube = UrdfObject( + "cube_small.urdf", world_pos=[-0.75, 0.0, 0.25], fixed=True + ) + self.goal_cube = UrdfObject( + "cube_small.urdf", world_pos=[-0.75, 0.0, 0.751], fixed=False + ) def reset(self, reset_robot=True): self.goal_cube.reset() @@ -58,10 +71,11 @@ def observe_state(self): sensor_readings = self.robot.get_sensor_readings() cube_pos = self.goal_cube.get_pose()[:3] - #print(sensor_readings) + # print(sensor_readings) - return np.concatenate([endeffector_pose, cube_pos, joint_pos[6:], - sensor_readings], axis=0) + return np.concatenate( + [endeffector_pose, cube_pos, joint_pos[6:], sensor_readings], axis=0 + ) def calculate_reward(self, state, action, next_state, done): if next_state[9] < 0.5: @@ -69,6 +83,6 @@ def calculate_reward(self, state, action, next_state, done): elif next_state[9] >= 1.0: return 10 else: - return -0.1 * np.linalg.norm(next_state[7:9] - np.array([-0.5, 0.5])) - - + return -0.1 * np.linalg.norm( + next_state[7:9] - np.array([-0.5, 0.5]) + ) diff --git a/deformable_gym/envs/ur10_shadow_grasp_env.py b/deformable_gym/envs/ur10_shadow_grasp_env.py index a6d2ce9..b9cd700 100644 --- a/deformable_gym/envs/ur10_shadow_grasp_env.py +++ b/deformable_gym/envs/ur10_shadow_grasp_env.py @@ -32,11 +32,11 @@ class UR10ShadowGraspEnv(GraspDeformableMixin, BaseBulletEnv): object2world = pt.transform_from(R=np.eye(3), p=np.array([-0.7, 0.1, 1.8])) def __init__( - self, - object_name="insole", - object_scale=1.0, - observable_object_pos: bool = False, - **kwargs + self, + object_name="insole", + object_scale=1.0, + observable_object_pos: bool = False, + **kwargs, ): self.insole = None self.velocity_commands = False @@ -44,37 +44,45 @@ def __init__( self.object_scale = object_scale self._observable_object_pos = observable_object_pos - super().__init__( - soft=True, - **kwargs - ) + super().__init__(soft=True, **kwargs) 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:], - ], axis=0) + lower_observations = np.concatenate( + [ + np.array([-2, -2, 0]), + -np.ones(4), + limits[0][6:], + ], + axis=0, + ) - upper_observations = np.concatenate([ - np.array([2, 2, 2]), np.ones(4), limits[1][6:], - ], axis=0) + upper_observations = np.concatenate( + [ + np.array([2, 2, 2]), + np.ones(4), + limits[1][6:], + ], + axis=0, + ) 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.0)) + upper_observations = np.append(upper_observations, np.full(3, 2.0)) self.observation_space = spaces.Box( - low=lower_observations, high=upper_observations) + low=lower_observations, high=upper_observations + ) - lower_actions = np.concatenate([ - np.array([-2, -2, 0]), -np.ones(4), limits[0][6:]], axis=0) + lower_actions = np.concatenate( + [np.array([-2, -2, 0]), -np.ones(4), limits[0][6:]], axis=0 + ) - upper_actions = np.concatenate([ - np.array([2, 2, 2]), np.ones(4), limits[1][6:]], axis=0) + upper_actions = np.concatenate( + [np.array([2, 2, 2]), np.ones(4), limits[1][6:]], axis=0 + ) self.action_space = spaces.Box(low=lower_actions, high=upper_actions) @@ -89,17 +97,20 @@ def _create_robot(self): task_space_limit=task_space_limit, end_effector_link="rh_forearm", verbose=self.verbose, - orn_limit=orn_limit) + 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) + orn_limit=orn_limit, + ) robot.set_initial_joint_positions( - dict(zip(robot.motors, robot.get_joint_positions()))) + dict(zip(robot.motors, robot.get_joint_positions())) + ) self.simulation.add_robot(robot) @@ -107,11 +118,15 @@ def _create_robot(self): def _load_objects(self): super()._load_objects() - 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) + ( + 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): @@ -122,7 +137,7 @@ def reset(self, seed=None, options=None): def _get_observation(self): joint_pos = self.robot.get_joint_positions() - #ee_pose = self.robot.get_ee_pose() + # ee_pose = self.robot.get_ee_pose() state = np.concatenate([joint_pos], axis=0) diff --git a/deformable_gym/envs/ur5_mia_grasp_env.py b/deformable_gym/envs/ur5_mia_grasp_env.py index 68df1d3..95d1523 100644 --- a/deformable_gym/envs/ur5_mia_grasp_env.py +++ b/deformable_gym/envs/ur5_mia_grasp_env.py @@ -76,17 +76,18 @@ class UR5MiaGraspEnv(GraspDeformableMixin, BaseBulletEnv): timing information (default: 0.1). :param pybullet_options: Options to pass to pybullet.connect(). """ + robot: ur5_mia.UR5Mia object2world = pt.transform_from(R=np.eye(3), p=np.array([-0.7, 0.1, 1.8])) def __init__( - self, - object_name: str = "insole", - thumb_adducted: bool = True, - object_scale: float = 1.0, - observable_object_pos: bool = False, - **kwargs + self, + object_name: str = "insole", + thumb_adducted: bool = True, + object_scale: float = 1.0, + observable_object_pos: bool = False, + **kwargs, ): self.velocity_commands = False @@ -95,44 +96,56 @@ def __init__( self.thumb_adducted = thumb_adducted self.object_scale = object_scale - super().__init__( - soft=True, - **kwargs - ) + super().__init__(soft=True, **kwargs) self.robot = self._create_robot() self._observable_object_pos = observable_object_pos 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([-5, -5, -5])], axis=0) + lower_observations = np.concatenate( + [ + np.array([-2, -2, 0]), + -np.ones(4), + limits[0][6:], + np.array([-5, -5, -5]), + ], + axis=0, + ) - upper_observations = np.concatenate([ - np.array([2, 2, 2]), np.ones(4), limits[1][6:], - np.array([5, 5, 5])], axis=0) + upper_observations = np.concatenate( + [ + np.array([2, 2, 2]), + np.ones(4), + limits[1][6:], + np.array([5, 5, 5]), + ], + axis=0, + ) 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.0)) + upper_observations = np.append(upper_observations, np.full(3, 2.0)) self.observation_space = spaces.Box( - low=lower_observations, high=upper_observations) + low=lower_observations, high=upper_observations + ) actuated_finger_ids = np.array([6, 7, 11], dtype=int) - lower_actions = np.concatenate([ - np.array([-2, -2, 0]), - -np.ones(4), - limits[0][actuated_finger_ids]], axis=0) + lower_actions = np.concatenate( + [ + np.array([-2, -2, 0]), + -np.ones(4), + limits[0][actuated_finger_ids], + ], + axis=0, + ) - upper_actions = np.concatenate([ - np.array([2, 2, 2]), - np.ones(4), - limits[1][actuated_finger_ids]], axis=0) + upper_actions = np.concatenate( + [np.array([2, 2, 2]), np.ones(4), limits[1][actuated_finger_ids]], + axis=0, + ) self.action_space = spaces.Box(low=lower_actions, high=upper_actions) @@ -146,14 +159,16 @@ def _create_robot(self): task_space_limit=task_space_limit, end_effector_link="palm", verbose=self.verbose, - orn_limit=orn_limit) + orn_limit=orn_limit, + ) else: robot = ur5_mia.UR5MiaPosition( pb_client=self.pb_client, task_space_limit=task_space_limit, end_effector_link="palm", verbose=self.verbose, - orn_limit=orn_limit) + orn_limit=orn_limit, + ) self.simulation.add_robot(robot) @@ -164,12 +179,15 @@ def _create_robot(self): def _load_objects(self): super()._load_objects() - (self.object_to_grasp, - self.object_position, - self.object_orientation) = ObjectFactory(self.pb_client).create( + ( + 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) + scale=self.object_scale, + ) def reset(self, seed=None, options=None): self.object_to_grasp.reset() @@ -178,7 +196,8 @@ def reset(self, seed=None, options=None): def _get_observation(self): joint_pos = self.robot.get_joint_positions( - self.robot.actuated_real_joints) + self.robot.actuated_real_joints + ) ee_pose = self.robot.get_ee_pose() sensor_readings = self.robot.get_sensor_readings() diff --git a/deformable_gym/helpers/pybullet_helper.py b/deformable_gym/helpers/pybullet_helper.py index 01a72f2..d2b16de 100644 --- a/deformable_gym/helpers/pybullet_helper.py +++ b/deformable_gym/helpers/pybullet_helper.py @@ -2,6 +2,10 @@ Helper convenience module to easily transform to and from PyBullet specific conventions. """ + +import os +import sys +from contextlib import contextmanager from enum import Enum from typing import Tuple @@ -9,13 +13,8 @@ import numpy.typing as npt import pybullet as pb import pytransform3d.rotations as pr - from pybullet_utils import bullet_client as bc -from contextlib import contextmanager -import sys -import os - @contextmanager def stdout_redirected(to=os.devnull): @@ -26,8 +25,8 @@ def _redirect_stdout(to_fd): # Duplicate the stdout file descriptor and open it for writing old_stdout_fd = os.dup(fd) - with os.fdopen(old_stdout_fd, 'w') as old_stdout: - with open(to, 'w') as file: + with os.fdopen(old_stdout_fd, "w") as old_stdout: + with open(to, "w") as file: # Redirect stdout to the provided file _redirect_stdout(file.fileno()) try: @@ -37,7 +36,6 @@ def _redirect_stdout(to_fd): _redirect_stdout(old_stdout_fd) - class JointType(Enum): revolute = pb.JOINT_REVOLUTE prismatic = pb.JOINT_PRISMATIC @@ -49,18 +47,18 @@ 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, - pb_client: bc.BulletClient, + 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 @@ -92,7 +90,7 @@ def set_position(self, position: float) -> None: self.body_id, self.joint_idx, targetValue=position, - targetVelocity=0.0 + targetVelocity=0.0, ) def set_limits(self, low: float, high: float) -> None: @@ -101,7 +99,7 @@ def set_limits(self, low: float, high: float) -> None: self.body_id, self.joint_idx, jointLowerLimit=low, - jointUpperLimit=high + jointUpperLimit=high, ) def get_position(self) -> float: @@ -123,8 +121,10 @@ def set_target_position(self, position: float) -> None: if self.max_vel + 0.1 < self.get_velocity(): if self.verbose: - print(f"Joint {self.name} has max velocity of {self.max_vel}" - f" but is moving with velocity {self.get_velocity()}") + print( + f"Joint {self.name} has max velocity of {self.max_vel}" + f" but is moving with velocity {self.get_velocity()}" + ) if self.activated: self.pb_client.setJointMotorControl2( @@ -139,7 +139,9 @@ def set_target_position(self, position: float) -> None: else: if self.verbose: - print(f"Warning: Trying to control deactivated motor {self.name}.") + print( + f"Warning: Trying to control deactivated motor {self.name}." + ) def set_target_velocity(self, velocity: float) -> None: """Sets the target position of the joint.""" @@ -154,7 +156,9 @@ def set_target_velocity(self, velocity: float) -> None: ) else: if self.verbose: - print(f"Warning: Trying to control deactivated motor {self.name}.") + print( + f"Warning: Trying to control deactivated motor {self.name}." + ) def deactivate(self): """Deactivates the motor.""" @@ -179,10 +183,7 @@ def merge_pose(pos, rot): return np.hstack((pos, [rot[-1]], rot[:-1])) # xyzw -> wxyz -def build_joint_list( - robot, - pb_client: bc.BulletClient, - verbose: bool = False): +def build_joint_list(robot, pb_client: bc.BulletClient, verbose: bool = False): """ Builds and returns a dictionary of all joints in the provided robot. @@ -200,23 +201,49 @@ def build_joint_list( # 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_client.getJointInfo(robot, joint_idx) + ( + _, + joint_name, + joint_type, + pos_idx, + vel_idx, + _, + _, + _, + lo, + hi, + max_force, + max_vel, + *_, + ) = pb_client.getJointInfo(robot, joint_idx) - joint_name = joint_name.decode('utf-8') + 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, pb_client)) + 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: str = None, - robot=None, - pb_client: bc.BulletClient = None, - verbose=0): + urdf_path: str = None, + robot=None, + pb_client: bc.BulletClient = None, + verbose=0, +): """ Compute mappings between joint and link names and their indices. """ @@ -247,9 +274,25 @@ def analyze_robot( 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_client.getJointInfo(robot, joint_idx) + ( + _, + 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") @@ -264,26 +307,36 @@ def analyze_robot( joint_type = JointType(joint_type).name if verbose: - print(f"Joint #{joint_idx}: {joint_name} ({joint_type}), " - f"child link: {child_link_name}, parent link index: {parent_idx}") + print( + f"Joint #{joint_idx}: {joint_name} ({joint_type}), " + f"child link: {child_link_name}, parent link index: {parent_idx}" + ) if joint_type == "fixed": continue print("=" * 80) - print(f"Index in positional state variables: {q_index}, " - f"Index in velocity state variables: {u_index}") - print(f"Joint limits: [{lo}, {hi}], max. force: {max_force}, " - f"max. velocity: {max_vel}") + print( + f"Index in positional state variables: {q_index}, " + f"Index in velocity state variables: {u_index}" + ) + print( + f"Joint limits: [{lo}, {hi}], max. force: {max_force}, " + f"max. velocity: {max_vel}" + ) print("=" * 80) if verbose: for link_idx in sorted(link_id_to_link_name.keys()): print(f"Link #{link_idx}: {link_id_to_link_name[link_idx]}") - return joint_name_to_joint_id, {v: k for k, v in link_id_to_link_name.items()} + return joint_name_to_joint_id, { + v: k for k, v in link_id_to_link_name.items() + } def get_limit_array(joint_list): - return np.asarray([j.low for j in joint_list]), np.asarray([j.high for j in joint_list]) + return np.asarray([j.low for j in joint_list]), np.asarray( + [j.high for j in joint_list] + ) def get_joint_by_name(joint_list, name): @@ -293,9 +346,7 @@ def get_joint_by_name(joint_list, name): def link_pose( - robot: int, - link: int, - pb_client: bc.BulletClient + robot: int, link: int, pb_client: bc.BulletClient ) -> Tuple[Tuple[float], Tuple[float]]: """Compute link pose from link state. @@ -310,30 +361,31 @@ def link_pose( :param link: Link ID. :return: Tuple of position and scalar-last quaternion. """ - link_inertial2world_pos, link_inertial2world_orn, \ - link_inertial2link_pos, link_inertial2link_orn = pb_client.getLinkState( - robot, link)[:4] + ( + link_inertial2world_pos, + link_inertial2world_orn, + link_inertial2link_pos, + link_inertial2link_orn, + ) = pb_client.getLinkState(robot, link)[:4] link2link_inertial_pos, link2link_inertial_orn = pb_client.invertTransform( - link_inertial2link_pos, link_inertial2link_orn) + link_inertial2link_pos, link_inertial2link_orn + ) link2world_pos, link2world_orn = pb_client.multiplyTransforms( - link_inertial2world_pos, link_inertial2world_orn, - link2link_inertial_pos, link2link_inertial_orn,) + link_inertial2world_pos, + link_inertial2world_orn, + link2link_inertial_pos, + link2link_inertial_orn, + ) return link2world_pos, link2world_orn -def start_recording( - path: str, - pb_client: bc.BulletClient): +def start_recording(path: str, pb_client: bc.BulletClient): # TODO: double check that recording is not already in progress - logging_id = pb_client.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: int, - pb_client: bc.BulletClient -): +def stop_recording(logging_id: int, pb_client: bc.BulletClient): pb_client.stopStateLogging(logging_id) @@ -360,20 +412,24 @@ class MultibodyPose: pb_client_id : int, optional (default: 0) Physics client ID. """ + def __init__( - self, - uuid, - initial_pos, - initial_orn, - pb_client: bc.BulletClient): + self, uuid, initial_pos, initial_orn, pb_client: bc.BulletClient + ): self.uuid = uuid self.pb_client = pb_client inertia_pos, inertia_orn = self.pb_client.getBasePositionAndOrientation( - self.uuid) + self.uuid + ) inv_pos, inv_orn = pb.invertTransform(inertia_pos, inertia_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)) + 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. @@ -396,7 +452,8 @@ def set_pose(self, pos, orn): """ new_pos, new_orn = self.translate_pose(pos, orn) self.pb_client.resetBasePositionAndOrientation( - self.uuid, new_pos, new_orn) + self.uuid, new_pos, new_orn + ) return new_pos, new_orn def translate_pose(self, pos, orn): @@ -419,7 +476,8 @@ def translate_pose(self, pos, orn): Inertial frame orientation. Provided as scalar-last quaternion. """ return self.pb_client.multiplyTransforms( - pos, orn, self.inertia_offset_pos, self.inertia_offset_orn) + pos, orn, self.inertia_offset_pos, self.inertia_offset_orn + ) def get_pose(self): """Get pose of the root link frame with respect to world frame. @@ -432,11 +490,16 @@ def get_pose(self): orn : array-like, shape (4,) Orientation. Provided as scalar-last quaternion. """ - measured_pos, measured_orn = self.pb_client.getBasePositionAndOrientation( - self.uuid) + 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)) + measured_pos, + measured_orn, + *self.pb_client.invertTransform( + self.inertia_offset_pos, self.inertia_offset_orn + ), + ) @staticmethod def external_pose_to_internal_pose(pose: npt.ArrayLike) -> np.ndarray: diff --git a/deformable_gym/objects/__init__.py b/deformable_gym/objects/__init__.py index c510706..3873e55 100644 --- a/deformable_gym/objects/__init__.py +++ b/deformable_gym/objects/__init__.py @@ -1,15 +1,39 @@ -from .bullet_object import (BoxObject, BulletObjectBase, CapsuleObject, - CylinderObject, Insole, InsoleOnConveyorBelt, - MeshObject, MocapObjectMixin, ObjectFactory, - PillowSmall, PositionEulerAngleMixin, - RigidPrimitiveObject, SoftObject, SoftObjectBase, - SphereObject, UrdfObject) +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", - "CapsuleObject", "SoftObject", "MocapObjectMixin", "RigidPrimitiveObject", - "Insole", "InsoleOnConveyorBelt", "PillowSmall", "PositionEulerAngleMixin", - "Conveyor" + "BulletObjectBase", + "SoftObjectBase", + "BoxObject", + "MeshObject", + "UrdfObject", + "SphereObject", + "CylinderObject", + "ObjectFactory", + "CapsuleObject", + "SoftObject", + "MocapObjectMixin", + "RigidPrimitiveObject", + "Insole", + "InsoleOnConveyorBelt", + "PillowSmall", + "PositionEulerAngleMixin", + "Conveyor", ] diff --git a/deformable_gym/objects/bullet_object.py b/deformable_gym/objects/bullet_object.py index a856110..36bf3f2 100644 --- a/deformable_gym/objects/bullet_object.py +++ b/deformable_gym/objects/bullet_object.py @@ -9,12 +9,11 @@ import pybullet as pb import pytransform3d.rotations as pr import pytransform3d.transformations as pt +from pybullet_utils import bullet_client as bc from ..helpers import pybullet_helper as pbh from ..robots.bullet_utils import draw_pose -from pybullet_utils import bullet_client as bc - base_path = Path(os.path.dirname(__file__)).parent.parent.absolute() @@ -23,13 +22,11 @@ class BulletObjectBase(abc.ABC): :param client_id: Physics client ID for PyBullet interface calls. """ + object_id: int pb_client: bc.BulletClient - def __init__( - self, - pb_client: bc.BulletClient - ): + def __init__(self, pb_client: bc.BulletClient): self.pb_client = pb_client def get_pose(self): @@ -63,6 +60,7 @@ def remove_anchors(self): class PositionEulerAngleMixin: """Controls pose of an object through position and Euler angles.""" + init_pos: npt.ArrayLike init_orn: npt.ArrayLike @@ -82,23 +80,25 @@ def _set_init_pose(self, world_pos=None, world_orn=None): def reset(self): """Resets the object to its initial pose.""" self.pb_client.resetBasePositionAndOrientation( - self.object_id, self.init_pos, self.init_orn) + self.object_id, self.init_pos, self.init_orn + ) class RigidPrimitiveObject(PositionEulerAngleMixin, BulletObjectBase): """Simple primitive object base.""" + def __init__( - 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 + 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 @@ -125,13 +125,20 @@ def _load_object(self): baseCollisionShapeIndex=primitive, baseVisualShapeIndex=-1, basePosition=self.init_pos, - baseOrientation=self.init_orn + baseOrientation=self.init_orn, ) if self.fixed: self.anchor = self.pb_client.createConstraint( - self.object, -1, -1, -1, pb.JOINT_FIXED, [0, 0, 1], [0, 0, 0], - self.init_pos) + self.object, + -1, + -1, + -1, + pb.JOINT_FIXED, + [0, 0, 1], + [0, 0, 0], + self.init_pos, + ) self.anchored = True dynamics_config = {} @@ -146,8 +153,7 @@ def _load_object(self): if self.contactDamping is not None: dynamics_config["contactDamping"] = self.contactDamping if dynamics_config: - self.pb_client.changeDynamics( - self.object, -1, **dynamics_config) + self.pb_client.changeDynamics(self.object, -1, **dynamics_config) return self.object @@ -166,20 +172,21 @@ def reset(self): class MeshObject(RigidPrimitiveObject): """Simple mesh object.""" + def __init__( - 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, + 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 @@ -193,30 +200,33 @@ def __init__( rollingFriction=rollingFriction, restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping + contactDamping=contactDamping, ) def _create_primitive(self): return self.pb_client.createCollisionShape( - pb.GEOM_MESH, fileName=self.filename, meshScale=self.scale) + pb.GEOM_MESH, fileName=self.filename, meshScale=self.scale + ) class BoxObject(RigidPrimitiveObject): """Simple box object.""" + def __init__( - 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, + 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__( pb_client=pb_client, @@ -228,30 +238,33 @@ def __init__( rollingFriction=rollingFriction, restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping + contactDamping=contactDamping, ) def _create_primitive(self): return self.pb_client.createCollisionShape( - pb.GEOM_BOX, halfExtents=self.half_extents) + pb.GEOM_BOX, halfExtents=self.half_extents + ) class SphereObject(RigidPrimitiveObject): """Simple sphere object.""" + def __init__( - 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, + 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__( pb_client=pb_client, @@ -263,30 +276,32 @@ def __init__( rollingFriction=rollingFriction, restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping + contactDamping=contactDamping, ) def _create_primitive(self): return self.pb_client.createCollisionShape( - pb.GEOM_SPHERE, radius=self.radius) + pb.GEOM_SPHERE, radius=self.radius + ) class CylinderObject(RigidPrimitiveObject): """Simple cylinder object.""" + def __init__( - 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, + 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 @@ -300,30 +315,32 @@ def __init__( rollingFriction=rollingFriction, restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping + contactDamping=contactDamping, ) def _create_primitive(self): return self.pb_client.createCollisionShape( - pb.GEOM_CYLINDER, radius=self.radius, height=self.height) + pb.GEOM_CYLINDER, radius=self.radius, height=self.height + ) class CapsuleObject(RigidPrimitiveObject): """Simple capsule object.""" + def __init__( - 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, + 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 @@ -337,14 +354,13 @@ def __init__( rollingFriction=rollingFriction, restitution=restitution, contactStiffness=contactStiffness, - contactDamping=contactDamping + contactDamping=contactDamping, ) def _create_primitive(self): return self.pb_client.createCollisionShape( - pb.GEOM_CAPSULE, - radius=self.radius, - height=self.height) + pb.GEOM_CAPSULE, radius=self.radius, height=self.height + ) class UrdfObject(PositionEulerAngleMixin, BulletObjectBase): @@ -352,13 +368,16 @@ class UrdfObject(PositionEulerAngleMixin, BulletObjectBase): Provides some basic meta functionality for URDF-based objects. """ - def __init__(self, - filename, - pb_client: bc.BulletClient, - verbose=0, - world_pos=None, - world_orn=None, - fixed=False): + + 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 @@ -373,8 +392,9 @@ def _load_object(self): self.init_pos, self.init_orn, useFixedBase=self.fixed, - 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 remove_anchors(self): if self.fixed: @@ -398,19 +418,19 @@ class SoftObjectBase(BulletObjectBase): constraints: Sequence[int] def __init__( - 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 + 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 @@ -444,7 +464,8 @@ def _load_object(self): useFaceContact=True, repulsionStiffness=self.repulsion_stiffness, mass=self.mass, - frictionCoeff=self.friction_coefficient) + frictionCoeff=self.friction_coefficient, + ) if self.fixed: self.__make_anchors(object_id) @@ -462,7 +483,8 @@ def __lame_parameters(nu, E): def __make_anchors(self, object_id): if self.fixed_nodes is None: warnings.warn( - "Object should be fixed, but no fixed nodes are given.") + "Object should be fixed, but no fixed nodes are given." + ) self.constraints = [] else: self.constraints = [ @@ -470,12 +492,12 @@ def __make_anchors(self, object_id): softBodyBodyUniqueId=object_id, nodeIndex=i, bodyUniqueId=-1, - linkIndex=-1) - for i in self.fixed_nodes] + linkIndex=-1, + ) + for i in self.fixed_nodes + ] for constraint in self.constraints: - self.pb_client.changeConstraint( - constraint, - maxForce=1) + self.pb_client.changeConstraint(constraint, maxForce=1) def remove_anchors(self): for anchor in self.constraints: @@ -484,20 +506,22 @@ def remove_anchors(self): class SoftObject(PositionEulerAngleMixin, SoftObjectBase): """Soft Bullet Object.""" - 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 - ): + + 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, pb_client=pb_client, @@ -508,7 +532,7 @@ def __init__(self, E=E, damping=damping, repulsion_stiffness=repulsion_stiffness, - mass=mass + mass=mass, ) self._set_init_pose(world_pos, world_orn) self.object_id = self._load_object() @@ -537,10 +561,7 @@ 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. @@ -556,17 +577,18 @@ def reset(self, self.object_id = self._load_object() if center_camera: self.pb_client.resetDebugVisualizerCamera( - 0.35, -60, -30, self.object_markers2world[:3, 3]) + 0.35, -60, -30, self.object_markers2world[:3, 3] + ) class Insole(MocapObjectMixin, SoftObjectBase): def __init__( - self, - insole_markers2world, - pb_client: bc.BulletClient, - scale=1.0, - E=100000.0, - fixed=False + 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"), @@ -580,10 +602,10 @@ def __init__( collision_margin=0.0005, repulsion_stiffness=8000.0, mass=0.1, - friction_coefficient=1.5) + friction_coefficient=1.5, + ) self.object_markers2world = insole_markers2world - self.init_pos, self.init_orn = self.mesh_pose( - self.object_markers2world) + self.init_pos, self.init_orn = self.mesh_pose(self.object_markers2world) self.object_id = self._load_object() @staticmethod @@ -593,21 +615,25 @@ def mesh_pose(object_markers2world): """ markers2mesh = pt.transform_from( R=pr.active_matrix_from_extrinsic_roll_pitch_yaw( - np.deg2rad([180, 0, -4.5])), - p=np.array([0.04, 0.07, -0.007])) + np.deg2rad([180, 0, -4.5]) + ), + p=np.array([0.04, 0.07, -0.007]), + ) pq = pt.pq_from_transform( - pt.concat(pt.invert_transform(markers2mesh), - object_markers2world)) + pt.concat(pt.invert_transform(markers2mesh), object_markers2world) + ) return pq[:3], pr.quaternion_xyzw_from_wxyz(pq[3:]) class PillowSmall(MocapObjectMixin, SoftObjectBase): - def __init__(self, - pillow_markers2world, - pb_client: bc.BulletClient, - scale=1.0, - fixed=False): + 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"), pb_client=pb_client, @@ -620,11 +646,10 @@ def __init__(self, collision_margin=0.0001, repulsion_stiffness=1000.0, mass=0.1, - friction_coefficient=1.5 + friction_coefficient=1.5, ) self.object_markers2world = pillow_markers2world - self.init_pos, self.init_orn = self.mesh_pose( - self.object_markers2world) + self.init_pos, self.init_orn = self.mesh_pose(self.object_markers2world) self.object_id = self._load_object() @staticmethod @@ -632,22 +657,27 @@ def mesh_pose(insole_markers2world): markers2mesh = pt.transform_from( R=pr.active_matrix_from_extrinsic_roll_pitch_yaw( - np.deg2rad([0, 0, 90])), - p=np.array([0.0, -0.02, 0.095])) + 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)) + pq = pt.pq_from_transform( + pt.concat(pt.invert_transform(markers2mesh), insole_markers2world) + ) return pq[:3], pr.quaternion_xyzw_from_wxyz(pq[3:]) class InsoleOnConveyorBelt(Insole): def __init__( - self, - insole_markers2world, - pb_client: bc.BulletClient, - grasp_point_name="back", - scale=1.0, - E=100000.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 @@ -656,7 +686,7 @@ def __init__( pb_client=pb_client, scale=scale, E=E, - fixed=False + fixed=False, ) def _load_object(self): @@ -664,7 +694,7 @@ def _load_object(self): object_id = super()._load_object() # extents: width, length, height - conveyor_extents = np.array([0.5, 1.5, self.init_pos[2]-.02]) + conveyor_extents = np.array([0.5, 1.5, self.init_pos[2] - 0.02]) if self.grasp_point_name == "back": conveyor_pos = (0, self.init_pos[1] + 0.88, self.init_pos[2] / 2) @@ -679,7 +709,8 @@ def _load_object(self): world_orn=(0, 0, 0), fixed=True, lateralFriction=0.3, - rollingFriction=0.3) + rollingFriction=0.3, + ) return object_id def remove_anchors(self): @@ -704,11 +735,13 @@ def reset(self, object_markers2world=None, center_camera=False, pos=None): self.object_id = self._load_object() if center_camera: self.pb_client.resetDebugVisualizerCamera( - 0.35, -60, -30, self.object_markers2world[:3, 3]) + 0.35, -60, -30, self.object_markers2world[:3, 3] + ) class ObjectFactory: """Creates objects to grasp.""" + OBJECT_POSITIONS = { "insole": [-0.15, 0.1, 1.03], "pillow_small": [0.00, 0.29, 1.05], @@ -718,30 +751,30 @@ class ObjectFactory: "box": [0.0, 0.14, 1.035], "sphere": [0.0, 0.15, 1.02], "cylinder": [0.0, 0.15, 1.02], - "capsule": [0.0, 0.15, 1.02] + "capsule": [0.0, 0.15, 1.02], } OBJECT_ORIENTATIONS = { "insole": [0, 0, 0], "pillow_small": [0.5, 0, 0], - "insole2": [0, 0, np.pi/2], + "insole2": [0, 0, np.pi / 2], "pillow_small2": [0, 0, 0], - "insole_on_conveyor_belt": [0, 0, np.pi/2], + "insole_on_conveyor_belt": [0, 0, np.pi / 2], "box": [0, 0, 0], "sphere": [0, 0, 0], "cylinder": [0, 0, 0], - "capsule": [0, 0, 0] + "capsule": [0, 0, 0], } def __init__(self, pb_client: bc.BulletClient): self.pb_client = pb_client def create( - self, - object_name: str, - object_position: Union[npt.ArrayLike, None] = None, - object_orientation: Union[npt.ArrayLike, None] = None, - object2world: Union[npt.ArrayLike, None] = None, - **additional_args + self, + object_name: str, + object_position: Union[npt.ArrayLike, None] = None, + object_orientation: Union[npt.ArrayLike, None] = None, + object2world: Union[npt.ArrayLike, None] = None, + **additional_args, ) -> Tuple[BulletObjectBase, np.ndarray, np.ndarray]: """Create object to grasp. @@ -761,51 +794,49 @@ def create( KeyError: If object is unknown. """ object_position, object_orientation, object2world = self.get_pose( - object_name, object_position, object_orientation, object2world) + object_name, object_position, object_orientation, object2world + ) if object_name == "insole": - args = dict(fixed=True, mass=0.1, E=200000.0, - fixed_nodes=[0, 40, 45]) + args = dict( + fixed=True, mass=0.1, E=200000.0, fixed_nodes=[0, 40, 45] + ) args.update(additional_args) object_to_grasp = SoftObject( os.path.join(base_path, "object_data/insole.vtk"), self.pb_client, world_pos=object_position, world_orn=object_orientation, - **args) + **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 = 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"), self.pb_client, world_pos=object_position, world_orn=object_orientation, - **args) + **args, + ) elif object_name == "insole2": args = dict(scale=1.0, fixed=True) args.update(additional_args) - object_to_grasp = Insole( - object2world, - self.pb_client, - **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, - self.pb_client, - **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, - self.pb_client, - **args) + 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) @@ -813,7 +844,8 @@ def create( self.pb_client, world_pos=object_position, world_orn=object_orientation, - **args) + **args, + ) elif object_name == "sphere": args = dict(radius=0.05, mass=0.1, fixed=True) args.update(additional_args) @@ -821,7 +853,8 @@ def create( self.pb_client, world_pos=object_position, world_orn=object_orientation, - **args) + **args, + ) elif object_name == "cylinder": args = dict(radius=0.025, height=0.05, mass=0.1, fixed=True) args.update(additional_args) @@ -829,7 +862,8 @@ def create( self.pb_client, world_pos=object_position, world_orn=object_orientation, - **args) + **args, + ) elif object_name == "capsule": args = dict(radius=0.025, height=0.05, mass=0.1, fixed=True) args.update(additional_args) @@ -837,17 +871,19 @@ def create( self.pb_client, world_pos=object_position, world_orn=object_orientation, - **args) + **args, + ) else: raise KeyError(f"Object '{object_name}' not available.") return object_to_grasp, object_position, object_orientation def get_pose( - self, object_name: str, - object_position: Union[npt.ArrayLike, None] = None, - object_orientation: Union[npt.ArrayLike, None] = None, - object2world: Union[npt.ArrayLike, None] = None + self, + object_name: str, + object_position: Union[npt.ArrayLike, None] = None, + object_orientation: Union[npt.ArrayLike, None] = None, + object2world: Union[npt.ArrayLike, None] = None, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """Translates between pose representations. @@ -867,38 +903,45 @@ 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]) + self.OBJECT_ORIENTATIONS[object_name] + ) object2world = pt.transform_from( - R=pr.active_matrix_from_extrinsic_euler_xyz( - object_orientation), - p=object_position) + R=pr.active_matrix_from_extrinsic_euler_xyz(object_orientation), + p=object_position, + ) else: object_position = object2world[:3, 3] object_orientation = pr.extrinsic_euler_xyz_from_active_matrix( - object2world[:3, :3]) + object2world[:3, :3] + ) return object_position, object_orientation, object2world class Pose(object): def __init__( - self, - position, - orientation, - pb_client: bc.BulletClient, - scale=0.1, - line_width=10,): + 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, pb_client=self.pb_client) + self.ids = draw_pose( + self.position, + self.orientation, + s=self.scale, + lw=self.line_width, + pb_client=self.pb_client, + ) def update(self, position, orientation): self.position = position @@ -909,7 +952,8 @@ def update(self, position, orientation): s=self.scale, lw=self.line_width, replace_item_unique_ids=self.ids, - pb_client=self.pb_client) + pb_client=self.pb_client, + ) def remove(self): for item in self.ids: diff --git a/deformable_gym/objects/conveyor.py b/deformable_gym/objects/conveyor.py index dc37064..b40d338 100644 --- a/deformable_gym/objects/conveyor.py +++ b/deformable_gym/objects/conveyor.py @@ -1,17 +1,19 @@ -from ..objects.bullet_object import BoxObject, ObjectFactory from pybullet_utils import bullet_client as bc +from ..objects.bullet_object import BoxObject, ObjectFactory + class Conveyor: - 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, - ): + 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, + ): self.height_m = height_m self.width_m = 0.5 self.length_m = 1.5 @@ -20,23 +22,26 @@ def __init__(self, self.conveyor = BoxObject( self.pb_client, half_extents=[ - self.width_m/2, self.length_m/2, self.height_m/2 + 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 + rollingFriction=0.3, ) self.object_factory = ObjectFactory(self.pb_client) - self.insole, self.insole_pos, self.insole_orn = \ + self.insole, self.insole_pos, self.insole_orn = ( self.object_factory.create( "insole2", object_position=insole_pos, object_orientation=insole_rot, - E=E + E=E, ) + ) self.insole.remove_anchors() def reset_insole(self, new_pos=None, new_orn=None): @@ -45,6 +50,7 @@ def reset_insole(self, new_pos=None, new_orn=None): if new_orn is not None: self.insole_orn = new_orn object2world = self.object_factory.get_pose( - "insole2", self.insole_pos, self.insole_orn)[2] + "insole2", self.insole_pos, self.insole_orn + )[2] self.insole.reset(object_markers2world=object2world) self.insole.remove_anchors() diff --git a/deformable_gym/robots/__init__.py b/deformable_gym/robots/__init__.py index a78cd10..02dfe05 100644 --- a/deformable_gym/robots/__init__.py +++ b/deformable_gym/robots/__init__.py @@ -6,8 +6,16 @@ __all__ = [ "BulletRobot", - "MiaHand", "MiaHandPosition", "MiaHandVelocity", - "ShadowHand", "ShadowHandPosition", "ShadowHandVelocity", - "UR5Mia", "UR5MiaPosition", "UR5MiaVelocity", - "UR10Shadow", "UR10ShadowPosition", "UR10ShadowVelocity" + "MiaHand", + "MiaHandPosition", + "MiaHandVelocity", + "ShadowHand", + "ShadowHandPosition", + "ShadowHandVelocity", + "UR5Mia", + "UR5MiaPosition", + "UR5MiaVelocity", + "UR10Shadow", + "UR10ShadowPosition", + "UR10ShadowVelocity", ] diff --git a/deformable_gym/robots/bullet_robot.py b/deformable_gym/robots/bullet_robot.py index b4fc05a..a496ee9 100644 --- a/deformable_gym/robots/bullet_robot.py +++ b/deformable_gym/robots/bullet_robot.py @@ -4,8 +4,8 @@ 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 pybullet_utils import bullet_client as bc from ..helpers import pybullet_helper as pbh from ..objects.bullet_object import Pose @@ -29,17 +29,18 @@ class BulletRobot(abc.ABC): :param base_commands: Control pose of the hand (add 7 components to action space). """ + def __init__( - 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, + 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, ): self.path = urdf_path self.verbose = verbose @@ -69,24 +70,26 @@ def __init__( self._id = self.initialise() - self._joint_name_to_joint_id, self._link_name_to_link_id = \ + self._joint_name_to_joint_id, self._link_name_to_link_id = ( pbh.analyze_robot( robot=self._id, pb_client=self.pb_client, verbose=verbose, ) + ) self.all_joints = pbh.build_joint_list( - self._id, self.pb_client, verbose=self.verbose) + self._id, self.pb_client, verbose=self.verbose + ) # separate fixed joints from controllable ones self.fixed_joints = { - j.name: j for j in self.all_joints - if j.joint_type == pb.JOINT_FIXED + j.name: j for j in self.all_joints if j.joint_type == pb.JOINT_FIXED } self.motors = { - j.name: j for j in self.all_joints + j.name: j + for j in self.all_joints if j.joint_type == pb.JOINT_REVOLUTE } @@ -97,9 +100,7 @@ def __init__( if "sensor" in name: self.sensors[name] = joint self.pb_client.enableJointForceTorqueSensor( - self._id, - joint.joint_idx, - enableSensor=True + self._id, joint.joint_idx, enableSensor=True ) # set the joints initial positions @@ -116,13 +117,13 @@ def __init__( if self.base_commands: 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 + -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 + [0, 0, 0], # joint axis in child frame + [0, 0, 0], # parent frame position + [0, 0, 1], # child frame position ) if verbose: @@ -132,16 +133,17 @@ def __init__( self.get_id(), self.init_pos, self.init_rot, - pb_client=self.pb_client) + 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)) + 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)) + np.full(len(self.motors), -1.0), np.full(len(self.motors), 1.0) + ) def initialise(self) -> int: """Initialize robot in simulation. @@ -154,7 +156,8 @@ def initialise(self) -> int: self.init_rot, useFixedBase=not self.base_commands, flags=pb.URDF_USE_SELF_COLLISION - | pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) + | pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT, + ) def get_id(self) -> int: """Get PyBullet UUID for the robot multi-body. @@ -189,10 +192,12 @@ def set_endeffector(self, link_id: int) -> None: self.end_effector = link_id if self.inverse_kinematics_solver is None: # default to PyBullet IK self.inverse_kinematics_solver = PyBulletSolver( - self._id, self.end_effector, self.pb_client) + self._id, self.end_effector, self.pb_client + ) def get_joint_positions( - self, keys: Union[Iterable[str], None] = None) -> np.ndarray: + self, keys: Union[Iterable[str], None] = None + ) -> np.ndarray: """Returns the robot's current joint positions. :param keys: Names of joints from which positions are requested. @@ -205,8 +210,7 @@ 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 + self, keys: Union[Iterable[str], None] = None ) -> np.ndarray: """Returns the robot's current joint velocities. @@ -224,8 +228,13 @@ def get_sensor_readings(self) -> np.ndarray: :return: Array of sensor readings. """ return np.array( - [el[2][-1] for el in self.pb_client.getJointStates( - self._id, [j.joint_idx for j in self.sensors.values()])]) + [ + 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]): """Updates the current command. @@ -242,8 +251,7 @@ def update_current_command(self, command: Dict[str, float]): self.current_command[key] = command[key] def send_current_command( - self, - keys: Union[Iterable[str], None] = None + self, keys: Union[Iterable[str], None] = None ) -> None: """Sends the currently stored commands to the selected motors. @@ -268,11 +276,11 @@ def actuate_motors(self, keys: Union[Iterable[str], None] = None) -> None: """ def compute_ik( - self, - joint_positions: np.ndarray, - pos_command: np.ndarray, - orn_command: Union[np.ndarray, None] = None, - velocities: bool = True + self, + joint_positions: np.ndarray, + pos_command: np.ndarray, + orn_command: Union[np.ndarray, None] = None, + velocities: bool = True, ) -> np.ndarray: """Translates task space command to motor commands. @@ -301,9 +309,8 @@ def compute_ik( print(f"Original {target_pos=}") target_pos = np.clip( - target_pos, - self.task_space_limit[0], - self.task_space_limit[1]) + target_pos, self.task_space_limit[0], self.task_space_limit[1] + ) if self.verbose: print(f"Clipped {target_pos=}") @@ -316,9 +323,9 @@ def compute_ik( if self.orn_limit is not None: 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_euler, self.orn_limit[0], self.orn_limit[1] + ) + target_orn = pb.getQuaternionFromEuler(target_orn_euler_clipped) target_orn /= np.maximum(np.linalg.norm(target_orn), 1e-10) @@ -329,7 +336,8 @@ def compute_ik( print("FOUND NAN IN TARGET ORN!") target = self.inverse_kinematics_solver( - target_pos, target_orn, joint_positions) + target_pos, target_orn, joint_positions + ) assert np.isfinite(target).all(), f"IK produced NAN:{target=}" @@ -343,16 +351,16 @@ def get_ee_pose(self) -> np.ndarray: """ if self.end_effector is not None: current_pos, current_orn = pbh.link_pose( - self._id, self.end_effector, self.pb_client) + self._id, self.end_effector, self.pb_client + ) else: current_pos, current_orn = ( - self.pb_client.getBasePositionAndOrientation(self._id)) + 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 + self, target_pos: np.ndarray, target_orn: np.ndarray ) -> None: """Sets the pose of the end-effector instantly. @@ -365,15 +373,14 @@ def set_ee_pose( 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)} + 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. @@ -388,10 +395,7 @@ def reset( 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. @@ -420,19 +424,23 @@ def move_base(self, offset: npt.ArrayLike) -> None: target_pos, target_orn = self._enforce_limits(pose) target_pos, target_orn = self.multibody_pose.translate_pose( - target_pos, target_orn) + target_pos, target_orn + ) self.pb_client.changeConstraint( self.base_constraint, target_pos, jointChildFrameOrientation=target_orn, - maxForce=100000) + maxForce=100000, + ) def _enforce_limits(self, pose) -> Tuple[Any, Any]: - assert self.base_commands, "tried to move base, but base commands " \ - "are not enabled" + assert self.base_commands, ( + "tried to move base, but base commands " "are not enabled" + ) if self.task_space_limit is not None: target_pos = np.clip( - pose[:3], self.task_space_limit[0], self.task_space_limit[1]) + pose[:3], self.task_space_limit[0], self.task_space_limit[1] + ) else: target_pos = pose[:3] target_orn = pose[3:] @@ -448,12 +456,13 @@ def reset_base(self, pose: npt.ArrayLike) -> None: target_pos, target_orn = self._enforce_limits(pose) target_pos, target_orn = self.multibody_pose.set_pose( - target_pos, target_orn) + target_pos, target_orn + ) self.pb_client.changeConstraint( self.base_constraint, target_pos, jointChildFrameOrientation=target_orn, - maxForce=100000 + maxForce=100000, ) def _get_link_id(self, link_name: str) -> int: @@ -465,16 +474,17 @@ 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] + 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. :return: Array of lower limits and array of upper limits. """ - return (np.array([self.motors[joint].low for joint in joints]), - np.array([self.motors[joint].high for joint in joints])) + return ( + np.array([self.motors[joint].low for joint in joints]), + np.array([self.motors[joint].high for joint in joints]), + ) class RobotCommandWrapper: @@ -485,6 +495,7 @@ class RobotCommandWrapper: :param robot: Robot. :param joint_names: Names of the actuated joints. """ + robot: BulletRobot def __init__(self, robot, joint_names): @@ -507,14 +518,16 @@ def _init_debug_visualizations(self): np.array([0.0, 0.0, 0.0, 1.0]), pb_client=self.pb_client, scale=0.1, - line_width=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) + line_width=1, + ) self.contact_normals = [] def get_contact_points(self, object_id: int): @@ -524,18 +537,20 @@ def get_contact_points(self, object_id: int): :return: List of contact point information from PyBullet. """ contact_points = self.pb_client.getContactPoints( - self.get_id(), object_id) + 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 = ( - self.pb_client.getBasePositionAndOrientation( - self.get_id())) + self.pb_client.getBasePositionAndOrientation(self.get_id()) + ) self.robot_pose.update(robot_position, robot_orientation) object_position, object_orientation = ( - self.pb_client.getBasePositionAndOrientation(object_id)) + self.pb_client.getBasePositionAndOrientation(object_id) + ) self.object_pose.update(object_position, object_orientation) self.contact_normals = [] @@ -544,12 +559,11 @@ def get_contact_points(self, object_id: int): 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)) + object_normal_end = np.array( + position_on_object + ) + normal_force * np.array(contact_normal_on_object) contact_normal_line = self.pb_client.addUserDebugLine( - position_on_object, - object_normal_end, - [1, 1, 1]) + 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 9c60a38..9adc232 100644 --- a/deformable_gym/robots/bullet_utils.py +++ b/deformable_gym/robots/bullet_utils.py @@ -1,16 +1,15 @@ import numpy as np import pytransform3d.rotations as pr import pytransform3d.transformations as pt - from pybullet_utils import bullet_client as bc def draw_transform( - pose2origin, - s, - pb_client: bc.BulletClient, - lw=1, - replace_item_unique_ids=(-1, -1, -1) + pose2origin, + s, + pb_client: bc.BulletClient, + lw=1, + replace_item_unique_ids=(-1, -1, -1), ): """Draw transformation matrix. @@ -42,29 +41,32 @@ def draw_transform( pose2origin[:3, 3] + s * pose2origin[:3, 0], [1, 0, 0], lw, - replaceItemUniqueId=replace_item_unique_ids[0]) + 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]) + 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]) + replaceItemUniqueId=replace_item_unique_ids[2], + ) return [line_x, line_y, line_z] def draw_pose( - pos, - rot, - s, - pb_client: bc.BulletClient, - lw=1, - replace_item_unique_ids=(-1, -1, -1), + pos, + rot, + s, + pb_client: bc.BulletClient, + lw=1, + replace_item_unique_ids=(-1, -1, -1), ): """Draw transformation matrix. @@ -96,15 +98,16 @@ def draw_pose( """ q_scalar_first = pr.quaternion_wxyz_from_xyzw(rot) pose2origin = pt.transform_from( - R=pr.matrix_from_quaternion(q_scalar_first), p=pos) + R=pr.matrix_from_quaternion(q_scalar_first), p=pos + ) return draw_transform( - pose2origin, s, pb_client, lw, replace_item_unique_ids) + pose2origin, s, pb_client, lw, replace_item_unique_ids + ) def draw_box( - corners_in_world, - pb_client: bc.BulletClient, - replace_item_unique_ids=None): + corners_in_world, pb_client: bc.BulletClient, replace_item_unique_ids=None +): """Draw box. Parameters @@ -118,14 +121,27 @@ def draw_box( if replace_item_unique_ids is None: replace_item_unique_ids = [-1] * 12 for line_idx, (i, j) in enumerate( - [(0, 1), (0, 2), (1, 3), (2, 3), - (4, 5), (4, 6), (5, 7), (6, 7), - (0, 4), (1, 5), (2, 6), (3, 7)]): + [ + (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_client.addUserDebugLine( corners_in_world[i], corners_in_world[j], (0, 0, 0), - replaceItemUniqueId=replace_item_unique_ids[line_idx]) + replaceItemUniqueId=replace_item_unique_ids[line_idx], + ) return replace_item_unique_ids @@ -136,14 +152,16 @@ def draw_limits(limits, pb_client): x_hi = limits[1][0] y_hi = limits[1][1] z_hi = limits[1][2] - task_space_corners = np.array([ - [x_lo, y_lo, z_hi], - [x_lo, y_hi, z_hi], - [x_hi, y_lo, z_hi], - [x_hi, y_hi, z_hi], - [x_lo, y_lo, z_lo], - [x_lo, y_hi, z_lo], - [x_hi, y_lo, z_lo], - [x_hi, y_hi, z_lo], - ]) + task_space_corners = np.array( + [ + [x_lo, y_lo, z_hi], + [x_lo, y_hi, z_hi], + [x_hi, y_lo, z_hi], + [x_hi, y_hi, z_hi], + [x_lo, y_lo, z_lo], + [x_lo, y_hi, z_lo], + [x_hi, y_lo, z_lo], + [x_hi, y_hi, z_lo], + ] + ) draw_box(task_space_corners, pb_client) diff --git a/deformable_gym/robots/control_mixins.py b/deformable_gym/robots/control_mixins.py index 2e42fcd..8be5fe3 100644 --- a/deformable_gym/robots/control_mixins.py +++ b/deformable_gym/robots/control_mixins.py @@ -3,6 +3,7 @@ class PositionControlMixin: """Mixin for position-controlled robots.""" + velocity_commands = False def actuate_motors(self, keys: Union[List[str], None] = None): @@ -20,6 +21,7 @@ def actuate_motors(self, keys: Union[List[str], None] = None): class VelocityControlMixin: """Mixin for velocity-controlled robots.""" + velocity_commands = True def actuate_motors(self, keys: Union[List[str], None] = None): diff --git a/deformable_gym/robots/inverse_kinematics.py b/deformable_gym/robots/inverse_kinematics.py index c6123ee..d041b61 100644 --- a/deformable_gym/robots/inverse_kinematics.py +++ b/deformable_gym/robots/inverse_kinematics.py @@ -1,20 +1,22 @@ import numpy as np - -from ..robots.ur_kinematics import (analytical_ik, - ee_kin2ee_options, - robot_params, - urdf_base2kin_base) from pybullet_utils import bullet_client as bc +from ..robots.ur_kinematics import ( + analytical_ik, + ee_kin2ee_options, + robot_params, + urdf_base2kin_base, +) + class PyBulletSolver: def __init__( - self, - robot, - end_effector, - pb_client: bc.BulletClient, - n_iter: int = 200, - threshold: float = 1e-6, + self, + robot, + end_effector, + pb_client: bc.BulletClient, + n_iter: int = 200, + threshold: float = 1e-6, ): self.robot = robot self.pb_client = pb_client @@ -35,16 +37,15 @@ def __call__(self, target_pos, target_orn, last_joint_angles): targetPosition=target_pos, targetOrientation=target_orn, maxNumIterations=self.n_iter, - residualThreshold=self.threshold)) + residualThreshold=self.threshold, + ) + ) return out[:6] class UniversalRobotAnalyticalInverseKinematics: def __init__( - self, - robot_type="ur5", - end_effector_name="ur5_tool0", - fallback=None + 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] @@ -59,7 +60,8 @@ def __call__(self, target_pos, target_orn, last_joint_angles): last_joint_angles, params=self.params, urdf_base2kin_base=urdf_base2kin_base, - ee_kin2ee=self.ee_kin2ee) + ee_kin2ee=self.ee_kin2ee, + ) if joint_angles is None: if self.fallback is None: diff --git a/deformable_gym/robots/mia_hand.py b/deformable_gym/robots/mia_hand.py index 96e3ec0..c204e82 100644 --- a/deformable_gym/robots/mia_hand.py +++ b/deformable_gym/robots/mia_hand.py @@ -5,27 +5,33 @@ import numpy as np import numpy.typing as npt +from pybullet_utils import bullet_client as bc from ..helpers.pybullet_helper import Joint from ..robots.bullet_robot import BulletRobot, HandMixin, RobotCommandWrapper from ..robots.control_mixins import PositionControlMixin, VelocityControlMixin from ..robots.sensors import MiaHandForceSensors -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") + "robots/urdf/mia_hand.urdf", +) class MiaHandMixin(HandMixin): # motor array, which includes all revolute joints, we omit j_thumb_opp, # j_thumb_opp cannot be controlled actively actuated_simulated_joints: List[str] = [ - "j_index_fle", "j_little_fle", "j_mrl_fle", "j_ring_fle", "j_thumb_fle" + "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 @@ -50,14 +56,16 @@ def set_thumb_opp(self, thumb_adducted: bool): self.motors["j_thumb_opp"].init_pos = -0.628 def update_current_hand_command( - self, motors: Dict[str, Joint], command: npt.ArrayLike): + self, motors: Dict[str, Joint], command: npt.ArrayLike + ): """Translates hand commands and updates current command. :param motors: Joints. :param command: 3D joint command array (fle_index, fle_mrl, fle_thumb) """ - assert len(command) == 3, \ - f"expected command to have length 3, got {command=} instead" + assert ( + len(command) == 3 + ), f"expected command to have length 3, got {command=} instead" hand_joint_target = self.convert_action_to_pybullet(command) @@ -76,7 +84,8 @@ def convert_action_to_pybullet(self, command): 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, 4, self.motors["j_thumb_opp"].init_pos + ) return hand_joint_target def get_joint_limits(self) -> Tuple[np.ndarray, np.ndarray]: @@ -122,20 +131,28 @@ class MiaHand(MiaHandMixin, BulletRobot, abc.ABC): :param base_commands: Control pose of the hand (add 7 components to action space). """ + def __init__( - 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): + 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__( - 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) + 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) @@ -144,7 +161,8 @@ def __init__( self._correct_index_limit() self.mia_hand_force_sensors = MiaHandForceSensors( - self._id, self._joint_name_to_joint_id, self.pb_client) + self._id, self._joint_name_to_joint_id, self.pb_client + ) self.thumb_adducted = True @@ -188,6 +206,3 @@ def __init__(self, *args, **kwargs): class MiaHandVelocity(VelocityControlMixin, MiaHand): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) - - - diff --git a/deformable_gym/robots/sensors.py b/deformable_gym/robots/sensors.py index c26fdd1..a50025c 100644 --- a/deformable_gym/robots/sensors.py +++ b/deformable_gym/robots/sensors.py @@ -1,6 +1,5 @@ import matplotlib.pyplot as plt import numpy as np - from pybullet_utils import bullet_client as bc INDEX_SENSOR_NAME = "j_index_sensor" @@ -69,15 +68,21 @@ class MiaHandForceSensors: Sensor values of an episode will be stored in debug mode. """ - SENSOR_NAMES = ["middle tangential", "index normal", "index tangential", - "thumb tangential", "thumb normal", "middle normal"] + SENSOR_NAMES = [ + "middle tangential", + "index normal", + "index tangential", + "thumb tangential", + "thumb normal", + "middle normal", + ] def __init__( - self, - robot_id: int, - joint_name_to_joint_id: dict, - pb_client: bc.BulletClient, - debug: bool = False, + 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] @@ -89,10 +94,11 @@ def __init__( for sensor_id in [ self.index_sensor_id, self.middle_sensor_id, - self.thumb_sensor_id + self.thumb_sensor_id, ]: self.pb_client.enableJointForceTorqueSensor( - self.robot_id, sensor_id) + self.robot_id, sensor_id + ) if self.debug: self.history = [] @@ -119,8 +125,10 @@ def get_limits(self): upper : array, shape (6,) Upper limits of sensor values. """ - return (np.array([-204.0, -512.0, -204.0, -285.0, -512.0, -512.0]), - np.array([204.0, 512.0, 204.0, 285.0, 512.0, 512.0])) + return ( + np.array([-204.0, -512.0, -204.0, -285.0, -512.0, -512.0]), + np.array([204.0, 512.0, 204.0, 285.0, 512.0, 512.0]), + ) def measure(self, out=None): """Take a force measurement. @@ -143,7 +151,8 @@ def measure(self, out=None): self.pb_client.getJointState( self.robot_id, self.index_sensor_id, - )[2]) + )[2] + ) out[1] = T_ind_z * MM_PER_M / 2.006 out[2] = T_ind_x * MM_PER_M / 2.854 @@ -151,7 +160,8 @@ def measure(self, out=None): self.pb_client.getJointState( self.robot_id, self.middle_sensor_id, - )[2]) + )[2] + ) out[5] = T_mrl_z * MM_PER_M / 2.006 out[0] = T_mrl_x * MM_PER_M / 2.854 @@ -159,7 +169,8 @@ def measure(self, out=None): self.pb_client.getJointState( self.robot_id, self.thumb_sensor_id, - )[2]) + )[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 57065c6..5de2002 100644 --- a/deformable_gym/robots/shadow_hand.py +++ b/deformable_gym/robots/shadow_hand.py @@ -5,14 +5,15 @@ import numpy as np import numpy.typing as npt +from pybullet_utils import bullet_client as bc from ..robots.bullet_robot import BulletRobot, HandMixin, RobotCommandWrapper from ..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") + "robots/urdf/shadow_hand.urdf", +) class ShadowHand(HandMixin, BulletRobot, abc.ABC): @@ -37,15 +38,18 @@ class ShadowHand(HandMixin, BulletRobot, abc.ABC): :param base_commands: Control pose of the hand (add 7 components to action space). """ + def __init__( - 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): + 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, pb_client=pb_client, @@ -54,7 +58,8 @@ def __init__( world_orn=world_orn, task_space_limit=task_space_limit, orn_limit=orn_limit, - **kwargs) + **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 67a0edd..6f65599 100644 --- a/deformable_gym/robots/ur10_shadow.py +++ b/deformable_gym/robots/ur10_shadow.py @@ -4,17 +4,21 @@ from typing import Any import numpy as np +from pybullet_utils import bullet_client as bc from ..robots.bullet_robot import BulletRobot, HandMixin, RobotCommandWrapper from ..robots.control_mixins import PositionControlMixin, VelocityControlMixin -from ..robots.inverse_kinematics import UniversalRobotAnalyticalInverseKinematics - -from pybullet_utils import bullet_client as bc +from ..robots.inverse_kinematics import ( + UniversalRobotAnalyticalInverseKinematics, +) # Shadow freq = 500 Hz # UR5 freq = 125 Hz -URDF_PATH = os.path.join(Path(os.path.dirname(__file__)).parent.parent.absolute(), "robots/urdf/shadow_hand_on_ur10.urdf") +URDF_PATH = os.path.join( + Path(os.path.dirname(__file__)).parent.parent.absolute(), + "robots/urdf/shadow_hand_on_ur10.urdf", +) class UR10Shadow(HandMixin, BulletRobot, abc.ABC): @@ -26,25 +30,30 @@ 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, - 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): + 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) + orn_limit=orn_limit, + ) self.command_counter = 0 self.debug_visualization = debug_visualization - self._arm_motors = {k: v for k, v in self.motors.items() if "ur10_" in k} + self._arm_motors = { + k: v for k, v in self.motors.items() if "ur10_" in k + } self._hand_motors = {k: v for k, v in self.motors.items() if "rh_" in k} def hand_command_wrapper(): @@ -65,7 +74,9 @@ def arm_command_wrapper(): UniversalRobotAnalyticalInverseKinematics( robot_type="ur10", end_effector_name=end_effector_link, - fallback=self.inverse_kinematics_solver)) + fallback=self.inverse_kinematics_solver, + ) + ) def perform_command(self, command): """ @@ -73,17 +84,17 @@ def perform_command(self, command): command and joint-space hand command. """ - assert command.shape[0] == 31, f"expected command to have shape 31, " \ - f"got {command.shape[0]} instead" + assert command.shape[0] == 31, ( + f"expected command to have shape 31, " + f"got {command.shape[0]} instead" + ) # get arm joint targets - arm_target = self.arm_command_to_joint_targets( - command[:7], False) + arm_target = self.arm_command_to_joint_targets(command[:7], False) super().update_current_command(arm_target) # get hand joint actions - hand_target = self.hand_command_to_joint_targets( - command[7:], False) + hand_target = self.hand_command_to_joint_targets(command[7:], False) super().update_current_command(hand_target) def arm_command_to_joint_targets(self, command, velocity_commands=False): @@ -94,14 +105,16 @@ def arm_command_to_joint_targets(self, command, velocity_commands=False): target positions or velocities :return: 6D array of UR10 target joint positions """ - assert command.shape[0] == 7, \ - f"expected command to have shape 7, got {command.shape[0]} instead" + assert ( + command.shape[0] == 7 + ), f"expected command to have shape 7, got {command.shape[0]} instead" # convert arm command to joint motor control arm_joint_angles = self.get_joint_positions(self._arm_motors) - arm_joint_target = self.compute_ik(arm_joint_angles, command[:3], - command[3:], velocity_commands) + arm_joint_target = self.compute_ik( + arm_joint_angles, command[:3], command[3:], velocity_commands + ) target_dict = {k: t for k, t in zip(self._arm_motors, arm_joint_target)} @@ -115,8 +128,9 @@ def hand_command_to_joint_targets(self, command, velocity_commands=False): target positions or velocities :return: 24D array of Mia hand target joint positions """ - assert command.shape[0] == 24, \ - f"expected command to have shape 24, got {command.shape[0]} instead" + assert ( + command.shape[0] == 24 + ), f"expected command to have shape 24, got {command.shape[0]} instead" if velocity_commands: current_joint_angles = self.get_joint_positions(self._hand_motors) @@ -126,7 +140,9 @@ def hand_command_to_joint_targets(self, command, velocity_commands=False): # add hand joint actions hand_joint_target = current_joint_angles + command - target_dict = {k: t for k, t in zip(self._hand_motors, hand_joint_target)} + target_dict = { + k: t for k, t in zip(self._hand_motors, hand_joint_target) + } return target_dict diff --git a/deformable_gym/robots/ur5_mia.py b/deformable_gym/robots/ur5_mia.py index 27f6ff7..cc4494d 100644 --- a/deformable_gym/robots/ur5_mia.py +++ b/deformable_gym/robots/ur5_mia.py @@ -3,20 +3,23 @@ from pathlib import Path import numpy.typing as npt +from pybullet_utils import bullet_client as bc from ..robots.bullet_robot import BulletRobot, RobotCommandWrapper from ..robots.control_mixins import PositionControlMixin, VelocityControlMixin -from ..robots.inverse_kinematics import UniversalRobotAnalyticalInverseKinematics +from ..robots.inverse_kinematics import ( + UniversalRobotAnalyticalInverseKinematics, +) from ..robots.sensors import MiaHandForceSensors - -from pybullet_utils import bullet_client as bc - from .mia_hand import MiaHandMixin # Mia freq = 20 Hz # UR5 freq = 125 Hz -URDF_PATH = os.path.join(Path(os.path.dirname(__file__)).parent.parent.absolute(), "robots/urdf/mia_hand_on_ur5.urdf") +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): @@ -28,21 +31,23 @@ 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, - pb_client: bc.BulletClient, - verbose: bool = False, - task_space_limit=None, - end_effector_link="ur5_tool0", - orn_limit=None, - debug_visualization=True): + 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 + orn_limit=orn_limit, ) self.debug_visualization = debug_visualization @@ -51,13 +56,15 @@ def __init__( self._hand_motors = {k: v for k, v in self.motors.items() if "j_" in k} hand_command_wrapper = RobotCommandWrapper( - self, self.actuated_simulated_joints) + self, self.actuated_simulated_joints + ) self.subsystems["hand"] = (20, hand_command_wrapper) arm_command_wrapper = RobotCommandWrapper(self, self._arm_motors) self.subsystems["arm"] = (125, arm_command_wrapper) self.mia_hand_force_sensors = MiaHandForceSensors( - self._id, self._joint_name_to_joint_id, self.pb_client) + self._id, self._joint_name_to_joint_id, self.pb_client + ) if self.debug_visualization: self._init_debug_visualizations() @@ -67,7 +74,9 @@ def __init__( self.set_inverse_kinematics_solver( UniversalRobotAnalyticalInverseKinematics( end_effector_name=end_effector_link, - fallback=self.inverse_kinematics_solver)) + fallback=self.inverse_kinematics_solver, + ) + ) self._correct_index_limit() @@ -84,8 +93,9 @@ def perform_command(self, command): self.command_to_joint_targets(command) def command_to_joint_targets(self, command: npt.ArrayLike): - assert len(command) == 10, \ - f"expected command to have shape 10, got {len(command)} instead" + assert ( + len(command) == 10 + ), f"expected command to have shape 10, got {len(command)} instead" # get arm joint targets self.arm_command_to_joint_targets(command[:7]) # get hand joint actions @@ -97,14 +107,16 @@ def arm_command_to_joint_targets(self, command: npt.ArrayLike): :param command: End-effector pose (x, y, z, qx, qy, qz, qw) target positions or velocities """ - assert len(command) == 7, \ - f"expected command to have shape 7, got {len(command)} instead" + assert ( + len(command) == 7 + ), f"expected command to have shape 7, got {len(command)} instead" # convert arm command to joint motor control arm_joint_angles = self.get_joint_positions(self._arm_motors) arm_joint_target = self.compute_ik( - arm_joint_angles, command[:3], command[3:], False) + arm_joint_angles, command[:3], command[3:], False + ) targets = {k: t for k, t in zip(self._arm_motors, arm_joint_target)} diff --git a/deformable_gym/robots/ur_kinematics.py b/deformable_gym/robots/ur_kinematics.py index 986fc3e..f6b6ba3 100644 --- a/deformable_gym/robots/ur_kinematics.py +++ b/deformable_gym/robots/ur_kinematics.py @@ -34,6 +34,7 @@ class UR10Params: d5 = 0.1157 d6 = 0.0922 + # TODO determine parameters for UR10e ZERO_THRESH = 1e-8 @@ -42,67 +43,107 @@ 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], - [-2.47332002e-01, -1.45329037e-05, 9.68930792e-01, -4.44860194e-03], - [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]]), - "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]]) -} - -urdf_base2kin_base = np.array([ - [1., 0., 0., 0.], - [0., 1., 0., 0.], - [0., 0., 1., -1.], - [0., 0., 0., 1.]]) -robot_params = { - "ur3": UR3Params, - "ur5": UR5Params, - "ur10": UR10Params + "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], + [-2.47332002e-01, -1.45329037e-05, 9.68930792e-01, -4.44860194e-03], + [0.00000000e00, 0.00000000e00, 0.00000000e00, 1.00000000e00], + ] + ), + "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.00000000e00, 0.00000000e00, 0.00000000e00, 1.00000000e00], + ] + ), + "rh_forearm": np.array( + [ # TODO: configure this? + [6.34136230e-07, -9.99999683e-01, -7.96326458e-04, 0.00000000e00], + [7.96326458e-04, 7.96326711e-04, -9.99999366e-01, 0.00000000e00], + [9.99999683e-01, 0.00000000e00, 7.96326711e-04, 0.00000000e00], + [0.00000000e00, 0.00000000e00, 0.00000000e00, 1.00000000e00], + ] + ), } - -joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", - "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] -link_names = ["base_link", "shoulder_link", "upper_arm_link", "forearm_link", - "wrist_1_link", "wrist_2_link", "wrist_3_link", "ee_link"] -joint_min = [-3.141592653589793, -3.141592653589793, 0.0, - -3.141592653589793, -3.141592653589793, -0.7853981633974483] -joint_max = [3.141592653589793, 0.7400196028455958, 2.93005874824808, - 3.141592653589793, 3.141592653589793, 5.497787143782138] +urdf_base2kin_base = np.array( + [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, -1.0], + [0.0, 0.0, 0.0, 1.0], + ] +) +robot_params = {"ur3": UR3Params, "ur5": UR5Params, "ur10": UR10Params} + + +joint_names = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] +link_names = [ + "base_link", + "shoulder_link", + "upper_arm_link", + "forearm_link", + "wrist_1_link", + "wrist_2_link", + "wrist_3_link", + "ee_link", +] +joint_min = [ + -3.141592653589793, + -3.141592653589793, + 0.0, + -3.141592653589793, + -3.141592653589793, + -0.7853981633974483, +] +joint_max = [ + 3.141592653589793, + 0.7400196028455958, + 2.93005874824808, + 3.141592653589793, + 3.141592653589793, + 5.497787143782138, +] def analytical_ik( - pose, - last_joint_angles, - params=UR5Params, - urdf_base2kin_base=urdf_base2kin_base, - ee_kin2ee=ee_kin2ee_options["ur5_tool0"] + pose, + 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.""" ee_link2urdf_base = pt.transform_from( R=pr.matrix_from_quaternion([pose[-1], pose[3], pose[4], pose[5]]), - p=pose[:3]) + p=pose[:3], + ) return analytical_ik_homogeneous( - ee_link2urdf_base, last_joint_angles, params, urdf_base2kin_base, - ee_kin2ee) + ee_link2urdf_base, + last_joint_angles, + params, + urdf_base2kin_base, + ee_kin2ee, + ) def analytical_ik_homogeneous( - ee_link2urdf_base, - last_joint_angles, - params=UR5Params, - urdf_base2kin_base=urdf_base2kin_base, - ee_kin2ee=ee_kin2ee_options["ur5_tool0"]): + 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.""" ee_link2kin_base = pt.concat(ee_link2urdf_base, urdf_base2kin_base) ee2kin_base = pt.concat(ee_kin2ee, ee_link2kin_base) @@ -118,8 +159,12 @@ def analytical_ik_homogeneous( if len(valid_solutions) > 0: solution = np.copy(last_joint_angles) best_solution_idx = np.argmin( - np.linalg.norm(np.array(valid_solutions) - - last_joint_angles[np.newaxis, :n_joints], axis=1)) + np.linalg.norm( + np.array(valid_solutions) + - last_joint_angles[np.newaxis, :n_joints], + axis=1, + ) + ) solution[:n_joints] = valid_solutions[best_solution_idx] return solution else: @@ -149,21 +194,34 @@ def _forward(q, params): T[0, 0] = c234 * c1 * s5 - c5 * s1 T[0, 1] = c6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * s6 T[0, 2] = -s6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * c6 - T[0, 3] = (params.d6 * c234 * c1 * s5 - params.a3 * c23 * c1 - - params.a2 * c1 * c2 - params.d6 * c5 * s1 - params.d5 * s234 * c1 - - params.d4 * s1) + T[0, 3] = ( + params.d6 * c234 * c1 * s5 + - params.a3 * c23 * c1 + - params.a2 * c1 * c2 + - params.d6 * c5 * s1 + - params.d5 * s234 * c1 + - params.d4 * s1 + ) T[1, 0] = c1 * c5 + c234 * s1 * s5 T[1, 1] = -c6 * (c1 * s5 - c234 * c5 * s1) - s234 * s1 * s6 T[1, 2] = s6 * (c1 * s5 - c234 * c5 * s1) - s234 * c6 * s1 - T[1, 3] = (params.d6 * (c1 * c5 + c234 * s1 * s5) + params.d4 * c1 - - params.a3 * c23 * s1 - params.a2 * c2 * s1 - - params.d5 * s234 * s1) + T[1, 3] = ( + params.d6 * (c1 * c5 + c234 * s1 * s5) + + params.d4 * c1 + - params.a3 * c23 * s1 + - params.a2 * c2 * s1 + - params.d5 * s234 * s1 + ) T[2, 0] = -s234 * s5 T[2, 1] = -c234 * s6 - s234 * c5 * c6 T[2, 2] = s234 * c5 * s6 - c234 * c6 - T[2, 3] = (params.d1 + params.a3 * s23 + params.a2 * s2 - - params.d5 * (c23 * c4 - s23 * s4) - - params.d6 * s5 * (c23 * s4 + s23 * c4)) + T[2, 3] = ( + params.d1 + + params.a3 * s23 + + params.a2 * s2 + - params.d5 * (c23 * c4 - s23 * s4) + - params.d6 * s5 * (c23 * s4 + s23 * c4) + ) T[3, 0] = 0.0 T[3, 1] = 0.0 T[3, 2] = 0.0 @@ -239,7 +297,7 @@ def _inverse(T, q6_des, params): q5 = np.empty((2, 2)) for i in range(2): - numer = (T03 * math.sin(q1[i]) - T13 * math.cos(q1[i]) - params.d4) + numer = T03 * math.sin(q1[i]) - T13 * math.cos(q1[i]) - params.d4 if abs(abs(numer) - abs(params.d6)) < ZERO_THRESH: div = _fast_sign(numer) * _fast_sign(params.d6) else: @@ -259,8 +317,10 @@ def _inverse(T, q6_des, params): if abs(s5) < ZERO_THRESH: q6 = q6_des else: - q6 = math.atan2(_fast_sign(s5) * -(T01 * s1 - T11 * c1), - _fast_sign(s5) * (T00 * s1 - T10 * c1)) + q6 = math.atan2( + _fast_sign(s5) * -(T01 * s1 - T11 * c1), + _fast_sign(s5) * (T00 * s1 - T10 * c1), + ) if abs(q6) < ZERO_THRESH: q6 = 0.0 @@ -270,14 +330,30 @@ def _inverse(T, q6_des, params): # RRR joints (q2,q3,q4) c6 = math.cos(q6) s6 = math.sin(q6) - x04x = -s5 * (T02 * c1 + T12 * s1) - c5 * (s6 * (T01 * c1 + T11 * s1) - c6 * (T00 * c1 + T10 * s1)) + x04x = -s5 * (T02 * c1 + T12 * s1) - c5 * ( + s6 * (T01 * c1 + T11 * s1) - c6 * (T00 * c1 + T10 * s1) + ) x04y = c5 * (T20 * c6 - T21 * s6) - T22 * s5 - p13x = (params.d5 * (s6 * (T00 * c1 + T10 * s1) + c6 * (T01 * c1 + T11 * s1)) - - params.d6 * (T02 * c1 + T12 * s1) + T03 * c1 + T13 * s1) - p13y = T23 - params.d1 - params.d6 * T22 + params.d5 * (T21 * c6 + T20 * s6) - - c3 = ((p13x * p13x + p13y * p13y - params.a2 * params.a2 - params.a3 * params.a3) - / (2.0 * params.a2 * params.a3)) + p13x = ( + params.d5 + * (s6 * (T00 * c1 + T10 * s1) + c6 * (T01 * c1 + T11 * s1)) + - params.d6 * (T02 * c1 + T12 * s1) + + T03 * c1 + + T13 * s1 + ) + p13y = ( + T23 + - params.d1 + - params.d6 * T22 + + params.d5 * (T21 * c6 + T20 * s6) + ) + + c3 = ( + p13x * p13x + + p13y * p13y + - params.a2 * params.a2 + - params.a3 * params.a3 + ) / (2.0 * params.a2 * params.a3) if abs(abs(c3) - 1.0) < ZERO_THRESH: c3 = _fast_sign(c3) elif abs(c3) > 1.0: @@ -285,18 +361,30 @@ def _inverse(T, q6_des, params): arccos = math.acos(c3) q3[0] = arccos q3[1] = TWO_PI - arccos - denom = params.a2 * params.a2 + params.a3 * params.a3 + 2 * params.a2 * params.a3 * c3 + denom = ( + params.a2 * params.a2 + + params.a3 * params.a3 + + 2 * params.a2 * params.a3 * c3 + ) s3 = math.sin(arccos) A = params.a2 + params.a3 * c3 B = params.a3 * s3 - q2[0] = math.atan2((A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom) - q2[1] = math.atan2((A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom) + q2[0] = math.atan2( + (A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom + ) + q2[1] = math.atan2( + (A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom + ) c23_0 = math.cos(q2[0] + q3[0]) s23_0 = math.sin(q2[0] + q3[0]) c23_1 = math.cos(q2[1] + q3[1]) s23_1 = math.sin(q2[1] + q3[1]) - q4[0] = math.atan2(c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0) - q4[1] = math.atan2(c23_1 * x04y - s23_1 * x04x, x04x * c23_1 + x04y * s23_1) + q4[0] = math.atan2( + c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0 + ) + q4[1] = math.atan2( + c23_1 * x04y - s23_1 * x04x, x04x * c23_1 + x04y * s23_1 + ) for k in range(2): if abs(q2[k]) < ZERO_THRESH: diff --git a/doc/source/conf.py b/doc/source/conf.py index c15b514..23d4d6c 100644 --- a/doc/source/conf.py +++ b/doc/source/conf.py @@ -17,12 +17,14 @@ # -- Project information ----------------------------------------------------- -project = 'DeformableGym' -copyright = '2022, Melvin Laux, Alexander Fabisch, Johannes Brust, Chandandeep Singh' -author = 'Melvin Laux, Alexander Fabisch, Johannes Brust, Chandandeep Singh' +project = "DeformableGym" +copyright = ( + "2022, Melvin Laux, Alexander Fabisch, Johannes Brust, Chandandeep Singh" +) +author = "Melvin Laux, Alexander Fabisch, Johannes Brust, Chandandeep Singh" # The full version, including alpha/beta/rc tags -release = '0.1' +release = "0.1" # -- General configuration --------------------------------------------------- @@ -36,7 +38,7 @@ ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. @@ -49,9 +51,9 @@ # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'alabaster' +html_theme = "alabaster" # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] diff --git a/examples/parallel_floating_mia_example.py b/examples/parallel_floating_mia_example.py index 3490612..6418e26 100644 --- a/examples/parallel_floating_mia_example.py +++ b/examples/parallel_floating_mia_example.py @@ -43,4 +43,3 @@ env.close() env2.close() - diff --git a/examples/uniform_initialisation_example.py b/examples/uniform_initialisation_example.py index 8a7172a..d479282 100644 --- a/examples/uniform_initialisation_example.py +++ b/examples/uniform_initialisation_example.py @@ -23,7 +23,8 @@ sampler = UniformSampler(low, high, seed=0) env = gymnasium.make( - "FloatingMiaGraspInsole-v0", initial_state_sampler=sampler, gui=True) + "FloatingMiaGraspInsole-v0", initial_state_sampler=sampler, gui=True +) obs, info = env.reset() episode_return = 0 diff --git a/examples/ur10_shadow_grasp_example.py b/examples/ur10_shadow_grasp_example.py index 39edb8a..184f7d1 100644 --- a/examples/ur10_shadow_grasp_example.py +++ b/examples/ur10_shadow_grasp_example.py @@ -10,10 +10,7 @@ """ -env = UR10ShadowGraspEnv( - gui=True, - object_name="insole" -) +env = UR10ShadowGraspEnv(gui=True, object_name="insole") env.reset() episode_return = 0 diff --git a/examples/ur5_mia_grasp_example.py b/examples/ur5_mia_grasp_example.py index 36d1182..68335ee 100644 --- a/examples/ur5_mia_grasp_example.py +++ b/examples/ur5_mia_grasp_example.py @@ -10,10 +10,7 @@ """ -env = UR5MiaGraspEnv( - gui=True, - object_name="insole2" -) +env = UR5MiaGraspEnv(gui=True, object_name="insole2") env.reset() episode_return = 0 diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..96baba5 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,7 @@ +[tool.black] +line-length = 80 +target-version = ["py38", "py39", "py310", "py311"] + +[tool.isort] +profile = "black" +line_length = 80 \ No newline at end of file diff --git a/setup.py b/setup.py index f82ba30..c8a9ac6 100644 --- a/setup.py +++ b/setup.py @@ -1,6 +1,5 @@ from setuptools import setup - if __name__ == "__main__": with open("README.md", "r") as f: long_description = f.read() diff --git a/tests/conftest.py b/tests/conftest.py index 22b3ea2..388507e 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -5,29 +5,58 @@ MIA_SENSORS = ["j_index_sensor", "j_middle_sensor", "j_thumb_sensor"] MIA_MOTORS = [ - "j_thumb_fle", "j_thumb_opp", + "j_thumb_fle", + "j_thumb_opp", "j_index_fle", - "j_mrl_fle", "j_ring_fle", "j_little_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" + "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_shoulder_pan_joint", + "ur5_shoulder_lift_joint", "ur5_elbow_joint", - "ur5_wrist_1_joint", "ur5_wrist_2_joint", "ur5_wrist_3_joint" + "ur5_wrist_1_joint", + "ur5_wrist_2_joint", + "ur5_wrist_3_joint", ] UR10_MOTORS = [ - "ur10_shoulder_pan_joint", "ur10_shoulder_lift_joint", + "ur10_shoulder_pan_joint", + "ur10_shoulder_lift_joint", "ur10_elbow_joint", - "ur10_wrist_1_joint", "ur10_wrist_2_joint", "ur10_wrist_3_joint" + "ur10_wrist_1_joint", + "ur10_wrist_2_joint", + "ur10_wrist_3_joint", ] diff --git a/tests/envs/test_floating_mia_grasp_env.py b/tests/envs/test_floating_mia_grasp_env.py index b347448..af8be17 100644 --- a/tests/envs/test_floating_mia_grasp_env.py +++ b/tests/envs/test_floating_mia_grasp_env.py @@ -11,7 +11,7 @@ def env(): verbose=False, horizon=10, object_name="insole_on_conveyor_belt/back", - #observable_object_pos=True, + # observable_object_pos=True, ) diff --git a/tests/envs/test_floating_shadow_grasp_env.py b/tests/envs/test_floating_shadow_grasp_env.py index 4bcbb0a..8420803 100644 --- a/tests/envs/test_floating_shadow_grasp_env.py +++ b/tests/envs/test_floating_shadow_grasp_env.py @@ -1,7 +1,7 @@ import pytest from numpy.testing import assert_allclose -from deformable_gym.envs.floating_shadow_grasp_env import (FloatingShadowGraspEnv) +from deformable_gym.envs.floating_shadow_grasp_env import FloatingShadowGraspEnv @pytest.fixture diff --git a/tests/envs/test_parallel_envs.py b/tests/envs/test_parallel_envs.py index 76837b6..e48483d 100644 --- a/tests/envs/test_parallel_envs.py +++ b/tests/envs/test_parallel_envs.py @@ -4,16 +4,8 @@ def test_parallel_envs(): - env = gymnasium.make( - "FloatingMiaGraspInsole-v0", - gui=False, - horizon=10 - ) - env2 = gymnasium.make( - "FloatingMiaGraspInsole-v0", - gui=False, - horizon=10 - ) + 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 diff --git a/tests/envs/test_samplers.py b/tests/envs/test_samplers.py index 6751370..ffb39fb 100644 --- a/tests/envs/test_samplers.py +++ b/tests/envs/test_samplers.py @@ -1,10 +1,14 @@ -import pytest import numpy as np import numpy.typing as npt +import pytest from numpy.testing import assert_allclose -from deformable_gym.envs.sampler import FixedSampler, GaussianSampler, \ - UniformSampler, GridSampler +from deformable_gym.envs.sampler import ( + FixedSampler, + GaussianSampler, + GridSampler, + UniformSampler, +) SEED = 0 @@ -42,18 +46,14 @@ def fixed_sampler(fixed_target_pose: npt.NDArray) -> FixedSampler: @pytest.fixture def gaussian_sampler() -> GaussianSampler: return GaussianSampler( - mu=np.array([1, 2, 3]), - sigma=np.array([2, 3, 4]), - seed=SEED + mu=np.array([1, 2, 3]), sigma=np.array([2, 3, 4]), seed=SEED ) @pytest.fixture def uniform_sampler() -> UniformSampler: return UniformSampler( - low=np.array([1, 2, 3]), - high=np.array([2, 3, 4]), - seed=SEED + low=np.array([1, 2, 3]), high=np.array([2, 3, 4]), seed=SEED ) @@ -62,38 +62,32 @@ def grid_sampler() -> GridSampler: return GridSampler( low=np.array([1, 2, 3, 4]), high=np.array([2, 3, 4, 5]), - n_points_per_axis=np.array([5, 3, 1, 1]) + n_points_per_axis=np.array([5, 3, 1, 1]), ) def test_fixed_sampler( - fixed_sampler: FixedSampler, - fixed_target_pose: npt.NDArray + fixed_sampler: FixedSampler, fixed_target_pose: npt.NDArray ): sampled_pose = fixed_sampler.sample_initial_pose() assert_allclose(sampled_pose, fixed_target_pose) def test_gaussian_sampler( - gaussian_sampler: GaussianSampler, - gaussian_target_pose: npt.NDArray + gaussian_sampler: GaussianSampler, gaussian_target_pose: npt.NDArray ): sampled_pose = gaussian_sampler.sample_initial_pose() assert_allclose(sampled_pose, gaussian_target_pose) def test_uniform_sampler( - uniform_sampler: UniformSampler, - uniform_target_pose: npt.NDArray + uniform_sampler: UniformSampler, uniform_target_pose: npt.NDArray ): sampled_pose = uniform_sampler.sample_initial_pose() assert_allclose(sampled_pose, uniform_target_pose) -def test_grid_sampler( - grid_sampler: GridSampler, - grid_target_pose: npt.NDArray -): +def test_grid_sampler(grid_sampler: GridSampler, grid_target_pose: npt.NDArray): sampled_pose = grid_sampler.sample_initial_pose() for i in range(20): @@ -101,4 +95,3 @@ def test_grid_sampler( assert len(grid_sampler.grid) == 15 assert_allclose(sampled_pose, grid_target_pose) - diff --git a/tests/envs/test_ur10_shadow_grasp_env.py b/tests/envs/test_ur10_shadow_grasp_env.py index 8089f58..77c8f54 100644 --- a/tests/envs/test_ur10_shadow_grasp_env.py +++ b/tests/envs/test_ur10_shadow_grasp_env.py @@ -1,6 +1,6 @@ import pytest - from numpy.testing import assert_allclose + 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 0ba097e..11af06f 100644 --- a/tests/envs/test_ur5_mia_grasp_env.py +++ b/tests/envs/test_ur5_mia_grasp_env.py @@ -1,6 +1,6 @@ import pytest - from numpy.testing import assert_allclose + from deformable_gym.envs.ur5_mia_grasp_env import UR5MiaGraspEnv @@ -11,7 +11,7 @@ def env(): verbose=False, horizon=10, object_name="insole", - #observable_object_pos=True, + # observable_object_pos=True, ) @@ -83,4 +83,3 @@ def test_eps_done(env): obs, reward, terminated, truncated, info = env.step(action) assert terminated - diff --git a/tests/objects/test_box.py b/tests/objects/test_box.py index c628ca5..8ec8323 100644 --- a/tests/objects/test_box.py +++ b/tests/objects/test_box.py @@ -12,7 +12,8 @@ def test_box(simulation): obj, _, _ = ObjectFactory(simulation.pb_client).create( - "box", object_position=TEST_POS, object_orientation=TEST_ORN) + "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])) @@ -20,4 +21,3 @@ def test_box(simulation): assert len(vertices) == 0 assert obj.get_id() == 0 - diff --git a/tests/objects/test_capsule.py b/tests/objects/test_capsule.py index 062d87d..b9c2492 100644 --- a/tests/objects/test_capsule.py +++ b/tests/objects/test_capsule.py @@ -9,7 +9,8 @@ def test_capsule_creation(simulation): obj, _, _ = ObjectFactory(simulation.pb_client).create( - "capsule", object_position=TEST_POS, object_orientation=TEST_ORN) + "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 f7a4a7c..fa6cabf 100644 --- a/tests/objects/test_cylinder.py +++ b/tests/objects/test_cylinder.py @@ -9,7 +9,8 @@ def test_cylinder_creation(simulation): obj, _, _ = ObjectFactory(simulation.pb_client).create( - "cylinder", object_position=TEST_POS, object_orientation=TEST_ORN) + "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 aa4c56a..7a0b3ef 100644 --- a/tests/objects/test_insole.py +++ b/tests/objects/test_insole.py @@ -12,7 +12,8 @@ def test_insole_creation(simulation): obj, _, _ = ObjectFactory(simulation.pb_client).create( - "insole", object_position=TEST_POS, object_orientation=TEST_ORN) + "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 7f82884..dcdb644 100644 --- a/tests/objects/test_insole_on_conveyor.py +++ b/tests/objects/test_insole_on_conveyor.py @@ -13,7 +13,8 @@ def test_insole_ob_conveyor_creation(simulation): obj, _, _ = ObjectFactory(simulation.pb_client).create( "insole_on_conveyor_belt/back", object_position=TEST_POS, - object_orientation=TEST_ORN) + 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 542fbf3..39facca 100644 --- a/tests/objects/test_pillow.py +++ b/tests/objects/test_pillow.py @@ -11,7 +11,9 @@ def test_pillow(simulation): - obj, _, _ = ObjectFactory(simulation.pb_client).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 e4c6233..c653ca1 100644 --- a/tests/objects/test_sphere.py +++ b/tests/objects/test_sphere.py @@ -12,9 +12,8 @@ def test_sphere(simulation): obj, _, _ = ObjectFactory(simulation.pb_client).create( - "sphere", - object_position=TEST_POS, - object_orientation=TEST_ORN) + "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 216c130..2e2345a 100644 --- a/tests/objects/test_urdf_object.py +++ b/tests/objects/test_urdf_object.py @@ -16,7 +16,8 @@ def test_urdf_object_creation(simulation): simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, - fixed=True) + 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 c3beffa..205ea28 100644 --- a/tests/robots/test_mia_hand_position.py +++ b/tests/robots/test_mia_hand_position.py @@ -15,7 +15,8 @@ def robot(simulation): pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, - base_commands=True) + base_commands=True, + ) robot.set_thumb_opp(thumb_adducted=True) return robot @@ -38,7 +39,9 @@ def test_mia_hand_position_creation(simulation, robot, mia_motors, mia_sensors): @pytest.mark.skip("TODO") def test_mia_hand_position_base_movement(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])) + 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]) + ) for _ in range(50): simulation.step_to_trigger("time_step") diff --git a/tests/robots/test_mia_hand_velocity.py b/tests/robots/test_mia_hand_velocity.py index 6b6672d..f3e8e2e 100644 --- a/tests/robots/test_mia_hand_velocity.py +++ b/tests/robots/test_mia_hand_velocity.py @@ -15,7 +15,8 @@ def robot(simulation): pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, - base_commands=True) + base_commands=True, + ) robot.set_thumb_opp(thumb_adducted=True) return robot @@ -30,7 +31,9 @@ def test_mia_hand_velocity_initial_pose(simulation, robot): assert_almost_equal(actual_pose, expected_pose) -def test_mia_hand_velocity_motor_creation(simulation, robot, mia_motors, mia_sensors): +def test_mia_hand_velocity_motor_creation( + simulation, robot, mia_motors, mia_sensors +): # check motor creation assert set(robot.motors.keys()) == set(mia_motors) @@ -43,7 +46,9 @@ def test_mia_hand_velocity_base_movement(simulation, robot): simulation.add_robot(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])) + 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]) + ) for _ in range(50): simulation.step_to_trigger("time_step") diff --git a/tests/robots/test_shadow_hand_position.py b/tests/robots/test_shadow_hand_position.py index 9fe7edc..57009d0 100644 --- a/tests/robots/test_shadow_hand_position.py +++ b/tests/robots/test_shadow_hand_position.py @@ -15,7 +15,8 @@ def robot(simulation): pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, - base_commands=True) + base_commands=True, + ) return robot @@ -45,8 +46,44 @@ def test_shadow_hand_position_motor_creation(simulation, robot, shadow_motors): def test_shadow_hand_position_base_movement(simulation, robot): simulation.add_robot(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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) + 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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) + ) for _ in range(50): simulation.step_to_trigger("time_step") diff --git a/tests/robots/test_shadow_hand_velocity.py b/tests/robots/test_shadow_hand_velocity.py index bed9aff..46b99bd 100644 --- a/tests/robots/test_shadow_hand_velocity.py +++ b/tests/robots/test_shadow_hand_velocity.py @@ -15,7 +15,8 @@ def robot(simulation): pb_client=simulation.pb_client, world_pos=TEST_POS, world_orn=TEST_ORN, - base_commands=True) + base_commands=True, + ) return robot @@ -39,8 +40,44 @@ def test_shadow_hand_velocity_base_movement(simulation, robot): simulation.add_robot(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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) + 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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) + ) for _ in range(50): simulation.step_to_trigger("time_step") diff --git a/tests/robots/test_ur10_shadow_position.py b/tests/robots/test_ur10_shadow_position.py index d493156..3be9639 100644 --- a/tests/robots/test_ur10_shadow_position.py +++ b/tests/robots/test_ur10_shadow_position.py @@ -21,6 +21,8 @@ def test_ur10_shadow_position_creation(simulation, robot): assert_almost_equal(actual_pose, expected_pose) -def test_ur10_shadow_position_motor_creation(simulation, robot, ur10_shadow_motors): +def test_ur10_shadow_position_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_ur10_shadow_velocity.py b/tests/robots/test_ur10_shadow_velocity.py index ad8cdac..34effb9 100644 --- a/tests/robots/test_ur10_shadow_velocity.py +++ b/tests/robots/test_ur10_shadow_velocity.py @@ -22,6 +22,7 @@ def test_ur10_shadow_velocity_creation(simulation, robot): def test_ur10_shadow_velocity_motor_creation( - simulation, robot, ur10_shadow_motors): + 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 d8b7eba..8741d71 100644 --- a/tests/robots/test_ur5_mia_position.py +++ b/tests/robots/test_ur5_mia_position.py @@ -20,7 +20,9 @@ def test_ur5_mia_position_initial_position(simulation, robot): assert_almost_equal(actual_pose, expected_pose) -def test_ur5_mia_position_creation(simulation, robot, ur5_mia_motors, ur5_mia_sensors): +def test_ur5_mia_position_creation( + simulation, robot, ur5_mia_motors, ur5_mia_sensors +): # check motor creation assert set(robot.motors.keys()) == set(ur5_mia_motors) diff --git a/tests/robots/test_ur5_mia_velocity.py b/tests/robots/test_ur5_mia_velocity.py index b40b356..98ccaac 100644 --- a/tests/robots/test_ur5_mia_velocity.py +++ b/tests/robots/test_ur5_mia_velocity.py @@ -19,13 +19,15 @@ def test_ur5_mia_velocity_creation(simulation, robot): 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) - 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])) + 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): + simulation, robot, ur5_mia_motors, ur5_mia_sensors +): # check motor creation assert set(robot.motors.keys()) == set(ur5_mia_motors) # check sensor creation assert set(robot.sensors.keys()) == set(ur5_mia_sensors) -