|
| 1 | +Using an operational space controller |
| 2 | +===================================== |
| 3 | + |
| 4 | +.. currentmodule:: omni.isaac.lab |
| 5 | + |
| 6 | +Sometimes, controlling the end-effector pose of the robot using a differential IK controller is not sufficient. |
| 7 | +For example, we might want to enforce a very specific pose tracking error dynamics in the task space, actuate the robot |
| 8 | +with joint effort/torque commands, or apply a contact force at a specific direction while controlling the motion of |
| 9 | +the other directions (e.g., washing the surface of the table with a cloth). In such tasks, we can use an |
| 10 | +operational space controller (OSC). |
| 11 | + |
| 12 | +.. rubric:: References for the operational space control: |
| 13 | + |
| 14 | +1. O Khatib. A unified approach for motion and force control of robot manipulators: |
| 15 | + The operational space formulation. IEEE Journal of Robotics and Automation, 3(1):43–53, 1987. URL http://dx.doi.org/10.1109/JRA.1987.1087068. |
| 16 | + |
| 17 | +2. Robot Dynamics Lecture Notes by Marco Hutter (ETH Zurich). URL https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf |
| 18 | + |
| 19 | +In this tutorial, we will learn how to use an OSC to control the robot. |
| 20 | +We will use the :class:`controllers.OperationalSpaceController` class to apply a constant force perpendicular to a |
| 21 | +tilted wall surface while tracking a desired end-effector pose in all the other directions. |
| 22 | + |
| 23 | +The Code |
| 24 | +~~~~~~~~ |
| 25 | + |
| 26 | +The tutorial corresponds to the ``run_osc.py`` script in the |
| 27 | +``source/standalone/tutorials/05_controllers`` directory. |
| 28 | + |
| 29 | + |
| 30 | +.. dropdown:: Code for run_osc.py |
| 31 | + :icon: code |
| 32 | + |
| 33 | + .. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py |
| 34 | + :language: python |
| 35 | + :linenos: |
| 36 | + |
| 37 | + |
| 38 | +Creating an Operational Space Controller |
| 39 | +---------------------------------------- |
| 40 | + |
| 41 | +The :class:`~controllers.OperationalSpaceController` class computes the joint |
| 42 | +efforts/torques for a robot to do simultaneous motion and force control in task space. |
| 43 | + |
| 44 | +The reference frame of this task space could be an arbitrary coordinate frame in Euclidean space. By default, |
| 45 | +it is the robot's base frame. However, in certain cases, it could be easier to define target coordinates w.r.t. a |
| 46 | +different frame. In such cases, the pose of this task reference frame, w.r.t. to the robot's base frame, should be |
| 47 | +provided in the ``set_command`` method's ``current_task_frame_pose_b`` argument. For example, in this tutorial, it |
| 48 | +makes sense to define the target commands w.r.t. a frame that is parallel to the wall surface, as the force control |
| 49 | +direction would be then only nonzero in the z-axis of this frame. The target pose, which is set to have the same |
| 50 | +orientation as the wall surface, is such a candidate and is used as the task frame in this tutorial. Therefore, all |
| 51 | +the arguments to the :class:`~controllers.OperationalSpaceControllerCfg` should be set with this task reference frame |
| 52 | +in mind. |
| 53 | + |
| 54 | +For the motion control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base, |
| 55 | +``target_types: "pose_abs"``) or relative the the end-effector's current pose (i.e., ``target_types: "pose_rel"``). |
| 56 | +For the force control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base, |
| 57 | +``target_types: "force_abs"``). If it is desired to apply pose and force control simultaneously, the ``target_types`` |
| 58 | +should be a list such as ``["pose_abs", "wrench_abs"]`` or ``["pose_rel", "wrench_abs"]``. |
| 59 | + |
| 60 | +The axes that the motion and force control will be applied can be specified using the ``motion_control_axes_task`` and |
| 61 | +``force_control_axes_task`` arguments, respectively. These lists should consist of 0/1 for all six axes (position and |
| 62 | +rotation) and be complementary to each other (e.g., for the x-axis, if the ``motion_control_axes_task`` is ``0``, the |
| 63 | +``force_control_axes_task`` should be ``1``). |
| 64 | + |
| 65 | +For the motion control axes, desired stiffness, and damping ratio values can be specified using the |
| 66 | +``motion_control_stiffness`` and ``motion_damping_ratio_task`` arguments, which can be a scalar (same value for all |
| 67 | +axes) or a list of six scalars, one value corresponding to each axis. If desired, the stiffness and damping ratio |
| 68 | +values could be a command parameter (e.g., to learn the values using RL or change them on the go). For this, |
| 69 | +``impedance_mode`` should be either ``"variable_kp"`` to include the stiffness values within the command or |
| 70 | +``"variable"`` to include both the stiffness and damping ratio values. In these cases, ``motion_stiffness_limits_task`` |
| 71 | +and ``motion_damping_limits_task`` should be set as well, which puts bounds on the stiffness and damping ratio values. |
| 72 | + |
| 73 | +For contact force control, it is possible to apply an open-loop force control by not setting the |
| 74 | +``contact_wrench_stiffness_task``, or apply a closed-loop force control (with the feed-forward term) by setting |
| 75 | +the desired stiffness values using the ``contact_wrench_stiffness_task`` argument, which can be a scalar or a list |
| 76 | +of six scalars. Please note that, currently, only the linear part of the contact wrench (hence the first three |
| 77 | +elements of the ``contact_wrench_stiffness_task``) is considered in the closed-loop control, as the rotational part |
| 78 | +cannot be measured with the contact sensors. |
| 79 | + |
| 80 | +For the motion control, ``inertial_dynamics_decoupling`` should be set to ``True`` to use the robot's inertia matrix |
| 81 | +to decouple the desired accelerations in the task space. This is important for the motion control to be accurate, |
| 82 | +especially for rapid movements. This inertial decoupling accounts for the coupling between all the six motion axes. |
| 83 | +If desired, the inertial coupling between the translational and rotational axes could be ignored by setting the |
| 84 | +``partial_inertial_dynamics_decoupling`` to ``True``. |
| 85 | + |
| 86 | +If it is desired to include the gravity compensation in the operational space command, the ``gravity_compensation`` |
| 87 | +should be set to ``True``. |
| 88 | + |
| 89 | +The included OSC implementation performs the computation in a batched format and uses PyTorch operations. |
| 90 | + |
| 91 | +In this tutorial, we will use ``"pose_abs"`` for controlling the motion in all axes except the z-axis and |
| 92 | +``"wrench_abs"`` for controlling the force in the z-axis. Moreover, we will include the full inertia decoupling in |
| 93 | +the motion control and not include the gravity compensation, as the gravity is disabled from the robot configuration. |
| 94 | +Finally, we set the impedance mode to ``"variable_kp"`` to dynamically change the stiffness values |
| 95 | +(``motion_damping_ratio_task`` is set to ``1``: the kd values adapt according to kp values to maintain a critically |
| 96 | +damped response). |
| 97 | + |
| 98 | +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py |
| 99 | + :language: python |
| 100 | + :start-at: # Create the OSC |
| 101 | + :end-at: osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device) |
| 102 | + |
| 103 | +Updating the states of the robot |
| 104 | +-------------------------------------------- |
| 105 | + |
| 106 | +The OSC implementation is a computation-only class. Thus, it expects the user to provide the necessary information |
| 107 | +about the robot. This includes the robot's Jacobian matrix, mass/inertia matrix, end-effector pose, velocity, and |
| 108 | +contact force, all in the root frame. Moreover, the user should provide gravity compensation vector, if desired. |
| 109 | + |
| 110 | +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py |
| 111 | + :language: python |
| 112 | + :start-at: # Update robot states |
| 113 | + :end-before: return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b |
| 114 | + |
| 115 | + |
| 116 | +Computing robot command |
| 117 | +----------------------- |
| 118 | + |
| 119 | +The OSC separates the operation of setting the desired command and computing the desired joint positions. |
| 120 | +To set the desired command, the user should provide command vector, which includes the target commands |
| 121 | +(i.e., in the order they appear in the ``target_types`` argument of the OSC configuration), |
| 122 | +and the desired stiffness and damping ratio values if the impedance_mode is set to ``"variable_kp"`` or ``"variable"``. |
| 123 | +They should be all in the same coordinate frame as the task frame (e.g., indicated with ``_task`` subscript) and |
| 124 | +concatanated together. |
| 125 | + |
| 126 | +In this tutorial, the desired wrench is already defined w.r.t. the task frame, and the desired pose is transformed |
| 127 | +to the task frame as the following: |
| 128 | + |
| 129 | +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py |
| 130 | + :language: python |
| 131 | + :start-at: # Convert the target commands to the task frame |
| 132 | + :end-at: return command, task_frame_pose_b |
| 133 | + |
| 134 | +The OSC command is set with the command vector in the task frame, the end-effector pose in the base frame, and the |
| 135 | +task (reference) frame pose in the base frame as the following. This information is needed, as the internal |
| 136 | +computations are done in the base frame. |
| 137 | + |
| 138 | +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py |
| 139 | + :language: python |
| 140 | + :start-at: # set the osc command |
| 141 | + :end-at: osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b) |
| 142 | + |
| 143 | +The joint effort/torque values are computed using the provided robot states and the desired command as the following: |
| 144 | + |
| 145 | +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py |
| 146 | + :language: python |
| 147 | + :start-at: # compute the joint commands |
| 148 | + :end-at: ) |
| 149 | + |
| 150 | + |
| 151 | +The computed joint effort/torque targets can then be applied on the robot. |
| 152 | + |
| 153 | +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py |
| 154 | + :language: python |
| 155 | + :start-at: # apply actions |
| 156 | + :end-at: robot.write_data_to_sim() |
| 157 | + |
| 158 | + |
| 159 | +The Code Execution |
| 160 | +~~~~~~~~~~~~~~~~~~ |
| 161 | + |
| 162 | +You can now run the script and see the result: |
| 163 | + |
| 164 | +.. code-block:: bash |
| 165 | +
|
| 166 | + ./isaaclab.sh -p source/standalone/tutorials/05_controllers/run_osc.py --num_envs 128 |
| 167 | +
|
| 168 | +The script will start a simulation with 128 robots. The robots will be controlled using the OSC. |
| 169 | +The current and desired end-effector poses should be displayed using frame markers in addition to the red tilted wall. |
| 170 | +You should see that the robot reaches the desired pose while applying a constant force perpendicular to the wall |
| 171 | +surface. |
| 172 | + |
| 173 | +.. figure:: ../../_static/tutorials/tutorial_operational_space_controller.jpg |
| 174 | + :align: center |
| 175 | + :figwidth: 100% |
| 176 | + :alt: result of run_osc.py |
| 177 | + |
| 178 | +To stop the simulation, you can either close the window or press ``Ctrl+C`` in the terminal. |
0 commit comments