Implement the skeleton of the OptimalControlElement class and SE3Task #167

GiulioRomualdi · 2020-12-23T18:46:07Z

This PR implements the first skeleton for the TSID library. It is the first PR that will allow achieving #166. In detail, I defined an OptimalControlElement that is an abstract class that described a generic task that can be achieved by TSID. I also developed SE3Task whose purpose is to implement J dν/dt + dJ/dt ν = dv/dt*.

lie-group-controllers is now added as a dependency of blf. Once this PR has been merged lie-group-controllers should be added to the superbuild (cc @traversaro )

Since I would like to keep the PR small, I will implement the other tasks into other PR.

GiulioRomualdi · 2020-12-23T18:47:36Z

lovely!

GiulioRomualdi · 2021-01-08T16:30:51Z

On macOS is failing because of

==> Installing dependencies for pybind11: python@3.9
==> Installing pybind11 dependency: python@3.9
==> Pouring python@3.9-3.9.1_5.catalina.bottle.tar.gz
Error: The `brew link` step did not complete successfully
The formula built, but is not symlinked into /usr/local
Could not symlink bin/2to3
Target /usr/local/bin/2to3
already exists. You may want to remove it:
  rm '/usr/local/bin/2to3'

traversaro · 2021-01-08T16:38:15Z

On macOS is failing because of

==> Installing dependencies for pybind11: python@3.9
==> Installing pybind11 dependency: python@3.9
==> Pouring python@3.9-3.9.1_5.catalina.bottle.tar.gz
Error: The `brew link` step did not complete successfully
The formula built, but is not symlinked into /usr/local
Could not symlink bin/2to3
Target /usr/local/bin/2to3
already exists. You may want to remove it:
  rm '/usr/local/bin/2to3'

See:

In short, the combination of homebrew and GitHub Actions is extremely unstable, and incidents like that are quite frequent. Once strategy may be probably to migrate to conda-forge provided dependencies at least for macOS.

traversaro · 2021-01-08T16:51:25Z

lie-group-controllers is now added as a dependency of blf. Once this PR has been merged lie-group-controllers should be added to the superbuild (cc @traversaro )

Done robotology/robotology-superbuild#574 .

DanielePucci · 2021-01-08T17:09:29Z

CC @diegoferigo

prashanthr05 · 2021-01-08T18:17:49Z

@GiulioRomualdi I will take a look at this PR soon. But anyhow, for starters, a naming clarification.
If I am not wrong SE3Task is technically not a $SE(3)$ space defined task, instead is a $SO(3) \times \mathbb{R}^3$ task?

Theoretically, there are differences between the two due to the different definitions of exponential maps and logarithm maps.

Also in the implementation, at a higher level you set the pose, but at the lower level you instead split it into a rotation and a position object to perform the control. (This is misleading).

So in that case, do you think we should rename the class?

src/TSID/src/SE3Task.cpp

S-Dafarra · 2021-01-10T13:46:49Z

src/TSID/include/BipedalLocomotion/TSID/OptimalControlElement.h

+class OptimalControlElement
+{
+protected:
+    Eigen::MatrixXd m_A; /**< Element Matrix */
+    Eigen::VectorXd m_b; /**< Element Vector */


I would suggest to name this in a way that is more specific to TSID, like TaskElement, hiding how that TSID problem is solved (either via optimal control, pseudoinverse, or other methods). OptimalControl is, in my opinion, generic and a bit misleading, considering that you are considering only linear elements.

here: f7cd662 I renamed the class as Task

GiulioRomualdi · 2021-01-12T10:16:56Z

@GiulioRomualdi I will take a look at this PR soon. But anyhow, for starters, a naming clarification.
If I am not wrong SE3Task is technically not a SE(3) space defined task, instead is a SO(3)×R3 task?

Yes exactly

Theoretically, there are differences between the two due to the different definitions of exponential maps and logarithm maps.

Also in the implementation, at a higher level you set the pose, but at the lower level you instead split it into a rotation and a position object to perform the control. (This is misleading).

So in that case, do you think we should rename the class?

I tried to follow what was done in TSID. However, I agree with you that the name could be misleading. Indeed here the MIXED representation is used to define the twist and as you know in Lie theory only left/right trivializations are well defined.

Do you have other names in mind?

prashanthr05 · 2021-01-12T10:37:07Z

Do you have other names in mind?

A wordy choice would be in the direction of SO3xR3TaskMixed. where the prefix for Task will specify the task space and the suffix will specify the velocity representation ?

Or suppress the suffix, so that different choices are handled dynamically within the class (in the future), as done in iDynTree for some computations.

traversaro · 2021-01-12T10:50:53Z

Do you have other names in mind?

A wordy choice would be in the direction of SO3xR3TaskMixed. where the prefix for Task will specify the task space and the suffix will specify the velocity representation ?

I think that even if not mathematically super-precise, SE3Task may be a good compromise.

…t::subA() methods

… manif objects in TSID

- Make Task class compliant with robotology/idyntree@64cbaf7

- Avoid to copy the jacobian matrix in SE3Task

GiulioRomualdi · 2021-01-12T12:04:10Z

@prashanthr05

Do you have other names in mind?

A wordy choice would be in the direction of SO3xR3TaskMixed. where the prefix for Task will specify the task space and the suffix will specify the velocity representation ?

Or suppress the suffix, so that different choices are handled dynamically within the class (in the future), as done in iDynTree for some computations.

In 7a776b2 I added a check to make sure that the KinDynComputations object handles quantities in mixed representation. About changing the name of the class I would like to keep it as shorter as possible. If you agree I can improve the documentation by explaining the "theoretical" issue you spot.

prashanthr05

minor comment.

src/TSID/include/BipedalLocomotion/TSID/SE3Task.h

prashanthr05 · 2021-01-12T10:49:17Z

src/TSID/src/SE3Task.cpp

+        return false;
+    }
+
+    if (m_robotAccelerationVariable.size != m_kinDyn->getNrOfDegreesOfFreedom() + 6)


