kinematic¶

Submodules¶

Classes¶

class brom_drake.productions.motion_planning.offline.kinematic.chem_lab1.ChemLab1(time_step: float = 0.001, meshcat_port_number: int = 7000, plan_execution_speed: float = 0.2, table_length: float = 0.6, table_width: float = 2.0, table_height: float = 0.1, shelf_pose: RigidTransform = None, **kwargs)[source]¶

Description

This production is the first in the chemistry lab series. It is used to test the motion planning capabilities of the robot in a chemistry lab setting with minimal constraints.

Parameters

time_step: float, optional: The time step for the production, by default 1e-3.
meshcat_port_number: int, optional: The port number for meshcat visualization, by default 7001.
plan_execution_speed: float, optional: The speed at which to execute the motion plan, by default 0.2.
pose_WorldBeaker: RigidTransform, optional: The pose of the beaker in the world frame, by default None.
table_length: float, optional: The length of the table, by default 0.6.
table_width: float, optional: The width of the table, by default 2.0.
table_height: float, optional: The height of the table, by default 0.1.
shelf_pose: RigidTransform, optional: The pose of the shelf in the world frame, by default None.

Usage

To use this production, create an instance of the ChemLab1 class provide it with a motion planning function, and then build it.

# Create the production
production = ChemLab1()

# Create a planner object which will be used to plan the motion
config = RRTConnectPlannerConfig(
    steering_step_size=0.1,
    prob_sample_goal=0.30,
    max_iterations=int(1e5),
    convergence_threshold=1e-3,
    debug_flag=True,
)
planner2 = RRTConnectPlanner(
    production.arm,
    production.plant,
    production.scene_graph,
    config=config,
)

# To build the production, we only need to provide a planning function
# (can come from anywhere, not just a BaseRRTPlanner object)
diagram, diagram_context = production.easy_cast_and_build(
    planner2.plan,
    with_watcher=True,
)

print("Simulating...")

# Simulate the diagram
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(1.0)

# Run simulation
simulator.Initialize()
simulator.AdvanceTo(0.1)
planned_trajectory = production.plan_dispenser.planned_trajectory
print(f"Expected end time of trajectory: {planned_trajectory.end_time()}")

# Advance to the end of the simulation
simulator.AdvanceTo(planned_trajectory.end_time()+1.0)

add_beaker()[source]¶

Description

This method adds a model of a beaker to the production. The beaker is welded in place at the pose pose_WorldBeaker with respect to the world origin.

The beaker’s URDF is specified in the brom_drake robots package.

add_cast_and_build(cast: List[Tuple[Role, LeafSystem | Diagram]] = [], with_watcher: bool = False) → Tuple[Diagram, Context][source]¶

Description

Modifies the normal add_cast_and_build, so that we share the context of the plant with the appropriate parts of the system.

In addition to using the parent class’s implementation of add_cast_and_build, this: - Configures the collision filter to avoid spurious collisions between the cupboard’s various parts and itself - Insert the motion planner role into the diagram and connect it properly

Parameters

cast: List[Tuple[Role, Performer]], optional: The main cast members that we would like to assign to the production. Default is None. TODO(Kwesi): Will using the default lead to errors? Should we even allow that?
with_watcher: bool, optional: A Boolean that determines whether to add a watcher to the diagram. Default is False.

Returns

diagram: pydrake.systems.framework.Diagram: The diagram that represents the fully assembled cast, all connected together.
diagram_context: pdrake.systems.framework.Context: The context for the diagram (used by the diagram watcher to monitor the system, if defined).

Note

This method returns a pydrake.systems.framework.Diagram and its associated pydrake.systems.framework.Context. When constructing a Drake simulation of this, you should use these objects to create the pydrake.systems.analysis.Simulator.

add_motion_planning_components()[source]¶

Description

Add the motion planning components to the builder.

add_shelf()[source]¶

Description

Add a model of a shelf to the production’s plant. The shelf’s pydrake.multibody.tree.ModelInstanceIndex will be saved in the internal variable self.shelf_model_index. The shelf is fixed at the pose self.pose_WorldShelf with respect to the world origin.

add_supporting_cast()[source]¶

Description

This method adds all secondary cast members to the builder. The secondary cast members in the production are the:

Table, where the robot exists
Test Tube Holders
Component which share’s the robot model reference
Motion Planning components (e.g., dispensers, etc.)
Start and Goal sources

add_table()[source]¶

Description

This method adds a model of a table to the production’s plant. The table will be a simple shape that the robot will interact with. The table is fixed in a pose with respect to the origin (pose chosen in this function).

add_test_tube_holders()[source]¶

Description

This method adds the test tube holders to the production. The test tube holder is fixed at pose self.pose_WorldHolder with respect to the world origin.

