Gazebo Customized Plugin: Read contact positions that contains both a variable sized element and an array of sub-buses

Hello,

I have tired to use the customized Gazebo plugin with Simulink and Gazebo in the virtual machine (Ubuntu).

The communication works between Gazebo in the virtual machine and Simulink.

However, when I try to read the contact-positions information (multiple contacts), the error occurs as shown below.

 Simulation 1
04:28 AM Elapsed: 3 sec
 Diagnostics 1
01:52 AM Elapsed: 20 sec
 Simulation 1
01:52 AM Elapsed: 2 sec
 Simulation 1
02:30 AM Elapsed: 3 sec
 Simulation 1
02:31 AM Elapsed: 3 sec
The bus object 'Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts' associated with 
the block 'gazeboCosimControl_ys_v3/Gazebo Robot/Bus Selector13' contains both a variable sized element 
and an array of sub-buses. Arrays of buses cannot be used in conjunction with bus objects that 
contain variable sized elements.
Component: Simulink | Category: Block error

In addition, the contact-positions have the following variable sized elements based on Gazebo simulation. I mean multiple contacts can include to this information as shown below.

To solve this issue, I have tried to use several blocks related to 'bus' such as, bus to vector, to virtual bus, bus selector, etc, but I couldn't solve it.

Do you have any advice to address this issue?

Thank you so much in advance,

Yun

Answer 1

[Gaurav Bhosale]

Hi Yunsik,

I created Simulink model for Contacts message and able to subscribe Contacts message data. To enable this, I did following changes.

1] The Contacts message is repeatative so you have to change few messages as follows.

You should change Dimensions as per you message length. Also, don't forget to change DimesnionMode to 'Variable' ( Highlighted in red ). The contacts message keep changing its size so it should be variable. Make sure, you do all following changes.

Contact

JointWrench

Time

Vector3d

2] The Simulink model look as follows,

Basically, you we get values in array form. e.g. If Contacts message contains 14 Contact messages/details, which you see as repeated fields in protobuf but in Simulink, these messages are in array form as shown above.

In above figure, <x> contains Wrench.force.x in array form, in which, each index define force.x value for each body.

You can separate these values in MATLAB function.

Thanks.

[Yunsik Jung]

Hi, Gaurav!

Great! I solved the issue with your comments! Thank you so much for the detailed comments.

I understood what you mentioned that I need to create MATLAB function to separate the x values in array form.

I have one more question, how did you convert body ids of the body_1_name to real names as shown below?

Can you let me know how to write the MATLAB function to convert it?

Thank you very much, again!

Yun

Answer 2

[Gaurav Bhosale]

Hi Yunsik,

You can directly get Position.x at Bus Selector 13 by selection/expanding Position in Bus Selector.

Any specific reason to have two bus selector?

[Yunsik Jung]

Hi, Gaurav.

Thank you for the quick response!

I have tried to use one bus selector to get Position.X as shown below.

But, I got the following error.

search
 
 Simulation 1
01:52 AM Elapsed: 2 sec
 Simulation 1
02:30 AM Elapsed: 3 sec
 Simulation 1
02:31 AM Elapsed: 3 sec
 Model Save (gazeboCosimControl_ys_v3) 
02:33 AM Elapsed: 0.156 sec
 Simulation 1
01:13 AM Elapsed: 3 sec
The bus object 'Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts' associated with the block 
'gazeboCosimControl_ys_v3/Gazebo Robot/Bus Selector13' contains both a variable sized element and 
an array of sub-buses. Arrays of buses cannot be used in conjunction with bus objects that contain 
variable sized elements.
Component: Simulink | Category: Block error

I guess the position information contains variable sized elements like having multiple contact points.

