Gazebo subscribe block error in Simulink

Hi, all. I have created Gazebo plugin including several message types, such as contacts, pose, link, joint, and etc. using gazebogenmsg to communicate Simulink and Gazebo.

The plugin works appropriately, and I can read forces and torques using the Gazebo subscribe block in Simulink.

However, when I try to read contacts, then there is an error as shown below.

[Error in Simulink]

An error occurred while running the simulation and the simulation was terminated

Caused by:

• For 'Output Port 2' of 'gazeboCosimControl_ys_v3/Gazebo Robot/Gazebo Subscribe con_info_redbox/SourceBlock', the method 'outputImpl' of the System object 'robotics.slgazebo.internal.GazeboSubscribeBlk' returns a bus type whose field Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts.contact.wrench.body_1_wrench.force.x size does not match the field Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts.contact.wrench.body_1_wrench.force.x size defined in the method 'getOutputDataTypeImpl'.

I have no idea how to solve this problem. Would you let me know if you have any ideas to solve it?

Thanks!

In addition, I defined the contacts to the Gazebo *.world file as shown below. And I have checked these topics are published in Gazebo.

        <collision name='m1n6s300_link_finger_tip_1_collision'>
          <pose frame=''>0 0 0 0 -0 0</pose>
          <geometry>
            <mesh>
              <scale>1 1 1</scale>
              <uri>/home/user/catkin_ws/src/kinova-ros/kinova_description/meshes/finger_distal.dae</uri>
            </mesh>
          </geometry>
          <max_contacts>100</max_contacts>
          <surface>
            <contact>
              <ode/>
            </contact>
            <bounce/>
            <friction>
              <torsional>
                <ode/>
              </torsional>
              <ode/>
            </friction>
          </surface>
        </collision>
        <visual name='m1n6s300_link_finger_tip_1_visual'>
          <pose frame=''>0 0 0 0 -0 0</pose>
          <geometry>
            <mesh>
              <scale>1 1 1</scale>
              <uri>/home/user/catkin_ws/src/kinova-ros/kinova_description/meshes/finger_distal.dae</uri>
            </mesh>
          </geometry>
        </visual>
        <self_collide>0</self_collide>
        <enable_wind>0</enable_wind>
        <kinematic>0</kinematic>
	<sensor name = 'con_info_finger_tip_1' type = 'contact'>
	  <contact>
	    <collision>m1n6s300_link_finger_tip_1_collision</collision>
	  </contact>
	</sensor>

Answer 1

[Gaurav Bhosale]

Hi Yunsik,

The Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts.contact.wrench.body_1_wrench.force.x is of variable length. The received message data is excceding dimension limit ( default is 128), so you need to increase dimension limit from Bus Editor.

Check Customize Bus Objects for Simulation from https://www.mathworks.com/help/robotics/ug/control-manipulator-robot-with-co-simulation.html

Try to change bus dimension and let me know if you face any issue in doing that.

[Yunsik Jung]

Hi, Gaurav.

Thank you so much for the quick response!

I really appreciate it, since I have suffered from this issue for a couple of days.

I have another question on this issue. How to determine the dimension limit? Even I increased dimensions, there are still errors.

I would like to ask how to set the bus editor for the following topics from Gazebo.

1) collision1: "redBox::link::collision_redbox"

2) collision2: "ground_plane::link::collision"

3) 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.0040215821663263434

y: -0.0035342300169850984

z: 0.00612709962277502

}

torque {

x: 8.4268671424788638e-05

y: -6.842890014103444e-05

z: 1.5792249417875155e-05

}

}

Current setting of the bus editor for them,

collision1, 2: Model=built in, Data type=unit8, Dimensions=128, DimensionModel=Variable

Wrench: Model=bus object, Dimensions=1, DimensionsMode=Fixed

Could you let me know how to modify them?

Thank you very much!

Yunsik

[Gaurav Bhosale]

Hi Yunsik,

You can check message sizes from Gazebo but that is not straightforward. Currently, there is no direct way to determine bus size is MATLAB.

You can keep larger dimension size such that you never exceed bus dimension limit.

Further, there are several fields in Contacts message which are variable size. So, I recommend you to check field name mentioned in the error and try to increase dimension size of that field.

I used Contacts message below. The Contacts message is repeatative Contact message. So, if you are using just Contact message then ignore 3rd modification from below.

