modelling elastic collisions in drake - drake

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.

Related

I'm getting some trouble with hector slam mapping (cyglidar_d1)

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

Correct tf frames setting in ndt_matching

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

Gazebo simulated PR2 robot maximum speed is not respected

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?

Gazebo / Ros: How to create a camera plugin with pixel-level segmentation?

I am looking to create a camera plugin where, at each pixel of the image, I'm able to output what object it belongs to, if any. I've struggled to find a solution to this problem. Any suggestions regarding where to begin?
If u want to make a camera is Gazebo simulation than u have to use the sensor plugin or sensor element in ur robot sdf/urdf model like described here,, U can find both type of camera their, depth and rgb. For example if u want a kinect sensor(camera) which contains both the rgb and depth image than u can use bolow sdf lines in ur robot model. Here when u run this code it will publish both rgb and depth datas as shown here:, here i've used ray sensor.
<gazebo reference="top">
<sensor name='camera1' type='depth'>
<always_on>1</always_on>
<visualize>1</visualize>
<camera name='__default__'>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<depth_camera>
<output>depths</output>
</depth_camera>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<plugin name='camera_controller' filename='libgazebo_ros_openni_kinect.so'>
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<cameraName>camera</cameraName>
<frameName>/camera_link</frameName>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudCutoff>0.4</pointCloudCutoff>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
<CxPrime>0.0</CxPrime>
<Cx>0.0</Cx>
<Cy>0.0</Cy>
<focalLength>0.0</focalLength>
</plugin>
</sens
or>
</gazebo

Describing 5 dimensional hdf5 matrix with Xdmf

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.

Resources