Connector mating simulation

[bsb808]

Objective is to be able to simulate and assess the process of mating and demating an electrical optical connection. As an example see these videos:

• teledyne fly-through, simulated
• ROPOS Ocean Observatory ODI connections
• OCEANEERING jumper install with two manipulator plugin, simulated
• BluHaptics demo, illustrates how force feedback can be used for teleoperation

The Teledyne ODI connectors serve as a surrogate product family to consider. Below is background information for considering how to model the interactions:

• The Gross Alignment Funnel / Enhanced Latch Indicator is a newer product with design elements to aid ROV installation.
• The Installation and operation manual for wet-mate hybrid connectors is a great overview of how to mate/demate with an ROV.
• The Electrical/Optical Flying Lead connector, Case Study - Connecting Subsea Networks

Some of the features that should be included to simulate the mating/demating

• Representation of parameterized mating/demating force.
• Mating/demating are represented visually as the location of the mating pair as well as a visual indicator (see "enhanced latching indicator" in the Installation and operation manual for wet-mate hybrid connectors
• ROS interface to indicate successful mate/demate
• Representation of mating/demating alignment requirements (rotational, angular (tilt) and linear).
• ROS interface for diagnostics including current alignment, location and forces.
• Performance evaluation of mating/demating attempts including metrics such as...
  • alignment, rotational, angular (tilt) and linear (see Figures 15-17 in the Installation and operation manual for wet-mate hybrid connectors
  • mating/demating forces and "smoothness"
  • off axis forces and torques
  • end effector location relative to connector grip (See Figure 2 in the Installation and operation manual for wet-mate hybrid connectors

[Youssef-Khaky]

Hi @caguero
I read the code from the DRCSim that you suggested here. I understood some of it but I didn't quite understand it fully.
if (!childJointsLinks.empty()) checks if the standpipe link has any children. I thought the relationship between links and joints is static, but from my reading it seems to be dynamic. As in, initially the hose isn't connected to the standpipe, but later on, they may be related to each other by a joint. How would that happen?
I stepped back a bit and decided to just start making a plugin that would simply detects the pose of an item then later on would detect if two items are aligned. I thought I can do this with a model plugin but because there is more than one item involved, I think I would have to use a world plugin. Am I going towards the right direction?
The problem that I am facing right now is that I am getting gzserver: symbol lookup error: /home/yk/uuv_ws/devel/lib/libLeadMating.so: undefined symbol: _ZN6gazebo10LeadMatingC1Ev during runtime I think I am missing something in my CMakelist.txt here, can you please check if I am doing it correctly?

[caguero]

Hi @Youssef-Khaky . There's a separate piece of code that was checking some alignment conditions between the hose and the valve to create a screw joint at runtime. See here.

[bsb808]

@Youssef-Khaky I wasn't able to find a quick fix, but I have a couple clues as to ways forward.

1. I notice in the error, undefined symbol: _ZN6gazebo10LeadMatingC1Ev, it seems to be prepending gazebo in the symbol. I couldn't find any where that the plugin was referred to as gazebo_LeadMating.
2. In 6b0c39b I added this tutorial example. I verified that this works correctly when you run

gzserver ~/uuv_ws/src/dave/gazebo_tutorials/worlds/hello.world --verbose

3. If you run

gzserver ~/uuv_ws/src/dave/uuv_dave/worlds/uuv_dave_ocean_waves_test.world --verbose

After some time this throws the same error (gzserver: symbol lookup error...)

I would recommend the following:

• I would reorganize your LeadMating example to all be on a single ROS package. I'd also give it a name that is consistent with the naming conventions for plugins. (I don't believe the naming convention has anything to do with it, but...)
• I would use a much simpler world SDF in your example. The ocean waves world is complicated, better to isolate the problem with a simple world similar to the hello.world.

If that doesn't work I'd probably start again, using the hello_uuv_world.cc source and the hello.world SDF as a working starting point to start building the plugin.

have anything to do with

[caguero]

That symbol seems associated with a constructor. Is it missing somehow?

$ c++filt _ZN6gazebo10LeadMatingC1Ev
gazebo::LeadMating::LeadMating

[bsb808]

@Youssef-Khaky were you able to sort this out?

[Youssef-Khaky]

Yes, thanks to your comments and Carlos' I was able to progress forward. The c++filt tool is pretty useful.
The issue has something to do with the constructor as Carlos mentioned.

Now I am able to print pose of an object, working on adding the detection of alignment.

[Youssef-Khaky]

Hi @caguero and @bsb808
In this branch if you run roslaunch gazebo_tutorials hello.launch, a world with two boxes will launch. After 5 second, a prismatic joint is added between these two links.
To check my work I do the following steps:

1. roslaunch gazebo_tutorials hello.launch

2. go into transition mode (T) to be able to move the objects

3. Move ones of the boxes along the X axis, Y axis and Z axis

Expectation:
When I drag one of the boxes along the X axis, it should slide and no forces should be transferred to the other box.
If I drag along Y or Z axis, this force should be transferred because the only degree of freedom is along the X axis.

What actually happens
Force is always transferred unless I move the object along the Z axis.

Here is a gif that shows what happens. (Sorry, its a bit too fast)

Also, on the menu on the left I expect a joint to appear under the "electrical socket" model, however, nothing appears.

[Youssef-Khaky]

Hi Carlos and Brian @caguero @bsb808
On the electrical_lead_mating branch there are two launch files in the uuv_mating pkg. One of them launches a world with two blocks already joined in the sdf files and the other one launches two blocks that links dynamically after the plugin is loaded.
In both the launches I have set one of the blocks to move along the axis of the prismatic joint, I expect a similar behavior from both of these launch files.
Dynamic launch:
roslaunch uuv_mating electrical_mating_dynamic.launch
Static launch:
roslaunch uuv_mating electrical_mating_static.launch

The static one works as expected, there is a joint that connects the two boxes and one of the boxes moves linearly along the x axis.

The dynamic however is not behaving the same way as the static, I would expect it to behave exactly the same way. It does move the box, however it moves it along the wrong axis (z-axis). Here is where I am setting the axis. I am not sure if I am using that function correctly. Regardless of the vector direction I use, the block still move along the z-axis..

[bsb808]

@Youssef-Khaky Nice work breaking down the problem and digging down to the root cause of the issues you are seeing.

One thought - I wonder what the 'scope' is of the programmatically generated joint. From the examples you included, I think the structure looks like this...

• In the static example, the joint is within the model.
• In the dynamic example, is the generated joint within one of the models or separate (as shown in red)
• I played around with trying to have a joint outside of both models, but connecting across the models. I haven't gotten that to work and it seems to require that the joint be encapsulated into a model containing at least one of the links.

The fact that the joint doesn't show up in the Gazebo GUI make it a bit tricky to verify where programmatically generated joint falls in the world/model heierarchy. Would you agree with the structure suggested on the right side of the image above - or is it something else?

I don't have a solution, but a few thoughts on how you might proceed...

• I would continue to try and get two comparable, apples-to-apples, solutions for the dynamic and static cases. As shown in the sketch, I think the hierarchy of the two examples is slightly different which may be a contributing factor.
• As a workaround, have you experimented with the joint pose when executing the Load method in the plugin? If we suspect that there may be a problem with setting the axis of the prismatic joint, it might be possible to effectively generate the functionality you need by changing the attitude of the joint.

At a higher-level, while this is frustrating, it might be good to consider how you might make progress on the feature while we let this shortcoming stand in the nearterm. Sometimes it is good to move on and leave the issue - coming back to it in a few days with fresh eyes.

A last thought is that you might consider stripping this down to a minimal example and creating a post on the Gazebo forums. The team is really responsive and if you do the work to allow them to quickly understand the issue they might be able to make a suggestion. The current example is too involved and you'd need to strip it down to the bear minimum to clearly illustrate the issue you are seeing.

I'm confident we'll sort this out - just may take some iteration.

[PenguineZhang]

@bsb808 @M1chaelM It looks like the issue is solved here. I have tried the simulation and am able to see the construction and destruction of the dynamic joint between the socket and the plug.

[M1chaelM]

@PenguineZhang Sounds good. Thanks. I will close it.