Description

This PR adds the OperationalSpaceController to the docs and provides some tests for its parametric features. Moreover, it implements the corresponding OperationalSpaceControllerAction and OperationalSpaceControllerActionCfg classes so they can be used with manager-based environments.

Fixes #873

Type of change

Bug fix (non-breaking change which fixes an issue)
New feature (non-breaking change which adds functionality)
This change requires a documentation update

Checklist

[x] I have run the pre-commit checks with ./isaaclab.sh --format
[x] I have made corresponding changes to the documentation
[x] My changes generate no new warnings
[x] I have added tests that prove my fix is effective or that my feature works
[ ] I have updated the changelog and the corresponding version in the extension's config/extension.toml file
[x] I have added my name to the CONTRIBUTORS.md or my name already exists there ``

Aug 31 '24 09:08 ozhanozen

@Dhoeller19, I have created this PR to replace #879 but haven't worked much on it yet. It is far from complete at the moment. I am currently working on it and will ping once it reaches a state worth reviewing. Sorry for the confusion.

Sep 03 '24 13:09 ozhanozen

Converted to a draft util ready for full review. @ozhanozen i will continue to work with you on this branch. Thanks for doing this!

Sep 03 '24 13:09 jtigue-bdai

@jtigue-bdai, no problem. It takes a bit of time to complete, though. I have some updates and questions regarding the progress.

I have added tests for cases where the target is abs/rel pos/pose, and w/wo inertial/gravity compensations. I took the differential IK controller test as a reference: if the robot ee reaches these targets well, the tests pass. The current tests pass successfully. The remaining tests to be implemented are:

[x] Force target (any idea what would be a test that makes sense here?)
[x] Hybrid target with selection matrix
[x] Variable stiffness (I will try varying the values within a reasonable range, for the sole purpose of testing but without any other function)

After these tests, the following steps would be:

[x] Implementing OperationSpaceControllerAction and OperationSpaceControllerActionCfg, which could basically be based on the test code.
[x] Adding OPC to the documentation.

Please let me know what you think and if you have any feedback.

Btw, I noticed that sim.clear() inside the tearDown() makes my tests hang, if LIVESTREAM is not 2 and started. This is the same for other tests, not only this controller. I am working with dockerized version, which might be relevant. Do you have any idea why this might be the case?

Sep 06 '24 16:09 ozhanozen

@Mayankm96, I also wanted to ping you here as you wrote the original OPC implementations.

I have made some changes to the implementation and wanted to ensure that these are accurate and that I haven't missed something that might break the functionality somewhere else. Mainly:

I observed some problems with the batched matrix dimensions being incompatible with multiplications, so I changed them.
The desired ee pos/rot/force was previously updated within the compute() function. However, this made the target unreachable when defined relative (e.g., the desired pose = ee pose + command was applied all the time). I moved this part to set_command() so the target is constant during the operation.
Some minor naming changes to be consistent with differential ik controller.
Separated OperationSpaceControllerCfg and OperationSpaceController to different files.

Please let me know if these changes are ok for you, or if you have any objections or feedback.

Moreover, there are some points regarding the current OPC implementation I was thinking about, which might be worth to consider for adding as a feature/modification:

Currently the target is assumed to have constant position/pose. We can generalize this by allowing inputting reference velocity/acceleration such that des_ee_acc = p_gains * (pos-pos_ref) + d_gains * (vel-vel_ref) + acc_ref. I believe this would be relevant as we one want that the robot follows a moving target.
We have the selection matrix to choose which axes we want to control the movement and which axes we want to apply the force. However, the axes are wrt the earth frame. In many cases, one might want that these axes are defined wrt the end-effector (e.g., we want to apply force in ee z-axis regardless of ee pose). We can achieve this by transforming the selection matrix using the rotation matrix between ee and the robot base. Maybe we can provide this as a parameter to the controller_cfg? (e.g., control_axes_frame: Literal["base", "ee"] )
There are scale coefficients for position and orientation commands. I see such coefficients are typically used within the corresponding Action implementations for other controllers. If these have the same purpose, does it make sense to remove them to be moved to future Action implementations?
The current implementation does not contain compensation for the velocity-dependent disturbances: the Coriolis/centrifugal forces and the velocity-induced acceleration due to the mapping of coordinates (i.e., J^{dot}*q^{dot}). Does it make sense to compensate for these? I believe it is easy to obtain the first one from the sim, but for the second element, we need the time derivative of the Jacobian, which I don't know if it is possible to obtain from the sim directly.
While the EE reaches the target successfully, the robot floats within its null space if a redundant robot is used. Shall we include some basic secondary targets (e.g., minimizing energy, maximizing manipulability, etc) to deal with this redundancy?

