How to make pendulum swing due to gravity in Drake - simulation

I have been trying to simulate a simple pendulum that can swing due to gravity after setting the initial angle. I am now able to create the urdf, and create a slider that will set the angle of the pendulum arm. But I am still facing trouble when trying to get the pendulum to move. I have been basing my code on the code present in the "Running a simple simulation" section in https://deepnote.com/project/Authoring-a-Multibody-Simulation-jnoKyVLkS5CYUgHG3ASsBA/%2Fauthor_multibody_sim.ipynb.
This is the code I have so far:
import os
from tempfile import mkdtemp
from pydrake.all import (
AddMultibodyPlantSceneGraph,
DiagramBuilder,
Meshcat,
MeshcatVisualizerCpp,
Parser,
Simulator,
)
from manipulation.meshcat_cpp_utils import (
StartMeshcat, MeshcatJointSlidersThatPublish)
meshcat = StartMeshcat("https://b8cf4725-e938-4907-9172-f08ccc4873ab.deepnoteproject.com")
tmp_dir = mkdtemp()
model_file = os.path.join(tmp_dir, "test_model.urdf")
model_text = f"""\
<?xml version="1.0"?>
<robot name="materials">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<joint name="world_2_base" type="fixed">
<parent link="world"/>
<child link="base"/>
<origin xyz="0 0 0.5"/>
</joint>
<link name="base">
<visual>
<geometry>
<box size="0.5 0.5 0.1"/>
</geometry>
</visual>
<inertial>
<mass value="1"/>
</inertial>
</link>
<joint name="base" type="revolute">
<parent link="base"/>
<child link="arm"/>
<axis xyz="0 1 0"/>
<origin xyz="0 0 -0.05"/>
</joint>
<link name="arm">
<visual>
<geometry>
<cylinder length="0.5" radius="0.01"/>
</geometry>
<material name="blue"/>
<origin xyz="0 0 -0.25"/>
</visual>
<inertial>
<mass value="1"/>
</inertial>
</link>
<joint name="base2" type="fixed">
<parent link="arm"/>
<child link="ball"/>
<origin xyz="0 0 -0.5"/>
</joint>
<link name="ball">
<visual>
<geometry>
<sphere radius="0.1"/>
</geometry>
<material name="green"/>
<origin xyz="0 0 -0.01"/>
</visual>
<inertial>
<mass value="10"/>
</inertial>
</link>
</robot>
"""
# Write temporary file.
with open(model_file, "w") as f:
f.write(model_text)
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0001)
# Add pendulum model.
model = Parser(plant, scene_graph).AddModelFromFile(model_file)
# - Weld base at origin.
base_link = plant.GetBodyByName("base", model)
#plant.WeldFrames(plant.world_frame(), base_link.body_frame())
plant.Finalize()
#visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph,
meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
sliders = MeshcatJointSlidersThatPublish(meshcat, plant, visualizer, context)
pend = plant.GetBodyByName(f"base", model)
def pose_callback(context):
pose = plant.EvalBodyPoseInWorld(context, pend)
sliders.Run(pose_callback)
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
#visualizer.StartRecording()
simulator.AdvanceTo(5.0)
#visualizer.PublishRecording()
simulator.AdvanceTo(5.0)
Please let me know what I am doing wrong. Thank you very much

You'd need to specify a non-zero initial angle in the joint between the base and the arm to observe anything interesting. One way of doing this is
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)
# Add pendulum model.
model = Parser(plant, scene_graph).AddModelFromFile(model_file)
plant.Finalize()
joint = plant.GetJointByName('base')
joint.set_default_angle(0.2)
visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat,
MeshcatVisualizerParams(role=Role.kIllustration, prefix="visual"))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
visualizer.StartRecording()
simulator.AdvanceTo(5.0)
visualizer.PublishRecording()
To set the initial condition with the slider, try
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01)
# Add pendulum model.
model = Parser(plant, scene_graph).AddModelFromFile(model_file)
plant.Finalize()
visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat,
MeshcatVisualizerParams(role=Role.kIllustration, prefix="visual"))
diagram = builder.Build()
simulator = Simulator(diagram)
# Get context of the diagram being simulated.
context = simulator.get_mutable_context()
# Set initial condition of the joint.
joint_slider = MeshcatJointSliders(meshcat, plant, context)
joint_slider.Run(visualizer, context)
# Run the simulation and record.
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
visualizer.StartRecording()
simulator.AdvanceTo(5.0)
visualizer.PublishRecording()

