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

1 次查看（过去 30 天）
Yunsik Jung
Yunsik Jung 2021-6-14
编辑： Yunsik Jung 2021-7-3
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

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

采纳的回答

Gaurav Bhosale
Gaurav Bhosale 2021-6-28
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 个评论
Yunsik Jung
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

请先登录，再进行评论。

更多回答（2 个）

Gaurav Bhosale
Gaurav Bhosale 2021-6-14
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
Yunsik Jung 2021-6-16
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
Yunsik Jung 2021-6-27
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

请先登录，再进行评论。

Gaurav Bhosale
Gaurav Bhosale 2021-6-29
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 个评论
Yunsik Jung
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

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 General Applications 的更多信息

标签

产品


版本

R2021a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by