I believe that some of these points might require significant work and make the implementation more complex. However, I think OPC might be beneficial for sim2real, and it is not a bad idea to have a complete implementation now. We could create another PR for some of these, though, as it goes beyond the scope of this PR.

What do you think about these points?

Sep 06 '24 17:09 ozhanozen

Thanks @ozhanozen, things are looking pretty good. I will give them a review.

As for you questions:

Force target: Maybe spawn the robot in a pose and have it press into a plane with a desired force and check steady state torques (minus gravity torques) and see if they match tau=J'w
Hybrid target: Similar to force target but have a desired normal force and a desired tangential position.
Varying stiffness, I think that is reasonable. Fully validating exact stiffnesses may not be worth the effort can probably drive the arm into a desired position past a wall a couple and make sure quasi statically the forces go up for higher stiffnesss.
As for the Action implementation do you mean an example or writing the actual action code?
The OPC is added into the documentation by adding the lines to docs/source/api/lab/omni.isaac.lab.controllers.rst that are already done. The docs are autogenerated by docstrings. We might clean up the docstrings along the way though. in addition we an add a standalon example and make a how-to or tutorial addition to the documentation.

I will try running your tests on my machine. I also use the docker workflow. I will let you know what I find.

Sep 06 '24 17:09 jtigue-bdai

Thanks @ozhanozen, things are looking pretty good. I will give them a review.

As for you questions:

Force target: Maybe spawn the robot in a pose and have it press into a plane with a desired force and check steady state torques (minus gravity torques) and see if they match tau=J'w

Hybrid target: Similar to force target but have a desired normal force and a desired tangential position.

Varying stiffness, I think that is reasonable. Fully validating exact stiffnesses may not be worth the effort can probably drive the arm into a desired position past a wall a couple and make sure quasi statically the forces go up for higher stiffnesss.

As for the Action implementation do you mean an example or writing the actual action code?

The OPC is added into the documentation by adding the lines to docs/source/api/lab/omni.isaac.lab.controllers.rst that are already done. The docs are autogenerated by docstrings. We might clean up the docstrings along the way though. in addition we an add a standalon example and make a how-to or tutorial addition to the documentation.

I will try running your tests on my machine. I also use the docker workflow. I will let you know what I find.

Thanks @jtigue-bdai for the suggestions, I will try to create test cases as you suggested.

Regarding the actions, I intended to implement the actual Action and ActionCfg code so that OPC is ready to be used with the manager-based environments. For this, I tried to write the test code in a way its functions could be re-used for the action implementation later.

Sep 09 '24 14:09 ozhanozen

Force target: Maybe spawn the robot in a pose and have it press into a plane with a desired force and check steady state torques (minus gravity torques) and see if they match tau=J'w

Hybrid target: Similar to force target but have a desired normal force and a desired tangential position.

Hi @jtigue-bdai, I have finished writing all the test cases but deviated a bit from what we discussed:

For the force control, I didn't see any way to measure interaction torques at the joints, and thought comparing applied torques with the desired wrench using tau=J'w didn't make sense. This is exactly how the joint torques are calculated in the open-loop case, and the equivalence is satisfied all the time. What I did instead was to put walls around the robot and attach contact sensors to these walls. The robot EE crashes into these walls; then I check if the first part of the desired wrench (i.e., forces) equals the measured contact force. In general, this works, but it is not very precise since you cannot easily make the robot stop at the contact point while using only force control, and any movement affects the contact force. Therefore, I have tuned the wall positions, gains, etc, as much as I could, and defined more "relaxed" thresholds for checking the force equivalence. If you change these parameters, it is possible that the robot goes somewhere else and the test fails, but this is, I believe, how the real robot would work as well, so it is expected.
For the hybrid control, I basically applied force control in only one axis pressing the wall, and I applied position control in the tangential directions, as you suggested. This time, the robot reaches a steady state pose more reliably. However, it is still not 100% robust as the force control can make the robot EE stuck if the path to the target pose is not simple or if the robot rotates in a bad way within its null space. Again, I tuned the tests so they pass, but they are a bit fragile whenever there is a force control component involved.

