mayataka / robotoc Goto Github PK

Thank you for such an excellent code. However, when I apply your code to the OCP of my snake robot, snake robot makes multiple contacts with the ground and Jacobian matrix is singular, then MJtJinv matrix can not be evaluated well. I try use pseudoinverse of JMinvJt to evaluate MJtJinv matrix, it also doesn't work because of the divergence. Could you give me some suggestions? Thanks.

related code gives as follows:
d.dbetamu().noalias() = - data.MJtJinv() * data.laf();

Hi Sotaro,

Thanks for open-sourcing such nice software!

When I am playing with your code, I bumped into a visualization error in C++. To be more exact, I would like to build the cpp files in examples/anymal with ENABLE_VIEWER to be 'ON' from the CMakeLists.txt to see the visualization effect. All examples (trot, crawl, pace, bounce, jump, run, jump_sto) can be successfully built. However, when executing, the following messages jumped out:

Failed in connecting to CORBA!!
omniORB: (0) 2022-08-24 15:51:55.371327: Error: the application attempted to invoke an operation on a nil reference.
terminate called after throwing an instance of 'CORBA::INV_OBJREF'
Aborted (core dumped)

After some investigation, I realized that it is at this line where the pinocchio::gepetto::Viewer cannot be initialized, which is pretty weird, because model_ and vmodel_ are all well-initialized according to my understanding. I did some search, but sadly found nothing valuable for debugging.

P.S. I have installed gepetto-viewer-corba and pinocchio-gepetto-viewer properly. The python visualization (from jupyter notebook) works well.

Any help are greatly appreciated! And thanks for your time in advance.

Best regards,
Shaohui Yang

The current install procedure of the Python binding is inconvenient.
It would be nice if we can install it through pip, .e.g, by pip install . --user.
An install script (setup.py or something like that) that satisfies

• Builds and installs C++ shared library as well as Python binding
• Enables CMake options (-DOPTIMIZE_FOR_NATIVE and -DBUILD_VIEWER)

is desired.

The new version Pinocchio 3.0 will be released and has many improvements from the past version.
We then have to update the followings:

• : PointContact
• : SurfaceContact
• : Robot
• : ContactDynamics
• : ImpulseDynamics

This is related to #52.

Surface contacts are necessary to model contacts in humanoid robots.
The following classes should be implemented:

• SurfaceContact (similar to PointContact)
• FrictionWrenchCone (similar to FrictionCone)
• ImpulseFrictionWrenchCone (similar to ImpulseFrictionCone)
• ContactWrenchCost (similar to ContactForceCost)

The real time iteration (RTI) scheme can improve the performance of MPC.
For this purpose, we need to decompose updateSolution() into the following two functions:

• preparationStepRTI(t, q, v)
• feedbackStepRTI(t, q, v)

Currently, only the filter line search is implemented.
To ensure global convergence, back-tracking line-search should be incorporated into the filter line-search method.
The following class should be implemented:

• LineSearchBackTracking

Hi I built the robotoc code but when I try to import it I get the following image

import robotoc
Traceback (most recent call last):
File "", line 1, in
File "/usr/local/lib/python3.10/site-packages/robotoc/init.py", line 1, in
from .robot import *
File "/usr/local/lib/python3.10/site-packages/robotoc/robot/init.py", line 1, in
from .contact_status import *
ModuleNotFoundError: No module named 'robotoc.robot.contact_status'
import math
import robotoc
Traceback (most recent call last):
File "", line 1, in
File "/usr/local/lib/python3.10/site-packages/robotoc/init.py", line 1, in
from .robot import *
File "/usr/local/lib/python3.10/site-packages/robotoc/robot/init.py", line 1, in
from .contact_status import *
ModuleNotFoundError: No module named 'robotoc.robot.contact_status'

Any idea what the issue is?

Hi, thanks for this very cool software. I am trying to use it on my custom URDFs, however I struggle to make the algorithm converge using the default parameters. I adapted the anymal_jump_sto.cpp example with my custom quadruped. However I cant make it converge. Do you have any tips for tuning? Thanks in advance!

The current implementation assumes that the contact surface is just the ground, i.e., its normal vector is z-axis and the height is zero.
The class

• ContactSurface