Before finalizing the plant, could you try calling
plant.mutable_gravity_field().set_gravity_vector([0, 0, -9.8])
?

I think you need to fix the pendulum input port to zero, you can put
diagram.get_input_port(0).FixValue(context, [0.])
after creating "context".
This says "apply zero torque at the pendulum joint actuator". So the pendulum will swing passively.

Related

Difficulty using buoyancy plugin (libBuoyancyPlugin.so) for my gazebo model(for ROS1)

RE-EDIT:
I want to apply bouyancy plugin my model which is made of 3 links. Do I need to apply the bouyancy plugin to all of the links for my model to float? Or do I just need to apply it to the base_link. The collision geometries for all of links are .dae files. But I have the gemotry of my base_link as a sphere for my Buoyancy plugin. Below is my .urdf.xacro file. For now, my model still phases through my world, so buoyancy isn't working. Not quite sure what I'm doing wrong.
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="wam-v">
<xacro:include filename="properties.urdf.xacro"/>
<link name="base_link">
<visual>
<geometry>
<mesh filename="package://freddy/mesh/WAM-V.dae"/>
</geometry>
</visual>
<collision name="base_link_collision">
<geometry>
<mesh filename="package://freddy/mesh/WAM-V.dae"/>
<!--sphere radius="1.5" /-->
</geometry>
</collision>
<inertial>
<mass value="247.07"/>
<inertia ixx="200" ixy="0.03" ixz="3.09" iyy="500" iyz="-0.05" izz="165.39"/>
<!--inertia ixx="198.828" ixy="0" ixz="0" iyy="198.828" iyz="0" izz="198.828"/-->
</inertial>
</link>
<joint name="base_link_joint" type="fixed">
<origin xyz="0 0 1.84"/>
<parent link="base_footprint"/>
<child link="base_link"/>
</joint>
<link name="base_footprint"/>
<link name="front_camera_link"/>
<joint name="front_camera_joint" type="fixed">
<origin xyz="1.2 0 0"/>
<parent link="base_link"/>
<child link="front_camera_link"/>
</joint>
<link name="left_camera_link"/>
<joint name="left_camera_joint" type="fixed">
<origin xyz="0 0.5 0" rpy="0 0 ${pi/2}"/>
<parent link="base_link"/>
<child link="left_camera_link"/>
</joint>
<link name="right_camera_link"/>
<joint name="right_camera_joint" type="fixed">
<origin xyz="0 -0.5 0" rpy="0 0 ${-pi/2}"/>
<parent link="base_link"/>
<child link="right_camera_link"/>
</joint>
<link name="rear_camera_link"/>
<joint name="rear_camera_joint" type="fixed">
<origin xyz="-0.5 0 0" rpy="0 0 ${pi}"/>
<parent link="base_link"/>
<child link="rear_camera_link"/>
</joint>
<link name="scan"/>
<joint name="scan_joint" type="fixed">
<origin xyz="1.5 0 -1" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="scan"/>
</joint>
<link name="right_motor">
<visual>
<geometry>
<mesh filename="package://freddy/mesh/PROPELLER.dae"/>
</geometry>
</visual>
<collision name="right_motor_collision">
<geometry>
<mesh filename="package://freddy/mesh/PROPELLER.dae"/>
</geometry>
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.00086" ixy="0.00" ixz="0.00" iyy="0.00086" iyz="0.00" izz="0.0017"/>
</inertial>
</link>
<joint name="right_motor_joint" type="continuous">
<origin xyz="${motor_position_x} -${motor_position_y} ${motor_position_z}"/>
<parent link="base_link"/>
<child link="right_motor"/>
<limit effort="0.5293" velocity="100${pi}" />
</joint>
<link name="left_motor">
<visual>
<geometry>
<mesh filename="package://freddy/mesh/PROPELLER.dae"/>
</geometry>
</visual>
<collision name="left_motor_collision">
<geometry>
<mesh filename="package://freddy/mesh/PROPELLER.dae"/>
</geometry>
</collision>
<inertial>
<mass value="0.01"/>
<inertia ixx="0.00086" ixy="0.00" ixz="0.00" iyy="0.00086" iyz="0.00" izz="0.0017"/>
</inertial>
</link>
<joint name="left_motor_joint" type="continuous">
<origin xyz="${motor_position_x} ${motor_position_y} ${motor_position_z}"/>
<parent link="base_link"/>
<child link="left_motor"/>
<limit effort="0.5293" velocity="100${pi}" />
</joint>
<link name="gps"/>
<joint name="gps_joint" type="fixed">
<origin xyz="0.8 0 0"/>
<parent link="base_link"/>
<child link="gps"/>
</joint>
<transmission name="right_motor_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="right_motor_joint">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="right_motor_actuator">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
<mechanicalReduction>30</mechanicalReduction>
</actuator>
</transmission>
<transmission name="left_motor_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="left_motor_joint">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="left_motor_actuator">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
<mechanicalReduction>30</mechanicalReduction>
</actuator>
</transmission>
<!--gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometrySource>world</odometrySource>
<publishOdomTF>true</publishOdomTF>
<robotBaseFrame>base_footprint</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishTf>true</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<legacyMode>false</legacyMode>
<updateRate>30</updateRate>
<leftJoint>left_motor_joint</leftJoint>
<rightJoint>right_motor_joint</rightJoint>
<wheelSeparation>2.054</wheelSeparation>
<wheelDiameter>0.098</wheelDiameter>
<wheelAcceleration>1</wheelAcceleration>
<wheelTorque>10</wheelTorque>
<rosDebugLevel>na</rosDebugLevel>
</plugin>
</gazebo-->
<gazebo reference="scan">
<plugin name="BuoyancyPlugin" filename="libBuoyancyPlugin.so">
<!--a viscous liquid -->
<fluid_density>1000</fluid_density>
<link name="base_link">
<center_of_volume>0 0 1.84</center_of_volume>
<volume>47.712</volume>
</link>
<geometry><sphere radius="2.25" /></geometry>
</plugin>
</gazebo>
<xacro:include filename="camera.urdf.xacro"/>
<xacro:camera prefix="front"/>
<xacro:camera prefix="right"/>
<xacro:camera prefix="left"/>
<xacro:camera prefix="rear"/>
<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
<VLP-16 parent="base_link" name="velodyne" topic="/velodyne_points">
<origin xyz="0.8 0 0.3" rpy="0 0 0" />
</VLP-16>
<gazebo reference="scan">
<material>Gazebo/Blue</material>
<turnGravityOff>false</turnGravityOff>
<sensor type="ray" name="scan_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>1</min>
<max>30.0</max>
<resolution>0.001</resolution>
</range>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>scan</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="front_camera_link">
<sensor type="camera" name="front">
<update_rate>30.0</update_rate>
<!--camera name="head"-->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].-->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<!--/camera-->
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>wam_v/front</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>front_camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<gazebo reference="right_camera_link">
<sensor type="camera" name="right">
<update_rate>30.0</update_rate>
<!--camera name="head"-->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].-->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<!--/camera-->
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>wam_v/right</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>right_camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<gazebo reference="left_camera_link">
<sensor type="camera" name="left">
<update_rate>30.0</update_rate>
<!--camera name="head"-->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].-->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<!--/camera-->
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>wam_v/left</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>left_camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<gazebo reference="rear_camera_link">
<sensor type="camera" name="rear">
<update_rate>30.0</update_rate>
<!--camera name="head"-->
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1].-->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<!--/camera-->
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>wam_v/rear</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>back_camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>