I will now proceed with writing the Action/ActionCfg classes. Feel free to provide any input for me to integrate.

Hi @Mayankm96, did you have a chance to look at the five improvement points I mentioned above? @jtigue-bdai, do you have any comments on these? I have thought about one more improvement, which could help with these force control issues I faced:

Let's say I would like robot EE to go to the table surface and apply force on the z-axis while I position-control the other axes. If the initial pose of the robot EE is too far from the target contact point, this sometimes creates problems as I mentioned; hence, I would normally first do a pure pose control until I reach the vicinity of the contact point, where I would switch to force control. I propose we include a new command specific to the force control (i.e., a binary or a float to fade smoothly) that allows us to switch/fade from motion control to force control for the axes the user wants to do eventually do force control. This way, not only can people use hybrid/force control in a more "controlled" manner, but also an RL policy could learn the optimal way to transition from motion to force control at the contact point. What do you think?

Sep 11 '24 19:09 ozhanozen

@ozhanozen thanks for all you hard work. I will take a look and get back with reviews.

Sep 12 '24 14:09 jtigue-bdai

Force target: Maybe spawn the robot in a pose and have it press into a plane with a desired force and check steady state torques (minus gravity torques) and see if they match tau=J'w

Hybrid target: Similar to force target but have a desired normal force and a desired tangential position.

Hi @jtigue-bdai, I have finished writing all the test cases but deviated a bit from what we discussed:

For the force control, I didn't see any way to measure interaction torques at the joints, and thought comparing applied torques with the desired wrench using tau=J'w didn't make sense. This is exactly how the joint torques are calculated in the open-loop case, and the equivalence is satisfied all the time. What I did instead was to put walls around the robot and attach contact sensors to these walls. The robot EE crashes into these walls; then I check if the first part of the desired wrench (i.e., forces) equals the measured contact force. In general, this works, but it is not very precise since you cannot easily make the robot stop at the contact point while using only force control, and any movement affects the contact force. Therefore, I have tuned the wall positions, gains, etc, as much as I could, and defined more "relaxed" thresholds for checking the force equivalence. If you change these parameters, it is possible that the robot goes somewhere else and the test fails, but this is, I believe, how the real robot would work as well, so it is expected.

For the hybrid control, I basically applied force control in only one axis pressing the wall, and I applied position control in the tangential directions, as you suggested. This time, the robot reaches a steady state pose more reliably. However, it is still not 100% robust as the force control can make the robot EE stuck if the path to the target pose is not simple or if the robot rotates in a bad way within its null space. Again, I tuned the tests so they pass, but they are a bit fragile whenever there is a force control component involved.

I will now proceed with writing the Action/ActionCfg classes. Feel free to provide any input for me to integrate.

Hi @Mayankm96, did you have a chance to look at the five improvement points I mentioned above? @jtigue-bdai, do you have any comments on these? I have thought about one more improvement, which could help with these force control issues I faced:

Let's say I would like robot EE to go to the table surface and apply force on the z-axis while I position-control the other axes. If the initial pose of the robot EE is too far from the target contact point, this sometimes creates problems as I mentioned; hence, I would normally first do a pure pose control until I reach the vicinity of the contact point, where I would switch to force control. I propose we include a new command specific to the force control (i.e., a binary or a float to fade smoothly) that allows us to switch/fade from motion control to force control for the axes the user wants to do eventually do force control. This way, not only can people use hybrid/force control in a more "controlled" manner, but also an RL policy could learn the optimal way to transition from motion to force control at the contact point. What do you think?

