RNEA method #125

Tianhuanyu · 2023-04-11T15:00:37Z

Description

The RNEA method is added to model module of optas for computing inverseDynamics. Given q, qd, qdd, the function could return a external force vector.

The RNEA is tested with test_rnea.py

The current version of RNEA only supports the robots with:

resolute joints,
the first joint of robot fixed,
no fixed joints except the first joint and the last joint.

Todo

Completed implementation of new feature.
Add unit-test for the new feature.
Add documentation for the new feature.
Ensure all unit tests pass.
Address all issues raised by unit-tests.
Consider adding an example that demonstrates the use of the new feature (not always necessary).
Added code reviewer.

…nto fix-issue-102

Fix issue 102

Remove the extF and Tau

cmower

Thanks @Tianhuanyu! This is great! I have some feedback (please see comments). Also, you should run black on the files you modify - this will ensure the code formatting is correct.

optas/models.py

example/TrackingBall.py

cmower · 2023-04-11T16:07:27Z

Please also address the todo list in the PR description above.

cmower · 2023-04-11T16:18:26Z

Could you also make sure to add a unit test in tests/test_models.py. It would be great if you could generate random q, dq, ddq and check the output of your function against the output of PyBullet RNEA method to ensure it is at-least numerically equivalent.

fixed problem

using black

Tianhuanyu · 2023-04-11T22:07:28Z

Thanks @Tianhuanyu! This is great! I have some feedback (please see comments). Also, you should run black on the files you modify - this will ensure the code formatting is correct.

Thanks, @cmower . I try black for reformatting my code. It's fantastic. All the following parts are done within this update.

Tianhuanyu · 2023-04-11T22:13:00Z

Could you also make sure to add a unit test in tests/test_models.py. It would be great if you could generate random q, dq, ddq and check the output of your function against the output of PyBullet RNEA method to ensure it is at-least numerically equivalent.

Sure, but I find there is always a small error between PyBullet output and our RNEA. (around 0.5% or less). Maybe Pybullet uses a different method for computing inverse dynamics or it may consider friction. Currently, the error doesn't have much effect on my application. It maybe a long-term work to find out why the numbers are slightly different.

cmower

Hi @Tianhuanyu, thanks for the updates. Just a couple more changes.

Also, could you please add a unit test (that compares RobotModel.rnea to PyBullet) into tests/test_models.py. The small error you see is fine. As you mention, PyBullet probably takes into account friction or maybe other things. The test is good to have though because it verifies i) you're implementation is working (i.e. the code runs), and ii) the values it outputs are "pretty much" what we expect. Take a look here for more details about the implementation of RNEA in PyBullet.

optas/models.py

Comments modification

Tianhuanyu · 2023-04-12T12:06:03Z

Hi @Tianhuanyu, thanks for the updates. Just a couple more changes.

Also, could you please add a unit test (that compares RobotModel.rnea to PyBullet) into tests/test_models.py. The small error you see is fine. As you mention, PyBullet probably takes into account friction or maybe other things. The test is good to have though because it verifies i) you're implementation is working (i.e. the code runs), and ii) the values it outputs are "pretty much" what we expect. Take a look here for more details about the implementation of RNEA in PyBullet.

Thanks. @cmower I've submitted the modification for the two minor suggestion. I will submit the testing part as soon as possible.

cmower · 2023-04-12T12:09:24Z

Great, thanks @Tianhuanyu.

Testing part error threshold is 0.08

cmower

Thanks @Tianhuanyu, some last requests. After you've made these changes we should be able to merge this PR :)

cmower · 2023-04-12T16:33:56Z

tests/test_rnea.py

+    # Setup model (with no parameterized joints)
+    model = optas.RobotModel(urdf_filename=urdf_path)
+
+    pb.connect(*[pb.DIRECT])


Replace line with:

pb.connect(pb.DIRECT)

cmower · 2023-04-12T16:34:35Z

tests/test_rnea.py

+    def test_calculate_inverse_dynamics_symbol(self):
+        for _ in range(NUM_RANDOM):
+            q = optas.SX.sym("q", self.model.ndof)
+            qd = optas.SX.sym("qd", self.model.ndof)
+            qdd = optas.SX.sym("qdd", self.model.ndof)
+            tau1 = self.model.rnea(q, qd, qdd)
+            assert isinstance(tau1, optas.SX)


No need to run this in a loop, checking symbolic output only needs to be done once.

cmower · 2023-04-12T16:37:28Z

tests/test_rnea.py

+            )
+            # print(tau1)
+            # print(tau2)
+            assert isclose(tau1.T, tau2)


The value for tau1 is being returned as an optas.DM variable, and is a vector. I suggest using

assert isclose(tau1.toarray().flatten(), tau2)

toarray converts the optas.DM variable to a numpy array

cmower · 2023-04-12T16:38:08Z

tests/test_rnea.py

+def main(q=None):
+    test = TestRnea()
+    test.test_calculate_inverse_dynamics_symbol()
+    test.test_calculate_inverse_dynamics()
+
+
+if __name__ == "__main__":
+    main()


Remove, pytest doesn't require a main function.

cmower · 2023-04-12T16:38:55Z

