What are the default input and output ports of a model that is loaded via
model = parser.AddModelFromFile("myfile.sdf")
?
In particular, I am wondering what the order of variables is. I'm quite confused since I am loading a robot with 10 Joints, thus I would expect
plant.get_state_output_port(model).size()
to be 20. I.e. 10 Joint positions, and 10 Joint velocities. However, when loading the robot the above yields 16, which to me seems rather strange. This also raises the question of how the variables are ordered, i.e. first all positions or by joint?
The sdf file I was loading is:
<?xml version="1.0"?>
<sdf version="1.7">
<model name="two_planar_cc_segments">
<link name="robot_base">
<pose> 0 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="robot_base_col">
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</collision>
<visual name="robot_base_vis">
<material>
<diffuse>1.0 0.0 0.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<!-- This is the first CC Segment -->
<link name="s1l0">
<pose relative_to="robot_base"> 0 0 0 1.570796327 0 0 </pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s1l0_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l0_vis">
<material>
<diffuse>1.0 1.0 1.0 1.0</diffuse>
</material>
<geometry>
<empty></empty>
</geometry>
</visual>
</link>
<joint name="s1q0" type="revolute">
<pose relative_to="robot_base"> 0 0 0 0 0 0</pose>
<parent> robot_base </parent>
<child> s1l0 </child>
<axis>
<xyz> 0 0 1 </xyz>
</axis>
</joint>
<link name="s1l1">
<pose relative_to="s1l0"> 0 0 0 0 0 0 </pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s1l1_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l1_vis">
<material>
<diffuse>1.0 1.0 1.0 1.0</diffuse>
</material>
<geometry>
<empty></empty>
</geometry>
</visual>
</link>
<joint name="s1q1" type="revolute">
<pose relative_to="s1q0">0 0 0 1.570796327 0 0 </pose>
<parent> s1l0 </parent>
<child> s1l1 </child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="s1l2">
<pose relative_to="s1l1"> 0 0 0 1.570796327 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s1l2_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l2_vis">
<material>
<diffuse>0.0 1.0 0.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s1q2" type="prismatic">
<pose relative_to="s1q1">0 0 0 0 0 0 </pose>
<parent> s1l1 </parent>
<child> s1l2 </child>
<axis>
<xyz>0 1 0</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s1l3">
<pose relative_to="s1l2"> 0 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s1l3_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l3_vis">
<material>
<diffuse>0.0 0.0 1.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s1q3" type="prismatic">
<pose relative_to="s1q2">0 0 0 -1.570796327 0 0 </pose>
<parent> s1l2 </parent>
<child> s1l3 </child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s1l4">
<pose relative_to="s1l3"> 0 0 0 1.570796327 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s1l4_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s1l4_vis">
<material>
<diffuse>0.0 0.0 1.0 1.0</diffuse>
</material>
<geometry>
<empty></empty>
</geometry>
</visual>
</link>
<joint name="s1q4" type="revolute">
<pose relative_to="s1q3">0 0 0 0 0 0 </pose>
<parent> s1l3 </parent>
<child> s1l4 </child>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<frame name="s1-ee-link">
<pose relative_to="s1q4">0 0 0 0 0 0</pose>
</frame>
<!-- Till Here -->
<!-- This is the first CC Segment -->
<link name="s2l0">
<pose relative_to="s1l4"> 0 0 0 0 0 0 </pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s2l0_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l0_vis">
<material>
<diffuse>1.0 1.0 1.0 1.0</diffuse>
</material>
<geometry>
<empty></empty>
</geometry>
</visual>
</link>
<joint name="s2q0" type="revolute">
<pose relative_to="s1l4"> 0 0 0 1.570796327 0 0</pose>
<parent> s1l4 </parent>
<child> s2l0 </child>
<axis>
<xyz> 0 0 1 </xyz>
</axis>
</joint>
<link name="s2l1">
<pose relative_to="s2l0"> 0 0 0 0 0 0 </pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s2l1_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l1_vis">
<material>
<diffuse>1.0 1.0 1.0 1.0</diffuse>
</material>
<geometry>
<empty></empty>
</geometry>
</visual>
</link>
<joint name="s2q1" type="revolute">
<pose relative_to="s2q0">0 0 0 1.570796327 0 0 </pose>
<parent> s2l0 </parent>
<child> s2l1 </child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="s2l2">
<pose relative_to="s2l1"> 0 0 0 1.570796327 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s2l2_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l2_vis">
<material>
<diffuse>0.0 1.0 0.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s2q2" type="prismatic">
<pose relative_to="s2q1">0 0 0 0 0 0 </pose>
<parent> s2l1 </parent>
<child> s2l2 </child>
<axis>
<xyz>0 1 0</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s2l3">
<pose relative_to="s2l2"> 0 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s2l3_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l3_vis">
<material>
<diffuse>0.0 0.0 1.0 1.0</diffuse>
</material>
<geometry>
<cylinder>
<length> 0.0001 </length>
<radius> 0.01 </radius>
</cylinder>
</geometry>
</visual>
</link>
<joint name="s2q3" type="prismatic">
<pose relative_to="s2q2">0 0 0 -1.570796327 0 0 </pose>
<parent> s2l2 </parent>
<child> s2l3 </child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<friction>1</friction>
<damping>1</damping>
</dynamics>
<limit>
<lower>0.0</lower>
<upper>0.1</upper>
</limit>
</axis>
</joint>
<link name="s2l4">
<pose relative_to="s2l3"> 0 0 0 1.570796327 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.001</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.001</iyy>
<iyz>0.0</iyz>
<izz>0.001</izz>
</inertia>
</inertial>
<collision name="s2l4_col">
<geometry>
<empty></empty>
</geometry>
</collision>
<visual name="s2l4_vis">
<material>
<diffuse>0.0 0.0 1.0 1.0</diffuse>
</material>
<geometry>
<empty></empty>
</geometry>
</visual>
</link>
<joint name="s2q4" type="revolute">
<pose relative_to="s2q3">0 0 0 0 0 0 </pose>
<parent> s2l3 </parent>
<child> s2l4 </child>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<frame name="s2-ee-link">
<pose relative_to="s2q4">0 0 0 0 0 0</pose>
</frame>
<!-- Till Here -->
</model>
</sdf>
As Xuchen mentioned, it's because your model is by default floating.
If you wish to fixture the model, then you should get the desired coordinates you want; alternatively, you may need to mask the DoFs you want.
Example: quick deepnote ipynb
Related
I made a URDF-File for a 4-wheeled Robot. I would like to test it in Gazebo. I have it working so far. I have 4 wheels, they also turn. I confirmed this by turning on View -> Wireframe. I am using the skid_steer_drive_controller and rosrun teleop_twist_keyboard teleop_twist_keyboard.py. I can see the wheels turning but the Robot does not move.
I am using an offset for my base_footprint, otherwise my robot collapses and starts jiggeling. I also took this offset in the z-Axis in account for the wheels.
I took this urdf as basis, here so i did not declare friction:
This is my URDF:
<?xml version="1.0" ?>
<robot name="explorer">
<link name="world">
</link>
<!-- -->
<link name="base_footprint">
<visual>
<origin xyz="0 0 0.046"/>
<geometry>
<box size="0.235 0.094 0.049"/>
</geometry>
<material name="gray">
<color rgba=".2 .2 .2 1" />
</material>
</visual>
<inertial>
<origin xyz="0 0 0.046"/>
<mass value="0.6174" />
<inertia ixx="2.84523645e-3" ixy="0.0" ixz="0" iyy="3.1763172e-3" iyz="0" izz="5.7814365e-4" />
</inertial>
<collision>
<origin xyz="0 0 0.046"/>
<geometry>
<box size="0.235 0.094 0.049"/>
</geometry>
</collision>
</link>
<joint name="dummy_joint" type="fixed">
<parent link="world"/>
<child link="base_footprint"/>
</joint>
<!-- -->
<link name="wheel_rf">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_rf_joint" type="continuous">
<origin xyz="0.04 0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_rf"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<link name="wheel_lf">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_lf_joint" type="continuous">
<origin xyz="0.04 -0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_lf"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- -->
<link name="wheel_rb">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_rb_joint" type="continuous">
<origin xyz="-0.04 0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_rb"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- -->
<link name="wheel_lb">
<inertial>
<mass value="0.036" />
<inertia ixx="9.2562e-5" ixy="0.0" ixz="0" iyy="9.2562e-5" iyz="0" izz="2.9124e-5" />
</inertial>
<visual>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.033" length="0.026"/>
</geometry>
</collision>
</link>
<joint name="wheel_lb_joint" type="continuous">
<origin xyz="-0.04 -0.063 0.033" rpy="1.57 0.0 0.0"/>
<parent link="base_footprint"/>
<child link="wheel_lb"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
<!-- Gazebo colors to links as upper defined colors are only visible in RVIZ-->
<gazebo reference="base_footprint">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="wheel_rf">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="wheel_lf">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="wheel_rb">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="wheel_lb">
<material>Gazebo/Orange</material>
</gazebo>
<!-- DIFFENERNTIAL DRIVEEEEEEEEEEEEE -->
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace>/</robotNamespace>
<leftFrontJoint>wheel_lf_joint</leftFrontJoint>
<rightFrontJoint>wheel_rf_joint</rightFrontJoint>
<leftRearJoint>wheel_lb_joint</leftRearJoint>
<rightRearJoint>wheel_rb_joint</rightRearJoint>
<wheelSeparation>0.08</wheelSeparation>
<wheelDiameter>0.066</wheelDiameter>
<robotBaseFrame>base_footprint</robotBaseFrame>
<torque>20</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>false</broadcastTF>
</plugin>
</gazebo>
</robot>
I need some help with a problem I encountered while adding ultrasonic sensors to a robot (loosely based on Linorobot), already equipped with an RPlidar. Hw/Sw: Raspi3B w/ Ubuntu 16.04.6 LTS, ROS kinetic, a Teensy, 2 Nano.
The robot was working fine with just the lidar, but I need to be able to detect correctly glass and some reflective surfaces, so I'm adding the ultrasonic sensors.
The hardware and microcontroller (rosserial) parts seem to be working fine, I suspect it's an error from my part, maybe related to namespaces or transform frames... or maybe I'm missing something gargantuan. I checked and re-checked against online tutorials, examples and other questions similar to this one, but I couldn't identify the culprit.
After executing the launch files I get the standard messages (same as before trying to setup the ultrasonic sensors), plus:
[ INFO] [1631195261.554945536]: global_costmap/sonar_layer: ALL as input_sensor_type given
[ INFO] [1631195261.596176257]: RangeSensorLayer: subscribed to topic /ultrasound_front
and I guess that's good.
Unfortunately from that moment onward I get (with increasingly high figures, of course):
[ WARN] [1631195265.533631740]: No range readings received for 4.02 seconds, while expected at least every 2.00 seconds.
here's a sensor message (from "rostopic echo /ultrasound_front"):
----
header:
seq: 1124
stamp:
secs: 1631192726
nsecs: 301432058
frame_id: "sonar_front"
radiation_type: 0
field_of_view: 0.259999990463
min_range: 0.0
max_range: 100.0
range: 52.0
----
So, the topic is published and the massages should be ok...
My costmap_common_params.yaml:
map_type: costmap
transform_tolerance: 1
footprint: [[-0.25, -0.25], [-0.25, 0.25], [0.25, 0.25], [0.25, -0.25]]
inflation_layer:
inflation_radius: 0.28
cost_scaling_factor: 3
obstacle_layer:
obstacle_range: 2.5
raytrace_range: 3.0
observation_sources: scan
observation_persistence: 0.0
scan:
data_type: LaserScan
topic: scan
marking: true
clearing: true
sonar_layer:
frame: sonar_front
topics: ["/ultrasound_front"]
no_readings_timeout: 2.0
clear_on_max_reading: true
clear_threshold: 0.2
mark_threshold: 0.80
My global_costmap_params.yaml:
global_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 1
publish_frequency: 0.5
static_map: true
transform_tolerance: 1
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
My local_costmap_params.yaml:
local_costmap:
global_frame: /odom
robot_base_frame: /base_footprint
update_frequency: 1
publish_frequency: 5.0
static_map: false
rolling_window: true
width: 3
height: 3
resolution: 0.02
transform_tolerance: 1
observation_persistence: 0.0
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
And my barebone URDF:
<?xml version="1.0"?>
<robot name="linorobot">
<link name="base_link">
<visual>
<geometry>
<box size="0.50 0.33 0.09"/>
</geometry>
<origin rpy="0 0 0" xyz="0.0 0.00 0.085"/>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
</link>
<link name="perception_deck">
<visual>
<geometry>
<box size="0.18 0.33 0.08"/>
</geometry>
<origin rpy="0 0 0" xyz="0.0 0.0 0.17"/>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
</link>
<link name="wheel_left_front">
<visual>
<geometry>
<cylinder length="0.03" radius="0.06"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0.163 0.222 0.03"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="wheel_right_front">
<visual>
<geometry>
<cylinder length="0.03" radius="0.06"/>
</geometry>
<origin rpy="1.57 0 0" xyz="0.163 -0.222 0.03"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="wheel_left_rear">
<visual>
<geometry>
<cylinder length="0.03" radius="0.06"/>
</geometry>
<origin rpy="1.57 0 0" xyz="-0.163 0.222 0.03"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="wheel_right_rear">
<visual>
<geometry>
<cylinder length="0.03" radius="0.06"/>
</geometry>
<origin rpy="1.57 0 0" xyz="-0.163 -0.222 0.03"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
</link>
<link name="laser">
<visual>
<geometry>
<cylinder length="0.065" radius="0.035"/>
</geometry>
<origin rpy="0 0 0" xyz="0.0 0.0 0.2825"/>
<material name="black"/>
</visual>
</link>
<link name="chassis">
<visual>
<geometry>
<box size="0.5 0.5 0.8"/>
</geometry>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<material name="silver">
<color rgba="192 192 192 0.6"/>
</material>
</visual>
</link>
<link name="sonar_front">
<visual>
</geometry>
<origin rpy="1.5708 0.2618 0" xyz="-0.21 0.0 0.235"/>
<material name="silver">
<color rgba="192 192 192 0.6"/>
</material>
</visual>
</link>
<link name="sonar_rear">
<visual>
<geometry>
<box size="0.02 0.025 0.07"/>
</geometry>
<origin rpy="1.5708 0.2618 3.1416" xyz="0.23 0.0 0.235"/>
<material name="silver">
<color rgba="192 192 192 0.6"/>
</material>
</visual>
</link>
<link name="sonar_left">
<visual>
<geometry>
<box size="0.02 0.025 0.07"/>
</geometry>
<origin rpy="1.5708 -0.2618 1.5708" xyz="0.0 0.18 0.235"/>
<material name="silver">
<color rgba="192 192 192 0.6"/>
</material>
</visual>
</link>
<link name="sonar_right">
<visual>
<geometry>
<box size="0.02 0.025 0.07"/>
</geometry>
<origin rpy="1.5708 -0.2618 -1.5708" xyz="0.0 -0.19 0.235"/>
<material name="silver">
<color rgba="192 192 192 0.6"/>
</material>
</visual>
</link>
<joint name="base_to_wheel_left_front" type="fixed">
<parent link="base_link"/>
<child link="wheel_left_front"/>
<origin xyz="0 0 0"/>
</joint>
<joint name="base_to_wheel_right_front" type="fixed">
<parent link="base_link"/>
<child link="wheel_right_front"/>
<origin xyz="0 0 0"/>
</joint>
<joint name="base_to_wheel_left_rear" type="fixed">
<parent link="base_link"/>
<child link="wheel_left_rear"/>
<origin xyz="0 0 0"/>
</joint>
<joint name="base_to_wheel_right_rear" type="fixed">
<parent link="base_link"/>
<child link="wheel_right_rear"/>
<origin xyz="0 0 0"/>
</joint>
<joint name="base_to_laser" type="fixed">
<parent link="base_link"/>
<child link="laser"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_left_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_left"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_right_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_right"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_rear_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_rear"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_front_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_front"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_perception_deck" type="fixed">
<parent link="base_link"/>
<joint name="base_to_laser" type="fixed">
<parent link="base_link"/>
<child link="laser"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_left_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_left"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_right_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_right"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_rear_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_rear"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_front_sonar" type="fixed">
<parent link="base_link"/>
<child link="sonar_front"/>
<origin xyz="0.0 0.0 0.0"/>
</joint>
<joint name="base_to_perception_deck" type="fixed">
<parent link="base_link"/>
<child link="perception_deck"/>
<origin xyz="0 0 0.0"/>
</joint>
<joint name="base_to_chassis" type="fixed">
<parent link="base_link"/>
<child link="chassis"/>
<origin xyz="0 0 0.44"/>
</joint>
</robot>
Thanks!
EDITS
after getting the messages, "rostopic hz /ultrasound_front" gives:
subscribed to [/ultrasound_front]
average rate: 3.494
min: 0.267s max: 0.305s std dev: 0.01919s window: 3
average rate: 3.384
min: 0.250s max: 0.353s std dev: 0.03533s window: 6
average rate: 3.362
min: 0.250s max: 0.353s std dev: 0.02813s window: 9
average rate: 3.352
min: 0.250s max: 0.353s std dev: 0.02625s window: 13
average rate: 3.349
min: 0.250s max: 0.353s std dev: 0.02447s window: 16
average rate: 3.344
min: 0.250s max: 0.353s std dev: 0.02547s window: 20
average rate: 3.341
min: 0.250s max: 0.353s std dev: 0.02368s window: 23
average rate: 3.256
min: 0.250s max: 0.490s std dev: 0.04349s window: 26
average rate: 3.336
min: 0.110s max: 0.490s std dev: 0.05406s window: 30
average rate: 3.335
min: 0.110s max: 0.490s std dev: 0.05176s window: 33
and so on. Publishing interval in the MCU code is 250ms.
"max_range:1.0" in "rostopic echo /ultrasound_front" has been corrected (was an error in the original MCU code), the behaviour doesn't change. I modified the output above to reflect the current version.
"rostopic info /ultrasound_front", after the massages started, gives: (Thank you #BTables!)
Type: sensor_msgs/Range
Publishers:
* /rosserial_NANO_sensors (http://192.168.2.54:34525/)
Subscribers:
* /move_base (http://192.168.2.54:40149/)
I finally solved some of the problems that emerged after adding the ultrasound sensors. Because of the nature of the errors, and the extremely large amount of different hw/sw configurations possible, I will put here my findings, with some more general info, hoping to help others:
Double check the UNIT of MEASURE used in the range fields in the microcontroller code. For example, the library and examples I used and referred to had everything in cm.
This isn't good for ROS navigation layer, the range/distance numbers passed in the messages should be in meters (min_range, max_range, range).
HOWEVER the microcontroller code could be passing the data, and using some internal calculations or logic, in centimeters (like here 'https://www.intorobotics.com/how-to-use-sensor_msgs-range-ros-for-multiple-sensors-with-rosserial/', for example), so some changes are probably needed (also regarding the logic behind the clearing of the costmap, but that's a problem for another question).
A message rate of 20Hz should be ok, it should not produce missing data messages, sync errors, etc. However please note that it's possible this frequency has to be modified, depending on the hardware involved.
The costmap YAML parameter clear_on_max_reading behaviour depends on how the data is presented by your ultrasound sensor (or sensors) MCU code. It's a good idea to try both settings and check which one is more appropriate for your case. You can then modify the MCU code to accomodate for the library logic behind this setting (or the other way around, modifying the libraries).
Verify that your RVIZ configuration contains all the necessary information to visualize your ultrasound (range) sensor data (http://wiki.ros.org/rviz/DisplayTypes/Range)
The URDF usually gives clear messages if something related to the transforms and related data is not working, once the real problems are solved, it's possible to see the cones and axes in Rviz (IF the unit of measure isn't too small!), so it's easy to correct orientation and position errors.
Use check_urdf to verify the validity of the URDF file (http://wiki.ros.org/urdf#Verification), and urdf_to_graphiz to have a visual representation with some more data, that could give some clues on malfunctions or errors (http://wiki.ros.org/urdf#Visualization). Also rqt_graph with enable_statistics set to "true" can give useful clues (http://wiki.ros.org/rqt_graph).
i make a ball model at gazebo by making sdf file but it's not moving
how can i make a ball rolling?
my sdf code right below
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="sphere">
<link name='base_link'>
<pose>0 0 .1 0 0 0</pose>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.03</radius>
</sphere>
</geometry>
<material>
<diffuse>1 0 0 1</diffuse>
<ambient>1 0 0 1</ambient>
</material>
</visual>
</link>
</model>
</sdf>
help me!!!
Hello Gazebo/ROS community on Stack Overflow, I'm trying to simulate the landing of a quadcopter on a pole using Gazebo/ROS Kinetic. However, as soon as the drone contacts the landing pole, it gets bounced away in a very unrealistic way. Both the drone and the pole have their mass, inertia, and friction defined.
A video is avaliable here:
https://youtu.be/8Iis60y5_yk
Can anyone give me a few pointers? Your help is much appreciated!
Here is the .world file:
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<!-- Only one ROS interface plugin is required per world, as any other plugin can connect a Gazebo
topic to a ROS topic (or vise versa). -->
<plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so"/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>47.3667</latitude_deg>
<longitude_deg>8.5500</longitude_deg>
<elevation>500.0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<physics type='ode'>
<ode>
<solver>
<type>quick</type>
<iters>1000</iters>
<sor>1.3</sor>
</solver>
<constraints>
<cfm>0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>100</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</ode>
<max_step_size>0.01</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>100</real_time_update_rate>
<gravity>0 0 -9.8</gravity>
</physics>
</world>
</sdf>
Here is the xacro file for the landing pole:
<?xml version="1.0"?>
<robot name="target" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="pole_height" value="1.2192" />
<xacro:property name="pole_radius" value="0.05" />
<!-- cone-shaped top compatible with the drone -->
<link name="target/cone">
<inertial>
<origin xyz="0 0 0"/>
<mass value="3.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://rotors_description/meshes/cone.dae" scale="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://rotors_description/meshes/cone.dae" scale="0.001 0.001 0.001" />
</geometry>
<contact_coefficients mu ="1" kp="1000" kd="1"/>
</collision>
</link>
<!-- connect cone to pole -->
<joint name="cone_to_pole" type="fixed">
<origin xyz="0 0 0.6096"/>
<parent link="target/base_link"/>
<child link="target/cone"/>
</joint>
<!-- landing pole -->
<link name="target/base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="${pole_height}" radius="${pole_radius}"/>
</geometry>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${pole_height}" radius="${pole_radius}"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0"/>
<mass value="3.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<!-- april tag -->
<link name="target/tag_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="1.7 1.7 0.05" />
</geometry>
<surface>
<friction>
<ode>
<mu>0.01</mu>
<mu2>0.01</mu2>
</ode>
</friction>
</surface>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://rotors_description/meshes/pole.dae" scale="1 1 1" />
</geometry>
</visual>
</link>
<!-- connect pole to tag -->
<joint name="target/tag_joint" type="fixed">
<origin xyz="0 0 -0.6096"/>
<parent link="target/base_link"/>
<child link="target/tag_link"/>
</joint>
<!-- make the landing pole transparent for testing purposes -->
<!-- <gazebo reference="target/base_link">
<visual>
<material>
<ambient> 1.0 1.0 1.0 0.0</ambient>
<diffuse> 1.0 1.0 1.0 0.0</diffuse>
<specular>1.0 1.0 1.0 0.0</specular>
</material>
</visual>
</gazebo>
<gazebo reference="target/cone">
<visual>
<material>
<ambient> 1.0 1.0 1.0 0.0</ambient>
<diffuse> 1.0 1.0 1.0 0.0</diffuse>
<specular>1.0 1.0 1.0 0.0</specular>
</material>
</visual>
</gazebo> -->
<xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro" />
<xacro:default_imu namespace="target" parent_link="target/base_link" />
<gazebo>
<plugin name="landing_target_plugin" filename="librotors_gazebo_landing_target_plugin.so">
<robotNamespace>target</robotNamespace>
<linkName>target/base_link</linkName>
</plugin>
</gazebo>
<gazebo reference="target/base_link">
<mu1>0.00000001</mu1>
<mu2>0.00000001</mu2>
</gazebo>
</robot>
Here is the xacro file for the base of the drone:
<?xml version="1.0"?>
<!--
Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Macro for the inertia of a box -->
<xacro:macro name="box_inertia" params="x y z mass">
<inertia ixx="${0.0833333 * mass * (y*y + z*z)}" ixy="0.0" ixz="0.0"
iyy="${0.0833333 * mass * (x*x + z*z)}" iyz="0.0"
izz="${0.0833333 * mass * (x*x + y*y)}" />
</xacro:macro>
<!-- Macro for the inertial properties of a box -->
<xacro:macro name="box_inertial" params="x y z mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<xacro:box_inertia x="${x}" y="${y}" z="${z}" mass="${mass}" />
</inertial>
</xacro:macro>
<!-- Main multirotor link -->
<xacro:macro name="multirotor_base_macro"
params="robot_namespace mass body_width body_height use_mesh_file mesh_file *inertia">
<link name="${robot_namespace}/base_link">
</link>
<joint name="${robot_namespace}/base_joint" type="fixed">
<origin xyz="0 0 0" rpy=" 0 0 0" />
<parent link="${robot_namespace}/base_link" />
<child link="${robot_namespace}/base_link_inertia" />
</joint>
<link name="${robot_namespace}/base_link_inertia">
<inertial>
<mass value="${mass}" /> <!-- [kg] -->
<origin xyz="0 0 0" />
<xacro:insert_block name="inertia" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<xacro:if value="${use_mesh_file}">
<mesh filename="${mesh_file}" scale="1 1 1" />
</xacro:if>
<xacro:unless value="${use_mesh_file}">
<box size="${body_width} ${body_width} ${body_height}"/> <!-- [m] [m] [m] -->
</xacro:unless>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<xacro:if value="${use_mesh_file}">
<mesh filename="${mesh_file}" scale="1 1 1" />
</xacro:if>
<xacro:unless value="${use_mesh_file}">
<box size="${body_width} ${body_width} ${body_height}"/> <!-- [m] [m] [m] -->
</xacro:unless>
</geometry>
<contact_coefficients mu ="1" kp="1000" kd="1"/>
</collision>
</link>
<!-- attach multirotor_base_plugin to the base_link -->
<gazebo>
<plugin filename="librotors_gazebo_multirotor_base_plugin.so" name="multirotor_base_plugin">
<robotNamespace>${robot_namespace}</robotNamespace>
<linkName>${robot_namespace}/base_link</linkName>
<rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
</xacro:macro>
<!-- Rotor joint and link -->
<xacro:macro name="vertical_rotor"
params="robot_namespace suffix direction motor_constant moment_constant parent mass_rotor radius_rotor time_constant_up time_constant_down max_rot_velocity motor_number rotor_drag_coefficient rolling_moment_coefficient color use_own_mesh mesh *origin *inertia">
<joint name="${robot_namespace}/rotor_${motor_number}_joint" type="continuous">
<xacro:insert_block name="origin" />
<axis xyz="0 0 1" />
<!-- TODO(ff): not currently set because it's not yet supported -->
<!-- <limit effort="2000" velocity="${max_rot_velocity}" /> -->
<parent link="${parent}" />
<child link="${robot_namespace}/rotor_${motor_number}" />
</joint>
<link name="${robot_namespace}/rotor_${motor_number}">
<inertial>
<mass value="${mass_rotor}" /> <!-- [kg] -->
<xacro:insert_block name="inertia" />
</inertial>
<visual>
<geometry>
<!-- <cylinder length="0.005" radius="${radius_rotor}"/> --> <!-- [m] -->
<xacro:if value="${use_own_mesh}">
<mesh filename="${mesh}"
scale="1 1 1" />
</xacro:if>
<xacro:unless value="${use_own_mesh}">
<mesh filename="package://rotors_description/meshes/propeller_${direction}.dae"
scale="${radius_rotor} ${radius_rotor} ${radius_rotor}" /> <!-- The propeller meshes have a radius of 1m -->
<!-- <box size="${2*radius_rotor} 0.01 0.005"/> -->
</xacro:unless>
</geometry>
</visual>
<collision>
<geometry>
<cylinder length="0.005" radius="${radius_rotor}" /> <!-- [m] -->
</geometry>
</collision>
</link>
<gazebo>
<plugin name="${robot_namespace}_${suffix}_motor_model" filename="librotors_gazebo_motor_model.so">
<robotNamespace>${robot_namespace}</robotNamespace>
<jointName>${robot_namespace}/rotor_${motor_number}_joint</jointName>
<linkName>${robot_namespace}/rotor_${motor_number}</linkName>
<turningDirection>${direction}</turningDirection>
<timeConstantUp>${time_constant_up}</timeConstantUp>
<timeConstantDown>${time_constant_down}</timeConstantDown>
<maxRotVelocity>${max_rot_velocity}</maxRotVelocity>
<motorConstant>${motor_constant}</motorConstant>
<momentConstant>${moment_constant}</momentConstant>
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
<motorNumber>${motor_number}</motorNumber>
<rotorDragCoefficient>${rotor_drag_coefficient}</rotorDragCoefficient>
<rollingMomentCoefficient>${rolling_moment_coefficient}</rollingMomentCoefficient>
<motorSpeedPubTopic>motor_speed/${motor_number}</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>${rotor_velocity_slowdown_sim}</rotorVelocitySlowdownSim>
</plugin>
</gazebo>
<gazebo reference="${robot_namespace}/rotor_${motor_number}">
<material>Gazebo/${color}</material>
</gazebo>
</xacro:macro>
</robot>
I have faced a similar problem. The robot started to bounce when it was in contact with a surface. In my case, the source of problem was unrealistic inertia values of robot parts. Even if one of link has a bad calculated/estimated inertia, the model may bounce. I would propose you to verify calculations under inertial tags.
I'm trying to model a small and light objects(the links of a rope). This is an example sdf. If I decrease the mass from 1.0 to 0.3 and adjust the inertias accordingly, it seems to disappear off the screen as soon as it touches the ground. With more links, I also get
RuntimeError: MultibodyPlant's discrete update solver failed to converge at simulation time = 0.004 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:
1. reduce the discrete update period set at construction,
2. decrease the high gains in your controller whenever possible,
3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.
or
terminated by signal SIGSEGV (Address boundary error)
I can work around a lot of these issues if I don't compute physically accurate inertias or if I put a mass of 1(and large inertias) on the dummy link, but I'm wondering if there's a proper way to resolve these errors. I'll also add that I'm not seeing weird behavior for a single link.
SDF: 2 links with mass 1kg
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>1.0</mass>
<inertia>
<ixx>0.0008395833333333335</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0008395833333333335</iyy>
<iyz>0</iyz>
<izz>1.25e-05</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>1.0</mass>
<inertia>
<ixx>0.0008395833333333335</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0008395833333333335</iyy>
<iyz>0</iyz>
<izz>1.25e-05</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose> 0 0 1.9 0 0 0 </pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 0.0 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-0.5</lower>
<upper>0.5</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 0.0 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
SDF: 2 links with mass 0.3kg
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose>0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
<sdf version='1.6'>
<model name='capsule'>
<link name="capsule_1">
<pose> 0 0 2.0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_2">
<pose> 0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 -0.05 0 0 0</pose>
<mass>0.3</mass>
<inertia>
<ixx>0.00025187500000000004</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00025187500000000004</iyy>
<iyz>0</iyz>
<izz>3.75e-06</izz>
</inertia>
</inertial>
<visual name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<diffuse>0.5569 0.349 0.2353 1.0</diffuse>
</material>
</visual>
<collision name='cylinder'>
<pose frame=''> 0 0 -0.05 0 0 0 </pose>
<geometry>
<cylinder>
<radius>0.005</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
<link name="capsule_1_extra">
<pose>0 0 1.9 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0 </pose>
<mass>1e-10</mass>
<inertia>
<ixx>1e-10</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-10</iyy>
<iyz>0</iyz>
<izz>1e-10</izz>
</inertia>
</inertial>
</link>
<joint name="capsule_1_capsule_2_joint_x" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_2</child>
<parent>capsule_1_extra</parent>
<axis>
<xyz> 1 0 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<joint name="capsule_1_capsule_2_joint_y" type="revolute">
<pose> 0 0 0 0 0 0 </pose>
<child>capsule_1_extra</child>
<parent>capsule_1</parent>
<axis>
<xyz> 0 1 0 </xyz>
<limit>
<effort>0</effort>
<lower>-3.14</lower>
<upper>3.14</upper>
</limit>
<dynamics>
<damping> 1 </damping>
<friction> 0 </friction>
<spring_reference> 0 </spring_reference>
<spring_stiffness> 10 </spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
That error is usually caused because our discrete solver fails. As the message suggests, did you attempt to reduce the discrete period (aka time step) at construction of the MultibodyPlant? What I imagine happens here is that your small links lead to fast dynamics and therefore you need smaller time steps to resolve it.
The link dissapearing is most likely due to the fact that the visualizer is already plotting a bad state (diverged, possibly with lots of NaNs). So the visualizer alone won't tell you much.
When setting up the discrete solver you always need an estimation of you system's time scale, stemming from the physics you want to resolve. For your case, and only because I don't know if you have applied forces, thinking of each link as a simple pendulum would allow you to estimate a time scale by for instance computing the period of that pendulum.
With your numbers, for a physical pendulum with inertia Iyy = 0.001 (about the link's end, motion about z seems restricted by your joints), m = 0.3, R=0.05 (distance to COM), I get a compound pendulum effective length of L = I/(m*R) = 0.0667 m. With this I get a period T = 2*pi*sqrt(L/g) = 0.52 seconds. Say I'd take 500 steps per period, then I'd estimate a time step of dt = 1ms.
That's a very rough estimate given I know nothing about your system or external forces. But summarizing, I believe your problem is the plant's time step.
Edit
What about penetration allowance? (MultibodyPlant::set_penetration_allowance()). When contact is present MultibodyPlant::get_contact_penalty_method_time_scale() will give you an estimate of the time scale due to contact. Usually you need to set a time step that is able to comfortably resolve that dynamics.