Wheeled Robots [omni.isaac.wheeled_robots]
Controllers (Python API)
- class DifferentialController(name: str, wheel_radius: float, wheel_base: float, max_linear_speed: float = 1e+20, max_angular_speed: float = 1e+20, max_wheel_speed: float = 1e+20)
Controller uses unicycle model for a differential drive
- Parameters
name (str) – [description]
wheel_radius (float) – Radius of left and right wheels in cms
wheel_base (float) – Distance between left and right wheels in cms
- forward(command: numpy.ndarray) omni.isaac.core.utils.types.ArticulationAction
Calculating the wheels speeds given the desired speed for the vehicle.
- Parameters
command (np.ndarray) – desired vehicle [forward, rotation] speed
- Returns
[description]
- Return type
- reset() None
[summary]
- class HolonomicController(name: str, wheel_radius: Optional[numpy.ndarray] = None, wheel_positions: Optional[numpy.ndarray] = None, wheel_orientations: Optional[numpy.ndarray] = None, mecanum_angles: Optional[numpy.ndarray] = None, wheel_axis: float = array([1, 0, 0]), up_axis: float = array([0, 0, 1]), max_linear_speed: float = 1e+20, max_angular_speed: float = 1e+20, max_wheel_speed: float = 1e+20, linear_gain: float = 1.0, angular_gain: float = 1.0)
[summary] Generic Holonomic drive controller. Model must have drive joints to mecanum wheels defined in the USD with the rollers angle and radius.
- Parameters
name (str) – [description]
wheel_radius (np.ndarray) – radius of the wheels, array of 1 can be used if all wheels are the same size
wheel_positions (np.ndarray) – position of the wheels relative to center of mass of the vehicle. number of wheels x [x,y,z]
wheel_orientations (np.ndarray) – orientation of the wheels in quaternions relative to center of mass frame of the vehicle. number of wheels x [quaternions]
mecanum_angles (np.ndarray) – mecanum wheel angles. Array of 1 can be used if all wheels have the same angle.
wheel_axis (np.ndarray) – the spinning axis of the wheels. Defaults to [1,0,0]
up_axis (np.ndarray) – Defaults to z- axis
- build_base()
- forward(command: numpy.ndarray) omni.isaac.core.utils.types.ArticulationAction
Calculating the wheels speed given the desired vehicle speed.
- Parameters
command (np.ndarray) – [forward_velocity, lateral_velocity, yaw_velocity].
- Returns
[description]
- Return type
- reset() None
[summary]
Utilities
- class HolonomicRobotUsdSetup(robot_prim_path: str, com_prim_path: str)
[summary] Generic Holonomic Robot Setup. Extract from USD or compile from user input the necessary information for holonomic controller. :param name: [description] :type name: str :param prim_path: path of the robot articulation :type prim_path: str :param com_prim_path: path of the xform representing the center of mass of the vehicle :type com_prim_path: str
- from_usd(robot_prim_path, com_prim_path)
if the USD contains all the necessary information, automatically extract them and compile
- get_articulation_controller_params()
- get_holonomic_controller_params()
- property mecanum_angles
- property up_axis
- property wheel_axis
- property wheel_dof_names
- property wheel_orientations
- property wheel_positions
- property wheel_radius
- class WheeledRobot(prim_path: str, robot_path: Optional[str] = None, wheel_dof_names: Optional[str] = None, wheel_dof_indices: Optional[int] = None, name: str = 'wheeled_robot', usd_path: Optional[str] = None, create_robot: Optional[bool] = False, position: Optional[numpy.ndarray] = None, orientation: Optional[numpy.ndarray] = None)
[summary]
- Parameters
prim_path (str) – [description]
robot_path (str) – relative path from prim path to the robot.
name (str) – [description]
wheel_dof_names ([str, str]) – name of the wheels, [left,right].
wheel_dof_indices – ([int, int]): indices of the wheels, [left, right]
usd_path (str, optional) – [description]
create_robot (bool) – create robot at prim_path if no robot exist at said path. Defaults to False
position (Optional[np.ndarray], optional) – [description]. Defaults to None.
orientation (Optional[np.ndarray], optional) – [description]. Defaults to None.
- apply_action(control_actions: omni.isaac.core.utils.types.ArticulationAction) None
[summary]
- Parameters
control_actions (ArticulationAction) – actions to be applied for next physics step.
indices (Optional[Union[list, np.ndarray]], optional) – degree of freedom indices to apply actions to. Defaults to all degrees of freedom.
- Raises
Exception – [description]
- apply_visual_material(visual_material: omni.isaac.core.materials.visual_material.VisualMaterial, weaker_than_descendants: bool = False) None
Used to apply visual material to the held prim and optionally its descendants.
- Parameters
visual_material (VisualMaterial) – visual material to be applied to the held prim. Currently supports PreviewSurface, OmniPBR and OmniGlass.
weaker_than_descendants (bool, optional) – True if the material shouldn’t override the descendants materials, otherwise False. Defaults to False.
- apply_wheel_actions(actions: omni.isaac.core.utils.types.ArticulationAction) None
applying action to the wheels to move the robot
- Parameters
actions (ArticulationAction) – [description]
- property articulation_handle: int
[summary]
- Returns
[description]
- Return type
int
- disable_gravity() None
Keep gravity from affecting the robot
- property dof_names: List[str]
List of prim names for each DOF.
- Returns
prim names
- Return type
list(string)
- property dof_properties: numpy.ndarray
[summary]
- Returns
[description]
- Return type
np.ndarray
- enable_gravity() None
Gravity will affect the robot
- get_angular_velocity() numpy.ndarray
[summary]
- Returns
[description]
- Return type
np.ndarray
- get_applied_action() omni.isaac.core.utils.types.ArticulationAction
[summary]
- Raises
Exception – [description]
- Returns
[description]
- Return type
- get_applied_joint_efforts(joint_indices: Optional[Union[List, numpy.ndarray]] = None) numpy.ndarray
Gets the efforts applied to the joints
- Parameters
joint_indices (Optional[Union[List, np.ndarray]], optional) – _description_. Defaults to None.
- Raises
Exception – _description_
- Returns
_description_
- Return type
np.ndarray
- get_applied_visual_material() omni.isaac.core.materials.visual_material.VisualMaterial
- Returns the current applied visual material in case it was applied using apply_visual_material OR
it’s one of the following materials that was already applied before: PreviewSurface, OmniPBR and OmniGlass.
- Returns
the current applied visual material if its type is currently supported.
- Return type
- get_articulation_body_count() int
[summary]
- Returns
[description]
- Return type
int
- get_articulation_controller() omni.isaac.core.controllers.articulation_controller.ArticulationController
- Returns
PD Controller of all degrees of freedom of an articulation, can apply position targets, velocity targets and efforts.
- Return type
- get_articulation_controller_properties()
- get_default_state() omni.isaac.core.utils.types.XFormPrimState
- Returns
returns the default state of the prim (position and orientation) that is used after each reset.
- Return type
- get_dof_index(dof_name: str) int
[summary]
- Parameters
dof_name (str) – [description]
- Returns
[description]
- Return type
int
- get_enabled_self_collisions() bool
[summary]
- Returns
[description]
- Return type
bool
- get_joint_positions(joint_indices: Optional[Union[List, numpy.ndarray]] = None) numpy.ndarray
_summary_
- Parameters
joint_indices (Optional[Union[List, np.ndarray]], optional) – _description_. Defaults to None.
- Returns
_description_
- Return type
np.ndarray
- get_joint_velocities(joint_indices: Optional[Union[List, numpy.ndarray]] = None) numpy.ndarray
_summary_
- Parameters
joint_indices (Optional[Union[List, np.ndarray]], optional) – _description_. Defaults to None.
- Returns
_description_
- Return type
np.ndarray
- get_joints_default_state() omni.isaac.core.utils.types.JointsState
Accessor for the default joints state.
- Returns
The defaults that the robot is reset to when post_reset() is called (often automatically called during world.reset()).
- Return type
- get_joints_state() omni.isaac.core.utils.types.JointsState
[summary]
- Returns
[description]
- Return type
- get_linear_velocity() numpy.ndarray
[summary]
- Returns
[description]
- Return type
np.ndarray
- get_local_pose() Tuple[numpy.ndarray, numpy.ndarray]
Gets prim’s pose with respect to the local frame (the prim’s parent frame).
- Returns
- first index is position in the local frame of the prim. shape is (3, ).
second index is quaternion orientation in the local frame of the prim. quaternion is scalar-first (w, x, y, z). shape is (4, ).
- Return type
Tuple[np.ndarray, np.ndarray]
- get_local_scale() numpy.ndarray
Gets prim’s scale with respect to the local frame (the parent’s frame).
- Returns
scale applied to the prim’s dimensions in the local frame. shape is (3, ).
- Return type
np.ndarray
- get_measured_joint_efforts(joint_indices: Optional[Union[List, numpy.ndarray]] = None) numpy.ndarray
Returns the dof efforts computed/measured by the physics solver
- Parameters
joint_indices (Optional[Union[List, np.ndarray]], optional) – _description_. Defaults to None.
- Raises
Exception – _description_
- Returns
_description_
- Return type
np.ndarray
- get_measured_joint_forces(joint_indices: Optional[Union[List, numpy.ndarray]] = None) numpy.ndarray
Gets the measured joint forces
- Parameters
joint_indices (Optional[Union[List, np.ndarray]], optional) – _description_. Defaults to None.
- Raises
Exception – _description_
- Returns
_description_
- Return type
np.ndarray
- get_sleep_threshold() float
[summary]
- Returns
[description]
- Return type
float
- get_solver_position_iteration_count() int
[summary]
- Returns
[description]
- Return type
int
- get_solver_velocity_iteration_count() int
[summary]
- Returns
[description]
- Return type
int
- get_stabilization_threshold() float
[summary]
- Returns
[description]
- Return type
float
- get_visibility() bool
- Returns
true if the prim is visible in stage. false otherwise.
- Return type
bool
- get_wheel_positions()
[summary]
- Returns
[description]
- Return type
Tuple[float, float]
- get_wheel_velocities()
[summary]
- Returns
[description]
- Return type
Tuple[np.ndarray, np.ndarray]
- get_world_pose() Tuple[numpy.ndarray, numpy.ndarray]
Gets prim’s pose with respect to the world’s frame.
- Returns
- first index is position in the world frame of the prim. shape is (3, ).
second index is quaternion orientation in the world frame of the prim. quaternion is scalar-first (w, x, y, z). shape is (4, ).
- Return type
Tuple[np.ndarray, np.ndarray]
- get_world_scale() numpy.ndarray
Gets prim’s scale with respect to the world’s frame.
- Returns
scale applied to the prim’s dimensions in the world frame. shape is (3, ).
- Return type
np.ndarray
- property handles_initialized: bool
[summary]
- Returns
[description]
- Return type
bool
- initialize(physics_sim_view=None) None
[summary]
- is_valid() bool
- Returns
True is the current prim path corresponds to a valid prim in stage. False otherwise.
- Return type
bool
- is_visual_material_applied() bool
- Returns
True if there is a visual material applied. False otherwise.
- Return type
bool
- property name: Optional[str]
Returns: str: name given to the prim when instantiating it. Otherwise None.
- property non_root_articulation_link: bool
_summary_
- Returns
_description_
- Return type
bool
- property num_dof: int
[summary]
- Returns
[description]
- Return type
int
- post_reset() None
[summary]
- property prim: pxr.Usd.Prim
Returns: Usd.Prim: USD Prim object that this object holds.
- property prim_path: str
Returns: str: prim path in the stage.
- set_angular_velocity(velocity: numpy.ndarray) None
[summary]
- Parameters
velocity (np.ndarray) – [description]
- set_default_state(position: Optional[Sequence[float]] = None, orientation: Optional[Sequence[float]] = None) None
Sets the default state of the prim (position and orientation), that will be used after each reset.
- Parameters
position (Optional[Sequence[float]], optional) – position in the world frame of the prim. shape is (3, ). Defaults to None, which means left unchanged.
orientation (Optional[Sequence[float]], optional) – quaternion orientation in the world frame of the prim. quaternion is scalar-first (w, x, y, z). shape is (4, ). Defaults to None, which means left unchanged.
- set_enabled_self_collisions(flag: bool) None
[summary]
- Parameters
flag (bool) – [description]
- set_joint_efforts(efforts: numpy.ndarray, joint_indices: Optional[Union[List, numpy.ndarray]] = None) None
[summary]
- Parameters
efforts (np.ndarray) – [description]
joint_indices (Optional[Union[list, np.ndarray]], optional) – [description]. Defaults to None.
- Raises
Exception – [description]
- set_joint_positions(positions: numpy.ndarray, joint_indices: Optional[Union[List, numpy.ndarray]] = None) None
[summary]
- Parameters
positions (np.ndarray) – [description]
indices (Optional[Union[list, np.ndarray]], optional) – [description]. Defaults to None.
- Raises
Exception – [description]
- set_joint_velocities(velocities: numpy.ndarray, joint_indices: Optional[Union[List, numpy.ndarray]] = None) None
[summary]
- Parameters
velocities (np.ndarray) – [description]
indices (Optional[Union[list, np.ndarray]], optional) – [description]. Defaults to None.
- Raises
Exception – [description]
- set_joints_default_state(positions: Optional[numpy.ndarray] = None, velocities: Optional[numpy.ndarray] = None, efforts: Optional[numpy.ndarray] = None) None
[summary]
- Parameters
positions (Optional[np.ndarray], optional) – [description]. Defaults to None.
velocities (Optional[np.ndarray], optional) – [description]. Defaults to None.
efforts (Optional[np.ndarray], optional) – [description]. Defaults to None.
- set_linear_velocity(velocity: numpy.ndarray)
Sets the linear velocity of the prim in stage.
- Parameters
velocity (np.ndarray) – linear velocity to set the rigid prim to. Shape (3,).
- set_local_pose(translation: Optional[Sequence[float]] = None, orientation: Optional[Sequence[float]] = None) None
Sets prim’s pose with respect to the local frame (the prim’s parent frame).
- Parameters
translation (Optional[Sequence[float]], optional) – translation in the local frame of the prim (with respect to its parent prim). shape is (3, ). Defaults to None, which means left unchanged.
orientation (Optional[Sequence[float]], optional) – quaternion orientation in the local frame of the prim. quaternion is scalar-first (w, x, y, z). shape is (4, ). Defaults to None, which means left unchanged.
- set_local_scale(scale: Optional[Sequence[float]]) None
Sets prim’s scale with respect to the local frame (the prim’s parent frame).
- Parameters
scale (Optional[Sequence[float]]) – scale to be applied to the prim’s dimensions. shape is (3, ). Defaults to None, which means left unchanged.
- set_sleep_threshold(threshold: float) None
[summary]
- Parameters
threshold (float) – [description]
- set_solver_position_iteration_count(count: int) None
[summary]
- Parameters
count (int) – [description]
- set_solver_velocity_iteration_count(count: int)
[summary]
- Parameters
count (int) – [description]
- set_stabilization_threshold(threshold: float) None
[summary]
- Parameters
threshold (float) – [description]
- set_visibility(visible: bool) None
Sets the visibility of the prim in stage.
- Parameters
visible (bool) – flag to set the visibility of the usd prim in stage.
- set_wheel_positions(positions) None
[summary]
- Parameters
positions (Tuple[float, float]) – [description]
- set_wheel_velocities(velocities) None
[summary]
- Parameters
velocities (Tuple[float, float]) – [description]
- set_world_pose(position: Optional[Sequence[float]] = None, orientation: Optional[Sequence[float]] = None) None
Sets prim’s pose with respect to the world’s frame.
- Parameters
position (Optional[Sequence[float]], optional) – position in the world frame of the prim. shape is (3, ). Defaults to None, which means left unchanged.
orientation (Optional[Sequence[float]], optional) – quaternion orientation in the world frame of the prim. quaternion is scalar-first (w, x, y, z). shape is (4, ). Defaults to None, which means left unchanged.
- property wheel_dof_indices
[summary]
- Returns
[description]
- Return type
int
Omnigraph Nodes
DifferentialController
Differential Controller
Use the wheel radius and the distance between the wheels to calculate the desired wheels speed when given a desired vehicle speed.
- Inputs
execIn (execution): The input execution.
wheelRadius (double): radius of the wheels.
wheelDistance (double): distance between the two wheels.
maxLinearSpeed (double): max linear speed allowed for vehicle.
maxAngularSpeed (double): max angular speed allowed for vehicle.
maxWheelSpeed (double): max wheel speed allowed.
linearVelocity (double): desired linear velocity.
angularVelocity (double): desired rotation velocity.
- Outputs
positionCommand (double[]): position commands.
velocityCommand (double[]): velocity commands.
effortCommand (double[]): effort commands.
QuinticPathPlanner
Quintic Path Planner For Wheeled robots
Use odometry from a robot and a target position/prim to calculate a route from the robot’s starting position to the target position.
- Inputs
execIn (execution): The input execution.
currentPosition (vectord[3]): Current position of the robot (recommended to use Get Prim Local to World Transform node).
currentOrientation (quatd[4]): Current rotation of the robot as a quaternion (recommended to use Get Prim Local to World Transform node).
targetPrim (target, optional): USD prim reference to the target position/orientation prim.
targetPosition (vectord[3]): Target position (used if no targetPrim provided).
targetOrientation (quatd[4]): Target orientation (used if no targetPrim provided).
initialVelocity (double): Initial velocity. Default to 0.5.
initialAccel (double): Initial acceleration. Default to 0.02.
goalVelocity (double): Goal velocity. Default to 0.5.
goalAccel (double): Goal acceleration. Default to 0.02.
maxAccel (double): Max acceleration. Default to 1.5.
maxJerk (double): Max jerk. Default to 0.3.
step (double): Step. Default to 0.16666666667.
- Outputs
execOut (execution): The output execution.
pathArrays (double[]): The path v, x, y, and yaw arrays.
target (double[3]): Target position and orientation.
targetChanged (bool): Target position/orientation has changed.
HolonomicController
Holonomic Controller
Calculating the desired wheel speeds when given a desired vehicle speed.
- Inputs
execIn (execution): The input execution.
wheelRadius (double[]): an array of wheel radius.
wheelPositions (double[3][]): position of the wheel with respect to chassis’ center of mass.
wheelOrientations (double[4][]): orientation of the wheel with respect to chassis’ center of mass frame.
mecanumAngles (double[]): angles of the mecanum wheels with respect to wheel’s rotation axis.
wheelAxis (double[3]): the rotation axis of the wheels.
upAxis (double[3]): the rotation axis of the vehicle.
velocityCommands ([‘float[3]’, ‘double[3]’]): velocity in x and y and rotation.
maxLinearSpeed (double, optional): maximum speed allowed for the vehicle.
maxAngularSpeed (double, optional): maximum angular rotation speed allowed for the vehicle.
maxWheelSpeed (double, optional): maximum rotation speed allowed for the wheel joints.
linearGain (double): linear gain. Default to 1.
angularGain (double): angular gain. Default to 1.
- Outputs
jointPositionCommand (double[]): position commands for the wheel joints.
jointVelocityCommand (double[]): velocity commands for the wheels joints.
jointEffortCommand (double[]): effort commands for the wheels joints.
StanleyControlPID
Drive to Target Steering
- Inputs
execIn (execution): The input execution.
currentPosition (vectord[3]): Current position of the robot (recommended to use Get Prim Local to World Transform node).
currentOrientation (quatd[4]): Current rotation of the robot as a quaternion (recommended to use Get Prim Local to World Transform node).
currentSpeed (vectord[3]): Current linear velocity of the robot.
maxVelocity (double): Maximum linear velocity of the robot. Default to 1.5.
reachedGoal (bool[]): Position and orientation thresholds at target. Default to [False, False].
pathArrays (double[]): The path v, x, y, and yaw arrays.
target (double[3]): Target position and orientation. Default to [0, 0, 0].
targetChanged (bool): Target position/orientation has changed. Default to False.
wheelBase (double): Distance between the centers of the front and rear wheels. Default to 0.4132.
thresholds (double[2]): Position and orientation thresholds at target. Default to [0.1, 0.1].
drawPath (bool): Draw the provided path curve onto the stage. Default to False.
step (double): Step. Default to 0.16666666667.
gains (double[3]): control, velocity and steering gains. Default to [0.5, 0.1, 0.0872665].
- Outputs
execOut (execution): The output execution.
linearVelocity (double): Current forward speed for robot drive.
angularVelocity (double): Current angular speed for robot drive.
HolonomicRobotUsdSetup
setup any robot to be ready to be used by the holonomic controller by extract attributes from USD
Use this node to extract all the holonomic drive information from USD if the listed information are stored in the USD file already. If they are not in USD, you can manually set those values in the HolonomicController node
- Inputs
robotPrim (target): prim for the robot’s articulation root.
comPrim (target): prim for the center of mass xform.
usePath (bool): use prim path instead of prim target. Default to False.
robotPrimPath (token): prim path to the robot’s articulation root link when usdPath is true.
comPrimPath (token): prim path to the robot’s center of mass xform.
- Outputs
wheelRadius (double[]): an array of wheel radius.
wheelPositions (double[3][]): position of the wheel with respect to chassis’ center of mass.
wheelOrientations (double[4][]): orientation of the wheel with respect to chassis’ center of mass frame.
mecanumAngles (double[]): angles of the mechanum wheels with respect to wheel’s rotation axis.
wheelAxis (double[3]): the rotation axis of the wheels, assuming all wheels have the same.
upAxis (double[3]): the rotation axis of the vehicle.
wheelDofNames (token[]): name of the left wheel joint.
CheckGoal2D
Check if wheeled robot has reached goal
- Inputs
execIn (execution): The input execution.
currentPosition (vectord[3]): Current position of the robot (recommended to use Get Prim Local to World Transform node).
currentOrientation (quatd[4]): Current rotation of the robot as a quaternion (recommended to use Get Prim Local to World Transform node).
target (double[3]): Target position and orientation. Default to [0, 0, 0].
targetChanged (bool): Target position/orientation has changed. Default to False.
thresholds (double[2]): Position and orientation thresholds at target. Default to [0.1, 0.1].
- Outputs
execOut (execution): The output execution.
reachedGoal (bool[]): Reached position and orientation goals.
AckermannSteering
Ackermann Steering Geometry
- Inputs
execIn (execution): The input execution.
accel (double): Desired acceleration for the robot.
curvature (double): Desired curvature for the robot, equal to 1/radius of curvature. By default it is positive for turning right and negative for turning left for front wheel drive.
linearVelocity (vectord[3]): Current linear velocity of the robot.
wheelBase (double): Distance between the front and rear axles of the robot.
trackWidth (double): Distance between the left and right rear wheels of the robot.
turningWheelRadius (double): Radius of the front wheels of the robot.
maxWheelVelocity (double): Maximum angular velocity of the robot wheel (rad/s).
invertCurvature (bool): Flips the sign of the curvature, Set to true for rear wheel drive.
maxWheelRotation (double): Maximum angle of rotation for the front wheels in degrees.
DT (double): Delta time for the simulation step.
- Outputs
execOut (execution): The output execution.
leftWheelAngle (double): Angle for the left turning wheel in degrees.
rightWheelAngle (double): Angle for the right turning wheel in degrees.
wheelRotationVelocity (double): Angular velocity for the turning wheels in rad/s.