For the force control one I agree with your approach. I am trying to figure out if there is a way to "fix" the end-effector is space (easily) and then allow you to just focus on the forces. I think your's is a good start. We can iterate if needed.

On hybrid control, one way to help is to change the default pose of the articulation to be at the desired location this may help reduce the ill conditioned poses that force control can take you in.

Sep 12 '24 15:09 jtigue-bdai

@Mayankm96, I also wanted to ping you here as you wrote the original OPC implementations.

I have made some changes to the implementation and wanted to ensure that these are accurate and that I haven't missed something that might break the functionality somewhere else. Mainly:

I observed some problems with the batched matrix dimensions being incompatible with multiplications, so I changed them.

The desired ee pos/rot/force was previously updated within the compute() function. However, this made the target unreachable when defined relative (e.g., the desired pose = ee pose + command was applied all the time). I moved this part to set_command() so the target is constant during the operation.

Some minor naming changes to be consistent with differential ik controller.

Separated OperationSpaceControllerCfg and OperationSpaceController to different files.

Please let me know if these changes are ok for you, or if you have any objections or feedback.

Moreover, there are some points regarding the current OPC implementation I was thinking about, which might be worth to consider for adding as a feature/modification:

Currently the target is assumed to have constant position/pose. We can generalize this by allowing inputting reference velocity/acceleration such that des_ee_acc = p_gains * (pos-pos_ref) + d_gains * (vel-vel_ref) + acc_ref. I believe this would be relevant as we one want that the robot follows a moving target.

We have the selection matrix to choose which axes we want to control the movement and which axes we want to apply the force. However, the axes are wrt the earth frame. In many cases, one might want that these axes are defined wrt the end-effector (e.g., we want to apply force in ee z-axis regardless of ee pose). We can achieve this by transforming the selection matrix using the rotation matrix between ee and the robot base. Maybe we can provide this as a parameter to the controller_cfg? (e.g., control_axes_frame: Literal["base", "ee"] )

There are scale coefficients for position and orientation commands. I see such coefficients are typically used within the corresponding Action implementations for other controllers. If these have the same purpose, does it make sense to remove them to be moved to future Action implementations?

The current implementation does not contain compensation for the velocity-dependent disturbances: the Coriolis/centrifugal forces and the velocity-induced acceleration due to the mapping of coordinates (i.e., J^{dot}*q^{dot}). Does it make sense to compensate for these? I believe it is easy to obtain the first one from the sim, but for the second element, we need the time derivative of the Jacobian, which I don't know if it is possible to obtain from the sim directly.

While the EE reaches the target successfully, the robot floats within its null space if a redundant robot is used. Shall we include some basic secondary targets (e.g., minimizing energy, maximizing manipulability, etc) to deal with this redundancy?

I believe that some of these points might require significant work and make the implementation more complex. However, I think OPC might be beneficial for sim2real, and it is not a bad idea to have a complete implementation now. We could create another PR for some of these, though, as it goes beyond the scope of this PR.

What do you think about these points?

I think for this feature its a reasonable thing to add, but lets hold off for a further PR. We are already adding quite a bit here.
I think this would be good to add especially for relative based targets (pose_rel, position_rel, and even force).
I think that is valid. You can move that to the action implementation.
Lets hold off on that for now, but I think it would be pretty beneficial later especially for faster motions.
An easy way to handle this would be to add some joint level damping in the actuators.
That would be useful, but we may want to save that for a further PR.

Sep 12 '24 15:09 jtigue-bdai

@Mayankm96, I also wanted to ping you here as you wrote the original OPC implementations. I have made some changes to the implementation and wanted to ensure that these are accurate and that I haven't missed something that might break the functionality somewhere else. Mainly:

I observed some problems with the batched matrix dimensions being incompatible with multiplications, so I changed them.

The desired ee pos/rot/force was previously updated within the compute() function. However, this made the target unreachable when defined relative (e.g., the desired pose = ee pose + command was applied all the time). I moved this part to set_command() so the target is constant during the operation.

Some minor naming changes to be consistent with differential ik controller.

Separated OperationSpaceControllerCfg and OperationSpaceController to different files.

