NonLinear mimic joints

[torydebra]

I am working on providing a way to put a nonLinear relationship for mimic joints. With ROS standards, only mimicPos = multiplierfatherPos + offset and mimicVel = multiplierfatherVel is possible.
I added an attribute nlFunPos (bad name, proposal accepted) in the <mimic> tag like this:

<mimic joint="SFB1__SFP1_1" multiplier="1.2" offset="0.2" nlFunPos="x^2"/>

• If present, multiplier and offset are not considered (in truth, "ovewritten")
• we need an hardcoded variable, so when parsing the expression we know which 'x' stand for "father position". I don't think this is a big problem
• It has any sense to have the possibility to put also a "nlFunVel" and "nlFunEff"?

Things that ROSEE uses that need to be updated

• joint_state_publisher (fork here) which we use to propagate position of actuated joints to their mimic joints, when using rviz only. (ROSEE correctly send commands to only actuated joints, not mimic ones). Easy to do, python code

  • python eval is really bad, use other thing to compute the expression
  • we need to update documentation, especially indicating to cloning also our joint_state_publisher fork, and install numexpr.
    sudo apt install python-numexpr Also explain syntax for expression (like use ^ for power)
    October 2021 update : added this install in the rosendeffector package manager.

• Move robot when looking for collisions. (branch nonLinear_moveit_col) For pinches, we use moveit and setRandomPosition() function, to then check for collisions.
  With moveit, each time a joint pos is updated, mimic joints are updated also (with the linear standard params). There is no way to not call the updateMimic method, because of private function. So I simply solved first calling the setRandomPosition(), and then overwriting the nonLinearMimic joint position with the correct relation.
  I used muparser to compute the equation, it seems nice

  • update doc to download muparser NOTE It can be installed auto from rosdep, so maybe doc does not need to be updated, check it
    October 2021 update : prerelease tests shows that muparser is installed automagically thanks to package.xml and cmakelists.

• Gazebo Plugin (fork here)
  We should need to fork the mimic joint gazebo plugin, and modify also this to consider the nonlinear relation.

  • update doc to download the mimi joint gazebo plugin fork

Pull request for ROSEE here #59

[torydebra]

At the end I used numexpr. It should be safe to use (not like eval), any info about?
I also need to substitute ^ with ** for python code.

[lia2790]

Hi Dav, I d like to know why you are doing that in ordet to understand. thanks :)

[torydebra]

Hi Dav, I d like to know why you are doing that in ordet to understand. thanks :)

I am working on providing a way to put a nonLinear relationship for mimic joints

For example, I can say joint_mimic_pos = joint_father_pos ^ 2, simply indicating x^2 in the urdf, among the mimic tags. We will use this for HERI (not x^2 obviusly)

[lia2790]

perfect! I get it!

[torydebra]

Maybe I also have to remember to specifically put some tests for this part

[lia2790]

yes! I suggest you too to provide some usage examples and or some explanation of how to use it :)

[torydebra]

Updates:
With nonlinear functions, it has become important to enforce the limit of mimic joints; because approximating non linear function can cause weird joint movements if the father move more than expected by the function approximation.

• Joint state publisher does not do that, there was a PR Enforce mimic joint position limits ros/joint_state_publisher#13 but it has not been implemented. I copied the line in my fork to enforce these limits.

• Gazebo mimic plugin does not need to enforce limits; it seems that gazebo already consider limits and stops the mimic children (I tested with default engine which is ODE, other engines do this? It is a feature of gazebo or of the specific engine?)

• FindActions has been modified in the setToRandomPosition, now also there after setting the non linear position, we enforce the joint limits.

[torydebra]

I noticed that we can have problem if the father of a mimic joint is a mimic joint itself (i.e., there is a "grand-father" or more). This because we iterate the mimic map, and we do not know if the "children" will be updated BEFORE the "nephews", as it should be.
BTW, I think this is a really strange case, and we can keep this. Only remember to warn the user that the father of a non linear mimic joint cannot be a mimic NL joint itself (this is instead possible with the "standard" official mimic tag). This warn is also for the fork of the mimic plugins and joint state publishers

[torydebra]

I should add a check too see that all the "father" joints are effectively contained in the "activeJoint" list (ie not mimic, nor passive)