diff --git a/docs/source/tutorials/05_controllers/run_osc.rst b/docs/source/tutorials/05_controllers/run_osc.rst index 661056b1a8..431dda1acb 100644 --- a/docs/source/tutorials/05_controllers/run_osc.rst +++ b/docs/source/tutorials/05_controllers/run_osc.rst @@ -86,14 +86,26 @@ If desired, the inertial coupling between the translational and rotational axes If it is desired to include the gravity compensation in the operational space command, the ``gravity_compensation`` should be set to ``True``. +A final consideration regarding the operational space control is what to do with the null-space of redundant robots. +The null-space is the subspace of the joint space that does not affect the task space coordinates. If nothing is done +to control the null-space, the robot joints will float without moving the end-effector. This might be undesired (e.g., +the robot joints might get close to their limits), and one might want to control the robot behaviour within its +null-space. One way to do is to set ``nullspace_control`` to ``"position"`` (by default it is ``"none"``) which +integrates a null-space PD controller to attract the robot joints to desired targets without affecting the task +space. The behaviour of this null-space controller can be defined using the ``nullspace_stiffness`` and +``nullspace_damping_ratio`` arguments. Please note that theoretical decoupling of the null-space and task space +accelerations is only possible when ``inertial_dynamics_decoupling`` is set to ``True`` and +``partial_inertial_dynamics_decoupling`` is set to ``False``. + The included OSC implementation performs the computation in a batched format and uses PyTorch operations. In this tutorial, we will use ``"pose_abs"`` for controlling the motion in all axes except the z-axis and ``"wrench_abs"`` for controlling the force in the z-axis. Moreover, we will include the full inertia decoupling in the motion control and not include the gravity compensation, as the gravity is disabled from the robot configuration. -Finally, we set the impedance mode to ``"variable_kp"`` to dynamically change the stiffness values +We set the impedance mode to ``"variable_kp"`` to dynamically change the stiffness values (``motion_damping_ratio_task`` is set to ``1``: the kd values adapt according to kp values to maintain a critically -damped response). +damped response). Finally, ``nullspace_control`` is set to use ``"position"`` where the joint set points are provided +to be the center of the joint position limits. .. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py :language: python @@ -104,13 +116,14 @@ Updating the states of the robot -------------------------------------------- The OSC implementation is a computation-only class. Thus, it expects the user to provide the necessary information -about the robot. This includes the robot's Jacobian matrix, mass/inertia matrix, end-effector pose, velocity, and -contact force, all in the root frame. Moreover, the user should provide gravity compensation vector, if desired. +about the robot. This includes the robot's Jacobian matrix, mass/inertia matrix, end-effector pose, velocity, contact +force (all in the root frame), and finally, the joint positions and velocities. Moreover, the user should provide +gravity compensation vector and null-space joint position targets if required. .. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py :language: python :start-at: # Update robot states - :end-before: return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b + :end-before: # Update the target commands Computing robot command diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 05f5be440a..d0ce0dae38 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.30.0" +version = "0.30.1" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index fe823cb371..a96a1c2c04 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,20 @@ Changelog --------- +0.30.1 (2024-12-17) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added null-space (position) control option to :class:`omni.isaac.lab.controllers.OperationalSpaceController`. +* Added test cases that uses null-space control for :class:`omni.isaac.lab.controllers.OperationalSpaceController`. +* Added information regarding null-space control to the tutorial script and documentation of + :class:`omni.isaac.lab.controllers.OperationalSpaceController`. +* Added arguments to set specific null-space joint position targets within + :class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction` class. + + 0.30.0 (2024-12-16) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py index 9f278248e2..9df58a03d4 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py @@ -77,11 +77,15 @@ def __init__(self, cfg: OperationalSpaceControllerCfg, num_envs: int, device: st self._selection_matrix_force_b = torch.zeros_like(self._selection_matrix_force_task) # -- commands self._task_space_target_task = torch.zeros(self.num_envs, self.target_dim, device=self._device) - # -- buffers for motion/force control + # -- Placeholders for motion/force control self.desired_ee_pose_task = None self.desired_ee_pose_b = None self.desired_ee_wrench_task = None self.desired_ee_wrench_b = None + # -- buffer for operational space mass matrix + self._os_mass_matrix_b = torch.zeros(self.num_envs, 6, 6, device=self._device) + # -- Placeholder for the inverse of joint space mass matrix + self._mass_matrix_inv = None # -- motion control gains self._motion_p_gains_task = torch.diag_embed( torch.ones(self.num_envs, 6, device=self._device) @@ -127,6 +131,14 @@ def __init__(self, cfg: OperationalSpaceControllerCfg, num_envs: int, device: st # -- end-effector contact wrench self._ee_contact_wrench_b = torch.zeros(self.num_envs, 6, device=self._device) + # -- buffers for null-space control gains + self._nullspace_p_gain = torch.tensor(self.cfg.nullspace_stiffness, dtype=torch.float, device=self._device) + self._nullspace_d_gain = ( + 2 + * torch.sqrt(self._nullspace_p_gain) + * torch.tensor(self.cfg.nullspace_damping_ratio, dtype=torch.float, device=self._device) + ) + """ Properties. """ @@ -338,6 +350,9 @@ def compute( current_ee_force_b: torch.Tensor | None = None, mass_matrix: torch.Tensor | None = None, gravity: torch.Tensor | None = None, + current_joint_pos: torch.Tensor | None = None, + current_joint_vel: torch.Tensor | None = None, + nullspace_joint_pos_target: torch.Tensor | None = None, ) -> torch.Tensor: """Performs inference with the controller. @@ -348,12 +363,18 @@ def compute( (``num_envs``, 7), which contains the position and quaternion ``(w, x, y, z)``. Defaults to ``None``. current_ee_vel_b: The current end-effector velocity in root frame. It is a tensor of shape (``num_envs``, 6), which contains the linear and angular velocities. Defaults to None. - current_ee_force_b: The current external force on the end-effector in root frame. - It is a tensor of shape (``num_envs``, 3), which contains the linear force. Defaults to ``None``. + current_ee_force_b: The current external force on the end-effector in root frame. It is a tensor of + shape (``num_envs``, 3), which contains the linear force. Defaults to ``None``. mass_matrix: The joint-space mass/inertia matrix. It is a tensor of shape (``num_envs``, ``num_DoF``, - ``num_DoF``). Defaults to ``None``. + ``num_DoF``). Defaults to ``None``. gravity: The joint-space gravity vector. It is a tensor of shape (``num_envs``, ``num_DoF``). Defaults - to ``None``. + to ``None``. + current_joint_pos: The current joint positions. It is a tensor of shape (``num_envs``, ``num_DoF``). + Defaults to ``None``. + current_joint_vel: The current joint velocities. It is a tensor of shape (``num_envs``, ``num_DoF``). + Defaults to ``None``. + nullspace_joint_pos_target: The target joint positions the null space controller is trying to enforce, when + possible. It is a tensor of shape (``num_envs``, ``num_DoF``). Raises: ValueError: When motion-control is enabled but the current end-effector pose or velocity is not provided. @@ -361,6 +382,14 @@ def compute( ValueError: When the current end-effector pose is not provided for the ``pose_rel`` command. ValueError: When closed-loop force control is enabled but the current end-effector force is not provided. ValueError: When gravity compensation is enabled but the gravity vector is not provided. + ValueError: When null-space control is enabled but the system is not redundant. + ValueError: When dynamically consistent pseudo-inverse is enabled but the mass matrix inverse is not + provided. + ValueError: When null-space control is enabled but the current joint positions and velocities are not + provided. + ValueError: When target joint positions are provided for null-space control but their dimensions do not + match the current joint positions. + ValueError: When an invalid null-space control method is provided. Returns: Tensor: The joint efforts computed by the controller. It is a tensor of shape (``num_envs``, ``num_DoF``). @@ -398,23 +427,21 @@ def compute( if mass_matrix is None: raise ValueError("Mass matrix is required for inertial decoupling.") # Compute operational space mass matrix - mass_matrix_inv = torch.inverse(mass_matrix) + self._mass_matrix_inv = torch.inverse(mass_matrix) if self.cfg.partial_inertial_dynamics_decoupling: - # Create a zero tensor representing the mass matrix, to fill in the non-zero elements later - os_mass_matrix = torch.zeros(self.num_envs, 6, 6, device=self._device) # Fill in the translational and rotational parts of the inertia separately, ignoring their coupling - os_mass_matrix[:, 0:3, 0:3] = torch.inverse( - jacobian_b[:, 0:3] @ mass_matrix_inv @ jacobian_b[:, 0:3].mT + self._os_mass_matrix_b[:, 0:3, 0:3] = torch.inverse( + jacobian_b[:, 0:3] @ self._mass_matrix_inv @ jacobian_b[:, 0:3].mT ) - os_mass_matrix[:, 3:6, 3:6] = torch.inverse( - jacobian_b[:, 3:6] @ mass_matrix_inv @ jacobian_b[:, 3:6].mT + self._os_mass_matrix_b[:, 3:6, 3:6] = torch.inverse( + jacobian_b[:, 3:6] @ self._mass_matrix_inv @ jacobian_b[:, 3:6].mT ) else: # Calculate the operational space mass matrix fully accounting for the couplings - os_mass_matrix = torch.inverse(jacobian_b @ mass_matrix_inv @ jacobian_b.mT) + self._os_mass_matrix_b[:] = torch.inverse(jacobian_b @ self._mass_matrix_inv @ jacobian_b.mT) # (Generalized) operational space command forces # F = (J M^(-1) J^T)^(-1) * \ddot(x_des) = M_task * \ddot(x_des) - os_command_forces_b = os_mass_matrix @ des_ee_acc_b + os_command_forces_b = self._os_mass_matrix_b @ des_ee_acc_b else: # Task-space impedance control: command forces = \ddot(x_des). # Please note that the definition of task-space impedance control varies in literature. @@ -455,4 +482,67 @@ def compute( # add gravity compensation joint_efforts += gravity + # Add null-space control + # -- Free null-space control + if self.cfg.nullspace_control == "none": + # No additional control is applied in the null space. + pass + else: + # Check if the system is redundant + if num_DoF <= 6: + raise ValueError("Null-space control is only applicable for redundant manipulators.") + + # Calculate the pseudo-inverse of the Jacobian + if self.cfg.inertial_dynamics_decoupling and not self.cfg.partial_inertial_dynamics_decoupling: + # Dynamically consistent pseudo-inverse allows decoupling of null space and task space + if self._mass_matrix_inv is None or mass_matrix is None: + raise ValueError("Mass matrix inverse is required for dynamically consistent pseudo-inverse") + jacobian_pinv_transpose = self._os_mass_matrix_b @ jacobian_b @ self._mass_matrix_inv + else: + # Moore-Penrose pseudo-inverse if full inertia matrix is not available (e.g., no/partial decoupling) + jacobian_pinv_transpose = torch.pinverse(jacobian_b).mT + + # Calculate the null-space projector + nullspace_jacobian_transpose = ( + torch.eye(n=num_DoF, device=self._device) - jacobian_b.mT @ jacobian_pinv_transpose + ) + + # Null space position control + if self.cfg.nullspace_control == "position": + + # Check if the current joint positions and velocities are provided + if current_joint_pos is None or current_joint_vel is None: + raise ValueError("Current joint positions and velocities are required for null-space control.") + + # Calculate the joint errors for nullspace position control + if nullspace_joint_pos_target is None: + nullspace_joint_pos_target = torch.zeros_like(current_joint_pos) + # Check if the dimensions of the target nullspace joint positions match the current joint positions + elif nullspace_joint_pos_target.shape != current_joint_pos.shape: + raise ValueError( + f"The target nullspace joint positions shape '{nullspace_joint_pos_target.shape}' does not" + f"match the current joint positions shape '{current_joint_pos.shape}'." + ) + + joint_pos_error_nullspace = nullspace_joint_pos_target - current_joint_pos + joint_vel_error_nullspace = -current_joint_vel + + # Calculate the desired joint accelerations + joint_acc_nullspace = ( + self._nullspace_p_gain * joint_pos_error_nullspace + + self._nullspace_d_gain * joint_vel_error_nullspace + ).unsqueeze(-1) + + # Calculate the projected torques in null-space + if mass_matrix is not None: + tau_null = (nullspace_jacobian_transpose @ mass_matrix @ joint_acc_nullspace).squeeze(-1) + else: + tau_null = nullspace_jacobian_transpose @ joint_acc_nullspace + + # Add the null-space joint efforts to the total joint efforts + joint_efforts += tau_null + + else: + raise ValueError(f"Invalid null-space control method: {self.cfg.nullspace_control}.") + return joint_efforts diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space_cfg.py index 8befaee6aa..9b47e802ac 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space_cfg.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space_cfg.py @@ -75,3 +75,16 @@ class OperationalSpaceControllerCfg: Note: since only the linear forces could be measured at the moment, only the first three elements are used for the feedback loop. """ + + nullspace_control: str = "none" + """The null space control method for redundant manipulators: ``"none"``, ``"position"``. + + Note: ``"position"`` is used to drive the redundant manipulator to zero configuration by default. If + ``target_joint_pos`` is provided in the ``compute()`` method, it will be driven to this configuration. + """ + + nullspace_stiffness: float = 10.0 + """The stiffness for null space control.""" + + nullspace_damping_ratio: float = 1.0 + """The damping ratio for null space control.""" diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py index 09ffd48522..f66a9f02b3 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py @@ -303,3 +303,10 @@ class OffsetCfg: damping_ratio_scale: float = 1.0 """Scale factor for the damping ratio commands. Defaults to 1.0.""" + + nullspace_joint_pos_target: str = "none" + """The joint targets for the null-space control: ``"none"``, ``"zero"``, ``"default"``, ``"center"``. + + Note: Functional only when ``nullspace_control`` is set to ``"position"`` within the + ``OperationalSpaceControllerCfg``. + """ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py index 8a96a2523c..eccfa26307 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py @@ -351,6 +351,10 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma self._ee_force_w = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now self._ee_force_b = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now + # create tensors for the joint states + self._joint_pos = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + self._joint_vel = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + # create the joint effort tensor self._joint_efforts = torch.zeros(self.num_envs, self._num_DoF, device=self.device) @@ -369,6 +373,10 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma self._damping_ratio_idx = None self._resolve_command_indexes() + # Nullspace position control joint targets + self._nullspace_joint_pos_target = None + self._resolve_nullspace_joint_pos_targets() + """ Properties. """ @@ -438,6 +446,7 @@ def apply_actions(self): self._compute_ee_pose() self._compute_ee_velocity() self._compute_ee_force() + self._compute_joint_states() # Calculate the joint efforts self._joint_efforts[:] = self._osc.compute( jacobian_b=self._jacobian_b, @@ -446,6 +455,9 @@ def apply_actions(self): current_ee_force_b=self._ee_force_b, mass_matrix=self._mass_matrix, gravity=self._gravity, + current_joint_pos=self._joint_pos, + current_joint_vel=self._joint_vel, + nullspace_joint_pos_target=self._nullspace_joint_pos_target, ) self._asset.set_joint_effort_target(self._joint_efforts, joint_ids=self._joint_ids) @@ -524,6 +536,35 @@ def _resolve_command_indexes(self): if self.action_dim != cmd_idx: raise ValueError("Not all command indexes have been resolved.") + def _resolve_nullspace_joint_pos_targets(self): + """Resolves the nullspace joint pos targets for the operational space controller. + + Raises: + ValueError: If the nullspace joint pos targets are set when null space control is not set to 'position'. + ValueError: If the nullspace joint pos targets are not set when null space control is set to 'position'. + ValueError: If an invalid value is set for nullspace joint pos targets. + """ + + if self.cfg.nullspace_joint_pos_target != "none" and self.cfg.controller_cfg.nullspace_control != "position": + raise ValueError("Nullspace joint targets can only be set when null space control is set to 'position'.") + + if self.cfg.nullspace_joint_pos_target == "none" and self.cfg.controller_cfg.nullspace_control == "position": + raise ValueError("Nullspace joint targets must be set when null space control is set to 'position'.") + + if self.cfg.nullspace_joint_pos_target == "zero" or self.cfg.nullspace_joint_pos_target == "none": + # Keep the nullspace joint targets as None as this is later processed as zero in the controller + self._nullspace_joint_pos_target = None + elif self.cfg.nullspace_joint_pos_target == "center": + # Get the center of the robot soft joint limits + self._nullspace_joint_pos_target = torch.mean( + self._asset.data.soft_joint_pos_limits[:, self._joint_ids, :], dim=-1 + ) + elif self.cfg.nullspace_joint_pos_target == "default": + # Get the default joint positions + self._nullspace_joint_pos_target = self._asset.data.default_joint_pos[:, self._joint_ids] + else: + raise ValueError("Invalid value for nullspace joint pos targets.") + def _compute_dynamic_quantities(self): """Computes the dynamic quantities for operational space control.""" @@ -605,6 +646,12 @@ def _compute_ee_force(self): # Rotate forces and torques into root frame self._ee_force_b[:] = math_utils.quat_rotate_inverse(self._asset.data.root_link_quat_w, self._ee_force_w) + def _compute_joint_states(self): + """Computes the joint states for operational space control.""" + # Extract joint positions and velocities + self._joint_pos[:] = self._asset.data.joint_pos[:, self._joint_ids] + self._joint_vel[:] = self._asset.data.joint_vel[:, self._joint_ids] + def _compute_task_frame_pose(self): """Computes the pose of the task frame in root frame.""" # Update task frame pose if task frame rigidbody is provided diff --git a/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py b/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py index bfde34681e..fa54adb0d6 100644 --- a/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py +++ b/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py @@ -574,6 +574,100 @@ def test_franka_taskframe_hybrid(self): self._run_op_space_controller(robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_set_tilted) + def test_franka_pose_abs_without_inertial_decoupling_with_nullspace_centering(self): + """Test absolute pose control with fixed impedance and nullspace centerin but without inertial decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=[400.0, 400.0, 400.0, 100.0, 100.0, 100.0], + motion_damping_ratio_task=[5.0, 5.0, 5.0, 0.001, 0.001, 0.001], + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_pose_abs_with_partial_inertial_decoupling_nullspace_centering(self): + """Test absolute pose control with fixed impedance, partial inertial decoupling and nullspace centering.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=True, + gravity_compensation=False, + motion_stiffness_task=1000.0, + motion_damping_ratio_task=1.0, + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_pose_abs_with_nullspace_centering(self): + """Test absolute pose control with fixed impedance, inertial decoupling and nullspace centering.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_taskframe_hybrid_with_nullspace_centering(self): + """Test hybrid control in task frame with fixed impedance, inertial decoupling and nullspace centering.""" + robot = Articulation(cfg=self.robot_cfg) + self.frame = "task" + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(2.0, 1.5, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(self.target_hybrid_set_tilted[0, 0] + 0.085, 0.0, 0.3), + orientation=(0.9238795325, 0.0, -0.3826834324, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + self.contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=400.0, + motion_damping_ratio_task=1.0, + contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], + motion_control_axes_task=[1, 1, 0, 1, 1, 1], + contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], + nullspace_control="position", + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_set_tilted) + """ Helper functions """ @@ -615,10 +709,22 @@ def _run_op_space_controller( # Note: We need to update buffers before the first step for the controller. robot.update(dt=sim_dt) + # Get the center of the robot soft joint limits + joint_centers = torch.mean(robot.data.soft_joint_pos_limits[:, arm_joint_ids, :], dim=-1) + # get the updated states - jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = self._update_states( - robot, ee_frame_idx, arm_joint_ids - ) + ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) = self._update_states(robot, ee_frame_idx, arm_joint_ids) # Track the given target command current_goal_idx = 0 # Current goal index for the arm @@ -653,7 +759,7 @@ def _run_op_space_controller( self.contact_forces.reset() # reset target pose robot.update(sim_dt) - _, _, _, ee_pose_b, _, _, _, _ = self._update_states( + _, _, _, ee_pose_b, _, _, _, _, _, _ = self._update_states( robot, ee_frame_idx, arm_joint_ids ) # at reset, the jacobians are not updated to the latest state command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = self._update_target( @@ -669,9 +775,18 @@ def _run_op_space_controller( ) else: # get the updated states - jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = ( - self._update_states(robot, ee_frame_idx, arm_joint_ids) - ) + ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) = self._update_states(robot, ee_frame_idx, arm_joint_ids) # compute the joint commands joint_efforts = osc.compute( jacobian_b=jacobian_b, @@ -680,6 +795,9 @@ def _run_op_space_controller( current_ee_force_b=ee_force_b, mass_matrix=mass_matrix, gravity=gravity, + current_joint_pos=joint_pos, + current_joint_vel=joint_vel, + nullspace_joint_pos_target=joint_centers, ) robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) robot.write_data_to_sim() @@ -715,6 +833,8 @@ def _update_states( root_pose_w (torch.tensor): The root pose in the world frame. ee_pose_w (torch.tensor): The end-effector pose in the world frame. ee_force_b (torch.tensor): The end-effector force in the root frame. + joint_pos (torch.tensor): The joint positions. + joint_vel (torch.tensor): The joint velocities. """ # obtain dynamics related quantities from simulation ee_jacobi_idx = ee_frame_idx - 1 @@ -757,7 +877,22 @@ def _update_states( # This is a simplification, only for the sake of testing. ee_force_b = ee_force_w - return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b + # Get joint positions and velocities + joint_pos = robot.data.joint_pos[:, arm_joint_ids] + joint_vel = robot.data.joint_vel[:, arm_joint_ids] + + return ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) def _update_target( self, diff --git a/source/extensions/omni.isaac.lab_tasks/config/extension.toml b/source/extensions/omni.isaac.lab_tasks/config/extension.toml index e82e19521f..7f8033187f 100644 --- a/source/extensions/omni.isaac.lab_tasks/config/extension.toml +++ b/source/extensions/omni.isaac.lab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.10.16" +version = "0.10.17" # Description title = "Isaac Lab Environments" diff --git a/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst index 69a4c04aea..53f2b01d2b 100644 --- a/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,17 @@ Changelog --------- +0.10.17 (2024-12-17) +~~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed the configuration of + :class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction` + inside the ``Isaac-Reach-Franka-OSC-v0`` environment to enable nullspace control. + + 0.10.16 (2024-12-16) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py index 410e33c467..517c2f39b0 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py @@ -49,7 +49,9 @@ def __post_init__(self): motion_stiffness_task=100.0, motion_damping_ratio_task=1.0, motion_stiffness_limits_task=(50.0, 200.0), + nullspace_control="position", ), + nullspace_joint_pos_target="center", position_scale=1.0, orientation_scale=1.0, stiffness_scale=100.0, diff --git a/source/standalone/tutorials/05_controllers/run_osc.py b/source/standalone/tutorials/05_controllers/run_osc.py index 252b93ce01..3088c41e08 100644 --- a/source/standalone/tutorials/05_controllers/run_osc.py +++ b/source/standalone/tutorials/05_controllers/run_osc.py @@ -134,6 +134,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], motion_control_axes_task=[1, 1, 0, 1, 1, 1], contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], + nullspace_control="position", ) osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device) @@ -177,10 +178,22 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): # Note: We need to update buffers before the first step for the controller. robot.update(dt=sim_dt) + # Get the center of the robot soft joint limits + joint_centers = torch.mean(robot.data.soft_joint_pos_limits[:, arm_joint_ids, :], dim=-1) + # get the updated states - jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = update_states( - sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces - ) + ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) = update_states(sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces) # Track the given target command current_goal_idx = 0 # Current goal index for the arm @@ -210,7 +223,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): contact_forces.reset() # reset target pose robot.update(sim_dt) - _, _, _, ee_pose_b, _, _, _, _ = update_states( + _, _, _, ee_pose_b, _, _, _, _, _, _ = update_states( sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces ) # at reset, the jacobians are not updated to the latest state command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = update_target( @@ -222,9 +235,18 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b) else: # get the updated states - jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = update_states( - sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces - ) + ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) = update_states(sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces) # compute the joint commands joint_efforts = osc.compute( jacobian_b=jacobian_b, @@ -233,6 +255,9 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): current_ee_force_b=ee_force_b, mass_matrix=mass_matrix, gravity=gravity, + current_joint_pos=joint_pos, + current_joint_vel=joint_vel, + nullspace_joint_pos_target=joint_centers, ) # apply actions robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) @@ -280,6 +305,8 @@ def update_states( root_pose_w (torch.tensor): Root pose in the world frame. ee_pose_w (torch.tensor): End-effector pose in the world frame. ee_force_b (torch.tensor): End-effector force in the body frame. + joint_pos (torch.tensor): The joint positions. + joint_vel (torch.tensor): The joint velocities. Raises: ValueError: Undefined target_type. @@ -324,7 +351,22 @@ def update_states( # This is a simplification, only for the sake of testing. ee_force_b = ee_force_w - return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b + # Get joint positions and velocities + joint_pos = robot.data.joint_pos[:, arm_joint_ids] + joint_vel = robot.data.joint_vel[:, arm_joint_ids] + + return ( + jacobian_b, + mass_matrix, + gravity, + ee_pose_b, + ee_vel_b, + root_pose_w, + ee_pose_w, + ee_force_b, + joint_pos, + joint_vel, + ) # Update the target commands diff --git a/tools/per_test_timeouts.py b/tools/per_test_timeouts.py index 340abde85c..dc6da9c704 100644 --- a/tools/per_test_timeouts.py +++ b/tools/per_test_timeouts.py @@ -18,5 +18,5 @@ "test_rsl_rl_wrapper.py": 200, "test_sb3_wrapper.py": 200, "test_skrl_wrapper.py": 200, - "test_operational_space.py": 200, + "test_operational_space.py": 300, }