Maybe declare a const int for this magic 6? So it can be used here and elsewhere?

Done bfa5eaa

prashanthr05 · 2021-01-12T12:15:39Z

@prashanthr05

Do you have other names in mind?

A wordy choice would be in the direction of SO3xR3TaskMixed. where the prefix for Task will specify the task space and the suffix will specify the velocity representation ?
Or suppress the suffix, so that different choices are handled dynamically within the class (in the future), as done in iDynTree for some computations.

In 7a776b2 I added a check to make sure that the KinDynComputations object handles quantities in mixed representation. About changing the name of the class I would like to keep it as shorter as possible. If you agree I can improve the documentation by explaining the "theoretical" issue you spot.

Yes. I think it's ok.

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>

GiulioRomualdi · 2021-01-12T13:19:25Z

@prashanthr05

Do you have other names in mind?

A wordy choice would be in the direction of SO3xR3TaskMixed. where the prefix for Task will specify the task space and the suffix will specify the velocity representation ?
Or suppress the suffix, so that different choices are handled dynamically within the class (in the future), as done in iDynTree for some computations.

In 7a776b2 I added a check to make sure that the KinDynComputations object handles quantities in mixed representation. About changing the name of the class I would like to keep it as shorter as possible. If you agree I can improve the documentation by explaining the "theoretical" issue you spot.

Yes. I think it's ok.

72041a7 let me know if it's fine with you.

prashanthr05 · 2021-01-12T13:21:00Z

@prashanthr05

Do you have other names in mind?

A wordy choice would be in the direction of SO3xR3TaskMixed. where the prefix for Task will specify the task space and the suffix will specify the velocity representation ?
Or suppress the suffix, so that different choices are handled dynamically within the class (in the future), as done in iDynTree for some computations.

In 7a776b2 I added a check to make sure that the KinDynComputations object handles quantities in mixed representation. About changing the name of the class I would like to keep it as shorter as possible. If you agree I can improve the documentation by explaining the "theoretical" issue you spot.

Yes. I think it's ok.

72041a7 let me know if it's fine with you.

Great, thanks!!

GiulioRomualdi marked this pull request as draft December 23, 2020 18:48

GiulioRomualdi self-assigned this Jan 8, 2021

GiulioRomualdi changed the title ~~Implement the skeleton of the OptimalControlElement class~~ Implement the skeleton of the OptimalControlElement class and SE3Task Jan 8, 2021

GiulioRomualdi marked this pull request as ready for review January 8, 2021 16:21

GiulioRomualdi requested review from prashanthr05 and S-Dafarra January 8, 2021 16:21

traversaro mentioned this pull request Jan 8, 2021

Add LieGroupControllers recipe robotology/robotology-superbuild#574

Merged

prashanthr05 reviewed Jan 8, 2021

View reviewed changes

src/TSID/src/SE3Task.cpp Show resolved Hide resolved

GiulioRomualdi mentioned this pull request Jan 8, 2021

Add a test device for YARP based ROS Publisher #160

Merged

S-Dafarra reviewed Jan 10, 2021

View reviewed changes

GiulioRomualdi force-pushed the feature/optimal_control_element branch from d15f1cc to 0b72bae Compare January 12, 2021 10:09

S-Dafarra approved these changes Jan 12, 2021

View reviewed changes

GiulioRomualdi added 9 commits January 12, 2021 12:57

Implement the skeleton of the OptimalControlElement class

