Skip to content

Commit

Permalink
formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@eaa3
eaa3 committed Nov 17, 2023
1 parent c77d042 commit 3689ef4
Show file tree
Hide file tree
Showing 3 changed files with 81 additions and 27 deletions.
67 changes: 51 additions & 16 deletions grip/robot/bullet_robot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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]:
"""
Expand All @@ -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)

Expand Down Expand Up @@ -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
Expand All @@ -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.
Expand All @@ -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)

Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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]
Expand All @@ -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)
Expand Down Expand Up @@ -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()
Expand All @@ -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):
Expand All @@ -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:
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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):
Expand Down
1 change: 0 additions & 1 deletion grip/robot/collision.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
40 changes: 30 additions & 10 deletions grip/robot/object.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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)
Expand All @@ -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))
Expand All @@ -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,
Expand Down Expand Up @@ -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:
"""
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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:
"""
Expand Down

0 comments on commit 3689ef4

Please sign in to comment.