Skip to content

Module pyastrobee.core.hybrid_bag

Testing to see if we can attach a deformable handle to a rigid bag

(work in progress)

View Source 
"""Testing to see if we can attach a deformable handle to a rigid bag

(work in progress)

"""

import time

from typing import Optional

import numpy as np

import numpy.typing as npt

from pybullet_utils.bullet_client import BulletClient

from pyastrobee.utils.bullet_utils import (

    initialize_pybullet,

    load_deformable_object,

    create_box,

    create_anchor,

)

from pyastrobee.utils.mesh_utils import get_closest_mesh_vertex, get_mesh_data

from pyastrobee.config.bag_properties import BOX_LENGTH, BOX_WIDTH, BOX_HEIGHT

from pyastrobee.core.abstract_bag import CargoBag

from pyastrobee.core.astrobee import Astrobee

def main():

    handle_corner_pos_local = [

        # Outer corners

        (0.075, 0.025, 0),  # ID 32

        (0.075, -0.025, 0),  # ID 1

        (-0.075, 0.025, 0),  # ID 21

        (-0.075, -0.025, 0),  # ID 19

        # Inner corners

        (0.06138287, 0.025, 0),  # ID 31

        (0.06138287, -0.025, 0),  # ID 0

        (-0.06138287, 0.025, 0),  # ID 22

        (-0.06138287, -0.025, 0),  # ID 20

    ]

    client = initialize_pybullet()

    box = create_box(

        (0, 0, -BOX_HEIGHT / 2),

        (0, 0, 0, 1),

        5,

        (BOX_LENGTH, BOX_WIDTH, BOX_HEIGHT),

        True,

        client=client,

    )

    handle = load_deformable_object(

        "pyastrobee/assets/meshes/handle_only.vtk",

        pos=(0, 0, 0),

        mass=0.2,

        client=client,

    )

    n_verts, vert_positions = get_mesh_data(handle, client=client)

    ids = []

    for i, pos in enumerate(handle_corner_pos_local):

        actual_pos, vert_id = get_closest_mesh_vertex(pos, vert_positions)

        print(

            f"Corner #{i}: ID = {vert_id}. Distance: {np.linalg.norm(pos - actual_pos)}"

        )

        ids.append(vert_id)

    anchor_ids = []

    for id in ids:

        aid, _ = create_anchor(handle, id, box, -1)

        anchor_ids.append(aid)

    while True:

        client.stepSimulation()

        time.sleep(1 / 120)

