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

Question

Yunsik Jung 2021-6-14

0
链接

此问题的直接链接

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

编辑： Yunsik Jung 2021-7-3

采纳的回答： Gaurav Bhosale

在 MATLAB Online 中打开

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Gaurav Bhosale 2021-6-28

0
链接

此回答的直接链接

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

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.

1 个评论
显示 -1更早的评论隐藏 -1更早的评论

Yunsik Jung 2021-6-29

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 2021-6-14

0
链接

此回答的直接链接

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

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?

2 个评论
显示无隐藏无

Yunsik Jung 2021-6-16

在 MATLAB Online 中打开

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 2021-6-27

在 MATLAB Online 中打开

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 2021-6-29

0
链接

此回答的直接链接

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

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

1 个评论
显示 -1更早的评论隐藏 -1更早的评论

Yunsik Jung 2021-6-30

编辑：Yunsik Jung 2021-7-3

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

请先登录，再进行评论。

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

1 个评论
显示 -1更早的评论隐藏 -1更早的评论

更多回答（2 个）

2 个评论
显示无隐藏无

1 个评论
显示 -1更早的评论隐藏 -1更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

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

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

1 个评论 显示 -1更早的评论隐藏 -1更早的评论

更多回答（2 个）

2 个评论 显示 无隐藏 无

1 个评论 显示 -1更早的评论隐藏 -1更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

1 个评论
显示 -1更早的评论隐藏 -1更早的评论

2 个评论
显示无隐藏无

1 个评论
显示 -1更早的评论隐藏 -1更早的评论