Please let me know if these changes are ok for you, or if you have any objections or feedback. Moreover, there are some points regarding the current OPC implementation I was thinking about, which might be worth to consider for adding as a feature/modification:

Currently the target is assumed to have constant position/pose. We can generalize this by allowing inputting reference velocity/acceleration such that des_ee_acc = p_gains * (pos-pos_ref) + d_gains * (vel-vel_ref) + acc_ref. I believe this would be relevant as we one want that the robot follows a moving target.

We have the selection matrix to choose which axes we want to control the movement and which axes we want to apply the force. However, the axes are wrt the earth frame. In many cases, one might want that these axes are defined wrt the end-effector (e.g., we want to apply force in ee z-axis regardless of ee pose). We can achieve this by transforming the selection matrix using the rotation matrix between ee and the robot base. Maybe we can provide this as a parameter to the controller_cfg? (e.g., control_axes_frame: Literal["base", "ee"] )

There are scale coefficients for position and orientation commands. I see such coefficients are typically used within the corresponding Action implementations for other controllers. If these have the same purpose, does it make sense to remove them to be moved to future Action implementations?

The current implementation does not contain compensation for the velocity-dependent disturbances: the Coriolis/centrifugal forces and the velocity-induced acceleration due to the mapping of coordinates (i.e., J^{dot}*q^{dot}). Does it make sense to compensate for these? I believe it is easy to obtain the first one from the sim, but for the second element, we need the time derivative of the Jacobian, which I don't know if it is possible to obtain from the sim directly.

While the EE reaches the target successfully, the robot floats within its null space if a redundant robot is used. Shall we include some basic secondary targets (e.g., minimizing energy, maximizing manipulability, etc) to deal with this redundancy?

I believe that some of these points might require significant work and make the implementation more complex. However, I think OPC might be beneficial for sim2real, and it is not a bad idea to have a complete implementation now. We could create another PR for some of these, though, as it goes beyond the scope of this PR. What do you think about these points?

I think for this feature its a reasonable thing to add, but lets hold off for a further PR. We are already adding quite a bit here.

I think this would be good to add especially for relative based targets (pose_rel, position_rel, and even force).

I think that is valid. You can move that to the action implementation.

Lets hold off on that for now, but I think it would be pretty beneficial later especially for faster motions.

An easy way to handle this would be to add some joint level damping in the actuators.

That would be useful, but we may want to save that for a further PR.

Thanks a lot for the answers.

The 3 is done. I will keep 1,4,6 for future PR. 2, I will try after Action implementations.

For 5, adding joint level damping might work for the tests but it is eventually undesired for sim2real as this damping is not available in the real robot. Also, somehow it is useful to have some control over the redundancy. What about I work on this separately and open a PR later if I have an elegant solution?

Sep 13 '24 15:09 ozhanozen

For 5, adding joint level damping might work for the tests but it is eventually undesired for sim2real as this damping is not available in the real robot. Also, somehow it is useful to have some control over the redundancy. What about I work on this separately and open a PR later if I have an elegant solution?

Yeah that sounds good.

Sep 16 '24 12:09 jtigue-bdai

Hi @jtigue-bdai,

I wanted to give an update:

I have finished implementing the OperationalSpaceControllerAction for the integration of OSC with manager-based environments. As an example, I have added a variation of the reach environment with minimal changes, opc_env_cfg.py, that allows learning/testing with OSC. Of course, not all combination of OSC parameters makes sense in this environment, since I have inherited from the existing reach environment config. Nevertheless, current parameters allow learning to reach, and it provides an example of OSC Action usage (you can use "Isaac-Reach-Franka-OSC-v0" and "Isaac-Reach-Franka-Play-OSC-v0").
There was a challenging issue regarding the contact sensors. If the OSC parameters are set in a way that a closed loop force feedback is necessary, I create a ContactSensor that belongs to the OperationalSpaceControllerAction. However, since the ContactSensorCfg is not part of the simulation scene directly, I couldn't make the simulation automatically handle the sensor (e.g., its initialization, reset, update, etc). Hence, I tried to manually handle the necessary ContactSensor operations (e.g., calling self._contact_sensor._initialize_impl()). It seems to work, but it is not an elegant solution. It would be better if you could check it once more. Is there a better way to integrate the ContactSensor? (e.g., making it a part of the articulation). I didn't see any other examples.