For Contacts message, I modify bus sizes as follows,

1] I modifed bus sizes for Vector3d message as,

2] I modified bus sizes for bodyname, body id from JointWrench message as,

3] I modified bus sizes for Contact message as follows,

Basically, these sizes can vary with input message. Also, there are many messages which are repeatative in Contacts message, so you need to tune those fields as per your Gazebo message.

Try to modify above fields and let me know if you still face same issue.

Thanks

Answer 2

[Yunsik Jung]

Dear, Gaurav.

Really appreciate giving the detailed comments on the issue!

I carefully followed your descrionts in the bus editor.

But, I am still have some issues.

[1] Extract contacts/positions from the Gazebo Subscribe

When I tried to get the positions from the Gazebo Subscribe over the bus selector, I got the following error.

Simulation 2 1
05:14 AM Elapsed: 5 sec
 Simulation 1 1
05:16 AM Elapsed: 2 sec
'Input Port 1' of 'gazeboCosimControl_ys_v3/Gazebo Robot/Display1' is not connected.
Component: Simulink | Category: Block warning
Simulink cannot propagate the variable-size mode from the 'Output Port 1' of 
'gazeboCosimControl_ys_v3/Gazebo Robot/extract1/Bus Selector1' to the 'Input Port 1' of 
'gazeboCosimControl_ys_v3/Gazebo Robot/extract1/Mux2'. This input port expects a fixed-size mode. 
Examine the configuration of 'gazeboCosimControl_ys_v3/Gazebo Robot/extract1/Mux2' 
for one of the following scenarios: 
1) the block does not support variable-size signals; 
2) the block supports variable-size signals but needs to be configured for them.
Component: Simulink | Category: Block error

And here is my simulink,

Can you see any problems with them?

[2] Extract contacts/collision from the Gazebo Subscribe

When I tried to get the pocollision[collision body name] from the Gazebo Subscribe over the bus selector, the simulation works! However, when a contact occurs, then I got the following error message.

 Simulation 2 1
05:14 AM Elapsed: 5 sec
 Simulation 1 1
05:16 AM Elapsed: 2 sec
 Model Save (gazeboCosimControl_ys_v3) 
05:37 AM Elapsed: 0.584 sec
 Model Save (gazeboCosimControl_ys_v3) 
05:37 AM Elapsed: 0.659 sec
 Simulation 1 1
05:38 AM Elapsed: 25 sec
Block 'gazeboCosimControl_ys_v3_2/Gazebo Robot/Display1' shows truncated signal data. 
The Display block can show up to 200 elements of vector or matrix signals.
Component: Simulink | Category: Block warning
An error occurred while running the simulation and the simulation was terminated
Caused by:
For 'Output Port 2' of 'gazeboCosimControl_ys_v3_2/Gazebo Robot/Gazebo Subscribe con_info_redbox/SourceBlock',
the method 'outputImpl' of the System object 'robotics.slgazebo.internal.GazeboSubscribeBlk' 
returns a bus type whose field Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts.contact.wrench.body_1_name 
size does not match the field Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts.contact.wrench.body_1_name 
size defined in the method 'getOutputDataTypeImpl'. 
Component: Simulink | Category: Block error

In addition, here is my simulink blocks.

Do you have any comments for me to solve this issue?

Thank you very much!

Best,

Yun

Answer 3

[Gaurav Bhosale]

Hi Yunsik,

1] This issue is more related to mux don't support variable size input. So, might be you need to try with fixed size bus. If you aware of size of incoming message then you can change bus to fixed size and add the bus size. Similar, settings are mentioned in the UR10 Gazebo Example in 'Customize Bus Objects for Simulation' section.

2] You need to increase bus size for Gazebo_SL_Bus_gazebo_msgs_custom_gazebo_msgs_Contacts.contact.wrench.body_1_name

Whenever collision occurs, the wrench.body_1_name contains body name string, which is exceeding bus limits of Contacts.contact.wrench.body_1_name.

So, try with higher value.

Thanks

[Yunsik Jung]

Hi, Garurav.

Thank you so much for the information. I can address those issues except the variable sized elements from the contact-positions.

So, I created new question related to this.

https://kr.mathworks.com/matlabcentral/answers/855615-gazebo-customized-plugin-read-contact-positions-that-contains-both-a-variable-sized-element-and-an

Thank you so much for your help, again!

Yun