diff --git a/grip/robot/bullet_robot.py b/grip/robot/bullet_robot.py index 8fda7b7..971055d 100644 --- a/grip/robot/bullet_robot.py +++ b/grip/robot/bullet_robot.py @@ -101,7 +101,9 @@ def __init__(self, **kwargs): self.ee_mount_link = kwargs.get("ee_mount_link", None) if self.ee_mount_link is not None and self._joint_names is None: - _, self._joint_names = self.get_movable_joints(upto_link_name=self.ee_mount_link) + _, self._joint_names = self.get_movable_joints( + upto_link_name=self.ee_mount_link + ) self.joint_dict, self.link_dict = self.get_joint_dict() self.actuated_joint_dict = self.joint_dict.copy() @@ -226,7 +228,9 @@ def setup_gui(self) -> None: if self._ee_sliders: ee_names = ["ee_x", "ee_y", "ee_z", "ee_roll", "ee_pitch", "ee_yaw"] ee_pos, ee_ori = self.ee_pose - ee_rpy = np.array(p.getEulerFromQuaternion(ee_ori, physicsClientId=self.phys_id)) + ee_rpy = np.array( + p.getEulerFromQuaternion(ee_ori, physicsClientId=self.phys_id) + ) min_pos = [c - 1.0 for c in ee_pos] max_pos = [c + 1.0 for c in ee_pos] @@ -411,7 +415,9 @@ def get_joint_info(self, joint_id: int) -> BulletJointInfo: """ - return BulletJointInfo(*p.getJointInfo(self.id, joint_id, physicsClientId=self.phys_id)) + return BulletJointInfo( + *p.getJointInfo(self.id, joint_id, physicsClientId=self.phys_id) + ) def get_base_pose(self) -> Tuple[np.ndarray, np.ndarray]: """ @@ -420,7 +426,9 @@ def get_base_pose(self) -> Tuple[np.ndarray, np.ndarray]: Returns: (Tuple[numpy.ndarray, numpy.ndarray]): tuple containing a position shape-(3,) and unit quaternion orientation shape-(4,) representing the pose of this robot. """ - pos, ori = p.getBasePositionAndOrientation(self.id, physicsClientId=self.phys_id) + pos, ori = p.getBasePositionAndOrientation( + self.id, physicsClientId=self.phys_id + ) return np.asarray(pos), np.asarray(ori) @@ -764,7 +772,9 @@ def reset_base_pose(self, position: np.ndarray, orientation: np.ndarray) -> None orientation (numpy.ndarray): desired orientation quaternion array shape(4,). """ if self._base_cid is not None: - p.changeConstraint(self._base_cid, position, orientation, maxForce=self.max_force) + p.changeConstraint( + self._base_cid, position, orientation, maxForce=self.max_force + ) else: p.resetBasePositionAndOrientation( self.id, position, orientation, physicsClientId=self.phys_id @@ -776,7 +786,9 @@ def reset_base_pose(self, position: np.ndarray, orientation: np.ndarray) -> None self._position = position self._orientation = orientation - def set_link_pose(self, link_name: str, position: np.ndarray, orientation: np.ndarray) -> None: + def set_link_pose( + self, link_name: str, position: np.ndarray, orientation: np.ndarray + ) -> None: """ Sets a corresponding base pose such that desired link pose matches position and orientation. @@ -785,7 +797,9 @@ def set_link_pose(self, link_name: str, position: np.ndarray, orientation: np.nd orientation (numpy.ndarray): desired orientation quaternion array shape(4,). """ - transform_wb_out = self.get_link_ref_base_pose("-1base", link_name, position, orientation) + transform_wb_out = self.get_link_ref_base_pose( + "-1base", link_name, position, orientation + ) self.reset_base_pose(*transform_wb_out) @@ -957,7 +971,9 @@ def set_joint_torques(self, cmd: np.ndarray, joint_names: List[str] = None) -> N physicsClientId=self.phys_id, ) - def enable_torque_mode(self, joint_names: List[str] = None, enable: bool = True) -> None: + def enable_torque_mode( + self, joint_names: List[str] = None, enable: bool = True + ) -> None: """ By default, each revolute joint and prismatic joint is motorized using a velocity motor. You can disable those default motor by using a maximum force of 0. This will let you perform torque control. Note: If torque control is enabled, it is better to call stepsimulation in the control loop, and disable realtime sim of the pybullet world. @@ -1124,7 +1140,9 @@ def inertia(self, joint_angles: np.ndarray = None) -> np.ndarray: joint_angles = self.angles inertia_tensor = np.array( - p.calculateMassMatrix(self.id, list(joint_angles), physicsClientId=self.phys_id) + p.calculateMassMatrix( + self.id, list(joint_angles), physicsClientId=self.phys_id + ) ) return inertia_tensor[begin:end, begin:end] @@ -1141,9 +1159,13 @@ def enable_torque_sensor(self, enable: bool) -> None: Enable or disable torque_sensor. """ - p.enableJointForceTorqueSensor(self.id, self.ee_index, enable, physicsClientId=self.phys_id) + p.enableJointForceTorqueSensor( + self.id, self.ee_index, enable, physicsClientId=self.phys_id + ) - def get_contact_force(self, target_links: list, ignore_self_contact: bool = True) -> np.ndarray: + def get_contact_force( + self, target_links: list, ignore_self_contact: bool = True + ) -> np.ndarray: """ Calculate contact forces applied onto given link(s) @@ -1249,7 +1271,9 @@ def disable_collision(self) -> None: link_ids = self.get_link_ids() for lid in link_ids: - p.setCollisionFilterGroupMask(self.id, lid, 0, 0, physicsClientId=self.phys_id) + p.setCollisionFilterGroupMask( + self.id, lid, 0, 0, physicsClientId=self.phys_id + ) def set_collision_filter(self, entity: Entity, enable: bool): link_ids = self.get_link_ids() @@ -1263,7 +1287,12 @@ def set_collision_filter(self, entity: Entity, enable: bool): entity_link_ids = entity.get_link_ids() for elid in entity_link_ids: p.setCollisionFilterPair( - self.id, entity.id, lid, elid, int(enable), physicsClientId=self.phys_id + self.id, + entity.id, + lid, + elid, + int(enable), + physicsClientId=self.phys_id, ) def contact_points(self): @@ -1281,7 +1310,9 @@ def contact_points(self): return contact_points - def aabb(self, link_id: int = -1, expansion: float = 0.0) -> Tuple[np.ndarray, np.ndarray]: + def aabb( + self, link_id: int = -1, expansion: float = 0.0 + ) -> Tuple[np.ndarray, np.ndarray]: """ Gets axis-aligned bounding box of a given link of this robot. Args: @@ -1400,7 +1431,9 @@ def setup_mimic_joints( physicsClientId=self.phys_id, ) - log.debug("Actuated joint names (updated): {}".format(self.actuated_joint_names)) + log.debug( + "Actuated joint names (updated): {}".format(self.actuated_joint_names) + ) log.debug("Actuated joint IDs (updated): {}".format(self.actuated_joint_ids)) def set_colour(self, rgba_colour: np.ndarray) -> None: @@ -1442,7 +1475,9 @@ def set_link_colour(self, rgba_colour: np.ndarray, link_ids: list[int]) -> None: ), "Link ids must be valid and exist/belong to this robot." for lid in link_ids: - p.changeVisualShape(self.id, lid, rgbaColor=rgba_colour, physicsClientId=self.phys_id) + p.changeVisualShape( + self.id, lid, rgbaColor=rgba_colour, physicsClientId=self.phys_id + ) class RobotKinematics(object): diff --git a/grip/robot/collision.py b/grip/robot/collision.py index de22f27..8cb7938 100644 --- a/grip/robot/collision.py +++ b/grip/robot/collision.py @@ -35,7 +35,6 @@ def get_collision_info(entity_a: Entity, entity_b: Entity, max_distance: float = def is_colliding(entity_a: Entity, entity_b: Entity, max_distance: float = 0.0): - """Returns collision information from pair of entities Args: diff --git a/grip/robot/object.py b/grip/robot/object.py index 1ef0e53..75a030a 100644 --- a/grip/robot/object.py +++ b/grip/robot/object.py @@ -8,7 +8,9 @@ from .world import BulletWorld import uuid, os -ObjectState = TypedDict("State", {"pose": Tuple[np.ndarray, np.ndarray], "entity": Entity}) +ObjectState = TypedDict( + "State", {"pose": Tuple[np.ndarray, np.ndarray], "entity": Entity} +) class BulletObject(Entity): @@ -152,7 +154,9 @@ def _init_from_mesh(self, mesh): log.info("Mesh kind: {}".format(mesh.visual.kind)) - vertices, faces, normals, uvs = self._get_mesh_data(mesh, flip_normals=self._flip_normals) + vertices, faces, normals, uvs = self._get_mesh_data( + mesh, flip_normals=self._flip_normals + ) # normals = -normals # the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) @@ -161,7 +165,9 @@ def _init_from_mesh(self, mesh): self._tid = None - if self._mesh_colour is None and isinstance(mesh.visual, trimesh.visual.color.ColorVisuals): + if self._mesh_colour is None and isinstance( + mesh.visual, trimesh.visual.color.ColorVisuals + ): try: colour = trimesh.visual.color.to_float(mesh.visual.vertex_colors) log.info("Has colours look: {}".format(colour)) @@ -181,7 +187,9 @@ def _init_from_mesh(self, mesh): self._tid = self._parent_world.texture(self._texture_path) if colour is None: - colour = [0.8, 0.8, 0.2, 1.0] if self._mesh_colour is None else self._mesh_colour + colour = ( + [0.8, 0.8, 0.2, 1.0] if self._mesh_colour is None else self._mesh_colour + ) vid = p.createVisualShape( shapeType=p.GEOM_MESH, @@ -255,7 +263,9 @@ def reset_pose(self, pos: np.ndarray, quat: np.ndarray) -> None: if self._base_cid is not None: p.changeConstraint(self._base_cid, pos, quat, maxForce=self._max_force) else: - p.resetBasePositionAndOrientation(self._id, pos, quat, physicsClientId=self.phys_id) + p.resetBasePositionAndOrientation( + self._id, pos, quat, physicsClientId=self.phys_id + ) def set_pose(self, pos, quat) -> None: """ @@ -284,7 +294,9 @@ def set_colour(self, rgba: List[float]) -> None: """ if self._vid is None: self._vid = -1 - p.changeVisualShape(self._id, -1, self._vid, rgbaColor=rgba, physicsClientId=self.phys_id) + p.changeVisualShape( + self._id, -1, self._vid, rgbaColor=rgba, physicsClientId=self.phys_id + ) def set_friction(self, lateral_friction: float) -> None: """Sets object lateral (linear) contact friction coefficient @@ -348,8 +360,12 @@ def load_texture(self, texture: Union[List[float], str]) -> None: self.set_colour(rgbaColor) elif isinstance(texture, str) or isinstance(texture, np.str_): texture_fn = texture - assert os.path.isfile(texture_fn), "Texture file {} doesn't exit".format(texture_fn) - texture_id = p.loadTexture(os.path.abspath(texture_fn), physicsClientId=self.phys_id) + assert os.path.isfile(texture_fn), "Texture file {} doesn't exit".format( + texture_fn + ) + texture_id = p.loadTexture( + os.path.abspath(texture_fn), physicsClientId=self.phys_id + ) p.changeVisualShape( objectUniqueId=self._id, linkIndex=-1, @@ -437,7 +453,9 @@ def contact_points( """ - contact_points = p.getContactPoints(entity.id, self.id, physicsClientId=self.phys_id) + contact_points = p.getContactPoints( + entity.id, self.id, physicsClientId=self.phys_id + ) return contact_points @@ -595,7 +613,9 @@ def disable_object_collision(self, object_entity: Entity) -> None: Args: object_entity (grip.robot.Entity): entity with which this object will no longer collide with. """ - p.setCollisionFilterPair(self.id, object_entity.id, -1, -1, 0, physicsClientId=self.phys_id) + p.setCollisionFilterPair( + self.id, object_entity.id, -1, -1, 0, physicsClientId=self.phys_id + ) def disable_collision(self) -> None: """