class HybridCargoBag(CargoBag):

    def __init__(

        self,

        bag_name: str,

        mass: float,

        pos: npt.ArrayLike,

        orn: npt.ArrayLike,

        client: Optional[BulletClient] = None,

    ):

        super().__init__(bag_name, mass, pos, orn, client)

    @property

    def pose(self) -> np.ndarray:

        # return super().pose

        pass

    @property

    def position(self) -> np.ndarray:

        # return super().position

        pass

    @property

    def orientation(self) -> np.ndarray:

        # return super().orientation

        pass

    @property

    def velocity(self) -> np.ndarray:

        # return super().velocity

        pass

    @property

    def angular_velocity(self) -> np.ndarray:

        # return super().angular_velocity

        pass

    @property

    def dynamics_state(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:

        # return super().dynamics_state

        pass

    @property

    def corner_positions(self) -> list[np.ndarray]:

        # return super().corner_positions

        pass

    def _load(

        self,

        pos: npt.ArrayLike,

        orn: npt.ArrayLike,

    ) -> int:

        # return super()._load(pos, orn)

        pass

    def unload(self) -> None:

        # return super().unload()

        pass

    def _attach(self, robot: Astrobee, handle_index: int) -> None:

        # return super()._attach(robot, handle_index)

        pass

    def detach(self) -> None:

        # return super().detach()

        pass

    def detach_robot(self, robot_id: int) -> None:

        # return super().detach_robot(robot_id)

        pass

    def reset_dynamics(

        self,

        pos: npt.ArrayLike,

        orn: npt.ArrayLike,

        lin_vel: npt.ArrayLike,

        ang_vel: npt.ArrayLike,

    ) -> None:

        # return super().reset_dynamics(pos, orn, lin_vel, ang_vel)

        pass

if __name__ == "__main__":

    main()

Variables

BOX_HEIGHT

BOX_LENGTH

BOX_WIDTH

Functions

main

def main(

)
View Source 
def main():

    handle_corner_pos_local = [

        # Outer corners

        (0.075, 0.025, 0),  # ID 32

        (0.075, -0.025, 0),  # ID 1

        (-0.075, 0.025, 0),  # ID 21

        (-0.075, -0.025, 0),  # ID 19

        # Inner corners

        (0.06138287, 0.025, 0),  # ID 31

        (0.06138287, -0.025, 0),  # ID 0

        (-0.06138287, 0.025, 0),  # ID 22

        (-0.06138287, -0.025, 0),  # ID 20

    ]

    client = initialize_pybullet()

    box = create_box(

        (0, 0, -BOX_HEIGHT / 2),

        (0, 0, 0, 1),

        5,

        (BOX_LENGTH, BOX_WIDTH, BOX_HEIGHT),

        True,

        client=client,

    )

    handle = load_deformable_object(

        "pyastrobee/assets/meshes/handle_only.vtk",

        pos=(0, 0, 0),

        mass=0.2,

        client=client,

    )

    n_verts, vert_positions = get_mesh_data(handle, client=client)

    ids = []

    for i, pos in enumerate(handle_corner_pos_local):

        actual_pos, vert_id = get_closest_mesh_vertex(pos, vert_positions)

        print(

            f"Corner #{i}: ID = {vert_id}. Distance: {np.linalg.norm(pos - actual_pos)}"

        )

        ids.append(vert_id)

    anchor_ids = []

    for id in ids:

        aid, _ = create_anchor(handle, id, box, -1)

        anchor_ids.append(aid)

    while True:

        client.stepSimulation()

        time.sleep(1 / 120)

Classes

HybridCargoBag

class HybridCargoBag(
    bag_name: str,
    mass: float,
    pos: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
    orn: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
    client: Optional[pybullet_utils.bullet_client.BulletClient] = None
)

Base (abstract) cargo bag class

See inherited cargo bags for full implementations (deformable bag, rigid bag, constraint bag, ...)

Attributes

Name Type Description Default
bag_name str Type of cargo bag to load. Single handle: "front_handle", "right_handle", "top_handle".
Dual handle: "front_back_handle", "right_left_handle", "top_bottom_handle"		 None
mass float Mass of the cargo bag, in kg None
pos npt.ArrayLike Initial XYZ position to load the bag None
orn npt.ArrayLike Initial XYZW quaternion to load the bag None
client BulletClient If connecting to multiple physics servers, include the client
(the class instance, not just the ID) here. Defaults to None (use default connected client)		 None
View Source 
class HybridCargoBag(CargoBag):

    def __init__(

        self,

        bag_name: str,

        mass: float,

        pos: npt.ArrayLike,

        orn: npt.ArrayLike,

        client: Optional[BulletClient] = None,

    ):

        super().__init__(bag_name, mass, pos, orn, client)

    @property

    def pose(self) -> np.ndarray:

        # return super().pose

        pass

    @property

    def position(self) -> np.ndarray:

        # return super().position

        pass

    @property

    def orientation(self) -> np.ndarray:

        # return super().orientation

        pass

    @property

    def velocity(self) -> np.ndarray:

        # return super().velocity

        pass

    @property

    def angular_velocity(self) -> np.ndarray:

        # return super().angular_velocity

        pass

    @property

    def dynamics_state(self) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:

        # return super().dynamics_state

        pass

    @property

    def corner_positions(self) -> list[np.ndarray]:

        # return super().corner_positions

        pass

    def _load(

        self,

        pos: npt.ArrayLike,

        orn: npt.ArrayLike,

    ) -> int:

        # return super()._load(pos, orn)

        pass

    def unload(self) -> None:

        # return super().unload()

        pass

    def _attach(self, robot: Astrobee, handle_index: int) -> None:

        # return super()._attach(robot, handle_index)

        pass

    def detach(self) -> None:

        # return super().detach()

        pass

    def detach_robot(self, robot_id: int) -> None:

        # return super().detach_robot(robot_id)

        pass

    def reset_dynamics(

        self,

        pos: npt.ArrayLike,

        orn: npt.ArrayLike,

        lin_vel: npt.ArrayLike,

        ang_vel: npt.ArrayLike,

    ) -> None:

        # return super().reset_dynamics(pos, orn, lin_vel, ang_vel)

        pass

Ancestors (in MRO)

  • pyastrobee.core.abstract_bag.CargoBag
  • abc.ABC

Class variables

BAG_NAMES

DUAL_HANDLE_BAGS

HANDLE_TRANSFORMS

HEIGHT

LENGTH

MESH_DIR

SINGLE_HANDLE_BAGS

URDF_DIR

WIDTH

Instance variables

angular_velocity

attached

ID(s) of the robot (or robots) grasping the bag. Empty if no robots are attached

bounding_box

Current axis-aligned bounding box of the bag (or just the main compartment), shape (2, 3)

corner_positions

dynamics_state

grasp_transforms

Transformation matrices "handle to bag" representing the grasp locations on the handles to the bag COM

In the case of a single-handled bag, this list will only have one entry

mass

Mass of the cargo bag

name

Type of cargo bag

num_handles

Number of handles on the cargo bag

orientation

pose

position

tmat

Current transformation matrix for the cargo bag: (Bag to world)

velocity

Methods

attach_to

def attach_to(
    self,
    robot_or_robots: Union[pyastrobee.core.astrobee.Astrobee, list[pyastrobee.core.astrobee.Astrobee], tuple[pyastrobee.core.astrobee.Astrobee]],
    object_to_move: str = 'robot'
) -> None

Attaches a robot (or multiple robots) to the handle(s) of the bag

Parameters:

Name Type Description Default
robot_or_robots Union[Astrobee, list[Astrobee], tuple[Astrobee]] Robot(s) to attach to the bag None
object_to_move str Either "robot" or "bag". This dictates what object will get its position
reset in order to make the grasp connection. In general, it makes more sense to move the robot to the
bag (default behavior)		 None

Raises:

Type Description
ValueError For invalid inputs, or if the bag does not have enough handles for each robot
NotImplementedError Multi-robot case with >2 robots
View Source 
    def attach_to(

        self,

        robot_or_robots: Union[Astrobee, list[Astrobee], tuple[Astrobee]],

        object_to_move: str = "robot",

    ) -> None:

        """Attaches a robot (or multiple robots) to the handle(s) of the bag

        Args:

            robot_or_robots (Union[Astrobee, list[Astrobee], tuple[Astrobee]]): Robot(s) to attach to the bag

            object_to_move (str, optional): Either "robot" or "bag". This dictates what object will get its position

                reset in order to make the grasp connection. In general, it makes more sense to move the robot to the

                bag (default behavior)

        Raises:

            ValueError: For invalid inputs, or if the bag does not have enough handles for each robot

            NotImplementedError: Multi-robot case with >2 robots

        """

        # Handle inputs

        if isinstance(robot_or_robots, Astrobee):  # Single robot

            num_robots = 1

        elif isinstance(robot_or_robots, (list, tuple)):  # Multi-robot

            if not all(isinstance(r, Astrobee) for r in robot_or_robots):

                raise ValueError("Non-Astrobee input detected")

            num_robots = len(robot_or_robots)

            if self.num_handles < num_robots:

                raise ValueError(

                    f"Bag does not have enough handles to support {num_robots} robots"

                )

            if num_robots == 1:  # Edge case: Unpack the list if only one robot

                robot_or_robots = robot_or_robots[0]

        else:

            raise ValueError(

                "Invalid input: Must provide either an Astrobee or a list of multiple Astrobees"

            )

        if object_to_move not in {"robot", "bag"}:

            raise ValueError("Invalid object to move: Must be either 'robot' or 'bag'.")

        bag_to_world = pos_quat_to_tmat(self.pose)

        if num_robots == 1:

            robot = robot_or_robots  # Unpack list

            if object_to_move == "robot":

                # Reset the position of the robot to interface with the handle

                handle_to_bag = self.grasp_transforms[0]

                handle_to_world = bag_to_world @ handle_to_bag

                handle_pose = tmat_to_pos_quat(handle_to_world)

                robot.reset_to_ee_pose(handle_pose)

            else:  # Move the bag to the robot

                self.reset_to_handle_pose(robot.ee_pose)

            self._attach(robot, 0)

        elif num_robots == 2:

            robot_1, robot_2 = robot_or_robots  # Unpack list

            if object_to_move == "robot":

                # Reset the position of each robot to interface with the two handles

                handle_1_to_bag = self.grasp_transforms[0]

                handle_2_to_bag = self.grasp_transforms[1]

                handle_1_to_world = bag_to_world @ handle_1_to_bag

                handle_2_to_world = bag_to_world @ handle_2_to_bag

                robot_1.reset_to_ee_pose(tmat_to_pos_quat(handle_1_to_world))

                robot_2.reset_to_ee_pose(tmat_to_pos_quat(handle_2_to_world))

                self._attach(robot_1, 0)

                self._attach(robot_2, 1)

            else:  # Move the bag while leaving the robots static

                raise NotImplementedError(

                    "Attaching the bag to multiple robots requires moving at least 1 robot"

                )

        else:

            raise NotImplementedError(

                "The multi-robot case is only implemented for 2 Astrobees"

            )

detach

def detach(
    self
) -> None

Detach all connections to the bag

View Source 
    def detach(self) -> None:

        # return super().detach()

        pass

detach_robot

def detach_robot(
    self,
    robot_id: int
) -> None

Detaches a specific robot from the bag

Parameters:

Name Type Description Default
robot_id int Pybullet ID of the robot to detach None
View Source 
    def detach_robot(self, robot_id: int) -> None:

        # return super().detach_robot(robot_id)

        pass

reset_dynamics

def reset_dynamics(
    self,
    pos: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
    orn: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
    lin_vel: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
    ang_vel: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]]
) -> None

