Extending mimic tag for non-linear relationships

[BenArtes]

The mimic tag can be used to model robots with closed kinematic loops / parallel linkages, but is currently limited by only allowing a single joint, multiplier, and offset.

I think that its capabilities could be expanded to support arbitrary combinations or equations using muparser in a similar way to ros_canopen's position / velocity / effort conversions.

I imagine the syntax as:

<joint name="mimic_joint" type="prismatic">
  <origin xyz="0 0 1" rpy="0 0 3.1416"/>
  <parent link="link1"/>
  <child link="link2"/>
  <mimic>
    <joints_to_mimic>
      <joint_to_mimic>mjoint1</joint_to_mimic>
      <joint_to_mimic>mjoint2</joint_to_mimic>
      <joint_to_mimic>mjoint3</joint_to_mimic>
    </joints_to_mimic>
    <mimc_relationship>"0.1 * sin(j_0) + (1.5 * j_1) + j_2"</mimic_relationship>
  </mimic>
</joint>

Where the mimic_relationship would contain a muparser expression with variables j_i as declared in the <joints_to_mimic> tag. Ordering of joints could be an issue so this could also be implemented such that the joints are referred to by their joint name in the muparser expression.

One issue with this proposal would be gazebo simulation of transmission as touched upon here. While simulation would be a large issue, the issue is currently present without a solid solution and the current linear mimic joint is limiting the ability to represent more complex kinematic configurations.

[JaehyunShim]

@BenArtes

Does this really work in the Gazebo simulation? It works with the mimic tag on Rviz but it doesn't on Gazebo for me.

Ryan

[saikishor]

Yes it would be awesome to have mimic joins that support non-linear relationship, in most of the applications, it is nonlinear.
👍 for this feature

[BenArtes]

@BenArtes

Does this really work in the Gazebo simulation? It works with the mimic tag on Rviz but it doesn't on Gazebo for me.

Ryan

https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins

This repo has a gazebo plugin that you can use for mimic joints in gazebo. I had commented on this issue to see if I can get it merged into gazebo_ros_pkgs; no response.

[Prologue-Z]

May I ask if URDF has realized this function？

[torydebra]

May I ask if URDF has realized this function？

If you really need it, I had done something to add parsing non linear functions with numexpr in the joint_state_publisher, https://github.com/torydebra/joint_state_publisher/tree/kinetic-devel-rosee

where you can add non linear relationship like:

    <mimic joint="motor_thumb" nlFunPos="0.3378*x^4 + -1.179*x^3 + 1.103*x^2 + 0.7363*x + 0.01468"/>

[Prologue-Z]

May I ask if URDF has realized this function？

If you really need it, I had done something to add parsing non linear functions with numexpr in the joint_state_publisher, https://github.com/torydebra/joint_state_publisher/tree/kinetic-devel-rosee

where you can add non linear relationship like:

    <mimic joint="motor_thumb" nlFunPos="0.3378*x^4 + -1.179*x^3 + 1.103*x^2 + 0.7363*x + 0.01468"/>

Thank you very much. This has been very useful for my work.

Also, I would like to ask if using your modified JointStatePublisher can adapt to MoveIt Setup Assistant.Or it can only be used in the GUI demo.

[torydebra]

Sorry but I do not know if moveit setup assistant use the joint state publisher and how, by the way the addition I made is just a simple parsing and probably can be added anywhere (torydebra/joint_state_publisher@9767eab)

Just come in my mind that I also had forked the roboticsgroup_gazebo_plugins linked above to accept these non linear relationship as well:
https://github.com/torydebra/roboticsgroup_gazebo_plugins/tree/devel-rosee

[saikishor]

I believe it might work, because MoveIt uses the topic /joint_states to update the state of the robot. As this is solved by @torydebra's joint state publisher, I think it should be fine.

[Prologue-Z]

Sorry but I do not know if moveit setup assistant use the joint state publisher and how, by the way the addition I made is just a simple parsing and probably can be added anywhere (torydebra/joint_state_publisher@9767eab)

Just come in my mind that I also had forked the roboticsgroup_gazebo_plugins linked above to accept these non linear relationship as well: https://github.com/torydebra/roboticsgroup_gazebo_plugins/tree/devel-rosee

Well, if it can be used in gazebo, moveit should also be adapted. Thank you very much for your solution.

[Prologue-Z]

I believe it might work, because MoveIt uses the topic /joint_states to update the state of the robot. As this is solved by @torydebra's joint state publisher, I think it should be fine.

Updating the robot state is certainly possible, and this is already implemented in the repository of @torydebra .
My question is whether it isadapted by other plugins of moveit, such as kinematics solvers, motion planning, etc. Does the plugins take into account the added non-linear relationships.

[saikishor]

My question is whether it isadapted by other plugins of moveit, such as kinematics solvers, motion planning, etc. Does the plugins take into account the added non-linear relationships.

There are more limitations in that area. May be it's not possible if you have closed linkages.On the other hand, It is certainly possible to set passive joints in the MoveIt configuration. Try it out. Moreover, the MoveIt specific thing has nothing to do with the changes in this repository. In case it doesn't work, open an issue in MoveIt repository.

Good luck ☺️

[Prologue-Z]

My question is whether it isadapted by other plugins of moveit, such as kinematics solvers, motion planning, etc. Does the plugins take into account the added non-linear relationships.

There are more limitations in that area. May be it's not possible if you have closed linkages.On the other hand, It is certainly possible to set passive joints in the MoveIt configuration. Try it out. Moreover, the MoveIt specific thing has nothing to do with the changes in this repository. In case it doesn't work, open an issue in MoveIt repository.

Good luck ☺️

Thank you for your reply. I will give it a try.
This does not seem to be an easy task, as multiple plugins in the moveit need to be modified.

Also I'll do some more investigation into other tech routes, maybe moveit isn't the best fit for me. 🤣