Module pyastrobee.core.rigid_bag
Rigid version of the cargo bag, using joints in the URDF to mimic what we would see from a deformable
Documentation for inherited methods can be found in the base class
View Source
"""Rigid version of the cargo bag, using joints in the URDF to mimic what we would see from a deformable
Documentation for inherited methods can be found in the base class
"""
# TODO tune the position control force on the handle
import time
from typing import Optional
import pybullet
from pybullet_utils.bullet_client import BulletClient
import numpy as np
import numpy.typing as npt
from pyastrobee.core.astrobee import Astrobee
from pyastrobee.core.abstract_bag import CargoBag
from pyastrobee.utils.bullet_utils import initialize_pybullet
from pyastrobee.utils.python_utils import print_green
from pyastrobee.utils.transformations import make_transform_mat
from pyastrobee.utils.rotations import quat_to_rmat
from pyastrobee.utils.dynamics import box_inertia
class RigidCargoBag(CargoBag):
"""Class for loading and managing properties associated with the rigid URDF-based cargo bags
Args:
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"
mass (float): Mass of the cargo bag, in kg
pos (npt.ArrayLike, optional): Initial XYZ position to load the bag. Defaults to (0, 0, 0)
orn (npt.ArrayLike, optional): Initial XYZW quaternion to load the bag. Defaults to (0, 0, 0, 1)
client (BulletClient, optional): If connecting to multiple physics servers, include the client
(the class instance, not just the ID) here. Defaults to None (use default connected client)
"""
LINKS_PER_HANDLE = 4 # 3 dummy links for roll/pitch/yaw, plus the handle itself
_urdfs = [
CargoBag.URDF_DIR + name + "_rigid_bag.urdf" for name in CargoBag.BAG_NAMES
]
URDFS = dict(zip(CargoBag.BAG_NAMES, _urdfs))
def __init__(
self,
bag_name: str,
mass: float,
pos: npt.ArrayLike = (0, 0, 0),
orn: npt.ArrayLike = (0, 0, 0, 1),
client: Optional[BulletClient] = None,
):
# This inertia is slightly approximate because some mass is in the handle and dummy links
self.inertia = box_inertia(mass, self.LENGTH, self.WIDTH, self.HEIGHT)
# Initializations
self._name = bag_name # HACK (this gets set in super.init but we need to know it before calling that)
self._constraints = {}
# Add position control to the handle(s) so its springs back into its natural position
# to provide some resistance to motion like we would see in a deformable
# The dummy joints are the joints associated with the motion of the handle
if self.num_handles == 1:
self.num_joints = 4 # 3 for rpy, 1 for handle
self.num_links = 5 # Base, 3 dummies for rpy, handle
dummy_joint_ids = [0, 1, 2]
elif self.num_handles == 2:
self.num_joints = 8
self.num_links = 9
dummy_joint_ids = [0, 1, 2, 4, 5, 6]
super().__init__(bag_name, mass, pos, orn, client)
self.client.setJointMotorControlArray(
self.id,
dummy_joint_ids,
pybullet.POSITION_CONTROL,
[0] * len(dummy_joint_ids),
forces=[0.1] * len(dummy_joint_ids), # TODO tune force
)
print_green("Bag is ready")
# TODO make this structure consistent with the constraint bag
@property
def constraints(self) -> list[int]:
"""Active IDs of constraints on the bag"""
return list(self._constraints.values())
def _load(self, pos: npt.ArrayLike, orn: npt.ArrayLike) -> int:
bag_id = self.client.loadURDF(self.URDFS[self.name], pos, orn)
# Update the mass of the bag based on the value provided at initialization
# The other links in the URDF (e.g. the handle(s)) have some mass so we need to account for this
link_mass = 0
for i in range(self.num_links - 1):
link_mass += self.client.getDynamicsInfo(bag_id, i)[0]
self.client.changeDynamics(bag_id, -1, self.mass - link_mass)
return bag_id
def _attach(self, robot: Astrobee, handle_index: int) -> None:
handle_link_index = (handle_index + 1) * self.LINKS_PER_HANDLE - 1
cid = self.client.createConstraint(
robot.id,
robot.Links.ARM_DISTAL.value,
self.id,
handle_link_index,
pybullet.JOINT_FIXED,
[0, 0, 1],
robot.TRANSFORMS.GRIPPER_TO_ARM_DISTAL[:3, 3],
[0, 0, 0],
)
self._constraints.update({robot.id: cid})
self._attached.append(robot.id)
def detach(self) -> None:
for cid in self.constraints:
self.client.removeConstraint(cid)
self._constraints = {}
self._attached = []
def detach_robot(self, robot_id: int) -> None:
if robot_id not in self.attached:
raise ValueError("Cannot detach robot: ID unknown")
self.client.removeConstraint(self._constraints[robot_id])
self._constraints.pop(robot_id)
self._attached.remove(robot_id)
def get_handle_transform(self, handle_index: int = 0) -> np.ndarray:
"""Calculates the transformation matrix (w.r.t the world) for a specified handle
Args:
handle_index (int): Index of the handle on the bag
Returns:
np.ndarray: Transformation matrix (handle to world). Shape = (4,4)
"""
if (handle_index + 1) > self.num_handles:
raise ValueError(
f"Invalid handle index: {handle_index}. Bag only has {self.num_handles} handles"
)
handle_link_index = (handle_index + 1) * self.LINKS_PER_HANDLE - 1
link_state = self.client.getLinkState(
self.id, handle_link_index, computeForwardKinematics=True
)
pos, quat = link_state[:2]
return make_transform_mat(quat_to_rmat(quat), pos)
def _single_handle_test(bag_name: str):
# Very simple example of loading the bag and attaching a robot
client = initialize_pybullet(bg_color=(0.8, 0.8, 1))
robot = Astrobee()
bag = RigidCargoBag(bag_name, 10)
bag.attach_to(robot)
while True:
client.stepSimulation()
time.sleep(1 / 120)
def _two_handle_test(bag_name: str):
# Load the bag and attach to two robots
client = initialize_pybullet(bg_color=(0.8, 0.8, 1))
robot_1 = Astrobee()
robot_2 = Astrobee()
bag = RigidCargoBag(bag_name, 10)
bag.attach_to([robot_1, robot_2])
while True:
client.stepSimulation()
time.sleep(1 / 120)
if __name__ == "__main__":
# _single_handle_test("top_handle")
_two_handle_test("top_bottom_handle")
Classes
RigidCargoBag
class RigidCargoBag(
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]]] = (0, 0, 0),
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]]] = (0, 0, 0, 1),
client: Optional[pybullet_utils.bullet_client.BulletClient] = None
)
Class for loading and managing properties associated with the rigid URDF-based cargo bags
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. Defaults to (0, 0, 0) | None |
| orn | npt.ArrayLike | Initial XYZW quaternion to load the bag. Defaults to (0, 0, 0, 1) | 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 RigidCargoBag(CargoBag):
"""Class for loading and managing properties associated with the rigid URDF-based cargo bags
Args:
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"
mass (float): Mass of the cargo bag, in kg
pos (npt.ArrayLike, optional): Initial XYZ position to load the bag. Defaults to (0, 0, 0)
orn (npt.ArrayLike, optional): Initial XYZW quaternion to load the bag. Defaults to (0, 0, 0, 1)
client (BulletClient, optional): If connecting to multiple physics servers, include the client
(the class instance, not just the ID) here. Defaults to None (use default connected client)
"""
LINKS_PER_HANDLE = 4 # 3 dummy links for roll/pitch/yaw, plus the handle itself
_urdfs = [
CargoBag.URDF_DIR + name + "_rigid_bag.urdf" for name in CargoBag.BAG_NAMES
]
URDFS = dict(zip(CargoBag.BAG_NAMES, _urdfs))
def __init__(
self,
bag_name: str,
mass: float,
pos: npt.ArrayLike = (0, 0, 0),
orn: npt.ArrayLike = (0, 0, 0, 1),
client: Optional[BulletClient] = None,
):
# This inertia is slightly approximate because some mass is in the handle and dummy links
self.inertia = box_inertia(mass, self.LENGTH, self.WIDTH, self.HEIGHT)
# Initializations
self._name = bag_name # HACK (this gets set in super.init but we need to know it before calling that)
self._constraints = {}
# Add position control to the handle(s) so its springs back into its natural position
# to provide some resistance to motion like we would see in a deformable
# The dummy joints are the joints associated with the motion of the handle
if self.num_handles == 1:
self.num_joints = 4 # 3 for rpy, 1 for handle
self.num_links = 5 # Base, 3 dummies for rpy, handle
dummy_joint_ids = [0, 1, 2]
elif self.num_handles == 2:
self.num_joints = 8
self.num_links = 9
dummy_joint_ids = [0, 1, 2, 4, 5, 6]
super().__init__(bag_name, mass, pos, orn, client)
self.client.setJointMotorControlArray(
self.id,
dummy_joint_ids,
pybullet.POSITION_CONTROL,
[0] * len(dummy_joint_ids),
forces=[0.1] * len(dummy_joint_ids), # TODO tune force
)
print_green("Bag is ready")
# TODO make this structure consistent with the constraint bag
@property
def constraints(self) -> list[int]:
"""Active IDs of constraints on the bag"""
return list(self._constraints.values())
def _load(self, pos: npt.ArrayLike, orn: npt.ArrayLike) -> int:
bag_id = self.client.loadURDF(self.URDFS[self.name], pos, orn)
# Update the mass of the bag based on the value provided at initialization
# The other links in the URDF (e.g. the handle(s)) have some mass so we need to account for this
link_mass = 0
for i in range(self.num_links - 1):
link_mass += self.client.getDynamicsInfo(bag_id, i)[0]
self.client.changeDynamics(bag_id, -1, self.mass - link_mass)
return bag_id
def _attach(self, robot: Astrobee, handle_index: int) -> None:
handle_link_index = (handle_index + 1) * self.LINKS_PER_HANDLE - 1
cid = self.client.createConstraint(
robot.id,
robot.Links.ARM_DISTAL.value,
self.id,
handle_link_index,
pybullet.JOINT_FIXED,
[0, 0, 1],
robot.TRANSFORMS.GRIPPER_TO_ARM_DISTAL[:3, 3],
[0, 0, 0],
)
self._constraints.update({robot.id: cid})
self._attached.append(robot.id)
def detach(self) -> None:
for cid in self.constraints:
self.client.removeConstraint(cid)
self._constraints = {}
self._attached = []
def detach_robot(self, robot_id: int) -> None:
if robot_id not in self.attached:
raise ValueError("Cannot detach robot: ID unknown")
self.client.removeConstraint(self._constraints[robot_id])
self._constraints.pop(robot_id)
self._attached.remove(robot_id)
def get_handle_transform(self, handle_index: int = 0) -> np.ndarray:
"""Calculates the transformation matrix (w.r.t the world) for a specified handle
Args:
handle_index (int): Index of the handle on the bag
Returns:
np.ndarray: Transformation matrix (handle to world). Shape = (4,4)
"""
if (handle_index + 1) > self.num_handles:
raise ValueError(
f"Invalid handle index: {handle_index}. Bag only has {self.num_handles} handles"
)
handle_link_index = (handle_index + 1) * self.LINKS_PER_HANDLE - 1
link_state = self.client.getLinkState(
self.id, handle_link_index, computeForwardKinematics=True
)
pos, quat = link_state[:2]
return make_transform_mat(quat_to_rmat(quat), pos)
Ancestors (in MRO)
- pyastrobee.core.abstract_bag.CargoBag
- abc.ABC
Class variables
BAG_NAMES
DUAL_HANDLE_BAGS
HANDLE_TRANSFORMS
HEIGHT
LENGTH
LINKS_PER_HANDLE
MESH_DIR
SINGLE_HANDLE_BAGS
URDFS
URDF_DIR
WIDTH
Instance variables
angular_velocity
Current [wx, wy, wz] angular velocity of the cargo bag's COM frame
- If both velocity and angular velocity are desired, use the dynamics_state property instead
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)
constraints
Active IDs of constraints on the bag
corner_positions
Positions of the 8 corners of the main compartment of the bag, shape (8, 3)
dynamics_state
Current state of the bag dynamics: Position, orientation, linear vel, and angular vel
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
Current XYZW quaternion orientation of the cargo bag's COM frame
pose
Current position + XYZW quaternion pose of the bag
position
Current XYZ position of the origin (COM frame) of the cargo bag
tmat
Current transformation matrix for the cargo bag: (Bag to world)
velocity
Current [vx, vy, vz] velocity of the cargo bag's COM frame
- If both velocity and angular velocity are desired, use the dynamics_state property instead
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:
for cid in self.constraints:
self.client.removeConstraint(cid)
self._constraints = {}
self._attached = []
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:
if robot_id not in self.attached:
raise ValueError("Cannot detach robot: ID unknown")
self.client.removeConstraint(self._constraints[robot_id])
self._constraints.pop(robot_id)
self._attached.remove(robot_id)
get_handle_transform
def get_handle_transform(
self,
handle_index: int = 0
) -> numpy.ndarray
Calculates the transformation matrix (w.r.t the world) for a specified handle
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
| handle_index | int | Index of the handle on the bag | None |
Returns:
| Type | Description |
|---|---|
| np.ndarray | Transformation matrix (handle to world). Shape = (4,4) |
View Source
def get_handle_transform(self, handle_index: int = 0) -> np.ndarray:
"""Calculates the transformation matrix (w.r.t the world) for a specified handle
Args:
handle_index (int): Index of the handle on the bag
Returns:
np.ndarray: Transformation matrix (handle to world). Shape = (4,4)
"""
if (handle_index + 1) > self.num_handles:
raise ValueError(
f"Invalid handle index: {handle_index}. Bag only has {self.num_handles} handles"
)
handle_link_index = (handle_index + 1) * self.LINKS_PER_HANDLE - 1
link_state = self.client.getLinkState(
self.id, handle_link_index, computeForwardKinematics=True
)
pos, quat = link_state[:2]
return make_transform_mat(quat_to_rmat(quat), pos)
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:
"""Resets the pose and velocities of the bag
Args:
pos (npt.ArrayLike): Position, shape (3,)
orn (npt.ArrayLike): XYZW quaternion orientation, shape (4,)
lin_vel (npt.ArrayLike): Linear velocity, shape (3,)
ang_vel (npt.ArrayLike): Angular velocity, shape (3,)
"""
self.client.resetBasePositionAndOrientation(self.id, pos, orn)
self.client.resetBaseVelocity(self.id, lin_vel, ang_vel)
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:
"""Removes the cargo bag from the simulation"""
self.detach()
self.client.removeBody(self.id)
self.id = None