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
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
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?
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.
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.
I have tried to detect something from a tutorial. When training have finished, stage files and cascade file is created. I have knowledge about the algorithm but I don't know meaning of information inside these file.
<internalNodes>
0 -1 13569 2.8149113059043884e-003</internalNodes>
<leafValues>
9.8837211728096008e-002 -8.5897433757781982e-001</leafValues></_>
and
<rects>
<_>
0 0 3 1 -1.</_>
<_>
1 0 1 1 3.</_></rects>
<tilted>0</tilted></_>
What are the meanings of these values?
Let's start with first block:
<internalNodes>
0 -1 13569 2.8149113059043884e-003</internalNodes>
<leafValues>
9.8837211728096008e-002 -8.5897433757781982e-001</leafValues></_>
It describes one of the weak classifier. In such case it's stump based, i.e. it's tree with max depth is equal to 1. 0 and -1 it's indexes of left and right child of root node. If indexes less or equal to zero it indicates that it's leaf nodes. Note that to calculate leaf index you need to negate it. Next number (13569) is index of feature in <features> section. And next number (2.8149113059043884e-003) is node threshold. In leafValues section presented weights of leafs in cascade tree.
For example, in this weak classifier we need to calculate value of 13569 feature. Next, compare this value with threshold (2.8149113059043884e-003) and if it less that threshold than you need to add the first leaf value (9.8837211728096008e-002) else you need to add the second leaf value (-8.5897433757781982e-001).
Next section describes one of the Haar feature:
<rects>
<_>
0 0 3 1 -1.</_>
<_>
1 0 1 1 3.</_></rects>
<tilted>0</tilted></_>
It obviously describes parameters of rectangle (x, y, width, height) and the weight of rectangle. It also may be tilted, that indicates by <tilted>0</tilted> flag.
I hope it will help.