How to spin a range sensor in gazebo?

I want to use the gazebo_range plugin to simulate a sonar sensor. I want to put it on a rover car then spin it to scan the environment.
I try to use the gazebo_ros_control controller to spin the sonar as it scans. But what actually happened is that the joint spin the entire car instead of the sensor, and the sensor just randomly spinning.
Can anyone help me and tell me how to fix it?
Thanks a lot!!
Here is my gazebo_ros_control plugin code:
<joint name="sonar_front_joint" type="continuous">
<axis xyz="0 0 1" />
<origin rpy="0 0 0" xyz="0 0 0.35" />
<parent link="base_footprint"/>
<child link="base_sonar_front"/>
</joint>
<transmission name="sonar_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="sonar_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="sonar_front_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
</transmission>
<link name="base_sonar_front">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/Rover</robotNamespace>
<controlPeriod>0.1</controlPeriod>
</plugin>
</gazebo>
Here is the yaml:
Rover:
# Position Controllers ---------------------------------------
joint_position_controller:
type: effort_controllers/JointPositionController
joint: sonar_front_joint
pid: {p: 1.0, i: 0.0, d: 0.0}
And here is the full /src repo in case my description is poor:
https://github.com/Benasking7124/gazebo_test
Thanks again ~

URDF model keeps disappearing whenever I add intertial properties to a link? How can I resolve this issue?

