Motion Policy Extension [omni.isaac.motion_generation]¶
Motion Policy Interface¶
-
class
MotionPolicy
(_stage: pxr.Usd.Stage, policy_type: omni.isaac.motion_generation.motion_policy_interface.PolicyType)¶ Interface for implementing motion policies for compatibility with MotionGenerator interface.
-
set_initialized
() → None¶ Set self.initialized to True after a successful initialization.
- Returns
None
-
update
(updated_obstacles: Optional[List[pxr.Usd.Prim]] = None) → None¶ Applies all necessary updates to the internal world/robot state.
- Parameters
updated_obstacles (list, optional) – If provided, only the given obstacles will have their poses updated. For motion policies that use obstacle poses relative to the robot base (e.g. Lula based planners), this list will be ignored if the robot base has moved because all object poses will have changed relative to the robot. Defaults to None.
- Returns
None
-
get_joint_velocity_targets
(joint_positions: numpy.array, joint_velocities: numpy.array, frame_duration: float) → numpy.array¶ Compute new velocity targets based on robot state.
This function will be used by MotionGenerator to set a velocity target at every frame. This function only needs to be implemented if the MotionPolicy has the PolicyType VELOCITY_POLICY
- Parameters
joint_positions (np.array) – (m x 1) vector with position of each active robot joint.
joint_velocities (np.array) – (m x 1) vector with velocity of each active robot joint.
frame_duration (float) – Duration of a single frame of simulation in seconds.
- Returns
(m x 1) vector with velocity target for each active joint.
- Return type
np.array
-
get_joint_position_targets
(joint_positions: numpy.array, joint_velocities: numpy.array, frame_duration: float) → numpy.array¶ Compute new position targets based on robot state.
This function will be used by MotionGenerator to set a position target at every frame. This function only needs to be implemented if the MotionPolicy has the PolicyType POSITION_POLICY
- Parameters
joint_positions (np.array) – (m x 1) vector with position of each active robot joint.
joint_velocities (np.array) – (m x 1) vector with velocity of each active robot joint.
frame_duration (float) – Duration of a single frame of simulation in seconds.
- Returns
(m x 1) vector with position target for each active joint.
- Return type
np.array
-
get_active_joints
() → List[str]¶ Return names of active joints.
Some articulated robot joints may be ignored by some policies. E.g., the gripper of the Franka arm is not used to follow targets, and the RMPflow config files excludes the joints in the gripper from the list of articulated joints.
- Returns
names of active joints.
- Return type
list of str
-
set_cspace_target
(target: numpy.array) → None¶ Set configuration space target for the robot.
- Parameters
target (np.array) – Desired configuration for the robot as (m x 1) vector where m is the number of active joints.
- Returns
None
-
set_end_effector_target
(target_prim: pxr.Usd.Prim, position_only: bool = False) → None¶ Set end effector target.
- Parameters
target_prim (Usd.Prim) – USD prim of the target. target_prim may also be None, in which case it is up to the policy to specify the desired behavior of the robot. Some policies store a default c-space configuration in their config files and drive the robot to that position when there is no target specified.
position_only (bool, optional) – When True, the policy will use only the position (not orientation) of the target_prim as the target. Defaults to False.
- Returns
None
-
get_end_effector_pose
(joint_positions: numpy.array) → Tuple[numpy.array, numpy.array, numpy.array]¶ Return pose of the end effector at the given end effector position.
- Parameters
joint_positions (np.array) – (m x 1) vector of joint positions for which to compute forward kinematics.
- Returns
End effector pose returned as translation vector (3 x 1) and rotation matrix (3 x 3).
- Return type
Tuple[np.array, np.array]
-
get_prim_pose
(prim: pxr.Usd.Prim, default_trans: numpy.array = array([0.0, 0.0, 0.0]), default_rot: numpy.array = array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])) → bool¶ Return pose of prim.
USD prims that lack translational information are placed on the stage at the point (0,0,0). USD prims that lack rotational information are placed on the stage with the identity rotation. Reading translation/rotation information from prims directly yields a 4x4 transform matrix, which necessitates filling in missing information with default information. This method allows the caller to know what information is actually present in the USD prims by passing in None for the defaults.
- Parameters
prim (Usd.Prim) – Prim for which pose will be returned.
default_trans (np.array, optional) – Translation component of pose to be returned if the prim has no translational information. Defaults to np.zeros(3).
default_rot (cp.array, optional) – Rotational component of pose to be returned if the prim contains no rotational information. Defaults to np.eye(3).
- Returns
Prim pose returned as translation vector (3 x 1) and rotation matrix (3 x 3).
- Return type
Tuple[np.array, np.array]
-
create_cube
(block_prim: pxr.Usd.Prim, side_length: Optional[float] = None, static: bool = False) → bool¶ Create a cube obstacle.
- Parameters
block_prim (Usd.Prim) – USD prim representing the cube. Must have pose information.
side_length (float, optional) – [description]. Length of each side of the cube. If not specified, side_length is read from ‘size’ attribute of block_prim. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if block_prim exists and has required attributes (i.e., size must be specified somewhere, either in block_prim or via side_length param).
- Return type
bool
-
create_block
(block_prim: pxr.Usd.Prim, dimensions: Optional[numpy.array] = None, static: bool = False) → bool¶ Create a block obstacle.
- Parameters
block_prim (Usd.Prim) – USD prim representing the block. Must have pose information.
dimensions (np.array, optional) – Length of block in (x,y,z) dimensions. If not specified, prim must have ‘xformOp:scale’ and “size” attribute. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if block_prim exists and has required attributes (i.e., side lengths must be specified somewhere, either in block_prim or via dimensions param).
- Return type
bool
-
create_sphere
(sphere_prim: pxr.Usd.Prim, radius: Optional[float] = None, static: bool = False) → bool¶ Create a sphere obstacle.
- Parameters
sphere_prim (Usd.Prim) – USD prim representing the sphere. Must have pose information.
radius (float, optional) – Radius of the sphere. If not specified, radius is read from ‘radius’ attribute of sphere_prim. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if sphere_prim exists and has required attributes (i.e., radius must be specified somewhere, either in sphere_prim or via radius param).
- Return type
bool
-
create_capsule
(capsule_prim: pxr.Usd.Prim, radius: Optional[float] = None, height: Optional[float] = None, static: bool = False) → bool¶ Create a capsule obstacle.
- Parameters
capsule_prim (Usd.Prim) – USD prim representing the capsule. Must have pose information.
radius (float, optional) – Radius of the capsule. If not specified, radius is read from ‘radius’ attribute of capsule_prim. Defaults to None.
height (float, optional) – Height of the capsule. If not specified, height is read from ‘height’ attribute of capsule_prim. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if capsule_prim exists and has required attributes (i.e., radius and height must be specified somewhere, either in capsule_prim or via radius and height params).
- Return type
bool
-
disable_obstacle
(obstacle_prim: pxr.Usd.Prim) → bool¶ Disable collision avoidance for obstacle.
- Parameters
obstacle_prim (Usd.Prim) – USD prim for obstacle to be disabled.
- Returns
Return true if obstacle was identified and successfully disabled.
- Return type
bool
-
enable_obstacle
(obstacle_prim: pxr.Usd.Prim) → bool¶ Enable collision avoidance for obstacle.
- Parameters
obstacle_prim (Usd.Prim) – USD prim for obstacle to be enabled.
- Returns
Return true if obstacle was identified and successfully enabled.
- Return type
bool
-
remove_obstacle
(obstacle_prim: pxr.Usd.Prim) → bool¶ Remove obstacle from collision avoidance. Obstacle cannot be re-enabled via enable_obstacle() after removal.
- Parameters
obstacle_prim (Usd.Prim) – USD prim for obstacle to be removed.
- Returns
Return true if obstacle was identified and successfully removed.
- Return type
bool
-
Motion Generator¶
-
class
MotionGenerator
(stage: pxr.Usd.Stage)¶ Interface for running MotionPolicy on simulated robots.
-
initialize
(policy_config: dict, robot_prim: pxr.Usd.Prim, sim_fps: float, velocity_control_damping: float = 100000000.0) → None¶ Initialize MotionGenerator.
- Parameters
policy_config (dict) – Dictionary containing the necessary configurations for a motion policy. policy_config must specify “policy_type” to select a specific motion policy such as “RMPflow”. All other specifications are policy dependent.
robot_prim (Usd.Prim) – USD prim for robot.
sim_fps (float) – Frequency at which Isaac Sim updates the world, typically 60 Hz
velocity_control_damping (float, optional) – Damping gain if using velocity control. Defaults to 1e8.
- Returns
None
-
is_initialized
() → bool¶ Indicates whether MotionGenerator has been initialized.
- Returns
Return True if MotionGenerator has been initialized. Else return False.
- Return type
bool
-
move
(updated_obstacles: Optional[List[pxr.Usd.Prim]] = None) → None¶ Generate new joint targets using MotionPolicy and apply joint targets to the robot.
- Parameters
updated_obstacles (list, optional) – List of obstacles that need to have their poses updated. If provided, only these obstacles will have their poses updated. Defaults to None.
- Returns
None
-
get_joint_states
() → Tuple[numpy.array, numpy.array, numpy.array]¶ Return the states (position, velocity and acceleration) of all robot joints.
- Returns
States (position, velcocity and acceleration) for all robot joints.
- Return type
Tuple[np.array, np.array, np.array]
-
get_active_joint_states
() → Tuple[numpy.array, numpy.array, numpy.array]¶ Return the states (position, velocity and acceleration) of active robot joints. Active joints are joints controlled by the underlying MotionPolicy.
- Returns
States (position, velcocity and acceleration) for active robot joints.
- Return type
Tuple[np.array, np.array, np.array]
-
get_end_effector_pose
() → Tuple[numpy.array, numpy.array, numpy.array]¶ Return current pose of the end effector.
- Returns
End effector pose returned as translation vector (3 x 1) and rotation matrix (3 x 3).
- Return type
Tuple[np.array, np.array]
-
set_cspace_target
(target: numpy.array) → None¶ Set configuration space target for the robot.
- Parameters
target (np.array) – Desired configuration for the robot as (m x 1) vector where m is the number of active joints.
- Returns
None
-
set_end_effector_target
(target_prim: pxr.Usd.Prim, position_only: bool = False) → None¶ Set end effector target.
- Parameters
target_prim (Usd.Prim) – USD prim of the target. target_prim may also be None, in which case it is up to the policy to specify the desired behavior of the robot. Some policies store a default c-space configuration in their config files and drive the robot to that position when there is no target specified.
position_only (bool, optional) – When True, the policy will use only the position (not orientation) of the target_prim as the target. Defaults to False.
- Returns
None
-
create_cube
(block_prim: pxr.Usd.Prim, side_length: Optional[float] = None, static: bool = False) → bool¶ Create a cube obstacle.
- Parameters
block_prim (Usd.Prim) – USD prim representing the cube. Must have pose information.
side_length (float, optional) – [description]. Length of each side of the cube. If not specified, side_length is read from ‘size’ attribute of block_prim. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if block_prim exists and has required attributes (i.e., size must be specified somewhere, either in block_prim or via side_length param).
- Return type
bool
-
create_block
(block_prim: pxr.Usd.Prim, dimensions: Optional[numpy.array] = None, static: bool = False) → bool¶ Create a block obstacle.
- Parameters
block_prim (Usd.Prim) – USD prim representing the block. Must have pose information.
dimensions (np.array, optional) – Length of block in (x,y,z) dimensions. If not specified, prim must have ‘xformOp:scale’ and “size” attribute. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if block_prim exists and has required attributes (i.e., side lengths must be specified somewhere, either in block_prim or via dimensions param).
- Return type
bool
-
create_sphere
(sphere_prim: pxr.Usd.Prim, radius: Optional[float] = None, static: bool = False) → bool¶ Create a sphere obstacle.
- Parameters
sphere_prim (Usd.Prim) – USD prim representing the sphere. Must have pose information.
radius (float, optional) – Radius of the sphere. If not specified, radius is read from ‘radius’ attribute of sphere_prim. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if sphere_prim exists and has required attributes (i.e., radius must be specified somewhere, either in sphere_prim or via radius param).
- Return type
bool
-
create_capsule
(capsule_prim: pxr.Usd.Prim, radius: Optional[float] = None, height: Optional[float] = None, static: bool = False) → bool¶ Create a capsule obstacle.
- Parameters
capsule_prim (Usd.Prim) – USD prim representing the capsule. Must have pose information.
radius (float, optional) – Radius of the capsule. If not specified, radius is read from ‘radius’ attribute of capsule_prim. Defaults to None.
height (float, optional) – Height of the capsule. If not specified, height is read from ‘height’ attribute of capsule_prim. Defaults to None.
static (bool, optional) – If True, indicate that cube will never change pose, and may be ignored in internal world updates. Defaults to False.
- Returns
Return True if capsule_prim exists and has required attributes (i.e., radius and height must be specified somewhere, either in capsule_prim or via radius and height params).
- Return type
bool
-
get_prim_pose
(prim: pxr.Usd.Prim, default_trans: numpy.array = array([0.0, 0.0, 0.0]), default_rot: numpy.array = array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])) → bool¶ Return pose of prim.
USD prims that lack translational information are placed on the stage at the point (0,0,0). USD prims that lack rotational information are placed on the stage with the identity rotation. Reading translation/rotation information from prims directly yields a 4x4 transform matrix, which necessitates filling in missing information with default information. This method allows the caller to know what information is actually present in the USD prims by passing in None for the defaults.
- Parameters
prim (Usd.Prim) – Prim for which pose will be returned.
default_trans (np.array, optional) – Translation component of pose to be returned if the prim has no translational information. Defaults to np.zeros(3).
default_rot (cp.array, optional) – Rotational component of pose to be returned if the prim contains no rotational information. Defaults to np.eye(3).
- Returns
Prim pose returned as translation vector (3 x 1) and rotation matrix (3 x 3).
- Return type
Tuple[np.array, np.array]
-
disable_obstacle
(obstacle_prim: pxr.Usd.Prim) → bool¶ Disable collision avoidance for obstacle.
- Parameters
obstacle_prim (Usd.Prim) – USD prim for obstacle to be disabled.
- Returns
Return true if obstacle was identified and successfully disabled.
- Return type
bool
-
enable_obstacle
(obstacle_prim: pxr.Usd.Prim) → bool¶ Enable collision avoidance for obstacle.
- Parameters
obstacle_prim (Usd.Prim) – USD prim for obstacle to be enabled.
- Returns
Return true if obstacle was identified and successfully enabled.
- Return type
bool
-
remove_obstacle
(obstacle_prim: pxr.Usd.Prim) → bool¶ Remove obstacle from collision avoidance. Obstacle cannot be re-enabled via enable_obstacle() after removal.
- Parameters
obstacle_prim (Usd.Prim) – USD prim for obstacle to be removed.
- Returns
Return true if obstacle was identified and successfully removed.
- Return type
bool
-
get_joint_velocity_targets
() → numpy.array¶ Compute and return joint velocity targets using underlying MotionPolicy.
- Returns
(m x 1) vector of velocity targets, where m is the number of active joints.
- Return type
np.array
-
get_joint_position_targets
() → numpy.array¶ Compute and return joint position targets using underlying MotionPolicy.
Active joint targets are computed by MotionPolicy. Position targets for non-active joints are kept identical to those currently set in the articulation.
- Returns
(m x 1) vector of position targets, where m is the total number of joints.
- Return type
np.array
-
get_motion_policy
() → omni.isaac.motion_generation.motion_policy_interface.MotionPolicy¶ It can be convenient to interact directly with the low level motion policy for testing and development, but in general the policy should be designed to function using only the motion_policy_interface.
- Returns
Underlying motion policy used by MotionGenerator.
- Return type
-
RMPFlow Base Controller¶
-
class
RMPFlowController
(name: str, robot_prim_path: str, policy_map_path: List[str], physics_dt: float = 0.016666666666666666)¶ [summary]
- Parameters
name (str) – [description]
robot_prim_path (str) – [description]
policy_map_path (List[str]) – [description]
physics_dt (float, optional) – [description]. Defaults to 1.0/60.0.
-
add_cube_obstacle
(cube_prim: pxr.Usd.Prim) → None¶ [summary]
- Parameters
cube_prim (Usd.Prim) – [description]
-
forward
(target_end_effector_position: numpy.ndarray, target_end_effector_orientation: Optional[numpy.ndarray] = None) → omni.isaac.core.utils.types.ArticulationAction¶ [summary]
- Parameters
target_end_effector_position (np.ndarray) – [description]
target_end_effector_orientation (Optional[np.ndarray], optional) – [description]. Defaults to None.
- Returns
[description]
- Return type
-
get_motion_generation
() → omni.isaac.motion_generation.motion_generation.MotionGenerator¶ [summary]
- Returns
[description]
- Return type
-
remove_cube_obstacle
(cube_prim: pxr.Usd.Prim) → None¶ [summary]
- Parameters
cube_prim (Usd.Prim) – [description]
-
reset
() → None¶ [summary]
Pick Place Controller¶
-
class
PickPlaceController
(name, cspace_controller: omni.isaac.core.controllers.base_controller.BaseController, gripper_controller: omni.isaac.core.controllers.base_gripper_controller.BaseGripperController, start_picking_height: Optional[float] = None, event_velocities: Optional[List[float]] = None)¶ Phase 0: Move end_effector above the cube center.
Phase 1: Lower end_effector down to encircle the target cube
Phase 2: close grip.
Phase 3: Move end_effector up again, keeping the grip tight (lifting the block).
Phase 4: Smoothly move the end_effector toward the goal xy, keeping the height constant.
Phase 5: Move end_effector vertically toward goal height.
Phase 6: loosen the grip.
Phase 7: Move end_effector vertically up again
Phase 8: Move end_effector towards the old xy position.
- Parameters
name ([type]) – [description]
cspace_controller (BaseController) – [description]
gripper_controller (GripperController) – [description]
start_picking_height (typing.Optional[float], optional) – [description]. Defaults to None.
event_velocities (typing.Optional[typing.List[float]], optional) – [description]. Defaults to None.
- Raises
Exception – [description]
Exception – [description]
-
forward
(picking_position: numpy.ndarray, placing_position: numpy.ndarray, current_joint_positions: numpy.ndarray, end_effector_offset: Optional[numpy.ndarray] = None, end_effector_orientation: Optional[numpy.ndarray] = None) → omni.isaac.core.utils.types.ArticulationAction¶ [summary]
- Parameters
picking_position (np.ndarray) – [description]
placing_position (np.ndarray) – [description]
current_joint_positions (np.ndarray) – [description]
end_effector_offset (typing.Optional[np.ndarray], optional) – [description]. Defaults to None.
end_effector_orientation (typing.Optional[np.ndarray], optional) – [description]. Defaults to None.
- Returns
[description]
- Return type
-
get_current_event
() → int¶ [summary]
- Returns
[description]
- Return type
int
-
is_done
() → bool¶ [summary]
- Returns
[description]
- Return type
bool
-
is_paused
() → bool¶ [summary]
- Returns
[description]
- Return type
bool
-
pause
() → None¶ [summary]
-
reset
(start_picking_height: Optional[float] = None, event_velocities: Optional[List[float]] = None) → None¶ [summary]
- Parameters
start_picking_height (typing.Optional[float], optional) – [description]. Defaults to None.
event_velocities (typing.Optional[typing.List[float]], optional) – [description]. Defaults to None.
- Raises
Exception – [description]
Exception – [description]
-
resume
() → None¶ [summary]
Stacking Controller¶
-
class
StackingController
(name: str, pick_place_controller: omni.isaac.motion_generation.pick_place_controller.PickPlaceController, picking_order_cube_names: List[str], robot_observation_name: str)¶ [summary]
- Parameters
name (str) – [description]
pick_place_controller (PickPlaceController) – [description]
picking_order_cube_names (typing.List[str]) – [description]
robot_observation_name (str) – [description]
-
forward
(observations: dict, end_effector_orientation: Optional[numpy.ndarray] = None, end_effector_offset: Optional[numpy.ndarray] = None) → omni.isaac.core.utils.types.ArticulationAction¶ - A controller should take inputs and returns an ArticulationAction to be then passed to the
ArticulationController.
- Parameters
observations (dict) – [description]
- Raises
NotImplementedError – [description]
- Returns
[description]
- Return type
-
is_done
() → bool¶ [summary]
- Returns
[description]
- Return type
bool
-
reset
(picking_order_cube_names: Optional[List[str]] = None) → None¶ [summary]
- Parameters
picking_order_cube_names (typing.Optional[typing.List[str]], optional) – [description]. Defaults to None.
Wheel Base Pose Controller¶
-
class
WheelBasePoseController
(name: str, open_loop_wheel_controller: omni.isaac.core.controllers.base_controller.BaseController, is_holonomic: bool = False)¶ [summary]
- Parameters
name (str) – [description]
open_loop_wheel_controller (BaseController) – A controller that takes in a command of [longitudinal velocity, steering angle] and returns the ArticulationAction to be applied to the wheels if non holonomic. and [longitudinal velocity, latitude velocity, steering angle] if holonomic.
is_holonomic (bool, optional) – [description]. Defaults to False.
-
forward
(start_position: numpy.ndarray, start_orientation: numpy.ndarray, goal_position: numpy.ndarray, lateral_velocity: float = 20.0, yaw_velocity: float = 0.5, heading_tol: float = 0.05, position_tol: float = 4.0) → omni.isaac.core.utils.types.ArticulationAction¶ [summary]
- Parameters
start_position (np.ndarray) – [description]
start_orientation (np.ndarray) – [description]
goal_position (np.ndarray) – [description]
lateral_velocity (float, optional) – [description]. Defaults to 20.0.
yaw_velocity (float, optional) – [description]. Defaults to 0.5.
heading_tol (float, optional) – [description]. Defaults to 0.05.
position_tol (float, optional) – [description]. Defaults to 4.0.
- Returns
[description]
- Return type
-
reset
() → None¶ [summary]