configure_collision_filter(scene_graph_context: Context)[source]¶

Description

This method configures the collision filter, so that:

self collisions between the shelf’s pieces,

are ignored during simulation.

Parameters

scene_graph_context: pydrake.systems.framework.Context: The context of the scenegraph for the constructed production (i.e., the diagram for the built Production).

easy_cast_and_build(planning_algorithm: Callable[[ndarray, ndarray, Callable[[ndarray], bool]], Tuple[DiGraph | ndarray, bool]], with_watcher: bool = False) → Tuple[Diagram, Context][source]¶

Description

This function is used to easily cast and build the production.

Parameters

planning_algorithm: Callable[[np.ndarray, np.ndarray, Callable[[np.ndarray], bool]], Tuple[MotionPlan, bool]: The algorithm that produces a path from the start configuration to goal configuration using the collision checker for the scene.
with_watcher: bool, optional: A Boolean that determines whether to add a watcher to the diagram. Default is False.

Returns

diagram: pydrake.systems.framework.Diagram: The diagram that represents the fully assembled cast, all connected together.
diagram_context: pdrake.systems.framework.Context: The context for the diagram (used by the diagram watcher to monitor the system, if defined).

Note

property goal_configuration: ndarray¶

Description

Goal configuration of the robot in the production.

Depending on the inputs, this may be computed the first time it is called.

Returns

q_goal: np.ndarray of shape (6,): The goal configuration of the robot.

property goal_pose: RigidTransform¶

Description

Get the goal pose of the end effector frame.

Returns

goal_pose: RigidTransform: The goal pose of the robot.

property id: ProductionID¶: Always returns ProductionID.kChemLab1.

solve_forward_kinematics_problem_for_arm(robot_joint_positions: ndarray, target_frame_name: str = 'ft_frame') → RigidTransform[source]¶

Description

This method solves the forward kinematics problem for the UR10e arm by itself.

Parameters

robot_joint_positions: np.ndarray of shape (6,): The joint configuration of the robot in the production.
target_frame_name: str, optional: The frame that we compute the pose of with respect to the world origin frame. Default is “ft_frame”

Returns

pose_WorldTarget: RigidTransform: The end effector pose of the robot expressed in the world frame.

property start_configuration¶

Description

Get the start configuration.

This may be computed using inverse kinematics on the first time it is requested, depending on the construction of the production.

Returns

q_start: np.ndarray of shape(6,): The starting configuration of the production’s robot.

property start_pose: RigidTransform¶

Description

Get the start pose.

Returns

pose_WorldStart: RigidTransform: The start pose of the robot’s end effector frame with respect to the world frame.

class brom_drake.productions.motion_planning.offline.kinematic.shelf.ShelfPlanning1(time_step: float = 0.001, shelf_pose: RigidTransform = None, meshcat_port_number: int = 7001, plan_execution_speed: float = 0.2, **kwargs)[source]¶

Description

This production contains a simple motion planning task where we attempt to reach to different places around the shelfs of a bookcase.

This is a Kinematic motion planning production, which means that the the robot’s actuators will not be driven. Instead, the robot’s links will be reset to “exactly” the configurations in the motion plan. This is not realistic but tests the correctness of the plan without worrying about the control problem of moving the arm.

Parameters

time_step: float, optional: The time step for the production, by default 1e-3.
shelf_pose: RigidTransform, optional: The pose of the shelf in the world frame, by default None.
meshcat_port_number: int, optional: The port number for meshcat visualization, by default 7001.
plan_execution_speed: float, optional: The speed at which to execute the motion plan, by default 0.2

Usage

You may use this in a similar way to all other KinematicMotionPlanningProduction objects.

# Other imports above...
from brom_drake.motion_planning.algorithms.rrt import RRTConnectPlannerConfig, RRTConnectPlanner
from brom_drake.productions.motion_planning.offline import ShelfPlanning1

# Define the goal pose
easy_goal_position = np.array([+0.0, 1.0, 1.05])
goal_orientation = RollPitchYaw(np.pi / 2.0, np.pi / 2.0, 0.0).ToQuaternion()
goal_pose = RigidTransform(goal_orientation, easy_goal_position)

# Create the production
production = ShelfPlanning1(
    meshcat_port_number=meshcat_port_number, # Use None for CI
    goal_pose=goal_pose,
)

# Create a planner object which will be used to plan the motion
config = RRTConnectPlannerConfig(
    steering_step_size=0.01,
    prob_sample_goal=0.05,
    max_iterations=int(1e5),
    convergence_threshold=1e-3,
)
planner2 = RRTConnectPlanner(
    production.arm,
    production.plant,
    production.scene_graph,
    config=config,
)

# To build the production, we only need to provide a planning function
# (can come from anywhere, not just a BaseRRTPlanner object)
diagram, diagram_context = production.easy_cast_and_build(
    planner2.plan,
    with_watcher=True,
)

print("Simulating...")

# Simulate the diagram
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(1.0)

# Run simulation
simulator.Initialize()
simulator.AdvanceTo(0.1)
planned_trajectory = production.plan_dispenser.planned_trajectory
print(f"Expected end time of the planned trajectory: {planned_trajectory.end_time()}")
# return
simulator.AdvanceTo(planned_trajectory.end_time()+1.0)

add_cast_and_build(cast: List[Tuple[Role, LeafSystem | Diagram]] = [], with_watcher: bool = False) → Tuple[Diagram, Context][source]¶

Description

Modifies the normal add_cast_and_build, so that we share the context of the plant with the appropriate parts of the system.

TODO(kwesi): Consider making this a standard part of the OfflineKinematicMotionPlanningProduction class.

Parameters

cast: List[Tuple[Role, Performer]], optional: The cast to add to the production, by default [].
with_watcher: bool, optional: A Boolean that determines whether to add a watcher to the diagram, by default False.

Returns

diagram: Diagram: The built diagram.
diagram_context: Context: The context for the built diagram. This context will be used to initialize the watcher.

add_motion_planning_components()[source]¶

Description

Connects the plan dispenser’s output port (i.e., the reference signal for the arm’s joint controller) to the input of the UR10eStation.

add_shelf(plant: MultibodyPlant)[source]¶

Description

Adds a model of the cupboard to the production’s plant.

Parameter

plant: pydrake.multibody.plant.MultibodyPlant: The plant that we want to add the cupboard to.

add_supporting_cast()[source]¶

Description

Modifies the plant and the builder by:

Adding UR10e station to the plant
Adding shelf to the plant
Adding a “ground” to the plant
Adding features required by the KinematicMotionPlanningProductionFeatures
- Robot’s ModelInstanceIndex Source
- Motion Planning Components
- Start and Goal Pose sources
Setting initial configuration of the arm for the plant

add_ur10e_station()[source]¶

Description

Add the Kinematic UR10e station to the production’s plant for simple motion planning.

configure_collision_filter(scene_graph_context: Context)[source]¶

Description

This method configures the collision filter, so that:

self collisions between the shelf’s pieces, and
self collisions between the arm’s parts,

are ignored during simulation.

Parameters

scene_graph_context: pydrake.systems.framework.Context: The context of the scenegraph for the constructed production (i.e., the diagram for the built Production).

Description

This function is used to easily cast and build the production.

Parameters

planning_algorithm: Callable[[np.ndarray, np.ndarray, Callable[[np.ndarray], bool]], Tuple[MotionPlan, bool]: The algorithm that produces a path from the start configuration to goal configuration using the collision checker for the scene.
with_watcher: bool, optional: A Boolean that determines whether to add a watcher to the diagram. Default is False.

Returns

diagram: pydrake.systems.framework.Diagram: The diagram that represents the fully assembled cast, all connected together.
diagram_context: pdrake.systems.framework.Context: The context for the diagram (used by the diagram watcher to monitor the system, if defined).

Note

property goal_pose: RigidTransform¶

Description

Get the goal pose.

Returns

pose_WorldGoal: RigidTransform: The goal pose.

property id: ProductionID¶: Always returns ProductionID.kShelfPlanning1

solve_forward_kinematics_problem_for_arm(robot_joint_positions: ndarray, target_frame_name: str = 'ft_frame') → RigidTransform[source]¶

Description

This method solves the forward kinematics problem for the UR10e arm by itself.

TODO(Kwesi): Attempt to make this a utility (it is duplicated multiple times!)

Parameters

robot_joint_positions: np.ndarray of shape (6,): The joint configuration of the robot in the production.
target_frame_name: str, optional: The frame that we compute the pose of with respect to the world origin frame. Default is “ft_frame”

Returns

pose_WorldTarget: RigidTransform: The end effector pose of the robot expressed in the world frame.

property start_pose: RigidTransform¶

Description

Get the start pose.

Returns

pose_WorldStart: RigidTransform: The start pose of the robot’s end effector frame with respect to the world frame.

Functions¶

(None found)

Variables¶

brom_drake.productions.motion_planning.offline.kinematic.chem_lab1.DEFAULT_BROM_MODELS_DIR = './brom/models'¶

str(object=’’) -> str str(bytes_or_buffer[, encoding[, errors]]) -> str

Create a new string object from the given object. If encoding or errors is specified, then the object must expose a data buffer that will be decoded using the given encoding and error handler. Otherwise, returns the result of object.__str__() (if defined) or repr(object). encoding defaults to sys.getdefaultencoding(). errors defaults to ‘strict’.