I am trying to simulate a simple pendulum using pydrake and am having a couple of issues creating the urdf file to creat the model for the pendulum. I have no background knowledge on urdf or drake and am open to any suggestions to resolve this issue.
This is the code I have so far:
import os
from tempfile import mkdtemp
from pydrake.all import (
AddMultibodyPlantSceneGraph,
DiagramBuilder,
Meshcat,
MeshcatVisualizerCpp,
Parser,
Simulator,
)
from manipulation.meshcat_cpp_utils import (
StartMeshcat, MeshcatJointSlidersThatPublish)
# Start the visualizer.
meshcat = StartMeshcat()
tmp_dir = mkdtemp()
model_file = os.path.join(tmp_dir, "test_model.urdf")
model_text = f"""\
<?xml version="1.0"?>
<robot name="materials">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<link name="base">
<visual>
<geometry>
<box size="0.5 0.5 0.1"/>
</geometry>
</visual>
<inertial>
<mass value="1"/>
</inertial>
</link>
<joint name="world_2_base" type="fixed">
<parent link="world"/>
<child link="base"/>
<origin xyz="0 0 0.5"/>
</joint>
<link name="arm">
<visual>
<geometry>
<cylinder length="0.5" radius="0.01"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<joint name="base" type="revolute">
<parent link="base"/>
<child link="arm"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 -0.3"/>
</joint>
<link name="ball">
<visual>
<geometry>
<sphere radius="0.1"/>
</geometry>
<material name="green"/>
</visual>
</link>
<joint name="base2" type="fixed">
<parent link="arm"/>
<child link="ball"/>
<origin xyz="0 0 -0.35"/>
</joint>
</robot>
"""
# Write temporary file.
with open(model_file, "w") as f:
f.write(model_text)
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
# Add pendulum model.
model = Parser(plant, scene_graph).AddModelFromFile(model_file)
# - Weld base at origin.
base_link = plant.GetBodyByName("base", model)
#plant.WeldFrames(plant.world_frame(), base_link.body_frame())
plant.Finalize()
visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(5.0)
This code gives me a blanck meshcat screen. But when I remove the inertia tags in the base link I can see a model of the pendulum. Please let me know what I am doing wrong. Also if you can reccomend some resources to learn about urdf and drake that would be really helpful.
Thank you very much.
I believe that the problem is that you have no mass in the arm, so when you go to simulate it, the simulation is ill-defined (probably you get NaNs in the state variables).
If you want to check just the visualization, I'd recommend doing
context = diagram.CreateDefaultContext()
diagram.Publish(context)
instead of the simulator step.
I have started working on a proper tutorial for precisely this. It's very much a draft right now, but in case it helps you can find it here. I intend for this to be one of the real Drake tutorials soon.