There are three more steps missing from my side for this PR:

Rotating the motion/force selection matrixes if the user wants to define them according to the end-effector (point 2 from the discussion before). I will work on this now, but it should be feasible.
A small tutorial, either similar to this, or more focused on the Action implementation. What do you think?
Whenever #966 is merged, I will recheck the test cases and tune them if necessary.

Please let me know if you have any feedback or reviews. We are almost there :)

Sep 19 '24 20:09 ozhanozen

Thanks @ozhanozen. I will review the code as it is currently and when #966 is merged I will let you know.

Sep 20 '24 15:09 jtigue-bdai

@jtigue-bdai, I have finished the following:

Added the feature to define an arbitrary frame (i.e., task reference frame) according to which all the targets could be defined. For example, if one wants to apply a force perpendicular to a tilted wall surface, it is easier to define the force targets w.r.t. a frame that is on the wall. All the relevant matrices/vectors (motion/force selection matrices, relative rotation vector, stiffness values, etc) are transformed according to this task reference frame now. I believe this would be useful: the recent Forge paper followed a similar approach and got promising results. It works well according to my tests, ~~but I haven't included this in the Action class, as this requires tracking a frame external to the robot asset. I will check how to do this later.~~ I have added this to the OperationalSpaceControllerAction: one can now provide the path of a RigidObject representing the task reference frame, and the action class creates a FrameTransformer within to keep track of the frame pose.
Added a tutorial script under source/standalone/tutorials/05_controllers/run_osc.py and a tutorial documentation under docs/source/tutorials/05_controllers/run_osc.rst that explains the controller and the script on an example in which the robot applies hybrid control on a tilted surface. However, I didn't manage to render the .rst file correctly on my side, so there might be problems with the format.

From my side, the PR is ready to be merged in terms of features, and I will be happy to integrate your feedback/reviews. Once it is merged, I will create other PRs for the other potential features as discussed.

Oct 04 '24 19:10 ozhanozen

@OOmotuyi this looks great. Just a few docstring/formatting stuff. If you are ready it seems like you can pull it out of draft and make it ready for review from the wider team.

@jtigue-bdai, Thanks a lot! I have addressed all your comments, updated the changelog, and marked the PR as ready for review.

Oct 11 '24 20:10 ozhanozen

Hi @jtigue-bdai,

I’ve addressed the comments (primarily regarding docstrings) currently marked as unresolved. Would it be okay if I go ahead and mark them as resolved?

I’ve also merged the latest changes from main and updated the relevant documentation. The CHANGELOG entries remain as [unreleased], assuming the version/date will be finalized when merging to main.

On a separate branch, I’ve implemented the nullspace controller feature for OSC and successfully deployed a trained policy on a real robot to test it. It’s working well, and I observed it significantly improves learning. I’ll create a new PR for it once this one is merged.

Thanks again for your review and feedback! 😊

Dec 10 '24 18:12 ozhanozen

Thanks a lot for the addition! Could you please help update the image from .png to .jpg (jpg takes up less storage), and add the new environment to the docs (https://isaac-sim.github.io/IsaacLab/main/source/overview/environments.html#single-agent and https://isaac-sim.github.io/IsaacLab/main/source/overview/environments.html#comprehensive-list-of-environments)?

Also looks like it'll need another rebase. feel free to update the version number based on the current main, I don't seem to have write access to update the file, but I can merge it in once everything is ready. thanks!

Dec 16 '24 03:12 kellyguo11

Hi @kellyguo11 ,

Thank you for the feedback on the PR and for merging it! While I noticed an issue related to the new clip ranges implementation (#1476) which I wanted to mention here, I wasn’t able to raise it before the PR was merged. I’ve now created a new issue (#1548) detailing my concerns and proposed solutions. Please let me know if you have any thoughts on it when convenient.

Dec 16 '24 18:12 ozhanozen

Add `OperationSpaceController` to docs and tests and implement corresponding action/action_cfg classes

Description

Type of change

Checklist