Resets the pose and velocities of the bag

Parameters:

Name Type Description Default
pos npt.ArrayLike Position, shape (3,) None
orn npt.ArrayLike XYZW quaternion orientation, shape (4,) None
lin_vel npt.ArrayLike Linear velocity, shape (3,) None
ang_vel npt.ArrayLike Angular velocity, shape (3,) None
View Source 
    def reset_dynamics(

        self,

        pos: npt.ArrayLike,

        orn: npt.ArrayLike,

        lin_vel: npt.ArrayLike,

        ang_vel: npt.ArrayLike,

    ) -> None:

        # return super().reset_dynamics(pos, orn, lin_vel, ang_vel)

        pass

reset_to_handle_pose

def reset_to_handle_pose(
    self,
    handle_pose: Union[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]], numpy._typing._nested_sequence._NestedSequence[numpy._typing._array_like._SupportsArray[numpy.dtype[Any]]], bool, int, float, complex, str, bytes, numpy._typing._nested_sequence._NestedSequence[Union[bool, int, float, complex, str, bytes]]],
    handle_index: int = 0
) -> None

Resets the position of the bag so that the handle is positioned at a desired pose

Parameters:

Name Type Description Default
handle_pose npt.ArrayLike Desired pose of the handle ("handle-to-world"), shape (7,) None
handle_index int Index of the handle to align to the desired pose. Defaults to 0. 0
View Source 
    def reset_to_handle_pose(

        self, handle_pose: npt.ArrayLike, handle_index: int = 0

    ) -> None:

        """Resets the position of the bag so that the handle is positioned at a desired pose

        Args:

            handle_pose (npt.ArrayLike): Desired pose of the handle ("handle-to-world"), shape (7,)

            handle_index (int, optional): Index of the handle to align to the desired pose. Defaults to 0.

        """

        handle_to_world = pos_quat_to_tmat(handle_pose)

        bag_to_handle = invert_transform_mat(self.grasp_transforms[handle_index])

        bag_to_world = handle_to_world @ bag_to_handle

        bag_pose = tmat_to_pos_quat(bag_to_world)

        # This assumes that we want the bag to be stationary

        self.reset_dynamics(bag_pose[:3], bag_pose[3:], np.zeros(3), np.zeros(3))

unload

def unload(
    self
) -> None

Removes the cargo bag from the simulation

View Source 
    def unload(self) -> None:

        # return super().unload()

        pass