Pressure topic is not published when add barometric pressure Sensor with geometry_msgs/PointStamped

To use http://wiki.ros.org/hector_pose_estimation?distro=noetic hector_pose_estimation package i need to fuse IMU and barometric pressure Sensor with geometry_msgs/PointStamped So I add such barometric pressure Sensor with geometry_msgs/PointStamped with gazebo_ros_plugins.
Here is the urdf.xacro of the barometric pressure Sensor
<?xml version="1.0"?>
<!-- Copyright (c) 2016 The UUV Simulator Authors.
All rights reserved.
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://www.ros.org/wiki/xacro">
<xacro:macro name="no_collision">
<collision>
<geometry>
<cylinder length="${0.000001}" radius="${0.000001}" />
</geometry>
<origin xyz="0 0 0" rpy="0 ${0.5*pi} 0"/>
</collision>
</xacro:macro>
<xacro:macro name="default_pressure_macro" params="namespace parent_link *origin">
<xacro:pressure_plugin_macro
namespace="${namespace}"
suffix=""
parent_link="${parent_link}"
topic="pressure"
mass="0.015"
update_rate="10"
range="30000"
noise_sigma="3.0"
noise_amplitude="0.0"
estimateDepth="false"
standardPressure="101.325"
kPaPerM="9.80638">
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
<xacro:insert_block name="origin" />>
</xacro:pressure_plugin_macro>
</xacro:macro>
<xacro:macro name="default_pressure" params="namespace parent_link *origin">
<xacro:pressure_plugin_macro
namespace="${namespace}"
suffix=""
parent_link="${parent_link}"
topic="pressure"
mass="0.015"
update_rate="10"
range="30000"
noise_sigma="3.0"
noise_amplitude="0.0"
estimateDepth="false"
standardPressure="101.325"
kPaPerM="9.80638">
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
<xacro:insert_block name="origin" />
</xacro:pressure_plugin_macro>
</xacro:macro>
<xacro:macro name="pressure_plugin_macro"
params="namespace suffix parent_link topic mass update_rate
range noise_sigma noise_amplitude estimateDepth standardPressure kPaPerM
*inertia *origin">
<link name="${namespace}/pressure${suffix}_link">
<inertial>
<xacro:insert_block name="inertia" />
<mass value="${mass}" /> <!-- [kg] -->
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
<visual>
<geometry>
<mesh filename="file://$(find uuv_sensor_ros_plugins)/meshes/pressure.dae" scale="1 1 1"/>
</geometry>
</visual>
<xacro:no_collision/>
</link>
<joint name="${namespace}/pressure${suffix}_joint" type="revolute">
<xacro:insert_block name="origin" />
<parent link="${parent_link}" />
<child link="${namespace}/pressure${suffix}_link" />
<limit upper="0" lower="0" effort="0" velocity="0" />
<axis xyz="1 0 0"/>
</joint>
<gazebo>
<plugin filename="librotors_gazebo_pressure_plugin.so" name="rotors_gazebo_pressure_sensor${suffix}_plugin">
<robot_namespace>${namespace}</robot_namespace> <!-- (string, required): ros namespace in which the messages are published -->
<linkName>${namespace}/pressure${suffix}_link</linkName> <!-- (string, required): name of the body which holds the IMU sensor -->
<sensor_topic>${topic}</sensor_topic> <!-- (string): name of the sensor output topic and prefix of service names (defaults to imu) -->
<update_rate>${update_rate}</update_rate> <!-- Update period of accelerometer and gyroscope [s] -->
<saturation>${range}</saturation> <!-- measurement range [kPa] -->
<noise_sigma>${noise_sigma}</noise_sigma> <!-- measurement stddev [kPa] -->
<noise_amplitude>${noise_amplitude}</noise_amplitude>
<estimate_depth_on>${estimateDepth}</estimate_depth_on> <!-- infer depth? -->
<standard_pressure>${standardPressure}</standard_pressure> <!-- pressure at sea level (depth 0 m) [kPa] -->
<kPa_per_meter>${kPaPerM}</kPa_per_meter> <!-- increase in pressure [kPa] per [m] in increased depth -->
<enable_gazebo_messages>false</enable_gazebo_messages>
</plugin>
</gazebo>
</xacro:macro>
</robot>
I can compile and run Gazebo and got this line in blue [Dbg] [gazebo_pressure_plugin.cpp:39] _model = rexrov2
When run rostopic list the pressure topic is not published.The plugin file librotors_gazebo_pressure_plugin is there.
Any help?

