fastdev.robo.articulation ========================= .. py:module:: fastdev.robo.articulation Module Contents --------------- .. py:class:: ArticulationSpec(urdf_or_mjcf_path: Union[str, pathlib.Path], mesh_dir: Optional[Union[str, pathlib.Path]] = None, format: Optional[Literal['urdf', 'mjcf']] = None, base_link_name: Optional[str] = None, ee_link_names: Optional[Union[str, List[str]]] = None, mjcf_assets: Optional[Dict[str, Any]] = None, enable_mimic_joints: bool = True) Specification for a single articulation. :param urdf_or_mjcf_path: Path to the URDF or MJCF file. :type urdf_or_mjcf_path: str :param mesh_dir: Directory to store mesh files. Defaults to None. :type mesh_dir: str, optional :param format: Format of the file, either "urdf" or "mjcf". Defaults to None. :type format: str, optional :param mjcf_assets: Assets for MJCF files. Defaults to None. :type mjcf_assets: Dict[str, Any], optional :param enable_mimic_joints: Whether to enable mimic joints. Defaults to True. :type enable_mimic_joints: bool, optional .. rubric:: Examples >>> arti_spec = ArticulationSpec(urdf_or_mjcf_path="assets/robot_description/panda.urdf") >>> arti_spec.num_dofs 8 .. py:attribute:: base_link_name :type: str .. py:attribute:: ee_link_names :type: List[str] .. py:attribute:: link_names :type: List[str] .. py:attribute:: num_links :type: int .. py:attribute:: num_dofs :type: int .. py:attribute:: active_joint_names :type: List[str] .. py:attribute:: has_mimic_joints :type: bool .. py:attribute:: mimic_joint_names :type: List[str] .. py:attribute:: full_joint_names :type: List[str] .. py:attribute:: num_full_joints :type: int .. py:attribute:: urdf_or_mjcf_path :value: '' .. py:attribute:: mesh_dir :value: '' .. py:attribute:: format :value: None .. py:attribute:: mjcf_assets :value: None .. py:attribute:: enable_mimic_joints :value: True .. py:attribute:: num_mimic_joints .. py:method:: parse_urdf() -> Tuple[Dict[str, Joint], Dict[str, Link]] .. py:method:: parse_mjcf() -> Tuple[Dict[str, Joint], Dict[str, Link]] .. py:method:: __repr__() -> str .. py:method:: __str__() -> str .. py:property:: joint_limits :type: Optional[jaxtyping.Float[numpy.ndarray, num_dofs 2]] .. py:property:: mimic_joint_limits :type: Optional[jaxtyping.Float[numpy.ndarray, num_mimic_joints 2]] .. py:property:: mimic_joint_indices :type: jaxtyping.Int[numpy.ndarray, num_mimic_joints] .. py:property:: mimic_multipliers :type: jaxtyping.Float[numpy.ndarray, num_mimic_joints] .. py:property:: mimic_offsets :type: jaxtyping.Float[numpy.ndarray, num_mimic_joints] .. py:property:: link_joint_indices :type: jaxtyping.Int[numpy.ndarray, num_links] .. py:property:: link_indices_topological_order :type: jaxtyping.Int[numpy.ndarray, num_links] .. py:property:: parent_link_indices :type: jaxtyping.Int[numpy.ndarray, num_links] .. py:property:: link_joint_axes :type: Optional[jaxtyping.Float[numpy.ndarray, num_links 3]] .. py:property:: full_joint_axes :type: jaxtyping.Float[numpy.ndarray, num_full_joints 3] .. py:property:: link_joint_origins :type: jaxtyping.Float[numpy.ndarray, num_links 4 4] .. py:property:: link_joint_types :type: jaxtyping.Int[numpy.ndarray, num_links] .. py:method:: get_ancestor_links_mask(link_name_or_idx: Union[str, int]) -> jaxtyping.Int[numpy.ndarray, num_links] .. py:method:: get_link_trimesh_meshes(mode: Literal['visual', 'collision'] = 'collision', return_empty_meshes: bool = True) -> Dict[str, trimesh.Trimesh] .. py:class:: Articulation(specs: Union[ArticulationSpec, Sequence[ArticulationSpec]], device: Device = 'cpu') Class to manage multiple articulations. :param specs: Articulation specifications. :type specs: Sequence[ArticulationSpec] :param device: Device to store tensors. Defaults to "cpu". :type device: str, optional .. rubric:: Examples >>> arti = Articulation(ArticulationSpec(urdf_or_mjcf_path="assets/robot_description/panda.urdf")) >>> link_poses = arti.forward_kinematics(torch.zeros(1, arti.total_num_dofs), clamp_joint_values=False) >>> torch.allclose(link_poses[0, -1, :3, 3], torch.tensor([0.0880, 0.0000, 0.8676]), atol=1e-3) True .. py:attribute:: specs :type: Sequence[ArticulationSpec] .. py:attribute:: num_arti :type: int .. py:attribute:: total_num_dofs :type: int .. py:attribute:: total_num_links :type: int .. py:attribute:: total_num_full_joints :type: int .. py:attribute:: device :value: 'cpu' .. py:method:: from_urdf_or_mjcf_paths(urdf_or_mjcf_paths: Union[Union[str, pathlib.Path], Sequence[Union[str, pathlib.Path]]], mesh_dirs: Optional[Union[Union[str, pathlib.Path], Sequence[Union[str, pathlib.Path]]]] = None, device: Device = 'cpu') -> Articulation :staticmethod: .. py:method:: from_urdf_or_mjcf_path(urdf_or_mjcf_path: Union[Union[str, pathlib.Path], Sequence[Union[str, pathlib.Path]]], mesh_dir: Optional[Union[Union[str, pathlib.Path], Sequence[Union[str, pathlib.Path]]]] = None, device: Device = 'cpu') -> Articulation :staticmethod: Keep this method for compatibility with old code. .. py:property:: first_spec :type: ArticulationSpec .. py:property:: has_none_joint_limits :type: bool .. py:property:: has_none_mimic_joint_limits :type: bool .. py:property:: has_mimic_joints :type: bool .. py:property:: joint_limits :type: Optional[jaxtyping.Float[torch.Tensor, total_num_dofs 2]] .. py:property:: num_dofs :type: int .. py:property:: active_joint_names :type: Union[List[str], List[List[str]]] .. py:property:: mimic_joint_names :type: Union[List[str], List[List[str]]] .. py:property:: link_names :type: Union[List[str], List[List[str]]] .. py:method:: get_packed_joint_limits(return_tensors: Literal['np', 'pt'] = 'np', return_mimic_joints: bool = False) -> Optional[Union[numpy.ndarray, torch.Tensor]] .. py:method:: get_packed_link_indices_topological_order(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_parent_link_indices(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_link_joint_indices(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_link_joint_types(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_link_joint_origins(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_link_joint_axes(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_full_joint_axes(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_joint_first_indices(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_link_first_indices(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_ancestor_links_mask(target_link_indices: jaxtyping.Int[torch.Tensor, num_arti], return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_packed_zero_joint_values(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_zero_joint_values(return_tensors: Literal['np', 'pt'] = 'np') -> Union[numpy.ndarray, torch.Tensor] .. py:method:: get_link_trimesh_meshes(mode: Literal['visual', 'collision'] = 'collision', return_empty_meshes: bool = False) -> Union[Dict[str, trimesh.Trimesh], List[Dict[str, trimesh.Trimesh]]] .. py:method:: apply_mimic_joints(joint_values: jaxtyping.Float[torch.Tensor, ... total_num_dofs], clamp_joint_values: bool = True) -> jaxtyping.Float[torch.Tensor, ... total_num_full_joints] .. py:method:: forward_kinematics(joint_values: Union[jaxtyping.Float[torch.Tensor, ... total_num_dofs]], joint_names: Optional[Union[List[List[str]], List[str]]] = None, root_poses: Optional[Union[jaxtyping.Float[torch.Tensor, ... num_arti 4 4], jaxtyping.Float[torch.Tensor, ... 4 4]]] = None, clamp_joint_values: bool = True, use_warp: bool = True) -> jaxtyping.Float[torch.Tensor, ... total_num_links 4 4] Forward kinematics. :param joint_values: Packed joint values of shape (..., total_num_dofs). :type joint_values: torch.Tensor :param joint_names: Joint names for each articulation. Defaults to None. Could be a single list of joint names if there is only one articulation. :type joint_names: List[List[str]], optional :param root_poses: Root poses of shape (..., num_arti, 4, 4). Defaults to None. The `num_arti` dimension can be omitted only if a single articulation is being used. :type root_poses: torch.Tensor, optional :param clamp_joint_values: Whether to clamp joint values to joint limits. Defaults to True. :type clamp_joint_values: bool, optional .. py:method:: jacobian(joint_values: jaxtyping.Float[torch.Tensor, ... total_num_dofs], joint_names: Optional[Union[List[List[str]], List[str]]] = None, root_poses: Optional[Union[jaxtyping.Float[torch.Tensor, ... num_arti 4 4], jaxtyping.Float[torch.Tensor, ... 4 4]]] = None, target_links: Optional[Union[str, List[str], int, List[int], jaxtyping.Int[torch.Tensor, num_arti]]] = None, clamp_joint_values: bool = True, use_warp: bool = True, return_target_link_poses: bool = False) -> Union[jaxtyping.Float[torch.Tensor, ... 6 total_num_dofs], Tuple[jaxtyping.Float[torch.Tensor, ... 6 total_num_dofs], jaxtyping.Float[torch.Tensor, ... num_arti 4 4]]] .. py:method:: inverse_kinematics(target_link_poses: Union[jaxtyping.Float[torch.Tensor, ... num_arti 4 4], jaxtyping.Float[torch.Tensor, ... 4 4]], target_links: Optional[Union[str, List[str], int, List[int], torch.Tensor]] = None, use_warp: bool = False, max_iterations: int = 30, learning_rate: float = 0.2, tolerance: float = 0.001, damping: float = 0.0001, num_retries: int = 50, init_joint_values: Optional[jaxtyping.Float[torch.Tensor, ... total_num_dofs]] = None, jitter_strength: float = 1.0, return_success: bool = False, force_insert_articulation_dim: bool = False) -> Union[jaxtyping.Float[torch.Tensor, ... total_num_dofs], Tuple[jaxtyping.Float[torch.Tensor, ... total_num_dofs], jaxtyping.Bool[torch.Tensor, ... num_arti]]] .. py:method:: apply_mimic_joints_numpy(joint_values: jaxtyping.Float[numpy.ndarray, ... total_num_dofs], clamp_joint_values: bool = True) -> jaxtyping.Float[numpy.ndarray, ... total_num_joints] .. py:method:: forward_kinematics_numpy(joint_values: Union[jaxtyping.Float[numpy.ndarray, ... total_num_dofs]], joint_names: Optional[Union[List[List[str]], List[str]]] = None, root_poses: Optional[Union[jaxtyping.Float[numpy.ndarray, ... num_arti 4 4], jaxtyping.Float[numpy.ndarray, ... 4 4]]] = None, clamp_joint_values: bool = True) -> jaxtyping.Float[numpy.ndarray, ... total_num_links 4 4] Forward kinematics. :param joint_values: Packed joint values of shape (..., total_num_dofs). :type joint_values: torch.Tensor :param joint_names: Joint names for each articulation. Defaults to None. Could be a single list of joint names if there is only one articulation. :type joint_names: List[List[str]], optional :param root_poses: Root poses of shape (..., num_arti, 4, 4). Defaults to None. The `num_arti` dimension can be omitted only if a single articulation is being used. :type root_poses: torch.Tensor, optional :param clamp_joint_values: Whether to clamp joint values to joint limits. Defaults to True. :type clamp_joint_values: bool, optional .. py:method:: to(device: Device) -> Articulation .. py:method:: __len__() -> int .. py:method:: __getitem__(idx: Union[int, slice]) -> Articulation .. py:method:: __repr__() -> str .. py:method:: __str__() -> str