Module pyastrobee.core.constraint_bag
Modeling a cargo bag as a single rigid box with a handle constructed from multiple point-to-point constraints
Documentation for inherited methods can be found in the base class
View Source
"""Modeling a cargo bag as a single rigid box with a handle constructed from multiple point-to-point constraints
Documentation for inherited methods can be found in the base class
"""
import time
from typing import Optional
import numpy as np
import numpy.typing as npt
import pybullet
from pybullet_utils.bullet_client import BulletClient
from pyastrobee.core.astrobee import Astrobee
from pyastrobee.core.abstract_bag import CargoBag
from pyastrobee.utils.bullet_utils import create_box
from pyastrobee.utils.transformations import transform_point
from pyastrobee.utils.python_utils import print_green
from pyastrobee.utils.bullet_utils import initialize_pybullet
# Different geometries of the constraint "constellation"
# First point is the central (primary) constraint
# Notes: tetrahedron seems to give a bit better behavior than diamond -- diamond will sometimes "snap" into place,
# which doesn't really make sense for a handle. Plus, tetrahedron has fewer constraints, which is better for sim
UNIT_CONSTRAINT_STRUCTURES = {
"diamond": np.array(
[
[0, 0, 0],
[1, 0, 0],
[-1, 0, 0],
[0, 1, 0],
[0, -1, 0],
[0, 0, 1],
[0, 0, -1],
]
),
"tetrahedron": np.array(
[
[0, 0, 0],
[-1 / 3, np.sqrt(8 / 9), 0],
[-1 / 3, -np.sqrt(2 / 9), np.sqrt(2 / 3)],
[-1 / 3, -np.sqrt(2 / 9), -np.sqrt(2 / 3)],
[1, 0, 0],
]
),
"xy_cross": np.array(
[
[0, 0, 0],
[1, 0, 0],
[-1, 0, 0],
[0, 1, 0],
[0, -1, 0],
]
),
"xz_cross": np.array(
[
[0, 0, 0],
[1, 0, 0],
[-1, 0, 0],
[0, 0, 1],
[0, 0, -1],
]
),
"yz_cross": np.array(
[
[0, 0, 0],
[0, 1, 0],
[0, -1, 0],
[0, 0, 1],
[0, 0, -1],
]
),
"x_inline": np.array(
[
[0, 0, 0],
[1, 0, 0],
[-1, 0, 0],
]
),
"y_inline": np.array(
[
[0, 0, 0],
[0, 1, 0],
[0, -1, 0],
]
),
"z_inline": np.array(
[
[0, 0, 0],
[0, 0, 1],
[0, 0, -1],
]
),
}
class ConstraintCargoBag(CargoBag):
"""Class for loading and managing properties associated with the constraint-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)
"""
def __init__(
self,
bag_name: str,
mass: float,
pos: npt.ArrayLike = (0, 0, 0),
orn: npt.ArrayLike = (0, 0, 0, 1),
client: BulletClient | None = None,
):
# Set up the geometric structure of the constraint-based handle
self.constraint_scaling = 0.05
self.constraint_structure_type = "tetrahedron"
self.constraint_structure = (
UNIT_CONSTRAINT_STRUCTURES[self.constraint_structure_type]
* self.constraint_scaling
)
# Define the forces applied by the constraints
self.primary_constraint_force = 3
self.secondary_constraint_force = 2
self.max_constraint_forces = np.concatenate(
[
[self.primary_constraint_force],
self.secondary_constraint_force
* np.ones(len(self.constraint_structure) - 1),
]
)
self._constraints = {}
self.num_contraints = len(self.constraint_structure)
super().__init__(bag_name, mass, pos, orn, client)
print_green("Bag is ready")
# Implement abstract methods
def _load(self, pos: npt.ArrayLike, orn: npt.ArrayLike) -> int:
return create_box(
pos,
orn,
self.mass,
(self.LENGTH, self.WIDTH, self.HEIGHT),
True,
(1, 1, 1, 1),
)
def _attach(self, robot: Astrobee, handle_index: int) -> None:
# Disable collisions with the arm for stability when resetting the position w.r.t the deformable
for link_id in [
robot.Links.GRIPPER_LEFT_DISTAL.value,
robot.Links.GRIPPER_RIGHT_DISTAL.value,
robot.Links.GRIPPER_LEFT_PROXIMAL.value,
robot.Links.GRIPPER_RIGHT_PROXIMAL.value,
robot.Links.ARM_DISTAL.value,
robot.Links.ARM_PROXIMAL.value,
]:
self.client.setCollisionFilterPair(robot.id, self.id, link_id, -1, 0)
constraints = form_constraint_grasp(
robot,
self.id,
self.grasp_transforms[handle_index],
self.constraint_structure_type,
self.constraint_scaling,
self.max_constraint_forces,
client=self.client,
)
self._constraints.update({robot.id: constraints})
self._attached.append(robot.id)
def detach(self) -> None:
for robot_id, cids in self.constraints.items():
for cid in cids:
self.client.removeConstraint(cid)
self._attached = []
self._constraints = {}
def detach_robot(self, robot_id: int) -> None:
if robot_id not in self.constraints:
raise ValueError("Cannot detach robot: ID unknown")
for cid in self.constraints[robot_id]:
self.client.removeConstraint(cid)
self._attached.remove(robot_id)
self._constraints.pop(robot_id)
# Functions and properties specific to the constraint-based bag
@property
def constraints(self) -> dict[int, list[int]]:
"""Constraints between the robot(s) and the handle(s). Key: robot ID; Value: list of constraint IDs"""
return self._constraints
def get_local_constraint_pos(self, handle_index: int) -> np.ndarray:
"""Determine the position of the handle's constraints in the bag frame
Args:
handle_index (int): Index of the handle of interest
Returns:
np.ndarray: Constraint positions, shape (n_constraints, 3)
"""
return np.array(
[
transform_point(self.grasp_transforms[handle_index], pt)
for pt in self.constraint_structure
]
)
def get_world_constraint_pos(self, handle_index: int) -> np.ndarray:
"""Determine the position of the handle's constraints in the world frame
Args:
handle_index (int): Index of the handle of interest
Returns:
np.ndarray: Constraint positions, shape (n_constraints, 3)
"""
tmat = self.tmat
local_constraint_pos = self.get_local_constraint_pos(handle_index)
return np.array([transform_point(tmat, pos) for pos in local_constraint_pos])
@property
def constraint_forces(self) -> dict[int, float]:
"""Forces on each constraint. Key: constraint ID; Value: Force, shape (3,)"""
# NOTE: this dictionary will maintain insertion order so we can also associate
# these constraint forces in the same order as the original structure
forces = {}
for robot_id, cids in self.constraints.items():
for cid in cids:
forces[cid] = self.client.getConstraintState(cid)
return forces
def form_constraint_grasp(
robot: Astrobee,
body_id: int,
grasp_transform: np.ndarray,
structure_type: str,
structure_scaling: float,
max_forces: list[float],
client: Optional[BulletClient] = None,
) -> list[int]:
"""Connects the Astrobee's gripper to an object via point-to-point constraints to mimic a non-rigid grasp
NOTE: Depending on the grasp transform used, it may be recommended to disable collisions between the Astrobee
gripper and the object before forming these constraints
Args:
robot (Astrobee): Astrobee performing the grasp
body_id (int): Pybullet ID of the object being grasped
grasp_transform (np.ndarray): Transformation matrix defining the grasp pose w.r.t the base frame of the object
structure_type (str, optional): Type of geometry to construct the series of constraints. Defaults to
"tetrahedron". Other options include "diamond"
structure_scaling (float, optional): Scale on the size of the constraint structure (if set to 1, the
constraints will be spaced along a unit (1 meter) sphere). Defaults to 0.05.
max_forces (list[float]): Maximum applied force for each constraint. Length must match with the number of
constraints in the desired structure type
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)
Returns:
list[int]: Pybullet IDs of the constraints
"""
client: pybullet = pybullet if client is None else client
constraint_structure = (
UNIT_CONSTRAINT_STRUCTURES[structure_type] * structure_scaling
)
if len(max_forces) != len(constraint_structure):
raise ValueError(
f"Invalid number of forces: Must be of length {len(constraint_structure)} "
+ f"for structure type {structure_type}.\nGot: {max_forces}"
)
body_local_constraint_pos = np.array(
[transform_point(grasp_transform, pt) for pt in constraint_structure]
)
robot_local_constraint_pos = np.array(
[
transform_point(Astrobee.TRANSFORMS.GRIPPER_TO_ARM_DISTAL, pt)
for pt in constraint_structure
]
)
constraints = []
for i in range(len(constraint_structure)):
cid = client.createConstraint(
robot.id,
robot.Links.ARM_DISTAL.value,
body_id,
-1,
client.JOINT_POINT2POINT,
(0, 0, 1),
robot_local_constraint_pos[i],
body_local_constraint_pos[i],
)
client.changeConstraint(cid, maxForce=max_forces[i])
constraints.append(cid)
return constraints
def _main():
client = initialize_pybullet(bg_color=(0.5, 0.5, 1))
client.configureDebugVisualizer(client.COV_ENABLE_WIREFRAME, 1)
robot = Astrobee()
# robot2 = Astrobee()
bag = ConstraintCargoBag("top_handle", 10)
# bag.attach_to([robot, robot2])
bag.attach_to(robot)
points_uid = None
while True:
forces = np.array(list(bag.constraint_forces.values()))
force_mags = np.linalg.norm(forces, axis=1)
rgbs = []
for i in range(bag.num_contraints):
r = min(1, force_mags[i] / bag.max_constraint_forces[i])
rgbs.append((r, 1 - r, 0))
world_constraint_pos = bag.get_world_constraint_pos(0)
if points_uid is None:
points_uid = client.addUserDebugPoints(world_constraint_pos, rgbs, 10, 0)
else:
points_uid = client.addUserDebugPoints(
world_constraint_pos, rgbs, 10, 0, replaceItemUniqueId=points_uid
)
client.stepSimulation()
time.sleep(1 / 120)
if __name__ == "__main__":
_main()
Variables
UNIT_CONSTRAINT_STRUCTURES
Functions
form_constraint_grasp
def form_constraint_grasp(
robot: pyastrobee.core.astrobee.Astrobee,
body_id: int,
grasp_transform: numpy.ndarray,
structure_type: str,
structure_scaling: float,
max_forces: list[float],
client: Optional[pybullet_utils.bullet_client.BulletClient] = None
) -> list[int]
Connects the Astrobee's gripper to an object via point-to-point constraints to mimic a non-rigid grasp
NOTE: Depending on the grasp transform used, it may be recommended to disable collisions between the Astrobee gripper and the object before forming these constraints
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
| robot | Astrobee | Astrobee performing the grasp | None |
| body_id | int | Pybullet ID of the object being grasped | None |
| grasp_transform | np.ndarray | Transformation matrix defining the grasp pose w.r.t the base frame of the object | None |
| structure_type | str | Type of geometry to construct the series of constraints. Defaults to "tetrahedron". Other options include "diamond" |
None |
| structure_scaling | float | Scale on the size of the constraint structure (if set to 1, the constraints will be spaced along a unit (1 meter) sphere). Defaults to 0.05. |
None |
| max_forces | list[float] | Maximum applied force for each constraint. Length must match with the number of constraints in the desired structure type |
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 |
Returns:
| Type | Description |
|---|---|
| list[int] | Pybullet IDs of the constraints |
View Source
def form_constraint_grasp(
robot: Astrobee,
body_id: int,
grasp_transform: np.ndarray,
structure_type: str,
structure_scaling: float,
max_forces: list[float],
client: Optional[BulletClient] = None,
) -> list[int]:
"""Connects the Astrobee's gripper to an object via point-to-point constraints to mimic a non-rigid grasp
NOTE: Depending on the grasp transform used, it may be recommended to disable collisions between the Astrobee
gripper and the object before forming these constraints
Args:
robot (Astrobee): Astrobee performing the grasp
body_id (int): Pybullet ID of the object being grasped
grasp_transform (np.ndarray): Transformation matrix defining the grasp pose w.r.t the base frame of the object
structure_type (str, optional): Type of geometry to construct the series of constraints. Defaults to
"tetrahedron". Other options include "diamond"
structure_scaling (float, optional): Scale on the size of the constraint structure (if set to 1, the
constraints will be spaced along a unit (1 meter) sphere). Defaults to 0.05.
max_forces (list[float]): Maximum applied force for each constraint. Length must match with the number of
constraints in the desired structure type
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)
Returns:
list[int]: Pybullet IDs of the constraints
"""
client: pybullet = pybullet if client is None else client
constraint_structure = (
UNIT_CONSTRAINT_STRUCTURES[structure_type] * structure_scaling
)
if len(max_forces) != len(constraint_structure):
raise ValueError(
f"Invalid number of forces: Must be of length {len(constraint_structure)} "
+ f"for structure type {structure_type}.\nGot: {max_forces}"
)
body_local_constraint_pos = np.array(
[transform_point(grasp_transform, pt) for pt in constraint_structure]
)
robot_local_constraint_pos = np.array(
[
transform_point(Astrobee.TRANSFORMS.GRIPPER_TO_ARM_DISTAL, pt)
for pt in constraint_structure
]
)
constraints = []
for i in range(len(constraint_structure)):
cid = client.createConstraint(
robot.id,
robot.Links.ARM_DISTAL.value,
body_id,
-1,
client.JOINT_POINT2POINT,
(0, 0, 1),
robot_local_constraint_pos[i],
body_local_constraint_pos[i],
)
client.changeConstraint(cid, maxForce=max_forces[i])
constraints.append(cid)
return constraints
Classes
ConstraintCargoBag
class ConstraintCargoBag(
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: pybullet_utils.bullet_client.BulletClient | None = None
)
Class for loading and managing properties associated with the constraint-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 ConstraintCargoBag(CargoBag):
"""Class for loading and managing properties associated with the constraint-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)
"""
def __init__(
self,
bag_name: str,
mass: float,
pos: npt.ArrayLike = (0, 0, 0),
orn: npt.ArrayLike = (0, 0, 0, 1),
client: BulletClient | None = None,
):
# Set up the geometric structure of the constraint-based handle
self.constraint_scaling = 0.05
self.constraint_structure_type = "tetrahedron"
self.constraint_structure = (
UNIT_CONSTRAINT_STRUCTURES[self.constraint_structure_type]
* self.constraint_scaling
)
# Define the forces applied by the constraints
self.primary_constraint_force = 3
self.secondary_constraint_force = 2
self.max_constraint_forces = np.concatenate(
[
[self.primary_constraint_force],
self.secondary_constraint_force
* np.ones(len(self.constraint_structure) - 1),
]
)
self._constraints = {}
self.num_contraints = len(self.constraint_structure)
super().__init__(bag_name, mass, pos, orn, client)
print_green("Bag is ready")
# Implement abstract methods
def _load(self, pos: npt.ArrayLike, orn: npt.ArrayLike) -> int:
return create_box(
pos,
orn,
self.mass,
(self.LENGTH, self.WIDTH, self.HEIGHT),
True,
(1, 1, 1, 1),
)
def _attach(self, robot: Astrobee, handle_index: int) -> None:
# Disable collisions with the arm for stability when resetting the position w.r.t the deformable
for link_id in [
robot.Links.GRIPPER_LEFT_DISTAL.value,
robot.Links.GRIPPER_RIGHT_DISTAL.value,
robot.Links.GRIPPER_LEFT_PROXIMAL.value,
robot.Links.GRIPPER_RIGHT_PROXIMAL.value,
robot.Links.ARM_DISTAL.value,
robot.Links.ARM_PROXIMAL.value,
]:
self.client.setCollisionFilterPair(robot.id, self.id, link_id, -1, 0)
constraints = form_constraint_grasp(
robot,
self.id,
self.grasp_transforms[handle_index],
self.constraint_structure_type,
self.constraint_scaling,
self.max_constraint_forces,
client=self.client,
)
self._constraints.update({robot.id: constraints})
self._attached.append(robot.id)
def detach(self) -> None:
for robot_id, cids in self.constraints.items():
for cid in cids:
self.client.removeConstraint(cid)
self._attached = []
self._constraints = {}
def detach_robot(self, robot_id: int) -> None:
if robot_id not in self.constraints:
raise ValueError("Cannot detach robot: ID unknown")
for cid in self.constraints[robot_id]:
self.client.removeConstraint(cid)
self._attached.remove(robot_id)
self._constraints.pop(robot_id)
# Functions and properties specific to the constraint-based bag
@property
def constraints(self) -> dict[int, list[int]]:
"""Constraints between the robot(s) and the handle(s). Key: robot ID; Value: list of constraint IDs"""
return self._constraints
def get_local_constraint_pos(self, handle_index: int) -> np.ndarray:
"""Determine the position of the handle's constraints in the bag frame
Args:
handle_index (int): Index of the handle of interest
Returns:
np.ndarray: Constraint positions, shape (n_constraints, 3)
"""
return np.array(
[
transform_point(self.grasp_transforms[handle_index], pt)
for pt in self.constraint_structure
]
)
def get_world_constraint_pos(self, handle_index: int) -> np.ndarray:
"""Determine the position of the handle's constraints in the world frame
Args:
handle_index (int): Index of the handle of interest
Returns:
np.ndarray: Constraint positions, shape (n_constraints, 3)
"""
tmat = self.tmat
local_constraint_pos = self.get_local_constraint_pos(handle_index)
return np.array([transform_point(tmat, pos) for pos in local_constraint_pos])
@property
def constraint_forces(self) -> dict[int, float]:
"""Forces on each constraint. Key: constraint ID; Value: Force, shape (3,)"""
# NOTE: this dictionary will maintain insertion order so we can also associate
# these constraint forces in the same order as the original structure
forces = {}
for robot_id, cids in self.constraints.items():
for cid in cids:
forces[cid] = self.client.getConstraintState(cid)
return forces
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
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)
constraint_forces
Forces on each constraint. Key: constraint ID; Value: Force, shape (3,)
constraints
Constraints between the robot(s) and the handle(s). Key: robot ID; Value: list of constraint IDs
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 robot_id, cids in self.constraints.items():
for cid in cids:
self.client.removeConstraint(cid)
self._attached = []
self._constraints = {}
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.constraints:
raise ValueError("Cannot detach robot: ID unknown")
for cid in self.constraints[robot_id]:
self.client.removeConstraint(cid)
self._attached.remove(robot_id)
self._constraints.pop(robot_id)
get_local_constraint_pos
def get_local_constraint_pos(
self,
handle_index: int
) -> numpy.ndarray
Determine the position of the handle's constraints in the bag frame
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
| handle_index | int | Index of the handle of interest | None |
Returns:
| Type | Description |
|---|---|
| np.ndarray | Constraint positions, shape (n_constraints, 3) |
View Source
def get_local_constraint_pos(self, handle_index: int) -> np.ndarray:
"""Determine the position of the handle's constraints in the bag frame
Args:
handle_index (int): Index of the handle of interest
Returns:
np.ndarray: Constraint positions, shape (n_constraints, 3)
"""
return np.array(
[
transform_point(self.grasp_transforms[handle_index], pt)
for pt in self.constraint_structure
]
)
get_world_constraint_pos
def get_world_constraint_pos(
self,
handle_index: int
) -> numpy.ndarray
Determine the position of the handle's constraints in the world frame
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
| handle_index | int | Index of the handle of interest | None |
Returns:
| Type | Description |
|---|---|
| np.ndarray | Constraint positions, shape (n_constraints, 3) |
View Source
def get_world_constraint_pos(self, handle_index: int) -> np.ndarray:
"""Determine the position of the handle's constraints in the world frame
Args:
handle_index (int): Index of the handle of interest
Returns:
np.ndarray: Constraint positions, shape (n_constraints, 3)
"""
tmat = self.tmat
local_constraint_pos = self.get_local_constraint_pos(handle_index)
return np.array([transform_point(tmat, pos) for pos in local_constraint_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