How to properly add a torsional_spring element to a RigidBodyTree in Drake

I am using the Drake simulator, and attempting to add joint stiffness to the Acrobot demo found in the underactuated robotics repo, and so far have been unable to successfully integrate such stiffness into the simulation.
I am running the script torque_slider_demo.py, which references the URDF file acrobot.urdf. I modified this URDF to include a torsional spring entry as follows:
<torsional_spring stiffness = "100000000" rest_angle = ".75">
<joint name = "shoulder"/>
</torsional_spring>
This element is added after defining joints (Full XML shown below), and I increased damping in both joints to more quickly see steady-state behavior.
<?xml version="1.0"?>
<robot xmlns="http://drake.mit.edu"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
name="Acrobot">
<link name="base_link">
<visual>
<geometry>
<box size="0.2 0.2 0.2" />
</geometry>
<material name="green">
<color rgba="0 1 0 1" />
</material>
</visual>
</link>
<link name="upper_link">
<inertial>
<origin xyz="0 0 -.5" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="0.083" iyz="0" izz="1" />
</inertial>
<visual>
<origin xyz="0 0 -0.5" rpy="0 0 0" />
<geometry>
<cylinder length="1.1" radius="0.05" />
</geometry>
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
</link>
<link name="lower_link">
<inertial>
<origin xyz="0 0 -1" rpy="0 0 0" />
<mass value="1" />
<inertia ixx="1" ixy="0" ixz="0" iyy="0.33" iyz="0" izz="1" />
</inertial>
<visual>
<origin xyz="0 0 -1" rpy="0 0 0" />
<geometry>
<cylinder length="2.1" radius=".05" />
</geometry>
<material name="blue">
<color rgba="0 0 1 1" />
</material>
</visual>
</link>
<joint name="shoulder" type="continuous">
<parent link="base_link" />
<child link="upper_link" />
<origin xyz="0 0.15 0" />
<axis xyz="0 1 0" />
<dynamics damping="1" />
</joint>
<joint name="elbow" type="continuous">
<parent link="upper_link" />
<child link="lower_link" />
<origin xyz="0 0.1 -1" />
<axis xyz="0 1 0" />
<dynamics damping="1" />
</joint>
<torsional_spring stiffness = "100000000" rest_angle = ".75">
<joint name = "shoulder"/>
</torsional_spring>
<transmission type="SimpleTransmission" name="elbow_trans">
<actuator name="elbow" />
<joint name="elbow" />
<mechanicalReduction>1</mechanicalReduction>
</transmission>
</robot>
With this absurdly high stiffness value, I would expect for the arm to stabilize around the resting angle of the spring and exhibit an obvious pull towards the resting angle, however I do not see these effects in the visualization produced by the script.
Am I improperly defining the torsional spring in the URDF? Do I need to do some other setup to tell the simulator to consider joint stiffness?
It looks like we have support for torsional springs, but don't actually load them from URDF (nor expose the interface in python):
https://github.com/RobotLocomotion/drake/blob/master/attic/multibody/test/rigid_body_tree/rigid_body_tree_spring_test.cc
It would not be hard to add, and we'd welcome the contribution if you want to add the few lines to the parser. The drake developers are focused on multibodyplant now (which will replace rigidbodyplant).