must be implemented, which contains information regarding the contact, .e.g, the normal vector and the height from the ground.
Accordingly, PointContact, Robot, or FrictionCone must also be modified.

Terminal constraints are often needed to ensure the stability of MPC.
Therefore, it is nice if we can design the terminal constraints independently from the constraints on the other time stages.
The following classes should be implemented:

• TerminalConstraintComponentBase
• TerminalJointPositionLowerLimit
• TerminalJointPositionUpperLimit
• TerminalBasePositionLowerLimit
• TerminalBasePositionUpperLimit
• TerminalJointVelocityLowerLimit
• TerminalJointVelocityUpperLimit
• TerminalBaseVelocityLowerLimit
• TerminalBaseVelocityUpperLimit

It is inconvenient to treat the ContactSequence through OCPSolver.
To expose ContactSequence, the following changes may be useful:

• OCPSolver has std::shared_ptr<ContactSequence> contact_sequence_ instead of ContactSequence contact_sequence_.

Dear @mayataka,

Very nice job. Your package seems really interesting, going in the common trend of developping real-time MPC for robots.
And I really appreciate that you rely on Pinocchio for computing the dynamics and their derivatives.

Among other things, I think the way you link your package to Pinocchio through CMake can now be updated.
You are no more obliged to rely on the pkg-config linkage procedure.

And again, if you need more featues from Pinocchio's side, do not hesitate to request them and/or to implement them inside Pinocchio. We do love external contributions: it makes our lives simpler.

Best,
Justin

In MPC for the legged robots, the problem structure of the OCP can change due to its discrete nature.
A little a bit complicated warm start strategy may be useful.
A class that represents the polynomially approximated continuous-time trajectory is useful for such cases, e.g., as

1. Solve an MPC problem
2. Construct the polynomially approximated continuous-time trajectory from the MPC solution
3. Proceed to the next sampling time, and then the problem structure of the OCP can change.
4. Guess the initial solution of the new MPC problem by the polynomially approximated continuous-time trajectory.

Robot has to have a function that transforms the local contact force (SplitSolution::f and ImpulseSplitSolution::f) into the contact force expressed in the world frame.
The transformation function for the Jacobian is also needed.
(Both of them are only implemented insides of FrictionCone and ImpulseFrictionCone)

The following Python bindings are not mandatory, but can be useful to check whether the formulation (settings of cost and constraints) are correct or not in Python:

• SplitOCP
• SplitKKTMatrix
• SplitKKTResidual
• StateEquation
• ContactDynamics
• ImpulseSplitOCP
• ImpulseSplitKKTMatrix
• ImpulseSplitKKTResidual
• ImpulseStateEquation
• ImpulseDynamics
• SwitchingConstraint
• SwitchingConstraintJacobian
• SwitchingConstraintResidual
• TerminalOCP

The name of this repo idocp is no longer appropriate, especially for new features to be implemented.
Therefore, we should immigrate to the new repo https://github.com/mayataka/roboc or change the name of this repo to robotoc.

Hello,

is it possible to define tight grasp contacts, i.e. fully constraining the movement of a frame like in the case of multi-arm robot manipulation? I have only seen the constraint placement for point or surface constraints.

Kind regards,
M.Dio

Hi. I found that this software requires CMake newer than 3.11 for the python binding.

As shown below, in building the python binding, CMake uses FetchContent module.

While this FetchContent module appears after version 3.11 (please see the 5th content of the CMake 3.11 Release Notes / Modules), the version requirement is still >=3.1.

I recommend you to set the minimum required version to 3.11 or higher

In my environment (Ubuntu 18.04 + default CMake (3.10.2)), a CMake error occurs (the error message is shown below), and updating CMake to the latest (3.22.2) resolves it.

