internutopia.core.robot.articulation#

articulation#

class internutopia.core.robot.articulation.IArticulation[source]#

Represents a kinematic chain of rigid bodies connected by joints.

Enables modeling of multi-link robotic mechanisms with realistic motion and constraints.

Note

Within Isaac Sim’s implementation, it is exclusively supported for single-DoF joints and methods prefixed with set_joint_***() are functionally equivalent to their set_dof_***() counterparts.

While Genesis natively supports multi-DoF joints (where a single joint may govern multiple degrees of freedom), we adhere to a one-to-one joint-to-DoF mapping convention for cross-simulator compatibility.

abstract apply_action(action: ArticulationAction) None[source]#

Apply joint positions, velocities and/or efforts to control an articulation.

Parameters:

action (ArticulationAction) – actions to be applied for next physics step.

Hint

High stiffness makes the joints snap faster and harder to the desired target, and higher damping smoothes but also slows down the joint’s movement to target

  • For position control, set relatively high stiffness and low damping (to reduce vibrations)

  • For velocity control, stiffness must be set to zero with a non-zero damping

  • For effort control, stiffness and damping must be set to zero

classmethod create(simulator_type: str = 'isaac_sim', usd_path: str | None = None, prim_path: str | None = None, name: str | None = None, position: ndarray | None = None, orientation: ndarray | None = None, scale: ndarray | None = None) IArticulation[source]#

Factory method to create IArticulation instances based on simulator_type.

Parameters:
  • simulator_type (str) – simulator type.

  • usd_path (str, optional) – The file path to the USD containing the robot definition.

  • prim_path (str, optional) – prim path of the Prim to encapsulate or create.

  • name (str, optional) – shortname to be used as a key by Scene class. Note: needs to be unique if the object is added to the Scene.

  • position (Optional[np.ndarray], optional) – position in the world frame of the prim. Shape is (3, ). Defaults to None, which means left unchanged.

  • orientation (Optional[np.ndarray], optional) – quaternion orientation in the world/ local frame of the prim quaternion is scalar-first (w, x, y, z). Shape is (4, ). Defaults to None, which means left unchanged.

  • scale (Optional[np.ndarray], optional) – local scale to be applied to the prim’s dimensions. Shape is (3, ). Defaults to None, which means left unchanged.

property dof_names: List[str]#

List of prim names for each DOF.

Returns:

prim names

Return type:

list(string)

abstract get_angular_velocity() ndarray[source]#

Get the articulation’s root angular velocity.

Returns:

3D angular velocity vector. Shape (3,)

Return type:

np.ndarray

abstract get_dof_index(dof_name: str) int[source]#

Get a DOF index given its name.

Parameters:

dof_name (str) – name of the DOF

Returns:

DOF index

Return type:

int

abstract get_joint_positions(joint_indices: List | ndarray | None = None) ndarray[source]#

Get the articulation joint positions.

Parameters:

joint_indices (Optional[Union[List, np.ndarray]], optional) – indices to specify which joints to read. Defaults to None (all joints)

Returns:

all or selected articulation joint positions

Return type:

np.ndarray

abstract get_joint_velocities(joint_indices: List | ndarray | None = None) ndarray[source]#

Get the articulation joint velocities.

Parameters:

joint_indices (Optional[Union[List, np.ndarray]], optional) – indices to specify which joints to read. Defaults to None (all joints)

Returns:

all or selected articulation joint velocities

Return type:

np.ndarray

abstract property handles_initialized: bool#

Check if articulation handler is initialized.

Returns:

whether the handler was initialized

Return type:

bool

abstract property num_bodies: int#

Number of articulation links.

Returns:

number of links

Return type:

int

abstract property num_dof: int#

Number of DOF of the articulation

Returns:

amount of DOFs

Return type:

int

abstract set_enabled_self_collisions(flag: bool) None[source]#

Set the enable self collisions flag.

Parameters:

flag (bool) – whether to enable self collisions

set_gains(kps: ndarray | None = None, kds: ndarray | None = None, joint_indices: ndarray | None = None) None[source]#

Set the implicit PD controller’s Kps (stiffnesses) and Kds (dampings) of the articulation.

Parameters:
  • kps (Optional[np.ndarray], optional) – stiffness of the drives. shape is (M, K). Defaults to None.

  • kds (Optional[np.ndarray], optional) – damping of the drives. shape is (M, K).. Defaults to None.

  • joint_indices (Optional[np.ndarray], optional) – joint indices to specify which joints to manipulate. Shape (K,). Where K <= num of dofs. Defaults to None (i.e: all dofs)

abstract set_joint_positions(positions: ndarray, joint_indices: List | ndarray | None = None) None[source]#

Set the articulation joint positions.

Parameters:
  • positions (np.ndarray) – articulation joint positions

  • joint_indices (Optional[Union[list, np.ndarray]], optional) – indices to specify which joints to manipulate. Defaults to None (all joints)

abstract set_joint_velocities(velocities: ndarray, joint_indices: List | ndarray | None = None) None[source]#

Set the articulation joint velocities.

Parameters:
  • velocities (np.ndarray) – articulation joint velocities

  • joint_indices (Optional[Union[list, np.ndarray]], optional) – indices to specify which joints to manipulate. Defaults to None (all joints)

set_solver_position_iteration_count(count: int) None[source]#

Set the solver (position) iteration count for the articulation.

Parameters:

count (int) – position iteration count

abstract set_solver_velocity_iteration_count(count: int) None[source]#

Set the solver (velocity) iteration count for the articulation.

Parameters:

count (int) – velocity iteration count