(That's why I have tried to use several kinds of simulink blocks related to 'bus')

In addition, it is not clear how to deal with multiple contact positions for me.

For instance, the number of contacts should be changed, such as 0, 1, 2, or more during the simulation.

Would you let me know how I can deal with it?

Really appreciate your support!

Thanks,

Yun

[Yunsik Jung]

Hi, Gaurav.

In addition to the previous ones, I have attached the information of contacts.

My Gazebo model has 3 fingered robotic hand and the red box.

I publiched the 3 contacts of the 3 fingertips (only one contact for each) and the ultiple contacts of the red box. So, The contacts of 3 fignertips have only one contact with the red box (when grasp it) and the contact of the red box has multiple contacts (with the fingertips and the ground) as shown below.

1. Contact between the finger_tip_3 and the redbox (only one contact point)

contact {
  collision1: "redBox::link::collision_redbox"
  collision2: "m1n6s300::m1n6s300_link_finger_tip_3::m1n6s300_link_finger_tip_3_collision"
  position {
    x: -0.056632018674152892
    y: -0.42469795648780345
    z: 0.024875600778013502
  }
  normal {
    x: 0.71547922093255889
    y: 0.69863401284489779
    z: -2.2582601418741355e-05
  }
  depth: 2.6248854961830048e-06
  wrench {
    body_1_name: "redBox::link::collision_redbox"
    body_1_id: 87
    body_2_name: "m1n6s300::m1n6s300_link_finger_tip_3::m1n6s300_link_finger_tip_3_collision"
    body_2_id: 80
    body_1_wrench {
      force {
        x: 0.014795303048279591
        y: 0.014795332132892173
        z: 0.0053169421890235311
      }
      torque {
        x: -0.00031781571041421079
        y: 0.00052897689231761564
        z: -0.00057880936339234552
      }
    }
    body_2_wrench {
      force {
        x: -0.016402243505716788
        y: 0.006562379650485198
        z: 0.012408638150760567
      }
      torque {
        x: -0.00013880548958348924
        y: -0.00037148702808840225
        z: 1.6748865645205459e-05
      }
    }
  }
  time {
    sec: 4
    nsec: 735000000
  }
  world: "default"
}

2. Contact between the redbox and the ground (4 contacts which are 4 corners' contact points of the redbox with the ground)

contact {
  collision1: "redBox::link::collision_redbox"
  collision2: "ground_plane::link::collision"
  position {
    x: 0.02939663887442602
    y: -0.41976679972114284
    z: -0.0010024591704583879
  }
  position {
    x: -0.016014571965647303
    y: -0.37326065034375422
    z: -0.0010021036116875867
  }
  position {
    x: -0.017109510486190312
    y: -0.46517801055606117
    z: -0.0010009913013661698
  }
  position {
    x: -0.062520721326263629
    y: -0.4186718611786725
    z: -0.0010006357425953684
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  normal {
    x: 0
    y: 0
    z: 1
  }
  depth: 0.0010024591704583877
  depth: 0.0010021036116875865
  depth: 0.0010009913013661696
  depth: 0.0010006357425953686
  wrench {
    body_1_name: "redBox::link::collision_redbox"
    body_1_id: 87
    body_2_name: "ground_plane::link::collision"
    body_2_id: 6
    body_1_wrench {
      force {
        x: -0.0099168260074715825
        y: -0.15414536541210239
        z: 0.0051114027837605869
      }
      torque {
        x: -0.0048436041801116526
        y: 0.00013869861625245231
        z: -0.0046874274350454607
      }
    }
    body_2_wrench {
      force {
        x: -0.11546865061100736
        y: -0.10194895975105885
        z: 0.097101250287424251
      }
      torque {
        x: 5.5136706719713968e-05
        y: -0.0026114362799532307
        z: -0.00256949188362319
      }
    }
  }
  wrench {
    body_1_name: "redBox::link::collision_redbox"
    body_1_id: 87
    body_2_name: "ground_plane::link::collision"
    body_2_id: 6
    body_1_wrench {
      force {
        x: -0.014632258645478114
        y: -0.1611667945219985
        z: -0.00478341549577067
      }
      torque {
        x: 0.0050824604845255868
        y: -0.00029058800275445535
        z: -0.0047623725773526075
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  wrench {
    body_1_name: "redBox::link::collision_redbox"
    body_1_id: 87
    body_2_name: "ground_plane::link::collision"
    body_2_id: 6
    body_1_wrench {
      force {
        x: -0.01805132048700674
        y: -0.11857466637778853
        z: -0.010768050052411758
      }
      torque {
        x: -0.00420363806484793
        y: 0.000246409935476471
        z: 0.00444034452002559
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  wrench {
    body_1_name: "redBox::link::collision_redbox"
    body_1_id: 87
    body_2_name: "ground_plane::link::collision"
    body_2_id: 6
    body_1_wrench {
      force {
        x: -0.025064265061970528
        y: -0.1259532718739578
        z: 0.0056684156029513828
      }
      torque {
        x: 0.004277704992998249
        y: -0.00062372288349323588
        z: 0.0049080699140838045
      }
    }
    body_2_wrench {
      force {
        x: 0
        y: 0
        z: 0
      }
      torque {
        x: 0
        y: 0
        z: 0
      }
    }
  }
  time {
    sec: 4
    nsec: 735000000
  }
  world: "default"
}
time {
  sec: 4
  nsec: 736000000
}

Even I try to get the one contact information (between the finger_tip_3 and the redbox), I got the same error message like the above.

Would you let me know if I have made any misunderstood or mistaken setups?

Thank you for your support!

Best,

Yun

Answer 3

[Gaurav Bhosale]

Hi Yunsik,

Add 'MATLAB function' block in Simulink and add following code lines in the function

function y = fcn(u)

y = string(char(u'));

Thanks

[Yunsik Jung]

Hi, Gaurav.

Thank you very much for your help!

However, when I tried to use the MATLAB function to convert it, I got an error.

And I have submitted another question to solve this error as:

https://www.mathworks.com/matlabcentral/answers/869593-gazebo-customized-plugin-read-the-body-names-of-contacts

Thanks,

Yun