tests/test_rnea.py

+NUM_RANDOM = 100
+
+
+class TestRnea:


Please move this class to tests/test_models.py. No need for the additional script.

optas/models.py

additional notes

Fixed test model

Torque Control Demo

Fix issue 102

Tianhuanyu and others added 7 commits March 16, 2023 11:43

update readme

bec8d98

1

ba9636f

Merge branch 'fix-issue-102' of https://github.com/Tianhuanyu/optas i…

332b3e4

…nto fix-issue-102

RNEA

7248326

Merge pull request #2 from Tianhuanyu/fix-issue-102

3eeff18

Fix issue 102

Remove the extF and Tau

11b315d

Merge pull request #3 from Tianhuanyu/fix-issue-102

cbe50d9

Remove the extF and Tau

cmower requested changes Apr 11, 2023

View reviewed changes

Tianhuanyu and others added 4 commits April 11, 2023 22:53

fixed problem

8042f1d

Merge pull request #4 from Tianhuanyu/fix-issue-102

6a8a87c

fixed problem

using black

63df249

Merge pull request #5 from Tianhuanyu/fix-issue-102

7f2b910

using black

cmower requested changes Apr 12, 2023

View reviewed changes

optas/models.py Outdated Show resolved Hide resolved

optas/models.py Show resolved Hide resolved

Tianhuanyu and others added 2 commits April 12, 2023 13:03

Comments modification

718a795

Merge pull request #6 from Tianhuanyu/fix-issue-102

5c7d031

Comments modification

Tianhuanyu and others added 2 commits April 12, 2023 16:43

Testing part error threshold is 0.08

0d6e59d

Merge pull request #7 from Tianhuanyu/fix-issue-102

bc4ee2d

Testing part error threshold is 0.08

cmower requested changes Apr 12, 2023

View reviewed changes

Tianhuanyu and others added 6 commits April 12, 2023 20:40

additional notes

4cf5b4d

Merge pull request #8 from Tianhuanyu/fix-issue-102

15bd365

additional notes

Fixed test model

7f22c5c

Merge pull request #9 from Tianhuanyu/fix-issue-102

560fda5

Fixed test model

Torque Control Demo

f0ded53

Merge pull request #10 from Tianhuanyu/fix-issue-102

49b0be2

Torque Control Demo

cmower and others added 29 commits June 6, 2023 11:56

Add is_discrete attribute for task model

a42a2f8

Integrate mixed integer solver

44126da

Implement mixed integer class

42df079

Format code with black

7a37ce7

Minor bugfixes

d5eeef1

Add mixed integer example

cac8409

Ensure optimization type is set correctly

9e4c42e

Bugfix

147e553

Add tests

2f27559

Update submodules

5109368

Compare against correct value. Fix cmower#136.

b9508cb

Format with black

9abc893

Add unit test for did_solve (and format code with black)

79e9e43

Disable velocity control

39f8efb

Merge pull request #11 from Tianhuanyu/fix-issue-102

8e7f22a

Fix issue 102

update readme

6a9ff8a

1

35bf184

RNEA

a55bf02

Remove the extF and Tau

0a8cf9f

fixed problem

d35a2f5

Comments modification

bbd6e76

Testing part error threshold is 0.08

82a7b2d

additional notes

bb6e62b

Fixed test model

f8be41d

Torque Control Demo

8179d86

Add is_discrete attribute for task model

ebe3c43

Disable velocity control

758ed03

Merge

d6057e6

Merge pull request #12 from Tianhuanyu/fix-issue-102

68a87d6

Fix issue 102

cmower merged commit b2cbbbe into cmower:master Jun 6, 2023
10 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

RNEA method #125

RNEA method #125

Tianhuanyu commented Apr 11, 2023 •

edited

cmower left a comment

cmower commented Apr 11, 2023

cmower commented Apr 11, 2023

Tianhuanyu commented Apr 11, 2023

Tianhuanyu commented Apr 11, 2023

cmower left a comment

Tianhuanyu commented Apr 12, 2023

cmower commented Apr 12, 2023

cmower left a comment

cmower Apr 12, 2023

cmower Apr 12, 2023

cmower Apr 12, 2023

cmower Apr 12, 2023

cmower Apr 12, 2023

RNEA method #125

RNEA method #125

Conversation

Tianhuanyu commented Apr 11, 2023 • edited

Description

Todo

cmower left a comment

Choose a reason for hiding this comment

cmower commented Apr 11, 2023

cmower commented Apr 11, 2023

Tianhuanyu commented Apr 11, 2023

Tianhuanyu commented Apr 11, 2023

cmower left a comment

Choose a reason for hiding this comment

Tianhuanyu commented Apr 12, 2023

cmower commented Apr 12, 2023

cmower left a comment

Choose a reason for hiding this comment

cmower Apr 12, 2023

Choose a reason for hiding this comment

cmower Apr 12, 2023

Choose a reason for hiding this comment

cmower Apr 12, 2023

Choose a reason for hiding this comment

cmower Apr 12, 2023

Choose a reason for hiding this comment

cmower Apr 12, 2023

Choose a reason for hiding this comment

Tianhuanyu commented Apr 11, 2023 •

edited