Source code for isaaclab.assets.articulation.articulation
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.# All rights reserved.## SPDX-License-Identifier: BSD-3-Clause# Flag for pyright to ignore type errors in this file.# pyright: reportPrivateUsage=falsefrom__future__importannotationsimporttorchfromcollections.abcimportSequencefromprettytableimportPrettyTablefromtypingimportTYPE_CHECKINGimportisaacsim.core.utils.stageasstage_utilsimportomni.logimportomni.physics.tensors.impl.apiasphysxfrompxrimportPhysxSchema,UsdPhysicsimportisaaclab.simassim_utilsimportisaaclab.utils.mathasmath_utilsimportisaaclab.utils.stringasstring_utilsfromisaaclab.actuatorsimportActuatorBase,ActuatorBaseCfg,ImplicitActuatorfromisaaclab.utils.typesimportArticulationActionsfrom..asset_baseimportAssetBasefrom.articulation_dataimportArticulationDataifTYPE_CHECKING:from.articulation_cfgimportArticulationCfg
[docs]classArticulation(AssetBase):"""An articulation asset class. An articulation is a collection of rigid bodies connected by joints. The joints can be either fixed or actuated. The joints can be of different types, such as revolute, prismatic, D-6, etc. However, the articulation class has currently been tested with revolute and prismatic joints. The class supports both floating-base and fixed-base articulations. The type of articulation is determined based on the root joint of the articulation. If the root joint is fixed, then the articulation is considered a fixed-base system. Otherwise, it is considered a floating-base system. This can be checked using the :attr:`Articulation.is_fixed_base` attribute. For an asset to be considered an articulation, the root prim of the asset must have the `USD ArticulationRootAPI`_. This API is used to define the sub-tree of the articulation using the reduced coordinate formulation. On playing the simulation, the physics engine parses the articulation root prim and creates the corresponding articulation in the physics engine. The articulation root prim can be specified using the :attr:`AssetBaseCfg.prim_path` attribute. The articulation class also provides the functionality to augment the simulation of an articulated system with custom actuator models. These models can either be explicit or implicit, as detailed in the :mod:`isaaclab.actuators` module. The actuator models are specified using the :attr:`ArticulationCfg.actuators` attribute. These are then parsed and used to initialize the corresponding actuator models, when the simulation is played. During the simulation step, the articulation class first applies the actuator models to compute the joint commands based on the user-specified targets. These joint commands are then applied into the simulation. The joint commands can be either position, velocity, or effort commands. As an example, the following snippet shows how this can be used for position commands: .. code-block:: python # an example instance of the articulation class my_articulation = Articulation(cfg) # set joint position targets my_articulation.set_joint_position_target(position) # propagate the actuator models and apply the computed commands into the simulation my_articulation.write_data_to_sim() # step the simulation using the simulation context sim_context.step() # update the articulation state, where dt is the simulation time step my_articulation.update(dt) .. _`USD ArticulationRootAPI`: https://openusd.org/dev/api/class_usd_physics_articulation_root_a_p_i.html """cfg:ArticulationCfg"""Configuration instance for the articulations."""actuators:dict[str,ActuatorBase]"""Dictionary of actuator instances for the articulation. The keys are the actuator names and the values are the actuator instances. The actuator instances are initialized based on the actuator configurations specified in the :attr:`ArticulationCfg.actuators` attribute. They are used to compute the joint commands during the :meth:`write_data_to_sim` function. """
[docs]def__init__(self,cfg:ArticulationCfg):"""Initialize the articulation. Args: cfg: A configuration instance. """super().__init__(cfg)
""" Properties """@propertydefdata(self)->ArticulationData:returnself._data@propertydefnum_instances(self)->int:returnself.root_physx_view.count@propertydefis_fixed_base(self)->bool:"""Whether the articulation is a fixed-base or floating-base system."""returnself.root_physx_view.shared_metatype.fixed_base@propertydefnum_joints(self)->int:"""Number of joints in articulation."""returnself.root_physx_view.shared_metatype.dof_count@propertydefnum_fixed_tendons(self)->int:"""Number of fixed tendons in articulation."""returnself.root_physx_view.max_fixed_tendons@propertydefnum_bodies(self)->int:"""Number of bodies in articulation."""returnself.root_physx_view.shared_metatype.link_count@propertydefjoint_names(self)->list[str]:"""Ordered names of joints in articulation."""returnself.root_physx_view.shared_metatype.dof_names@propertydeffixed_tendon_names(self)->list[str]:"""Ordered names of fixed tendons in articulation."""returnself._fixed_tendon_names@propertydefbody_names(self)->list[str]:"""Ordered names of bodies in articulation."""returnself.root_physx_view.shared_metatype.link_names@propertydefroot_physx_view(self)->physx.ArticulationView:"""Articulation view for the asset (PhysX). Note: Use this view with caution. It requires handling of tensors in a specific way. """returnself._root_physx_view""" Operations. """
[docs]defreset(self,env_ids:Sequence[int]|None=None):# use ellipses object to skip initial indices.ifenv_idsisNone:env_ids=slice(None)# reset actuatorsforactuatorinself.actuators.values():actuator.reset(env_ids)# reset external wrenchself._external_force_b[env_ids]=0.0self._external_torque_b[env_ids]=0.0
[docs]defwrite_data_to_sim(self):"""Write external wrenches and joint commands to the simulation. If any explicit actuators are present, then the actuator models are used to compute the joint commands. Otherwise, the joint commands are directly set into the simulation. Note: We write external wrench to the simulation here since this function is called before the simulation step. This ensures that the external wrench is applied at every simulation step. """# write external wrenchifself.has_external_wrench:self.root_physx_view.apply_forces_and_torques_at_position(force_data=self._external_force_b.view(-1,3),torque_data=self._external_torque_b.view(-1,3),position_data=None,indices=self._ALL_INDICES,is_global=False,)# apply actuator modelsself._apply_actuator_model()# write actions into simulationself.root_physx_view.set_dof_actuation_forces(self._joint_effort_target_sim,self._ALL_INDICES)# position and velocity targets only for implicit actuatorsifself._has_implicit_actuators:self.root_physx_view.set_dof_position_targets(self._joint_pos_target_sim,self._ALL_INDICES)self.root_physx_view.set_dof_velocity_targets(self._joint_vel_target_sim,self._ALL_INDICES)
[docs]deffind_bodies(self,name_keys:str|Sequence[str],preserve_order:bool=False)->tuple[list[int],list[str]]:"""Find bodies in the articulation based on the name keys. Please check the :meth:`isaaclab.utils.string_utils.resolve_matching_names` function for more information on the name matching. Args: name_keys: A regular expression or a list of regular expressions to match the body names. preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. Returns: A tuple of lists containing the body indices and names. """returnstring_utils.resolve_matching_names(name_keys,self.body_names,preserve_order)
[docs]deffind_joints(self,name_keys:str|Sequence[str],joint_subset:list[str]|None=None,preserve_order:bool=False)->tuple[list[int],list[str]]:"""Find joints in the articulation based on the name keys. Please see the :func:`isaaclab.utils.string.resolve_matching_names` function for more information on the name matching. Args: name_keys: A regular expression or a list of regular expressions to match the joint names. joint_subset: A subset of joints to search for. Defaults to None, which means all joints in the articulation are searched. preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. Returns: A tuple of lists containing the joint indices and names. """ifjoint_subsetisNone:joint_subset=self.joint_names# find jointsreturnstring_utils.resolve_matching_names(name_keys,joint_subset,preserve_order)
[docs]deffind_fixed_tendons(self,name_keys:str|Sequence[str],tendon_subsets:list[str]|None=None,preserve_order:bool=False)->tuple[list[int],list[str]]:"""Find fixed tendons in the articulation based on the name keys. Please see the :func:`isaaclab.utils.string.resolve_matching_names` function for more information on the name matching. Args: name_keys: A regular expression or a list of regular expressions to match the joint names with fixed tendons. tendon_subsets: A subset of joints with fixed tendons to search for. Defaults to None, which means all joints in the articulation are searched. preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. Returns: A tuple of lists containing the tendon indices and names. """iftendon_subsetsisNone:# tendons follow the joint names they are attached totendon_subsets=self.fixed_tendon_names# find tendonsreturnstring_utils.resolve_matching_names(name_keys,tendon_subsets,preserve_order)
""" Operations - State Writers. """
[docs]defwrite_root_state_to_sim(self,root_state:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root state over selected environment indices into the simulation. The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear and angular velocity. All the quantities are in the simulation frame. Args: root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """# set into simulationself.write_root_pose_to_sim(root_state[:,:7],env_ids=env_ids)self.write_root_velocity_to_sim(root_state[:,7:],env_ids=env_ids)
[docs]defwrite_root_com_state_to_sim(self,root_state:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root center of mass state over selected environment indices into the simulation. The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear and angular velocity. All the quantities are in the simulation frame. Args: root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """# set into simulationself.write_root_com_pose_to_sim(root_state[:,:7],env_ids=env_ids)self.write_root_com_velocity_to_sim(root_state[:,7:],env_ids=env_ids)
[docs]defwrite_root_link_state_to_sim(self,root_state:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root link state over selected environment indices into the simulation. The root state comprises of the cartesian position, quaternion orientation in (w, x, y, z), and linear and angular velocity. All the quantities are in the simulation frame. Args: root_state: Root state in simulation frame. Shape is (len(env_ids), 13). env_ids: Environment indices. If None, then all indices are used. """# set into simulationself.write_root_link_pose_to_sim(root_state[:,:7],env_ids=env_ids)self.write_root_link_velocity_to_sim(root_state[:,7:],env_ids=env_ids)
[docs]defwrite_root_pose_to_sim(self,root_pose:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root pose over selected environment indices into the simulation. The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). Args: root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7). env_ids: Environment indices. If None, then all indices are used. """# resolve all indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICES# note: we need to do this here since tensors are not set into simulation until step.# set into internal buffersself._data.root_state_w[env_ids,:7]=root_pose.clone()# convert root quaternion from wxyz to xyzwroot_poses_xyzw=self._data.root_state_w[:,:7].clone()root_poses_xyzw[:,3:]=math_utils.convert_quat(root_poses_xyzw[:,3:],to="xyzw")# Need to invalidate the buffer to trigger the update with the new root pose.self._data._body_state_w.timestamp=-1.0self._data._body_link_state_w.timestamp=-1.0self._data._body_com_state_w.timestamp=-1.0# set into simulationself.root_physx_view.set_root_transforms(root_poses_xyzw,indices=physx_env_ids)
[docs]defwrite_root_link_pose_to_sim(self,root_pose:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root link pose over selected environment indices into the simulation. The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). Args: root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7). env_ids: Environment indices. If None, then all indices are used. """# resolve all indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICES# note: we need to do this here since tensors are not set into simulation until step.# set into internal buffersself._data.root_link_state_w[env_ids,:7]=root_pose.clone()self._data.root_state_w[env_ids,:7]=self._data.root_link_state_w[env_ids,:7]# convert root quaternion from wxyz to xyzwroot_poses_xyzw=self._data.root_link_state_w[:,:7].clone()root_poses_xyzw[:,3:]=math_utils.convert_quat(root_poses_xyzw[:,3:],to="xyzw")# Need to invalidate the buffer to trigger the update with the new root pose.self._data._body_state_w.timestamp=-1.0self._data._body_link_state_w.timestamp=-1.0self._data._body_com_state_w.timestamp=-1.0# set into simulationself.root_physx_view.set_root_transforms(root_poses_xyzw,indices=physx_env_ids)
[docs]defwrite_root_com_pose_to_sim(self,root_pose:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root center of mass pose over selected environment indices into the simulation. The root pose comprises of the cartesian position and quaternion orientation in (w, x, y, z). The orientation is the orientation of the principle axes of inertia. Args: root_pose: Root center of mass poses in simulation frame. Shape is (len(env_ids), 7). env_ids: Environment indices. If None, then all indices are used. """# resolve all indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICEScom_pos=self.data.com_pos_b[env_ids,0,:]com_quat=self.data.com_quat_b[env_ids,0,:]root_link_pos,root_link_quat=math_utils.combine_frame_transforms(root_pose[...,:3],root_pose[...,3:7],math_utils.quat_rotate(math_utils.quat_inv(com_quat),-com_pos),math_utils.quat_inv(com_quat),)root_link_pose=torch.cat((root_link_pos,root_link_quat),dim=-1)self.write_root_link_pose_to_sim(root_pose=root_link_pose,env_ids=physx_env_ids)
[docs]defwrite_root_velocity_to_sim(self,root_velocity:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root center of mass velocity over selected environment indices into the simulation. The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. NOTE: This sets the velocity of the root's center of mass rather than the roots frame. Args: root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). env_ids: Environment indices. If None, then all indices are used. """# resolve all indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICES# note: we need to do this here since tensors are not set into simulation until step.# set into internal buffersself._data.root_state_w[env_ids,7:]=root_velocity.clone()self._data.body_acc_w[env_ids]=0.0# set into simulationself.root_physx_view.set_root_velocities(self._data.root_state_w[:,7:],indices=physx_env_ids)
[docs]defwrite_root_com_velocity_to_sim(self,root_velocity:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root center of mass velocity over selected environment indices into the simulation. The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. NOTE: This sets the velocity of the root's center of mass rather than the roots frame. Args: root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6). env_ids: Environment indices. If None, then all indices are used. """# resolve all indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICES# note: we need to do this here since tensors are not set into simulation until step.# set into internal buffersself._data.root_com_state_w[env_ids,7:]=root_velocity.clone()self._data.root_state_w[env_ids,7:]=self._data.root_com_state_w[env_ids,7:]self._data.body_acc_w[env_ids]=0.0# set into simulationself.root_physx_view.set_root_velocities(self._data.root_com_state_w[:,7:],indices=physx_env_ids)
[docs]defwrite_root_link_velocity_to_sim(self,root_velocity:torch.Tensor,env_ids:Sequence[int]|None=None):"""Set the root link velocity over selected environment indices into the simulation. The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. NOTE: This sets the velocity of the root's frame rather than the roots center of mass. Args: root_velocity: Root frame velocities in simulation world frame. Shape is (len(env_ids), 6). env_ids: Environment indices. If None, then all indices are used. """# resolve all indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESroot_com_velocity=root_velocity.clone()quat=self.data.root_link_state_w[env_ids,3:7]com_pos_b=self.data.com_pos_b[env_ids,0,:]# transform given velocity to center of massroot_com_velocity[:,:3]+=torch.linalg.cross(root_com_velocity[:,3:],math_utils.quat_rotate(quat,com_pos_b),dim=-1)# write center of mass velocity to simself.write_root_com_velocity_to_sim(root_velocity=root_com_velocity,env_ids=physx_env_ids)
[docs]defwrite_joint_state_to_sim(self,position:torch.Tensor,velocity:torch.Tensor,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|slice|None=None,):"""Write joint positions and velocities to the simulation. Args: position: Joint positions. Shape is (len(env_ids), len(joint_ids)). velocity: Joint velocities. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the targets for. Defaults to None (all joints). env_ids: The environment indices to set the targets for. Defaults to None (all environments). """# set into simulationself.write_joint_position_to_sim(position,joint_ids=joint_ids,env_ids=env_ids)self.write_joint_velocity_to_sim(velocity,joint_ids=joint_ids,env_ids=env_ids)
[docs]defwrite_joint_position_to_sim(self,position:torch.Tensor,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|slice|None=None,):"""Write joint positions to the simulation. Args: position: Joint positions. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the targets for. Defaults to None (all joints). env_ids: The environment indices to set the targets for. Defaults to None (all environments). """# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set into internal buffersself._data.joint_pos[env_ids,joint_ids]=position# Need to invalidate the buffer to trigger the update with the new root pose.self._data._body_state_w.timestamp=-1.0self._data._body_link_state_w.timestamp=-1.0self._data._body_com_state_w.timestamp=-1.0# set into simulationself.root_physx_view.set_dof_positions(self._data.joint_pos,indices=physx_env_ids)
[docs]defwrite_joint_velocity_to_sim(self,velocity:torch.Tensor,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|slice|None=None,):"""Write joint velocities to the simulation. Args: velocity: Joint velocities. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the targets for. Defaults to None (all joints). env_ids: The environment indices to set the targets for. Defaults to None (all environments). """# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set into internal buffersself._data.joint_vel[env_ids,joint_ids]=velocityself._data._previous_joint_vel[env_ids,joint_ids]=velocityself._data.joint_acc[env_ids,joint_ids]=0.0# set into simulationself.root_physx_view.set_dof_velocities(self._data.joint_vel,indices=physx_env_ids)
[docs]defwrite_joint_stiffness_to_sim(self,stiffness:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Write joint stiffness into the simulation. Args: stiffness: Joint stiffness. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the stiffness for. Defaults to None (all joints). env_ids: The environment indices to set the stiffness for. Defaults to None (all environments). """# note: This function isn't setting the values for actuator models. (#128)# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set into internal buffersself._data.joint_stiffness[env_ids,joint_ids]=stiffness# set into simulationself.root_physx_view.set_dof_stiffnesses(self._data.joint_stiffness.cpu(),indices=physx_env_ids.cpu())
[docs]defwrite_joint_damping_to_sim(self,damping:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Write joint damping into the simulation. Args: damping: Joint damping. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the damping for. Defaults to None (all joints). env_ids: The environment indices to set the damping for. Defaults to None (all environments). """# note: This function isn't setting the values for actuator models. (#128)# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set into internal buffersself._data.joint_damping[env_ids,joint_ids]=damping# set into simulationself.root_physx_view.set_dof_dampings(self._data.joint_damping.cpu(),indices=physx_env_ids.cpu())
[docs]defwrite_joint_position_limit_to_sim(self,limits:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,warn_limit_violation:bool=True,):"""Write joint position limits into the simulation. Args: limits: Joint limits. Shape is (len(env_ids), len(joint_ids), 2). joint_ids: The joint indices to set the limits for. Defaults to None (all joints). env_ids: The environment indices to set the limits for. Defaults to None (all environments). warn_limit_violation: Whether to use warning or info level logging when default joint positions exceed the new limits. Defaults to True. """# note: This function isn't setting the values for actuator models. (#128)# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set into internal buffersself._data.joint_pos_limits[env_ids,joint_ids]=limits# update default joint pos to stay within the new limitsiftorch.any((self._data.default_joint_pos[env_ids,joint_ids]<limits[...,0])|(self._data.default_joint_pos[env_ids,joint_ids]>limits[...,1])):self._data.default_joint_pos[env_ids,joint_ids]=torch.clamp(self._data.default_joint_pos[env_ids,joint_ids],limits[...,0],limits[...,1])violation_message=("Some default joint positions are outside of the range of the new joint limits. Default joint positions"" will be clamped to be within the new joint limits.")ifwarn_limit_violation:# warn level will show in consoleomni.log.warn(violation_message)else:# info level is only written to log fileomni.log.info(violation_message)# set into simulationself.root_physx_view.set_dof_limits(self._data.joint_pos_limits.cpu(),indices=physx_env_ids.cpu())# compute the soft limits based on the joint limits# TODO: Optimize this computation for only selected joints# soft joint position limits (recommended not to be too close to limits).joint_pos_mean=(self._data.joint_pos_limits[...,0]+self._data.joint_pos_limits[...,1])/2joint_pos_range=self._data.joint_pos_limits[...,1]-self._data.joint_pos_limits[...,0]soft_limit_factor=self.cfg.soft_joint_pos_limit_factor# add to dataself._data.soft_joint_pos_limits[...,0]=joint_pos_mean-0.5*joint_pos_range*soft_limit_factorself._data.soft_joint_pos_limits[...,1]=joint_pos_mean+0.5*joint_pos_range*soft_limit_factor
[docs]defwrite_joint_velocity_limit_to_sim(self,limits:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Write joint max velocity to the simulation. The velocity limit is used to constrain the joint velocities in the physics engine. The joint will only be able to reach this velocity if the joint's effort limit is sufficiently large. If the joint is moving faster than this velocity, the physics engine will actually try to brake the joint to reach this velocity. Args: limits: Joint max velocity. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the max velocity for. Defaults to None (all joints). env_ids: The environment indices to set the max velocity for. Defaults to None (all environments). """# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# move tensor to cpu if neededifisinstance(limits,torch.Tensor):limits=limits.to(self.device)# set into internal buffersself._data.joint_vel_limits[env_ids,joint_ids]=limits# set into simulationself.root_physx_view.set_dof_max_velocities(self._data.joint_vel_limits.cpu(),indices=physx_env_ids.cpu())
[docs]defwrite_joint_effort_limit_to_sim(self,limits:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Write joint effort limits into the simulation. The effort limit is used to constrain the computed joint efforts in the physics engine. If the computed effort exceeds this limit, the physics engine will clip the effort to this value. Args: limits: Joint torque limits. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). """# note: This function isn't setting the values for actuator models. (#128)# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# move tensor to cpu if neededifisinstance(limits,torch.Tensor):limits=limits.to(self.device)# set into internal buffersself._data.joint_effort_limits[env_ids,joint_ids]=limits# set into simulationself.root_physx_view.set_dof_max_forces(self._data.joint_effort_limits.cpu(),indices=physx_env_ids.cpu())
[docs]defwrite_joint_armature_to_sim(self,armature:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Write joint armature into the simulation. The armature is directly added to the corresponding joint-space inertia. It helps improve the simulation stability by reducing the joint velocities. Args: armature: Joint armature. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). """# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set into internal buffersself._data.joint_armature[env_ids,joint_ids]=armature# set into simulationself.root_physx_view.set_dof_armatures(self._data.joint_armature.cpu(),indices=physx_env_ids.cpu())
[docs]defwrite_joint_friction_coefficient_to_sim(self,joint_friction_coeff:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):r"""Write joint friction coefficients into the simulation. The joint friction is a unitless quantity. It relates the magnitude of the spatial force transmitted from the parent body to the child body to the maximal friction force that may be applied by the solver to resist the joint motion. Mathematically, this means that: :math:`F_{resist} \leq \mu F_{spatial}`, where :math:`F_{resist}` is the resisting force applied by the solver and :math:`F_{spatial}` is the spatial force transmitted from the parent body to the child body. The simulated friction effect is therefore similar to static and Coulomb friction. Args: joint_friction: Joint friction. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). """# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:env_ids=slice(None)physx_env_ids=self._ALL_INDICESifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set into internal buffersself._data.joint_friction_coeff[env_ids,joint_ids]=joint_friction_coeff# set into simulationself.root_physx_view.set_dof_friction_coefficients(self._data.joint_friction_coeff.cpu(),indices=physx_env_ids.cpu())
""" Operations - Setters. """
[docs]defset_external_force_and_torque(self,forces:torch.Tensor,torques:torch.Tensor,body_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set external force and torque to apply on the asset's bodies in their local frame. For many applications, we want to keep the applied external force on rigid bodies constant over a period of time (for instance, during the policy control). This function allows us to store the external force and torque into buffers which are then applied to the simulation at every step. .. caution:: If the function is called with empty forces and torques, then this function disables the application of external wrench to the simulation. .. code-block:: python # example of disabling external wrench asset.set_external_force_and_torque(forces=torch.zeros(0, 3), torques=torch.zeros(0, 3)) .. note:: This function does not apply the external wrench to the simulation. It only fills the buffers with the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function right before the simulation step. Args: forces: External forces in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). torques: External torques in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). body_ids: Body indices to apply external wrench to. Defaults to None (all bodies). env_ids: Environment indices to apply external wrench to. Defaults to None (all instances). """ifforces.any()ortorques.any():self.has_external_wrench=Trueelse:self.has_external_wrench=False# resolve all indices# -- env_idsifenv_idsisNone:env_ids=self._ALL_INDICESelifnotisinstance(env_ids,torch.Tensor):env_ids=torch.tensor(env_ids,dtype=torch.long,device=self.device)# -- body_idsifbody_idsisNone:body_ids=torch.arange(self.num_bodies,dtype=torch.long,device=self.device)elifisinstance(body_ids,slice):body_ids=torch.arange(self.num_bodies,dtype=torch.long,device=self.device)[body_ids]elifnotisinstance(body_ids,torch.Tensor):body_ids=torch.tensor(body_ids,dtype=torch.long,device=self.device)# note: we need to do this complicated indexing since torch doesn't support multi-indexing# create global body indices from env_ids and env_body_ids# (env_id * total_bodies_per_env) + body_idindices=body_ids.repeat(len(env_ids),1)+env_ids.unsqueeze(1)*self.num_bodiesindices=indices.view(-1)# set into internal buffers# note: these are applied in the write_to_sim functionself._external_force_b.flatten(0,1)[indices]=forces.flatten(0,1)self._external_torque_b.flatten(0,1)[indices]=torques.flatten(0,1)
[docs]defset_joint_position_target(self,target:torch.Tensor,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None):"""Set joint position targets into internal buffers. This function does not apply the joint targets to the simulation. It only fills the buffers with the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. Args: target: Joint position targets. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the targets for. Defaults to None (all joints). env_ids: The environment indices to set the targets for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)ifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set targetsself._data.joint_pos_target[env_ids,joint_ids]=target
[docs]defset_joint_velocity_target(self,target:torch.Tensor,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None):"""Set joint velocity targets into internal buffers. This function does not apply the joint targets to the simulation. It only fills the buffers with the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. Args: target: Joint velocity targets. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the targets for. Defaults to None (all joints). env_ids: The environment indices to set the targets for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)ifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set targetsself._data.joint_vel_target[env_ids,joint_ids]=target
[docs]defset_joint_effort_target(self,target:torch.Tensor,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None):"""Set joint efforts into internal buffers. This function does not apply the joint targets to the simulation. It only fills the buffers with the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. Args: target: Joint effort targets. Shape is (len(env_ids), len(joint_ids)). joint_ids: The joint indices to set the targets for. Defaults to None (all joints). env_ids: The environment indices to set the targets for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)ifjoint_idsisNone:joint_ids=slice(None)# broadcast env_ids if needed to allow double indexingifenv_ids!=slice(None)andjoint_ids!=slice(None):env_ids=env_ids[:,None]# set targetsself._data.joint_effort_target[env_ids,joint_ids]=target
""" Operations - Tendons. """
[docs]defset_fixed_tendon_stiffness(self,stiffness:torch.Tensor,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set fixed tendon stiffness into internal buffers. This function does not apply the tendon stiffness to the simulation. It only fills the buffers with the desired values. To apply the tendon stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function. Args: stiffness: Fixed tendon stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). fixed_tendon_ids: The tendon indices to set the stiffness for. Defaults to None (all fixed tendons). env_ids: The environment indices to set the stiffness for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)iffixed_tendon_idsisNone:fixed_tendon_ids=slice(None)ifenv_ids!=slice(None)andfixed_tendon_ids!=slice(None):env_ids=env_ids[:,None]# set stiffnessself._data.fixed_tendon_stiffness[env_ids,fixed_tendon_ids]=stiffness
[docs]defset_fixed_tendon_damping(self,damping:torch.Tensor,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set fixed tendon damping into internal buffers. This function does not apply the tendon damping to the simulation. It only fills the buffers with the desired values. To apply the tendon damping, call the :meth:`write_fixed_tendon_properties_to_sim` function. Args: damping: Fixed tendon damping. Shape is (len(env_ids), len(fixed_tendon_ids)). fixed_tendon_ids: The tendon indices to set the damping for. Defaults to None (all fixed tendons). env_ids: The environment indices to set the damping for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)iffixed_tendon_idsisNone:fixed_tendon_ids=slice(None)ifenv_ids!=slice(None)andfixed_tendon_ids!=slice(None):env_ids=env_ids[:,None]# set dampingself._data.fixed_tendon_damping[env_ids,fixed_tendon_ids]=damping
[docs]defset_fixed_tendon_limit_stiffness(self,limit_stiffness:torch.Tensor,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set fixed tendon limit stiffness efforts into internal buffers. This function does not apply the tendon limit stiffness to the simulation. It only fills the buffers with the desired values. To apply the tendon limit stiffness, call the :meth:`write_fixed_tendon_properties_to_sim` function. Args: limit_stiffness: Fixed tendon limit stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). fixed_tendon_ids: The tendon indices to set the limit stiffness for. Defaults to None (all fixed tendons). env_ids: The environment indices to set the limit stiffness for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)iffixed_tendon_idsisNone:fixed_tendon_ids=slice(None)ifenv_ids!=slice(None)andfixed_tendon_ids!=slice(None):env_ids=env_ids[:,None]# set limit_stiffnessself._data.fixed_tendon_limit_stiffness[env_ids,fixed_tendon_ids]=limit_stiffness
[docs]defset_fixed_tendon_position_limit(self,limit:torch.Tensor,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set fixed tendon limit efforts into internal buffers. This function does not apply the tendon limit to the simulation. It only fills the buffers with the desired values. To apply the tendon limit, call the :meth:`write_fixed_tendon_properties_to_sim` function. Args: limit: Fixed tendon limit. Shape is (len(env_ids), len(fixed_tendon_ids)). fixed_tendon_ids: The tendon indices to set the limit for. Defaults to None (all fixed tendons). env_ids: The environment indices to set the limit for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)iffixed_tendon_idsisNone:fixed_tendon_ids=slice(None)ifenv_ids!=slice(None)andfixed_tendon_ids!=slice(None):env_ids=env_ids[:,None]# set limitself._data.fixed_tendon_pos_limits[env_ids,fixed_tendon_ids]=limit
[docs]defset_fixed_tendon_rest_length(self,rest_length:torch.Tensor,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set fixed tendon rest length efforts into internal buffers. This function does not apply the tendon rest length to the simulation. It only fills the buffers with the desired values. To apply the tendon rest length, call the :meth:`write_fixed_tendon_properties_to_sim` function. Args: rest_length: Fixed tendon rest length. Shape is (len(env_ids), len(fixed_tendon_ids)). fixed_tendon_ids: The tendon indices to set the rest length for. Defaults to None (all fixed tendons). env_ids: The environment indices to set the rest length for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)iffixed_tendon_idsisNone:fixed_tendon_ids=slice(None)ifenv_ids!=slice(None)andfixed_tendon_ids!=slice(None):env_ids=env_ids[:,None]# set rest_lengthself._data.fixed_tendon_rest_length[env_ids,fixed_tendon_ids]=rest_length
[docs]defset_fixed_tendon_offset(self,offset:torch.Tensor,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set fixed tendon offset efforts into internal buffers. This function does not apply the tendon offset to the simulation. It only fills the buffers with the desired values. To apply the tendon offset, call the :meth:`write_fixed_tendon_properties_to_sim` function. Args: offset: Fixed tendon offset. Shape is (len(env_ids), len(fixed_tendon_ids)). fixed_tendon_ids: The tendon indices to set the offset for. Defaults to None (all fixed tendons). env_ids: The environment indices to set the offset for. Defaults to None (all environments). """# resolve indicesifenv_idsisNone:env_ids=slice(None)iffixed_tendon_idsisNone:fixed_tendon_ids=slice(None)ifenv_ids!=slice(None)andfixed_tendon_ids!=slice(None):env_ids=env_ids[:,None]# set offsetself._data.fixed_tendon_offset[env_ids,fixed_tendon_ids]=offset
[docs]defwrite_fixed_tendon_properties_to_sim(self,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Write fixed tendon properties into the simulation. Args: fixed_tendon_ids: The fixed tendon indices to set the limits for. Defaults to None (all fixed tendons). env_ids: The environment indices to set the limits for. Defaults to None (all environments). """# resolve indicesphysx_env_ids=env_idsifenv_idsisNone:physx_env_ids=self._ALL_INDICESiffixed_tendon_idsisNone:fixed_tendon_ids=slice(None)# set into simulationself.root_physx_view.set_fixed_tendon_properties(self._data.fixed_tendon_stiffness,self._data.fixed_tendon_damping,self._data.fixed_tendon_limit_stiffness,self._data.fixed_tendon_pos_limits,self._data.fixed_tendon_rest_length,self._data.fixed_tendon_offset,indices=physx_env_ids,)
""" Internal helper. """def_initialize_impl(self):# create simulation viewself._physics_sim_view=physx.create_simulation_view(self._backend)self._physics_sim_view.set_subspace_roots("/")# obtain the first prim in the regex expression (all others are assumed to be a copy of this)template_prim=sim_utils.find_first_matching_prim(self.cfg.prim_path)iftemplate_primisNone:raiseRuntimeError(f"Failed to find prim for expression: '{self.cfg.prim_path}'.")template_prim_path=template_prim.GetPath().pathString# find articulation root primsroot_prims=sim_utils.get_all_matching_child_prims(template_prim_path,predicate=lambdaprim:prim.HasAPI(UsdPhysics.ArticulationRootAPI))iflen(root_prims)==0:raiseRuntimeError(f"Failed to find an articulation when resolving '{self.cfg.prim_path}'."" Please ensure that the prim has 'USD ArticulationRootAPI' applied.")iflen(root_prims)>1:raiseRuntimeError(f"Failed to find a single articulation when resolving '{self.cfg.prim_path}'."f" Found multiple '{root_prims}' under '{template_prim_path}'."" Please ensure that there is only one articulation in the prim path tree.")# resolve articulation root prim back into regex expressionroot_prim_path=root_prims[0].GetPath().pathStringroot_prim_path_expr=self.cfg.prim_path+root_prim_path[len(template_prim_path):]# -- articulationself._root_physx_view=self._physics_sim_view.create_articulation_view(root_prim_path_expr.replace(".*","*"))# check if the articulation was createdifself._root_physx_view._backendisNone:raiseRuntimeError(f"Failed to create articulation at: {self.cfg.prim_path}. Please check PhysX logs.")# log information about the articulationomni.log.info(f"Articulation initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.")omni.log.info(f"Is fixed root: {self.is_fixed_base}")omni.log.info(f"Number of bodies: {self.num_bodies}")omni.log.info(f"Body names: {self.body_names}")omni.log.info(f"Number of joints: {self.num_joints}")omni.log.info(f"Joint names: {self.joint_names}")omni.log.info(f"Number of fixed tendons: {self.num_fixed_tendons}")# container for data accessself._data=ArticulationData(self.root_physx_view,self.device)# create buffersself._create_buffers()# process configurationself._process_cfg()self._process_actuators_cfg()self._process_fixed_tendons()# validate configurationself._validate_cfg()# update the robot dataself.update(0.0)# log joint informationself._log_articulation_info()def_create_buffers(self):# constantsself._ALL_INDICES=torch.arange(self.num_instances,dtype=torch.long,device=self.device)# external forces and torquesself.has_external_wrench=Falseself._external_force_b=torch.zeros((self.num_instances,self.num_bodies,3),device=self.device)self._external_torque_b=torch.zeros_like(self._external_force_b)# asset named dataself._data.joint_names=self.joint_namesself._data.body_names=self.body_names# tendon names are set in _process_fixed_tendons function# -- joint propertiesself._data.default_joint_pos_limits=self.root_physx_view.get_dof_limits().to(self.device).clone()self._data.default_joint_stiffness=self.root_physx_view.get_dof_stiffnesses().to(self.device).clone()self._data.default_joint_damping=self.root_physx_view.get_dof_dampings().to(self.device).clone()self._data.default_joint_armature=self.root_physx_view.get_dof_armatures().to(self.device).clone()self._data.default_joint_friction_coeff=(self.root_physx_view.get_dof_friction_coefficients().to(self.device).clone())self._data.joint_pos_limits=self._data.default_joint_pos_limits.clone()self._data.joint_vel_limits=self.root_physx_view.get_dof_max_velocities().to(self.device).clone()self._data.joint_effort_limits=self.root_physx_view.get_dof_max_forces().to(self.device).clone()self._data.joint_stiffness=self._data.default_joint_stiffness.clone()self._data.joint_damping=self._data.default_joint_damping.clone()self._data.joint_armature=self._data.default_joint_armature.clone()self._data.joint_friction_coeff=self._data.default_joint_friction_coeff.clone()# -- body propertiesself._data.default_mass=self.root_physx_view.get_masses().clone()self._data.default_inertia=self.root_physx_view.get_inertias().clone()# -- joint commands (sent to the actuator from the user)self._data.joint_pos_target=torch.zeros(self.num_instances,self.num_joints,device=self.device)self._data.joint_vel_target=torch.zeros_like(self._data.joint_pos_target)self._data.joint_effort_target=torch.zeros_like(self._data.joint_pos_target)# -- joint commands (sent to the simulation after actuator processing)self._joint_pos_target_sim=torch.zeros_like(self._data.joint_pos_target)self._joint_vel_target_sim=torch.zeros_like(self._data.joint_pos_target)self._joint_effort_target_sim=torch.zeros_like(self._data.joint_pos_target)# -- computed joint efforts from the actuator modelsself._data.computed_torque=torch.zeros_like(self._data.joint_pos_target)self._data.applied_torque=torch.zeros_like(self._data.joint_pos_target)# -- other data that are filled based on explicit actuator modelsself._data.soft_joint_vel_limits=torch.zeros(self.num_instances,self.num_joints,device=self.device)self._data.gear_ratio=torch.ones(self.num_instances,self.num_joints,device=self.device)# soft joint position limits (recommended not to be too close to limits).joint_pos_mean=(self._data.joint_pos_limits[...,0]+self._data.joint_pos_limits[...,1])/2joint_pos_range=self._data.joint_pos_limits[...,1]-self._data.joint_pos_limits[...,0]soft_limit_factor=self.cfg.soft_joint_pos_limit_factor# add to dataself._data.soft_joint_pos_limits=torch.zeros(self.num_instances,self.num_joints,2,device=self.device)self._data.soft_joint_pos_limits[...,0]=joint_pos_mean-0.5*joint_pos_range*soft_limit_factorself._data.soft_joint_pos_limits[...,1]=joint_pos_mean+0.5*joint_pos_range*soft_limit_factordef_process_cfg(self):"""Post processing of configuration parameters."""# default state# -- root state# note: we cast to tuple to avoid torch/numpy type mismatch.default_root_state=(tuple(self.cfg.init_state.pos)+tuple(self.cfg.init_state.rot)+tuple(self.cfg.init_state.lin_vel)+tuple(self.cfg.init_state.ang_vel))default_root_state=torch.tensor(default_root_state,dtype=torch.float,device=self.device)self._data.default_root_state=default_root_state.repeat(self.num_instances,1)# -- joint stateself._data.default_joint_pos=torch.zeros(self.num_instances,self.num_joints,device=self.device)self._data.default_joint_vel=torch.zeros_like(self._data.default_joint_pos)# joint posindices_list,_,values_list=string_utils.resolve_matching_names_values(self.cfg.init_state.joint_pos,self.joint_names)self._data.default_joint_pos[:,indices_list]=torch.tensor(values_list,device=self.device)# joint velindices_list,_,values_list=string_utils.resolve_matching_names_values(self.cfg.init_state.joint_vel,self.joint_names)self._data.default_joint_vel[:,indices_list]=torch.tensor(values_list,device=self.device)""" Internal simulation callbacks. """def_invalidate_initialize_callback(self,event):"""Invalidates the scene elements."""# call parentsuper()._invalidate_initialize_callback(event)# set all existing views to None to invalidate themself._physics_sim_view=Noneself._root_physx_view=None""" Internal helpers -- Actuators. """def_process_actuators_cfg(self):"""Process and apply articulation joint properties."""# create actuatorsself.actuators=dict()# flag for implicit actuators# if this is false, we by-pass certain checks when doing actuator-related operationsself._has_implicit_actuators=False# iterate over all actuator configurationsforactuator_name,actuator_cfginself.cfg.actuators.items():# type annotation for type checkersactuator_cfg:ActuatorBaseCfg# create actuator groupjoint_ids,joint_names=self.find_joints(actuator_cfg.joint_names_expr)# check if any joints are foundiflen(joint_names)==0:raiseValueError(f"No joints found for actuator group: {actuator_name} with joint name expression:"f" {actuator_cfg.joint_names_expr}.")# resolve joint indices# we pass a slice if all joints are selected to avoid indexing overheadiflen(joint_names)==self.num_joints:joint_ids=slice(None)else:joint_ids=torch.tensor(joint_ids,device=self.device)# create actuator collection# note: for efficiency avoid indexing when over all indicesactuator:ActuatorBase=actuator_cfg.class_type(cfg=actuator_cfg,joint_names=joint_names,joint_ids=joint_ids,num_envs=self.num_instances,device=self.device,stiffness=self._data.default_joint_stiffness[:,joint_ids],damping=self._data.default_joint_damping[:,joint_ids],armature=self._data.default_joint_armature[:,joint_ids],friction=self._data.default_joint_friction_coeff[:,joint_ids],effort_limit=self._data.joint_effort_limits[:,joint_ids],velocity_limit=self._data.joint_vel_limits[:,joint_ids],)# log information on actuator groupsmodel_type="implicit"ifactuator.is_implicit_modelelse"explicit"omni.log.info(f"Actuator collection: {actuator_name} with model '{actuator_cfg.class_type.__name__}'"f" (type: {model_type}) and joint names: {joint_names} [{joint_ids}].")# store actuator groupself.actuators[actuator_name]=actuator# set the passed gains and limits into the simulationifisinstance(actuator,ImplicitActuator):self._has_implicit_actuators=True# the gains and limits are set into the simulation since actuator model is implicitself.write_joint_stiffness_to_sim(actuator.stiffness,joint_ids=actuator.joint_indices)self.write_joint_damping_to_sim(actuator.damping,joint_ids=actuator.joint_indices)else:# the gains and limits are processed by the actuator model# we set gains to zero, and torque limit to a high value in simulation to avoid any interferenceself.write_joint_stiffness_to_sim(0.0,joint_ids=actuator.joint_indices)self.write_joint_damping_to_sim(0.0,joint_ids=actuator.joint_indices)# Set common properties into the simulationself.write_joint_effort_limit_to_sim(actuator.effort_limit_sim,joint_ids=actuator.joint_indices)self.write_joint_velocity_limit_to_sim(actuator.velocity_limit_sim,joint_ids=actuator.joint_indices)self.write_joint_armature_to_sim(actuator.armature,joint_ids=actuator.joint_indices)self.write_joint_friction_coefficient_to_sim(actuator.friction,joint_ids=actuator.joint_indices)# Store the configured values from the actuator model# note: this is the value configured in the actuator model (for implicit and explicit actuators)self._data.default_joint_stiffness[:,actuator.joint_indices]=actuator.stiffnessself._data.default_joint_damping[:,actuator.joint_indices]=actuator.dampingself._data.default_joint_armature[:,actuator.joint_indices]=actuator.armatureself._data.default_joint_friction_coeff[:,actuator.joint_indices]=actuator.friction# perform some sanity checks to ensure actuators are prepared correctlytotal_act_joints=sum(actuator.num_jointsforactuatorinself.actuators.values())iftotal_act_joints!=(self.num_joints-self.num_fixed_tendons):omni.log.warn("Not all actuators are configured! Total number of actuated joints not equal to number of"f" joints available: {total_act_joints} != {self.num_joints-self.num_fixed_tendons}.")def_process_fixed_tendons(self):"""Process fixed tendons."""# create a list to store the fixed tendon namesself._fixed_tendon_names=list()# parse fixed tendons properties if they existifself.num_fixed_tendons>0:stage=stage_utils.get_current_stage()joint_paths=self.root_physx_view.dof_paths[0]# iterate over all joints to find tendons attached to themforjinrange(self.num_joints):usd_joint_path=joint_paths[j]# check whether joint has tendons - tendon name follows the joint name it is attached tojoint=UsdPhysics.Joint.Get(stage,usd_joint_path)ifjoint.GetPrim().HasAPI(PhysxSchema.PhysxTendonAxisRootAPI):joint_name=usd_joint_path.split("/")[-1]self._fixed_tendon_names.append(joint_name)# store the fixed tendon namesself._data.fixed_tendon_names=self._fixed_tendon_names# store the current USD fixed tendon propertiesself._data.default_fixed_tendon_stiffness=self.root_physx_view.get_fixed_tendon_stiffnesses().clone()self._data.default_fixed_tendon_damping=self.root_physx_view.get_fixed_tendon_dampings().clone()self._data.default_fixed_tendon_limit_stiffness=(self.root_physx_view.get_fixed_tendon_limit_stiffnesses().clone())self._data.default_fixed_tendon_pos_limits=self.root_physx_view.get_fixed_tendon_limits().clone()self._data.default_fixed_tendon_rest_length=self.root_physx_view.get_fixed_tendon_rest_lengths().clone()self._data.default_fixed_tendon_offset=self.root_physx_view.get_fixed_tendon_offsets().clone()# store a copy of the default values for the fixed tendonsself._data.fixed_tendon_stiffness=self._data.default_fixed_tendon_stiffness.clone()self._data.fixed_tendon_damping=self._data.default_fixed_tendon_damping.clone()self._data.fixed_tendon_limit_stiffness=self._data.default_fixed_tendon_limit_stiffness.clone()self._data.fixed_tendon_pos_limits=self._data.default_fixed_tendon_pos_limits.clone()self._data.fixed_tendon_rest_length=self._data.default_fixed_tendon_rest_length.clone()self._data.fixed_tendon_offset=self._data.default_fixed_tendon_offset.clone()def_apply_actuator_model(self):"""Processes joint commands for the articulation by forwarding them to the actuators. The actions are first processed using actuator models. Depending on the robot configuration, the actuator models compute the joint level simulation commands and sets them into the PhysX buffers. """# process actions per groupforactuatorinself.actuators.values():# prepare input for actuator model based on cached data# TODO : A tensor dict would be nice to do the indexing of all tensors togethercontrol_action=ArticulationActions(joint_positions=self._data.joint_pos_target[:,actuator.joint_indices],joint_velocities=self._data.joint_vel_target[:,actuator.joint_indices],joint_efforts=self._data.joint_effort_target[:,actuator.joint_indices],joint_indices=actuator.joint_indices,)# compute joint command from the actuator modelcontrol_action=actuator.compute(control_action,joint_pos=self._data.joint_pos[:,actuator.joint_indices],joint_vel=self._data.joint_vel[:,actuator.joint_indices],)# update targets (these are set into the simulation)ifcontrol_action.joint_positionsisnotNone:self._joint_pos_target_sim[:,actuator.joint_indices]=control_action.joint_positionsifcontrol_action.joint_velocitiesisnotNone:self._joint_vel_target_sim[:,actuator.joint_indices]=control_action.joint_velocitiesifcontrol_action.joint_effortsisnotNone:self._joint_effort_target_sim[:,actuator.joint_indices]=control_action.joint_efforts# update state of the actuator model# -- torquesself._data.computed_torque[:,actuator.joint_indices]=actuator.computed_effortself._data.applied_torque[:,actuator.joint_indices]=actuator.applied_effort# -- actuator dataself._data.soft_joint_vel_limits[:,actuator.joint_indices]=actuator.velocity_limit# TODO: find a cleaner way to handle gear ratio. Only needed for variable gear ratio actuators.ifhasattr(actuator,"gear_ratio"):self._data.gear_ratio[:,actuator.joint_indices]=actuator.gear_ratio""" Internal helpers -- Debugging. """def_validate_cfg(self):"""Validate the configuration after processing. Note: This function should be called only after the configuration has been processed and the buffers have been created. Otherwise, some settings that are altered during processing may not be validated. For instance, the actuator models may change the joint max velocity limits. """# check that the default values are within the limitsjoint_pos_limits=self.root_physx_view.get_dof_limits()[0].to(self.device)out_of_range=self._data.default_joint_pos[0]<joint_pos_limits[:,0]out_of_range|=self._data.default_joint_pos[0]>joint_pos_limits[:,1]violated_indices=torch.nonzero(out_of_range,as_tuple=False).squeeze(-1)# throw error if any of the default joint positions are out of the limitsiflen(violated_indices)>0:# prepare message for violated jointsmsg="The following joints have default positions out of the limits: \n"foridxinviolated_indices:joint_name=self.data.joint_names[idx]joint_limit=joint_pos_limits[idx]joint_pos=self.data.default_joint_pos[0,idx]# add to messagemsg+=f"\t- '{joint_name}': {joint_pos:.3f} not in [{joint_limit[0]:.3f}, {joint_limit[1]:.3f}]\n"raiseValueError(msg)# check that the default joint velocities are within the limitsjoint_max_vel=self.root_physx_view.get_dof_max_velocities()[0].to(self.device)out_of_range=torch.abs(self._data.default_joint_vel[0])>joint_max_velviolated_indices=torch.nonzero(out_of_range,as_tuple=False).squeeze(-1)iflen(violated_indices)>0:# prepare message for violated jointsmsg="The following joints have default velocities out of the limits: \n"foridxinviolated_indices:joint_name=self.data.joint_names[idx]joint_limit=[-joint_max_vel[idx],joint_max_vel[idx]]joint_vel=self.data.default_joint_vel[0,idx]# add to messagemsg+=f"\t- '{joint_name}': {joint_vel:.3f} not in [{joint_limit[0]:.3f}, {joint_limit[1]:.3f}]\n"raiseValueError(msg)def_log_articulation_info(self):"""Log information about the articulation. Note: We purposefully read the values from the simulator to ensure that the values are configured as expected. """# read out all joint parameters from simulation# -- gainsstiffnesses=self.root_physx_view.get_dof_stiffnesses()[0].tolist()dampings=self.root_physx_view.get_dof_dampings()[0].tolist()# -- propertiesarmatures=self.root_physx_view.get_dof_armatures()[0].tolist()frictions=self.root_physx_view.get_dof_friction_coefficients()[0].tolist()# -- limitsposition_limits=self.root_physx_view.get_dof_limits()[0].tolist()velocity_limits=self.root_physx_view.get_dof_max_velocities()[0].tolist()effort_limits=self.root_physx_view.get_dof_max_forces()[0].tolist()# create table for term informationjoint_table=PrettyTable()joint_table.title=f"Simulation Joint Information (Prim path: {self.cfg.prim_path})"joint_table.field_names=["Index","Name","Stiffness","Damping","Armature","Friction","Position Limits","Velocity Limits","Effort Limits",]joint_table.float_format=".3"joint_table.custom_format["Position Limits"]=lambdaf,v:f"[{v[0]:.3f}, {v[1]:.3f}]"# set alignment of table columnsjoint_table.align["Name"]="l"# add info on each termforindex,nameinenumerate(self.joint_names):joint_table.add_row([index,name,stiffnesses[index],dampings[index],armatures[index],frictions[index],position_limits[index],velocity_limits[index],effort_limits[index],])# convert table to stringomni.log.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n"+joint_table.get_string())# read out all tendon parameters from simulationifself.num_fixed_tendons>0:# -- gainsft_stiffnesses=self.root_physx_view.get_fixed_tendon_stiffnesses()[0].tolist()ft_dampings=self.root_physx_view.get_fixed_tendon_dampings()[0].tolist()# -- limitsft_limit_stiffnesses=self.root_physx_view.get_fixed_tendon_limit_stiffnesses()[0].tolist()ft_limits=self.root_physx_view.get_fixed_tendon_limits()[0].tolist()ft_rest_lengths=self.root_physx_view.get_fixed_tendon_rest_lengths()[0].tolist()ft_offsets=self.root_physx_view.get_fixed_tendon_offsets()[0].tolist()# create table for term informationtendon_table=PrettyTable()tendon_table.title=f"Simulation Fixed Tendon Information (Prim path: {self.cfg.prim_path})"tendon_table.field_names=["Index","Stiffness","Damping","Limit Stiffness","Limits","Rest Length","Offset",]tendon_table.float_format=".3"joint_table.custom_format["Limits"]=lambdaf,v:f"[{v[0]:.3f}, {v[1]:.3f}]"# add info on each termforindexinrange(self.num_fixed_tendons):tendon_table.add_row([index,ft_stiffnesses[index],ft_dampings[index],ft_limit_stiffnesses[index],ft_limits[index],ft_rest_lengths[index],ft_offsets[index],])# convert table to stringomni.log.info(f"Simulation parameters for tendons in {self.cfg.prim_path}:\n"+tendon_table.get_string())""" Deprecated methods. """
[docs]defwrite_joint_friction_to_sim(self,joint_friction:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Write joint friction coefficients into the simulation. .. deprecated:: 2.1.0 Please use :meth:`write_joint_friction_coefficient_to_sim` instead. """omni.log.warn("The function 'write_joint_friction_to_sim' will be deprecated in a future release. Please"" use 'write_joint_friction_coefficient_to_sim' instead.")self.write_joint_friction_coefficient_to_sim(joint_friction,joint_ids=joint_ids,env_ids=env_ids)
[docs]defwrite_joint_limits_to_sim(self,limits:torch.Tensor|float,joint_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,warn_limit_violation:bool=True,):"""Write joint limits into the simulation. .. deprecated:: 2.1.0 Please use :meth:`write_joint_position_limit_to_sim` instead. """omni.log.warn("The function 'write_joint_limits_to_sim' will be deprecated in a future release. Please"" use 'write_joint_position_limit_to_sim' instead.")self.write_joint_position_limit_to_sim(limits,joint_ids=joint_ids,env_ids=env_ids,warn_limit_violation=warn_limit_violation)
[docs]defset_fixed_tendon_limit(self,limit:torch.Tensor,fixed_tendon_ids:Sequence[int]|slice|None=None,env_ids:Sequence[int]|None=None,):"""Set fixed tendon position limits into internal buffers. .. deprecated:: 2.1.0 Please use :meth:`set_fixed_tendon_position_limit` instead. """omni.log.warn("The function 'set_fixed_tendon_limit' will be deprecated in a future release. Please"" use 'set_fixed_tendon_position_limit' instead.")self.set_fixed_tendon_position_limit(limit,fixed_tendon_ids=fixed_tendon_ids,env_ids=env_ids)