Running ROS melodic, gazebo version 9.11.0 and the official Gazebo/PR2 plugin.
I am using a PR2 robot simulated within gazebo and sending control commands through ROS. However the robot is moving at most within 0.25 m/s while the maximum speed is 1 m/s (as per specification). I'm using the teleop application provided by PR2/Gazebo plugin.
The PR2 teleop launch file teleop_keyboard.launch correctly indicates the correct value
<launch>
<node pkg="pr2_teleop" type="teleop_pr2_keyboard" name="spawn_teleop_keyboard" output="screen">
<remap from="cmd_vel" to="robot1/base_controller/command" />
<param name="walk_vel" value="0.5" />
<param name="run_vel" value="1.0" />
<param name="yaw_rate" value="1.0" />
<param name="yaw_run_rate" value="1.5" />
</node>
</launch>
Accessing the robot1 odometry directly indicates the problem: the maximum speed is somehow capped at 0.261 m/s. But the walking velocity in pr2_teleop is set to 0.5 m/s!
twist:
twist:
linear:
x: 0.261318236589
y: 4.47927095593e-06
z: 0.0
angular:
x: 0.0
y: 0.0
z: -1.72120462594e-06
covariance: <removed>
According to teleop_pr2_keyboard.cpp the values of geometry_msgs::Twist have to be:
0.5 for twist.twist.linear.x -> forward moving
-0.5 for twist.twist.linear.x -> backward moving
and so on, see http://docs.ros.org/en/melodic/api/pr2_teleop/html/teleop__pr2__keyboard_8cpp_source.html.
My question is how do you get a so minimal deviation in linear.y and angular.z and did you change something in the above mentioned file?
Related
I really want to solve this problem.
This is my environment: jetson nano ubuntu 18.04 ros: melodic imu:
mpu6050 lidar: cyglidar_d1 and I got
transform from map to base_link failed this error
cyglidar_d1 launch file I will just use 2d so modify run_mode to 0. [1]
hector slam mapping_default file [2]base frame -> base_linkodom_frame -> base_link
<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser_link 100"/>
<include file="$(find hector_imu_attitude_to_tf)/launch/example.launch"/>
hector slam tutorial sim time = false
tf_rqt_tree image [3] [fixed frame = map][4] [fixed frame = base_link][5]
rqt graph [6] I can see lidar scan_laser, imu data on rostopic list I want to use navigation after mapping. I'd appreciate it if you could
tell me which part I should edit here Thank you [1]:
https://i.stack.imgur.com/cailz.png [2]:
https://i.stack.imgur.com/xB9nb.png [3]:
https://i.stack.imgur.com/0aHSM.png [4]:
https://i.stack.imgur.com/AOyNn.png [5]:
https://i.stack.imgur.com/zX8i2.png [6]:
https://i.stack.imgur.com/eeWXU.png
ndt_matching succeeded in autoware, but the vehicle model cannot be set correctly.
How do I set the correct angle for the vehicle model?
What does the frame "mobility" mean?
tf.launch
<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map 10" />
<node pkg="tf" type="static_transform_publisher" name="map_to_points_map" args="0 0 0 0 0 0 /map /points_map 10" />
<node pkg="tf" type="static_transform_publisher" name="velodyne_to_lidar_top" args="0 0 0 0 0 0 /velodyne /lidar_top 10" />
Image for RViz
Image for TF Tree
The settings in the TF file were correct.
To change the angle of the vehicle model, I made the following settings.
Change the yaw setting of Baselink to Localizer in the Setup tab (in the direction you want the vehicle model to point).
Set the yaw setting of ndt_matching to offset it.(if baselink angle(1) is -1.55, here it is +1.55)
I wrote an article about these issues, Thank you JWCS!
https://medium.com/yodayoda/localization-with-autoware-3e745f1dfe5d
I am trying to model an elastic bouncing ball in drake. However, I have not figured out how to set something like the coefficient of restitution for the urdf model I load. Does drake support elastic collisions for the point contact model? If yes how can I set the respective parameters?
Edit: I already tried setting the penetration allowance with plant.set_penetration_allowance(0.0001) but I got the following error: AttributeError: 'MultibodyPlant_[float]' object has no attribute 'set_penetration_allowance'. But since it models a critically damped system I assume it would not help with my problem anyways.
My current code looks as follows:
plane_friction_coef = CoulombFriction(static_friction=1.0, dynamic_friction=1.0)
# generate the diagram of the system
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant=plant)
# connect to Drake Visualizer
lcm = DrakeLcm()
ConnectDrakeVisualizer(builder, scene_graph, lcm=lcm)
# add plane to plant
X_WP = xyz_rpy_deg(xyz=[0, 0, 0], rpy_deg=[0,0,0]) # offset and orientation of plane wrt.
world frame
plant.RegisterVisualGeometry(plant.world_body(), X_BG=X_WP, shape=HalfSpace(),
name='InclinedPlaneVisualGeometry',
diffuse_color=np.array([1, 1, 1, 0.999]))
plant.RegisterCollisionGeometry(plant.world_body(), X_BG=X_WP, shape=HalfSpace(),
name='InclinedPlaneCollisionGeometry',
coulomb_friction=plane_friction_coef)
# set gravity in world
plant.mutable_gravity_field().set_gravity_vector(gravity_vec)
# add object from sdf or urdf file
my_object = parser.AddModelFromFile(obj_file_path, model_name='my_object')
plant.Finalize()
# add a logger
logger = LogOutput(plant.get_state_output_port(), builder)
logger.set_name('logger')
logger.set_publish_period(1 / recording_rate)
# build diagram and set its context
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)
plant.SetPositionsAndVelocities(plant_context, gen_pos)
# start simulation
simulator = Simulator(diagram, diagram_context)
simulator.Initialize()
simulator.set_target_realtime_rate(1)
simulator.AdvanceTo(sim_time)
time_log = logger.sample_times()
state_log = logger.data()
The urdf file I load looks like this:
<?xml version="1.0"?>
<robot name="my_ball">
<material name="Black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<link name="base_link">
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="5"/>
<inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.05"/>
</inertial>
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="Black"/>
</visual>
<collision name='collision'>
<geometry>
<sphere radius="0.2"/>
</geometry>
<drake:proximity_properties>
<drake:mu_dynamic value="1.0" />
<drake:mu_static value="1.0" />
</drake:proximity_properties>
</collision>
</link>
</robot>
Nicolas,
No, currently we do not support elastic collisions given we focused our efforts on slowly approaching contact surfaces as it is the case with manipulation applications. We will definitely support this as our contact solver matures.
That being said, currently there is no support to specify a coefficient of restitution for your model.
The best solution will depend on your particular problem. Is this a vertically bouncing ball? is friction important? (i.e. can the ball also move horizontally?) is it a 2D or 3D case?
From simpler to more complex I'd suggest:
If a vertically bouncing ball, 1DOF, then I'd suggest writing a the dynamics by hand in a LeafSystem.
An "event driven" hybrid model is also possible. And you have an example in Drake here, though probably an advanced used of Drake.
You can create your own LeafSystem that given the state of an MBP as input, computes a contact force as its output (for instance using something like a Hertz model with Hunt-Crossley dissipation). You'd then wire up the applied force through the MBP port MBP::get_applied_spatial_force_input_port().
I hope this helps you out.
My xml file is test.xml. See below
<?xml version="1.0" ?>
<Output>
<partialstore /> <!-- The code writes the spectrum at Sun position for each species on a FITS file (optional) -->
<fullstore /> <!-- The code writes the complete (r,z,p) grid of propagated particles for each species on a FITS file (optional) -->
<feedback value="2" /> </Output>
<Diffusion type="Constant"> <!-- Spatial distribution of the diffusion coefficient; options: Constant, Exp, Qtau -->
<!-- In Constant mode: D(Rigidity) = beta^etaT * D0 * (Rigidity/4GV)^delta -->
<!-- In Exp mode: D(Rigidity,z) = beta^etaT * D0 * (Rigidity/4GV)^delta * exp(z/zt) -->
<D0_1e28 value="2.7" /> <!-- Normalization of the diffusion coefficient at reference rigidity DiffRefRig Unit: 10^28 cm^2/s -->
<DiffRefRig value = "4" /> <!-- Reference rigidity for the normalization of the diffusion coefficient -->
<!-- NOTE: the reference rigidity 4 GV is stored in the const D_ref_rig defined in include/constants.h -->
<Delta value="0.6" /> <!-- Slope of the diffusion coefficient spectrum -->
<zt value="4" /> <!-- Scale heigth of the diffusion coefficient, useful in Exp mode: D(z) \propto exp(z/zt) (optional) -->
<etaT value="1." /> <!-- Low energy correction factor of the diffusion coefficient: D \propto beta^etaT --> </Diffusion>
I want to parse this xml with python. There are two root elements output and diffusion. I want to add 0.6 with D0_1e28 value and Delta_value so that those become 3.1 and 1.2 respectively and save the test.xml as a test_mod.xml with the modification. How can I do it with python code?
If I understand you correctly, this should work, using xpath with lxml:
xml = """
<root>
[your xml above; note that it was an invalid xml file, so I added a root]
</root>
"""
import lxml.etree as et
tree = et.fromstring(xml)
to_change = ['//Diffusion/D0_1e28','//Delta'] #those are the items you are changing
for i in range(len(to_change)):
item=tree.xpath(to_change[i])[0] #xpath returns a list of 1 item, so [0] selects it
item.attrib['value']= str(float(item.attrib['value'])+0.6)
#the "value" attribute has a value of type 'string' which needs to be cast to
#'float' to increment it by 0.6, and then back to 'string` to change the value
#of the 'value' attribute
mydata=et.tostring(tree)
with open("test_mod.xml", "wb") as myfile:
myfile.write(mydata)
I have a 5 dimensional matrix in an hdf5 data file. I would like to plot this data using paraview. The solution I have in mind is describing the data via the Xdmf Format.
The 5 dimensional matrix is structured as follows:
matrix[time][type][x][y][z]
The 'time' index specifies a time step. The 'type' selects the matrices for different particle types. And x,y,z describes the spatial coordinates of a grid. The value of the matrix is a Scalar that I would like to plot.
My question is: How can I select a specific 3 dimensional matrix for a given time step and type to plot, using the xdmf format? Ideally the timestep can be represented by the <time> functionality of Xdmf.
I tried the 'hyperslab' functionality of xdmf, but that seems to not reduce the dimensionallity to, which I need to to plot the grid.
I also had a look at the 'SubSet' functionality, but I did not understand how to use it, by reading the official documentation at xdmf.
With help of the mailing list of Xdmf I found a solution that works for me.
My input matrix is 5-dim (1,2,12,6,6) in the hdf5 file "ana.h5" and I select timestep 0 and type 1.
<?xml version="1.0" ?>
<!DOCTYPE Xdmf SYSTEM "Xdmf.dtd" []>
<Xdmf xmlns:xi="http://www.w3.org/2003/XInclude" Version="2.2">
<Domain>
<Topology name="topo" TopologyType="3DCoRectMesh" Dimensions="12 6 6"></Topology>
<Geometry name="geo" Type="ORIGIN_DXDYDZ">
<!-- ORigin -->
<DataItem Format="XML" Dimensions="3">
0.0 0.0 0.0
</DataItem>
<!-- DxDyDz -->
<DataItem Format="XML" Dimensions="3">
1 1 1
</DataItem>
</Geometry>
<Grid Name="TimeStep_0" GridType="Uniform">
<Topology Reference="/Xdmf/Domain/Topology[1]"/>
<Geometry Reference="/Xdmf/Domain/Geometry[1]"/>
<Time Value="64"/>
<Attribute Type="Scalar" Center="Cell" Name="Type1">
<!-- Result will be 3 dimensions -->
<DataItem ItemType="HyperSlab" Dimensions="12 6 6 ">
<!-- The source is 5 dimensions -->
<!-- Origin=0,1,0,0,0 Stride=1,1,1,1,1 Count=1,1,12,6,6 -->
<DataItem Dimensions="3 5" Format="XML">
0 1 0 0 0
1 1 1 1 1
1 1 12 6 6
</DataItem>
<DataItem Format="HDF" NumberType="UInt" Precision="2" Dimensions="1 2 12 6 6 ">
ana.h5:/density_field
</DataItem>
</DataItem>
</Attribute>
</Grid>
</Domain>
</Xdmf>
The resulting matrix is 3 dimensional (12,6,6) and plotable with paraview.