update scene
- update to new bunker poses - remove condition and add repeat decorator - update grasping positions due to the new bunker model - format gazebo world file xml and update positions of new scene
This commit is contained in:
parent
255c7b1b18
commit
1ad460f4a1
5 changed files with 116 additions and 104 deletions
BIN
rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL
Normal file
BIN
rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL
Normal file
Binary file not shown.
|
@ -6,41 +6,41 @@
|
|||
<visual name="bunker_visual">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://bunker/meshes/bunker.stl</uri>
|
||||
<uri>model://bunker/meshes/Bunker_for_labels_60х40.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<!-- <material> -->
|
||||
<!-- <diffuse>1 1 1 1</diffuse> -->
|
||||
<!-- <ambient>1 1 1 1</ambient> -->
|
||||
<!-- <specular>0.5 0.5 0.5 1</specular> -->
|
||||
<!-- <emissive>0 0 0 1</emissive> -->
|
||||
<!-- <texture> -->
|
||||
<!-- <diffuse_map>model://bunker/textures/shildik_sh.png</diffuse_map> -->
|
||||
<!-- </texture> -->
|
||||
<!-- <pbr> -->
|
||||
<!-- <metal> -->
|
||||
<!-- <albedo_map>model://bunker/textures/shildik_sh_d.png</albedo_map> -->
|
||||
<!-- <normal_map>model://bunker/textures/shildik_sh_n.png</normal_map> -->
|
||||
<!-- <roughness_map>model://bunker/textures/shildik_sh_r.png</roughness_map> -->
|
||||
<!-- <metalness_map>model://bunker/textures/shildik_sh_m.png</metalness_map> -->
|
||||
<!-- <ambient_occlusion_map>model://bunker/textures/shildik_sh_o.png</ambient_occlusion_map> -->
|
||||
<!-- </metal> -->
|
||||
<!-- </pbr> -->
|
||||
<!-- </material> -->
|
||||
<material>
|
||||
<diffuse>1 1 1 1</diffuse>
|
||||
<ambient>1 1 1 1</ambient>
|
||||
<specular>0.5 0.5 0.5 1</specular>
|
||||
<emissive>0 0 0 1</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<collision name="laser_collision">
|
||||
<collision name="bunker_collision">
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://bunker/meshes/bunker.stl</uri>
|
||||
<uri>model://bunker/meshes/Bunker_for_labels_60х40.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode />
|
||||
<ode>
|
||||
<kp>1e6</kp>
|
||||
<kd>1e3</kd>
|
||||
<max_vel>0.1</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
<bounce />
|
||||
<bounce>
|
||||
<restitution>0.3</restitution>
|
||||
</bounce>
|
||||
<friction>
|
||||
<ode />
|
||||
<ode>
|
||||
<mu>0.61</mu>
|
||||
<mu2>0.47</mu2>
|
||||
<slip1>0.0</slip1>
|
||||
<slip2>0.0</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
|
|
|
@ -1,8 +1,9 @@
|
|||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="Main">
|
||||
<Repeat num_cycles="5">
|
||||
<Sequence>
|
||||
<Condition ID="IsInPose" topic_name="/ee_pose"/>
|
||||
<!-- <Condition ID="IsInPose" topic_name="/ee_pose"/> -->
|
||||
<SubTree ID="ToGraver" robot_name="arm" />
|
||||
|
||||
<Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
|
||||
|
@ -13,10 +14,11 @@
|
|||
<Delay delay_msec="2000">
|
||||
<SubTree ID="FromGraver" robot_name="arm" />
|
||||
</Delay>
|
||||
<Action ID="MoveToJointState" joint_state="0.0;0.85;1.0;0.0;1.0;0.0" duration="3"
|
||||
robot_name="arm" action_name="/mtjs_jtc" />
|
||||
<!-- <Action ID="MoveToJointState" joint_state="0.0;0.85;1.0;0.0;1.0;0.0" duration="3" -->
|
||||
<!-- robot_name="arm" action_name="/mtjs_jtc" /> -->
|
||||
|
||||
</Sequence>
|
||||
</Repeat>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
|
||||
|
|
|
@ -3,7 +3,7 @@ to_graver:
|
|||
position:
|
||||
x: -0.3
|
||||
y: -0.20
|
||||
z: 0.1
|
||||
z: 0.2
|
||||
orientation:
|
||||
x: 0.707
|
||||
y: -0.707
|
||||
|
@ -23,7 +23,7 @@ to_graver:
|
|||
position:
|
||||
x: -0.3
|
||||
y: -0.20
|
||||
z: 0.1
|
||||
z: 0.2
|
||||
orientation:
|
||||
x: 0.707
|
||||
y: -0.707
|
||||
|
@ -96,7 +96,7 @@ from_graver:
|
|||
position:
|
||||
x: -0.3
|
||||
y: 0.20
|
||||
z: 0.1
|
||||
z: 0.2
|
||||
orientation:
|
||||
x: 0.707
|
||||
y: -0.707
|
||||
|
@ -116,7 +116,7 @@ from_graver:
|
|||
position:
|
||||
x: -0.3
|
||||
y: 0.20
|
||||
z: 0.1
|
||||
z: 0.2
|
||||
orientation:
|
||||
x: 0.707
|
||||
y: -0.707
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
name="gz::sim::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-contact-system"
|
||||
name="gz::sim::systems::Contact">
|
||||
name="gz::sim::systems::Contact">
|
||||
</plugin>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
|
@ -56,27 +56,27 @@
|
|||
|
||||
<include>
|
||||
<name>shildik_0</name>
|
||||
<pose>-0.5 -0.2 0.0 0 0 1.57</pose>
|
||||
<pose>-0.5 -0.2 0.02 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_1</name>
|
||||
<pose>-0.5 -0.2 0.0 0 0 1.57</pose>
|
||||
<pose>-0.5 -0.2 0.04 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_2</name>
|
||||
<pose>-0.5 -0.2 0.0 0 0 1.57</pose>
|
||||
<pose>-0.5 -0.2 0.05 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_3</name>
|
||||
<pose>-0.5 -0.2 0.0 0 0 1.57</pose>
|
||||
<pose>-0.5 -0.2 0.06 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
<include>
|
||||
<name>shildik_4</name>
|
||||
<pose>-0.5 -0.2 0.0 0 0 1.57</pose>
|
||||
<pose>-0.5 -0.2 0.07 0 0 1.57</pose>
|
||||
<uri>model://shildik</uri>
|
||||
</include>
|
||||
|
||||
|
@ -86,67 +86,67 @@
|
|||
<uri>model://laser</uri>
|
||||
</include>
|
||||
|
||||
<!-- <include> -->
|
||||
<!-- <static>true</static> -->
|
||||
<!-- <pose>0.0 -0.3 0.0752 1.57 0 3.14159</pose> -->
|
||||
<!-- <uri>model://bunker</uri> -->
|
||||
<!-- </include> -->
|
||||
<include>
|
||||
<static>true</static>
|
||||
<pose>-0.505 -0.20 0.06 0.0 0.0 1.57</pose>
|
||||
<uri>model://bunker</uri>
|
||||
</include>
|
||||
|
||||
<model name="rgbd_camera">
|
||||
<pose>0 -0.93 0.42 0 0.4 1.81</pose>
|
||||
<static>1</static>
|
||||
<link name="rgbd_camera">
|
||||
<model name="rgbd_camera">
|
||||
<pose>0 -0.93 0.42 0 0.4 1.81</pose>
|
||||
<static>1</static>
|
||||
<link name="rgbd_camera">
|
||||
|
||||
<!-- Visual -->
|
||||
<visual name="visual">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.01 0.01 0.01</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 0 1</ambient>
|
||||
<diffuse>0 0 0 1</diffuse>
|
||||
<specular>0 0 0 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- Visual -->
|
||||
<visual name="visual">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.01 0.01 0.01</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0 0 0 1</ambient>
|
||||
<diffuse>0 0 0 1</diffuse>
|
||||
<specular>0 0 0 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- Collision -->
|
||||
<collision name="collision">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.01 0.01 0.01</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<!-- Collision -->
|
||||
<collision name="collision">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.01 0.01 0.01</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<!-- RGBD Sensor -->
|
||||
<sensor name="rgbd_camera" type="rgbd_camera">
|
||||
<camera>
|
||||
<horizontal_fov>1.047</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.1</near>
|
||||
<far>3</far>
|
||||
</clip>
|
||||
<depth_camera>
|
||||
<output>depth</output>
|
||||
</depth_camera>
|
||||
</camera>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>/rgbd_camera/image</topic>
|
||||
<enable_metrics>true</enable_metrics>
|
||||
</sensor>
|
||||
</link>
|
||||
</model>
|
||||
<!-- RGBD Sensor -->
|
||||
<sensor name="rgbd_camera" type="rgbd_camera">
|
||||
<camera>
|
||||
<horizontal_fov>1.047</horizontal_fov>
|
||||
<image>
|
||||
<width>1920</width>
|
||||
<height>1080</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.1</near>
|
||||
<far>3</far>
|
||||
</clip>
|
||||
<depth_camera>
|
||||
<output>depth</output>
|
||||
</depth_camera>
|
||||
</camera>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>/rgbd_camera/image</topic>
|
||||
<enable_metrics>true</enable_metrics>
|
||||
</sensor>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
|
||||
<model name="table">
|
||||
|
@ -174,10 +174,20 @@
|
|||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode/>
|
||||
<ode>
|
||||
<mu>1.0</mu>
|
||||
<mu2>1.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<bounce/>
|
||||
<contact/>
|
||||
<bounce />
|
||||
<contact>
|
||||
<ode>
|
||||
<soft_cfm>0.00001</soft_cfm>
|
||||
<soft_erp>0.8</soft_erp>
|
||||
<kp>1e5</kp>
|
||||
<kd>1e3</kd>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='table_link_visual'>
|
||||
|
@ -187,17 +197,17 @@
|
|||
<size>1.2 0.69999999999999996 0.80000000000000004</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0 0 1 1</diffuse>
|
||||
<ambient>0 0 1 1</ambient>
|
||||
</material>
|
||||
<material>
|
||||
<diffuse>0.6 0.4 0.2 1</diffuse>
|
||||
<ambient>0.6 0.4 0.2 1</ambient>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<emissive>0 0 0 1</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
<enable_wind>false</enable_wind>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
|
||||
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue