Add the possibility to create custom LinearTasks in python #480

GiulioRomualdi · 2021-12-05T10:17:16Z

If you are a python user and you want to create inverse kinematics (IK) or a task-based inverse
dynamics (TSID) task, you do need to write a C++ code. Indeed it is possible to write a class that inherits from TSIDLinearTask or IKLinearTask and passes it to tsid or ik solvers. The following snippet can be used as starting point to build a custom TSID task, a similar approach can be used to define a custom IK task

import bipedal_locomotion_framework.bindings as blf


class CustomTSIDTask(blf.tsid.TSIDLinearTask):
    """ CustomTSIDTask represents a custom task for the tsid solver.

    The class must define the following functions:
    __init__(self): the constructor
    size(self) -> Get the size of the task. (I.e the number of rows of the vector b)
    is_valid(self) -> Determines the validity of the objects retrieved with getA() and getB()
    type(self) -> Get the type of the task. Namely equality or inequality

    Furthemore you have to populate the following attrubutes defined in TSIDLinearTask:
    self._description:str -> contains the description of the task
    self._A:numpy.ndarray[float64[m, n]]) -> matrix that describes the task A * x = b
    self._b: numpy.ndarray[float64[m, 1]]) -> vector that describes the task A * x = b

    Depending on the content of the task you may need to reimplement the following methods
    initialize(self, param_handler: blf.parameters_handler.IParameterHandler) -> usefull function to set parameters to the task
    set_variables_handler(self, variables_handler: blf.system.VariablesHandler) -> usefull function to set the variables
    update(self) -> update the content of the task
    """

    def __init__(self):

        blf.tsid.TSIDLinearTask.__init__(self)  # Without this, a TypeError is raised.

        # set the description of the task
        self._description = "Custom task"

    def size(self):
        # this is the number of rows of the vector b (in this example is equal to 1)
        return 1

    def is_valid(self):
        return True

    def type(self):
        # the types are defined in blf.tsid.TSIDLinearTask.Type
        return blf.tsid.TSIDLinearTask.Type.equality

Then you can use your custom task as follows

import bipedal_locomotion_framework.bindings as blf

# Instantiate the task
custom_task = CustomTSIDTask()

# Initialize your task
custom_task.initialize(param_handler)

# Create the solver
solver = blf.tsid.QPTSID()
priority = 0
solver.add_task(custom_task, 'custom_task', priority)

GiulioRomualdi · 2021-12-06T13:04:58Z

On conda macos there is this nice failure

../bindings/python/TSID/tests/test_TSID.py Fatal Python error: Illegal instruction

Current thread 0x000000011c3e5dc0 (most recent call first):
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/python.py", line 183 in pytest_pyfunc_call
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_callers.py", line 39 in _multicall
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_manager.py", line 80 in _hookexec
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_hooks.py", line 265 in __call__
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/python.py", line 1641 in runtest
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/runner.py", line 162 in pytest_runtest_call
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_callers.py", line 39 in _multicall
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_manager.py", line 80 in _hookexec
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_hooks.py", line 265 in __call__
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/runner.py", line 255 in <lambda>
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/runner.py", line 311 in from_call
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/runner.py", line 254 in call_runtest_hook
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/runner.py", line 215 in call_and_report
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/runner.py", line 126 in runtestprotocol
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/runner.py", line 109 in pytest_runtest_protocol
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_callers.py", line 39 in _multicall
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_manager.py", line 80 in _hookexec
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_hooks.py", line 265 in __call__
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/main.py", line 348 in pytest_runtestloop
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_callers.py", line 39 in _multicall
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_manager.py", line 80 in _hookexec
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_hooks.py", line 265 in __call__
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/main.py", line 323 in _main
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/main.py", line 269 in wrap_session
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/main.py", line 316 in pytest_cmdline_main
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_callers.py", line 39 in _multicall
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_manager.py", line 80 in _hookexec
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pluggy/_hooks.py", line 265 in __call__
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/config/__init__.py", line 162 in main
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/_pytest/config/__init__.py", line 185 in console_main
  File "/usr/local/miniconda/envs/test/lib/python3.9/site-packages/pytest/__main__.py", line 5 in <module>
  File "/usr/local/miniconda/envs/test/lib/python3.9/runpy.py", line 87 in _run_code
  File "/usr/local/miniconda/envs/test/lib/python3.9/runpy.py", line 197 in _run_module_as_main

traversaro · 2021-12-06T13:40:46Z

On conda macos there is this nice failure

Probably related to conda/conda#9678 . Not sure what a quick workaround is.

GiulioRomualdi · 2021-12-06T13:45:47Z

Probably related to conda/conda#9678 . Not sure what a quick workaround is.

Why is this happening now?

DanielePucci · 2021-12-06T13:48:26Z

CC @paolo-viceconte @diegoferigo @CarlottaSartore

traversaro · 2021-12-06T13:56:27Z

Probably related to conda/conda#9678 . Not sure what a quick workaround is.

Why is this happening now?

It is possible that the other tests were not using numpy or they were not using some specific numpy function that is instead used by this test?

GiulioRomualdi · 2021-12-06T14:03:15Z

It is possible that the other tests were not using numpy or they were not using some specific numpy function that is instead used by this test?

well np.zeros(), slicing and np.ones() were not used before

temp = np.zeros((10, variables_handler.get_number_of_variables()))
temp[:, variable.offset] = np.ones(10)
self._b = np.ones(10)

traversaro · 2021-12-06T14:04:59Z

It is possible that the other tests were not using numpy or they were not using some specific numpy function that is instead used by this test?

well np.zeros(), slicing and np.ones() were not used before
temp = np.zeros((10, variables_handler.get_number_of_variables()))
temp[:, variable.offset] = np.ones(10)
self._b = np.ones(10)

I suspect this is what is triggering it. Note that I guess this is a problem that will not occur on real machines, just something that is occuring with virtual machine that do not support AVX instructions. For the time being you can just skip those tests on macOS.

paolo-viceconte

Looks good to me.

paolo-viceconte · 2021-12-06T14:22:48Z

docs/pages/python-additional-info.md

+
+### Create a custom TSID or IK task directly in Python
+If you are a `python` user and you want create a inverse kinematics (IK) or a task based inverse
+dynamics (TSID) tasks, you do need to write a `C++` code. Indeed it is possible to write a class that inherits from `TSIDLinearTask` or `IKLinearTask` and pass it to `tsid` or `ik` solvers. The following snippet can be used as starting point to build a custom TSID task, a similar approach can be used to define a custom IK task


Should this be "you don't need to write a C++ code"?

traversaro · 2021-12-06T16:11:31Z

In #481 I tried to switch the blas implementation to netlib and blis, but the issue is still there, so I guess it is not a problem of blas implementation.

GiulioRomualdi · 2021-12-07T10:42:52Z

Bingo this is the problem:

I tested with https://github.com/GiulioRomualdi/blf-fastmac

blf.tsid.TSIDLinearTask.__init__(self)

however, I don't know how to fix it since that line is required as explained in https://pybind11.readthedocs.io/en/stable/advanced/classes.html#overriding-virtual-functions-in-python

GiulioRomualdi · 2021-12-07T11:23:01Z

(robotologyenv) Mac-1638874300540:build runner$ python3 test.py
[2021-12-07 11:21:46.641] [thread: 106743] [blf] [info] [QPTSID::initialize] 'verbosity' not found. T
he following parameter will be used 'false'.
Illegal instruction: 4

https://stackoverflow.com/questions/14268887/what-is-the-illegal-instruction-4-error-and-why-does-mmacosx-version-min-10

However

a = blf.tsid.TSIDLinearTask()

run without any problem

traversaro · 2021-12-07T11:31:06Z

Are you sure that python3 is the one provided in the conda environment? What is the output of which python3?

GiulioRomualdi · 2021-12-07T11:32:07Z

Are you sure that python3 is the one provided in the conda environment? What is the output of which python3?

/Users/runner/miniforge3/envs/robotologyenv/bin/python3

traversaro · 2021-12-07T15:34:52Z

In the recent past we had an "Illegal instruction" error for what in the end was just wrong memory access (see robotology/robometry#110). Did you tried to run python that runs this test on Linux under valgrind?

traversaro · 2021-12-07T16:05:01Z

However, at this point for me we can disable the conda/macOS test and proceed.

traversaro · 2021-12-08T16:00:17Z

How did you found the problem (or at least a problem) of missing virtual destrutor?

GiulioRomualdi · 2021-12-08T16:05:42Z

I've just tried to fix the warnings

GiulioRomualdi · 2021-12-08T16:07:34Z

That one was actually the problem, the test runs on fastmac

…anager

Add the possibility to create custom LinearTasks in python

70fa269

github-actions bot added 📖 documentation 🐍 bindings labels Dec 5, 2021

GiulioRomualdi force-pushed the bindings/custom_tasks branch 2 times, most recently from eec2308 to 7ed7cc1 Compare December 5, 2021 10:37

GiulioRomualdi self-assigned this Dec 5, 2021

GiulioRomualdi marked this pull request as ready for review December 5, 2021 14:00

GiulioRomualdi force-pushed the bindings/custom_tasks branch 3 times, most recently from 488a9aa to a46ef6e Compare December 6, 2021 09:37

GiulioRomualdi requested review from paolo-viceconte and traversaro December 6, 2021 09:37

GiulioRomualdi force-pushed the bindings/custom_tasks branch from a46ef6e to f90491e Compare December 6, 2021 09:44

traversaro mentioned this pull request Dec 6, 2021

[DO NOT MERGE] Add print of macOS conda environment #481

Closed

paolo-viceconte approved these changes Dec 6, 2021

View reviewed changes

traversaro approved these changes Dec 7, 2021

View reviewed changes

github-actions bot added the 🔨 component: System label Dec 8, 2021

GiulioRomualdi force-pushed the bindings/custom_tasks branch from a2fd93b to 9a8ff4a Compare December 8, 2021 15:47

github-actions bot added 🔨 component: IK 🔨 component: TSID labels Dec 8, 2021

GiulioRomualdi added 9 commits December 8, 2021 17:17

Update the python documentation

df6cefa

Update the CHANGELOG

eff2b62

Add the IK custom task implementation in the python bindings

fe4e8a1

Add the TSID custom task implementation in the python bindings

435d874

Avoid to use slicing the IK and TSID python tests

b8eaec8

Define missing virtual destructors in LinearTask and ITaskControllerM…

3e8e242

…anager

Add virtual destructors in TSIDLinearTask and IKLinearTask classes

f015c80

Add virtual destructors in Sink and Source classes

01fa364

Update the CHANGELOG

8ddeb0d

GiulioRomualdi force-pushed the bindings/custom_tasks branch from 2dade84 to 8ddeb0d Compare December 8, 2021 16:17

Merge branch 'master' into bindings/custom_tasks

24013c9

GiulioRomualdi merged commit 62fdd2f into master Dec 8, 2021

GiulioRomualdi deleted the bindings/custom_tasks branch December 8, 2021 18:04

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add the possibility to create custom LinearTasks in python #480

Add the possibility to create custom LinearTasks in python #480

GiulioRomualdi commented Dec 5, 2021 •

edited

Loading

GiulioRomualdi commented Dec 6, 2021

traversaro commented Dec 6, 2021

GiulioRomualdi commented Dec 6, 2021

DanielePucci commented Dec 6, 2021

traversaro commented Dec 6, 2021

GiulioRomualdi commented Dec 6, 2021

traversaro commented Dec 6, 2021

paolo-viceconte left a comment

paolo-viceconte Dec 6, 2021

traversaro commented Dec 6, 2021

GiulioRomualdi commented Dec 7, 2021

GiulioRomualdi commented Dec 7, 2021 •

edited

Loading

traversaro commented Dec 7, 2021

GiulioRomualdi commented Dec 7, 2021

traversaro commented Dec 7, 2021

traversaro commented Dec 7, 2021

traversaro commented Dec 8, 2021 •

edited

Loading

GiulioRomualdi commented Dec 8, 2021

GiulioRomualdi commented Dec 8, 2021

Add the possibility to create custom LinearTasks in python #480

Add the possibility to create custom LinearTasks in python #480

Conversation

GiulioRomualdi commented Dec 5, 2021 • edited Loading

GiulioRomualdi commented Dec 6, 2021

traversaro commented Dec 6, 2021

GiulioRomualdi commented Dec 6, 2021

DanielePucci commented Dec 6, 2021

traversaro commented Dec 6, 2021

GiulioRomualdi commented Dec 6, 2021

traversaro commented Dec 6, 2021

paolo-viceconte left a comment

Choose a reason for hiding this comment

paolo-viceconte Dec 6, 2021

Choose a reason for hiding this comment

traversaro commented Dec 6, 2021

GiulioRomualdi commented Dec 7, 2021

GiulioRomualdi commented Dec 7, 2021 • edited Loading

traversaro commented Dec 7, 2021

GiulioRomualdi commented Dec 7, 2021

traversaro commented Dec 7, 2021

traversaro commented Dec 7, 2021

traversaro commented Dec 8, 2021 • edited Loading

GiulioRomualdi commented Dec 8, 2021

GiulioRomualdi commented Dec 8, 2021

GiulioRomualdi commented Dec 5, 2021 •

edited

Loading

GiulioRomualdi commented Dec 7, 2021 •

edited

Loading

traversaro commented Dec 8, 2021 •

edited

Loading