Mimic Joints: Send joint positions when commanding only the actuated joints

[torydebra]

When moving a joint, also all the other joints that mimic this one should move.

The joint_state_publisher deal with it sendind right values to all mimicking joints (I think), and with schunk hand we use their plugins that deal with this problem (I think), so I do not notice this problem until now.

Actually when a command arrives, UniversalRosEndEffector moves only the actuated joints and not the ones that mimic the actuated.

[liesrock]

Yes the above is right: currently UniversaRosEndEffector does not support mimic joints: we can work on it in the future.

[torydebra]

I solved the problem when using gazebo, using an external gazebo plugin (as explained in the readme of rosee_gazebo_plugins ).

We still need a method to solve it if we use only rviz

[torydebra]

I think that orocos kdl parser can not deal with mimic joints, thus we can not parse them with it. I found no info about this on Internet and neither on kdl api doc.

MoveIt (which uses kdl) probably developed some additional code to parse them, as I suppose from https://github.com/ros-planning/moveit/pull/1517

@liesrock, are you aware of someone who uses orocos kdl for parsing urdf with mimic tags?

[liesrock]

no unfortunately I am not aware of anyone using orocos_kdl for this: the feature is currently missing as you mentioned.

Can we extend it or use another parser only for mimic joints ?

[torydebra]

As you know I used moveit functions for parsing. In Parser, we could parse urdf also with moveit, then, for each mimic joint, delete it from the finger_jointmap and __urdf_jointmap, and decrease the jointsnum counter.

But at this point is not easier to parse the model directly with moveit structures? So we do not have to "loop" twice.

However, these problems are related to the issue about passive joints ( #16 )

[torydebra]

I think we have two issues here.

• The first is about the control: we have to move all joint that mimic the "father". This is the problem depicted in the first message. I think that it is easy solvable: we have to pass through a joint_state_publisher node, that will move all the mimic joint for us, so our universalrosee must publish only state of actuated joints. If @liesrock agrees, I will proceed with this fix.

• Second problem is about parsing, stated in the last messages, that has to be thinked a bit more

Sorry for the mess: should we create another issue or we can consider both here?

EDIT second problem moved to #30

[liesrock]

ok, thanks for clearing this out: yes please open an issue about the parsing part, which I should also think more about to find/discuss a proper solution.

We keep here the control problem: can you elaborate more on your solution? Let's say UniversalROSEE receive a Pinch action request, it will set the _qref vector and then in the main looop we have:

_hal->setPositionReference(_qref);

How to use the joint state publisher?

[torydebra]

It is simple, we only need to publish on a different topic the joint position.

At the moment, when using only rviz (thus no gazebo) we send position in /ros_end_effector/joint_states. From here robot_state_publisher take values and tranform into tranformation matrix for rviz, like below.

Instead, we should publish joint position on a different topic, where _/input_joint_statepublisher will take values (I will change the node name). The joint_state_publisher is aware of all the mimic, so take as input only the actuated joints, and outputs position of all joints, mimic included. Then there is the usual robot_state_publisher for rviz.

With this method we do not need to change our node for mimic. This would be wrong: a real robot "knows" which are the mimic joints and we should send to it only command for actuated joint

This scheme is similar when using gazebo: in that case it is the gazebo_ros_plugin that take as input actuated joint pos and output all joint positions "converted" in gazebo data structures.

[liesrock]

I agree with you, it is a simple mechanism, but it only works with the Dummy mode (joint state publisher based): once we will define better the working principle of the HAL for real-hardware I will come back to this with further details.

[torydebra]

But why a real hardware needs to receive commands also for "mimic" joint? It is the interal mechanism of actuators that move the dipendent joints when the actuated ones move...

[liesrock]

This is right, I was thinking more from a software abstraction point of view related with the HAL: but the proposed solution works, relying on ROS, both on kinematic simulation (robot_state_publisher) and on dynamic simulation (ad-hoc gazebo plugin).

[torydebra]

Done in 02a521c081d45c46c74550c2e16426feef51de83 Without gazebo With gazebo