$ cmake .. -DCMAKE_BUILD_TYPE=Release -DOPTIMIZE_FOR_NATIVE=ON
-- pinocchio FOUND. pinocchio at /opt/openrobots/lib/libpinocchio.so
-- boost_filesystem FOUND. boost_filesystem at /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
-- boost_serialization FOUND. boost_serialization at /usr/lib/x86_64-linux-gnu/libboost_serialization.so
-- boost_system FOUND. boost_system at /usr/lib/x86_64-linux-gnu/libboost_system.so
-- Boost version: 1.65.1
-- Found the following Boost libraries:
--   filesystem
--   serialization
--   system
-- hpp-fcl FOUND. hpp-fcl at /opt/openrobots/lib/libhpp-fcl.so
-- Boost version: 1.65.1
-- Found the following Boost libraries:
--   chrono
--   serialization
--   system
-- Default C++ standard: 201402
-- C++ standard sufficient: Minimal required 11, currently defined: 11
-- C++ standard sufficient: Minimal required 11, currently defined: 11
-- Found OpenMP_CXX: -fopenmp  
-- Found OpenMP: TRUE   
CMake Error at bindings/python/CMakeLists.txt:1 (include):
  include could not find load file:
    FetchContent

CMake Error at bindings/python/CMakeLists.txt:2 (FetchContent_GetProperties):
  Unknown CMake command "FetchContent_GetProperties".

-- Configuring incomplete, errors occurred!

References: CMake error "include could not find load file: FetchContent" - stackoverflow

Hello,

first of all, I'd like to say that what you're doing is amazing. I really like this project a simply reading through the code teaches me a lot.

I wanted to play around with the python bindings the other day, so I followed the installation procedure and installed everything according to the installation guide. However, when I try to import robotoc, the python interpreter complains that there's no robotoc.robot.contact_status module.

Do you know, why this could be?

Thanks.

The current cost function data is for all the cost function components.
It is convenient that users can define data to be only used with the cost function component as the ConstraintComponentData.

• : CostFunctionComponentData or something like that

Each cost component should have functions to return const reference to the reference and weight parameters.
For example, ConfigurationSpaceCost should have const Eigen::VectorXd& get_q_ref() const and const Eigen::VectorXd& get_q_weight() const.

It is nice if we can generate Python API documentation from C++ Doxygen.

Since the new STO algorithm is also valuable for the centroidal dynamics model, it is worth implementing such models.

will line contact be supported in the future?

Hi,
Can you guide me on how I can go about simulating a quadruped for which I have an array of joint angle values? Are there any examples that I can look at to better understand this?
Thanks

Does Robotoc support other dynamics? Such as SRBD?
I only see the Whole-body dynamics in the robotoc, what should i do if i want to use other dynamics formulation?

　（日本語で失礼いたします）私は2足歩行ロボットの動作の生成にこのライブラリを使っており，実際のロボットでの実験を目指しています．今のところ，私達のロボットのURDFにおいても，面接触において動作生成は上手く行っているように見えます．素晴らしいライブラリを公開してくださり，ありがとうございます．
現在，画像のようにロボットのつま先を使う動作，つまり面接触から線接触に移行するような動作を生成したいと考えており，画像に示すA,B,C,Dの4つの方法を検討しましたが上手くいきませんでした．

• Aについては，このissue(#124) と同じ問題で出来ませんでした．（kkt errorが発散してしまいます）
• Bについても面接触から点接触に移行する際に滞空期を経由しなければkkt errorが発散してしまいます(私の実装の問題かもしれません）
• C,Dについて，ロボットのつま先にあたる部分に，小さく質量が小さいダミーのつま先リンクをURDFに追加し，つま先接地を擬似的に行おうとしましたが，(B)と同様に出来ませんでした．

もし他につま先を使う方法があればお伺いしたいです．よろしくお願いいたします．

[English]
　I am using this library to generate motions for a biped robot and am aiming to experiment with it on a actual robot. So far, the motion generation seems to work well on our robot's URDF in surface contact. Thank you very much for releasing this excellent library.
Currently, we are trying to generate a motion that uses the robot's toes as shown in the image, i.e., a transition from surface contact to line contact, and we have considered four methods (A, B, C, and D) shown in the image, but they did not work.

• For A, I could not do it due to the same problem as in this issue (#124) . (kkt error diverges).
• For B, the kkt error diverges if the transition from surface contact to point contact does not go through a dwell period (this may be a problem with my implementation).
• For C and D, I tried to add a small, low-mass dummy toe link to the URDF at the toe of the robot to simulate toe grounding, but as in (B), this did not work.

If there is another way to use the toe, I would like to hear about it. Thank you in advance.

Each constraint component should have functions to return parameters.
For example, FrictionCone should have double get_mu() const.