a8f6dfc

General refactor of VariablesHandler class

cb77ba6

Implement toManifRot function in ManifConversions.h

170651d

Implement OptimalControlElement::setKinDyn() and OptimalControlElemen…

fed3abc

…t::subA() methods

Implement SE3Task into TSID component

c6dcfa2

Add LieGroupControllers as dependency of TSID component

9cfb95a

Implement SE3TaskTest

12eac02

Improve the TSID CMakeLists.txt file

99d9a6b

Add LieGroupControllers to the Framework public dependencies

505681c

GiulioRomualdi added 7 commits January 12, 2021 12:57

Install lie-group-controllers in GitHub CI

4694d90

Update CHANGELOG.md

ba453ed

Use v() and w() methods to get the linear and angular velocity of the…

348ee16

… manif objects in TSID

- Rename OptimalControlElement into Task in TSID

36802eb

- Make Task class compliant with robotology/idyntree@64cbaf7

- Make SE3Task compatible with f7cd662

d3c3f9d

- Avoid to copy the jacobian matrix in SE3Task

Bump iDynTree version in GitHub CI

e2bb904

Check that MIXED_REPRESENTATION is used in the SE3Task

7a776b2

GiulioRomualdi force-pushed the feature/optimal_control_element branch from 1037d3e to 7a776b2 Compare January 12, 2021 11:58

prashanthr05 reviewed Jan 12, 2021

View reviewed changes

GiulioRomualdi and others added 3 commits January 12, 2021 14:03

Fix typo in SE3Task documentation

f488554

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>

Improve the documentation of the SE3Task

72041a7

Avoid to use magic numbers in SE3Task class

bfa5eaa

prashanthr05 approved these changes Jan 12, 2021

View reviewed changes

GiulioRomualdi merged commit 580bf41 into master Jan 12, 2021

GiulioRomualdi deleted the feature/optimal_control_element branch January 12, 2021 13:31

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implement the skeleton of the OptimalControlElement class and SE3Task #167

Implement the skeleton of the OptimalControlElement class and SE3Task #167

GiulioRomualdi commented Dec 23, 2020 •

edited

Loading

GiulioRomualdi commented Dec 23, 2020 •

edited

Loading

GiulioRomualdi commented Jan 8, 2021

traversaro commented Jan 8, 2021

traversaro commented Jan 8, 2021

DanielePucci commented Jan 8, 2021

prashanthr05 commented Jan 8, 2021 •

edited

Loading

S-Dafarra Jan 10, 2021

GiulioRomualdi Jan 12, 2021

GiulioRomualdi commented Jan 12, 2021

prashanthr05 commented Jan 12, 2021 •

edited

Loading

traversaro commented Jan 12, 2021

GiulioRomualdi commented Jan 12, 2021 •

edited

Loading

prashanthr05 left a comment

prashanthr05 Jan 12, 2021

GiulioRomualdi Jan 12, 2021

prashanthr05 commented Jan 12, 2021

GiulioRomualdi commented Jan 12, 2021

prashanthr05 commented Jan 12, 2021

Implement the skeleton of the OptimalControlElement class and SE3Task #167

Implement the skeleton of the OptimalControlElement class and SE3Task #167

Conversation

GiulioRomualdi commented Dec 23, 2020 • edited Loading

GiulioRomualdi commented Dec 23, 2020 • edited Loading

GiulioRomualdi commented Jan 8, 2021

traversaro commented Jan 8, 2021

traversaro commented Jan 8, 2021

DanielePucci commented Jan 8, 2021

prashanthr05 commented Jan 8, 2021 • edited Loading

S-Dafarra Jan 10, 2021

Choose a reason for hiding this comment

GiulioRomualdi Jan 12, 2021

Choose a reason for hiding this comment

GiulioRomualdi commented Jan 12, 2021

prashanthr05 commented Jan 12, 2021 • edited Loading

traversaro commented Jan 12, 2021

GiulioRomualdi commented Jan 12, 2021 • edited Loading

prashanthr05 left a comment

Choose a reason for hiding this comment

prashanthr05 Jan 12, 2021

Choose a reason for hiding this comment

GiulioRomualdi Jan 12, 2021

Choose a reason for hiding this comment

prashanthr05 commented Jan 12, 2021

GiulioRomualdi commented Jan 12, 2021

prashanthr05 commented Jan 12, 2021

GiulioRomualdi commented Dec 23, 2020 •

edited

Loading

GiulioRomualdi commented Dec 23, 2020 •

edited

Loading

prashanthr05 commented Jan 8, 2021 •

edited

Loading

prashanthr05 commented Jan 12, 2021 •

edited

Loading

GiulioRomualdi commented Jan 12, 2021 •

edited

Loading