Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
.vscode/
7 changes: 7 additions & 0 deletions stonefish_sim/config/structure_config.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
drone_position: "2.25 -16.5 6.5"
drone_orientation: "-1.57 0.0 0.0"
water_jerlow: "0.20"
seafloor_position: "0.0 0.0 3.0"
seafloor_orientation: "1.57 0.0 0.0"
structure_position: "1.0 -18.0 8.1"
structure_orientation: "-1.57 0.0 -1.57"
Comment on lines +1 to +7
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could be nice to add a comment behind every field here to indicate what it represents? For example

drone_position: "2.25 -16.5 6.5" # In NED coordinates

4 changes: 3 additions & 1 deletion stonefish_sim/config/tacc_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -4,4 +4,6 @@ water_jerlow: "0.20"
seafloor_position: "0.0 0.0 3.0"
seafloor_orientation: "1.57 0.0 0.0"
pipeline_position: "1.85 0.0 5.65"
pipeline_orientation: "-0.14 0.175 0.0"
pipeline_orientation: "-0.14 0.175 0.0"
structure_position: "1.0 -18.0 8.1"
structure_orientation: "-1.57 0.0 -1.57"
1 change: 1 addition & 0 deletions stonefish_sim/metadata/looks.scn
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
<scenario>
<looks>
<look name="Yellow" rgb="1.0 0.9 0.0" roughness="0.3"/>
<look name="Orange" rgb="1.0 0.55 0.0" roughness="0.3"/>
<look name="Gray" gray="0.1" roughness="0.4" metalness="0.5"/>
<look name="Green" rgb="0.0 0.2 0.0" roughness="0.9"/>
<look name="Red" rgb="0.8 0.0 0.0" roughness="0.9"/>
Expand Down
112 changes: 82 additions & 30 deletions stonefish_sim/objects/structure.scn
Original file line number Diff line number Diff line change
@@ -1,8 +1,5 @@
<?xml version="1.0"?>
<scenario>
<!-- ===========================================
Static structure (stable concave collisions)
=========================================== -->
<static name="structure_with_valves_base" type="model">
<physical>
<mesh filename="object_files/structure_mesh.obj" scale="1.0"/>
Expand All @@ -14,14 +11,19 @@
</visual>
<material name="Neutral"/>
<look name="Yellow"/>
<world_transform xyz="1.0 -18.0 8.1" rpy="-1.57 0.0 -1.57"/>
<world_transform
xyz="$(param structure_position)"
rpy="$(param structure_orientation)"/>
</static>

<!-- =========================
Valve 1 as its own robot
========================= -->
<robot name="valve1" fixed="true" self_collisions="false">
<base_link name="valve1_base" type="model" physics="submerged">
<base_link name="valve1_frame" type="box" physics="submerged">
<dimensions xyz="0.001 0.001 0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="Neutral"/><look name="Orange"/>
</base_link>

<link name="valve1_base" type="model" physics="submerged">
<physical>
<mesh filename="object_files/valve_base_mesh.obj" scale="1.0"/>
<origin rpy="-1.57 0 1.57" xyz="0 0 0"/>
Expand All @@ -30,9 +32,16 @@
<mesh filename="object_files/valve_base.obj" scale="1.0"/>
<origin rpy="-1.57 0 1.57" xyz="0 0 0"/>
</visual>
<material name="Steel"/>
<look name="Yellow"/>
</base_link>
<material name="Steel"/><look name="Orange"/>
</link>

<joint name="valve1_mount" type="fixed">
<parent name="valve1_frame"/>
<child name="valve1_base"/>
<origin
xyz="1.196441 1.42144 1.807916"
rpy="0.0 0.0 1.57"/>
Comment on lines +42 to +43
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These are still magic numbers, which could be moved to config file if that is wanted. Maybe in a future pull requests all such relationships (like the thruster positions in orca.scn) could be moved to yaml files too. That is issue material

</joint>

<link name="valve1_turnable" type="model" physics="submerged">
<physical>
Expand All @@ -43,8 +52,7 @@
<mesh filename="object_files/valve_turnable.obj" scale="1.0"/>
<origin rpy="-1.57 0 1.57" xyz="0 0 0.1"/>
</visual>
<material name="Steel"/>
<look name="Yellow"/>
<material name="Steel"/><look name="Orange"/>
</link>

<joint name="valve1_joint" type="revolute">
Expand All @@ -53,20 +61,21 @@
<axis xyz="0 1 0"/>
<limits min="0.0" max="1.57"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<!-- <damping value="2.0"/> -->
</joint>

<!-- Composed world pose for valve 1 -->
<world_transform
xyz="2.80999954 -19.19500003 6.68000014"
rpy="0.0 0.0 1.57079632"/>
xyz="$(param structure_position)"
rpy="$(param structure_orientation)"/>
</robot>

<!-- =========================
Valve 2 as its own robot
========================= -->
<robot name="valve2" fixed="true" self_collisions="false">
<base_link name="valve2_base" type="model" physics="submerged">
<base_link name="valve2_frame" type="box" physics="submerged">
<dimensions xyz="0.001 0.001 0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="Neutral"/><look name="Orange"/>
</base_link>

<link name="valve2_base" type="model" physics="submerged">
<physical>
<mesh filename="object_files/valve_base_mesh.obj" scale="1.0"/>
<origin rpy="-1.57 0 1.57" xyz="0 0 0"/>
Expand All @@ -75,9 +84,16 @@
<mesh filename="object_files/valve_base.obj" scale="1.0"/>
<origin rpy="-1.57 0 1.57" xyz="0 0 0"/>
</visual>
<material name="Steel"/>
<look name="Yellow"/>
</base_link>
<material name="Steel"/><look name="Orange"/>
</link>

<joint name="valve2_mount" type="fixed">
<parent name="valve2_frame"/>
<child name="valve2_base"/>
<origin
xyz="0.665 0.95140878 1.76872493"
rpy="0.0 0.0 0.0"/>
</joint>

<link name="valve2_turnable" type="model" physics="submerged">
<physical>
Expand All @@ -88,8 +104,7 @@
<mesh filename="object_files/valve_turnable.obj" scale="1.0"/>
<origin rpy="-1.57 0 1.57" xyz="0 0 0.1"/>
</visual>
<material name="Steel"/>
<look name="Yellow"/>
<material name="Steel"/><look name="Orange"/>
</link>

<joint name="valve2_joint" type="revolute">
Expand All @@ -98,12 +113,49 @@
<axis xyz="0 1 0"/>
<limits min="0.0" max="1.57"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<!-- <damping value="2.0"/> -->
</joint>

<!-- Composed world pose for valve 2 -->
<world_transform
xyz="2.77000017 -18.64999971 7.15"
rpy="0 1.57079632 1.57079632"/>
xyz="$(param structure_position)"
rpy="$(param structure_orientation)"/>
</robot>

<robot name="structure_markers" fixed="true" self_collisions="false">
<base_link name="markers_frame" type="box" physics="submerged">
<dimensions xyz="0.001 0.001 0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="Neutral"/>
<look name="Invisible"/>
Comment on lines +127 to +128
Copy link

Copilot AI Sep 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The formatting is inconsistent - these lines should be properly indented to match the surrounding XML structure.

Suggested change
<material name="Neutral"/>
<look name="Invisible"/>
<material name="Neutral"/>
<look name="Invisible"/>

Copilot uses AI. Check for mistakes.
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think you are wrong here buddy.

Comment on lines +127 to +128
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
<material name="Neutral"/>
<look name="Invisible"/>
<material name="Neutral"/>
<look name="Invisible"/>

my bad, didnt think it would mess up the indentation. I think this should be correct.

</base_link>

<link name="aruco_structure_01" type="box">
<dimensions xyz="0.15 0.15 0.002"/>
<material name="Neutral"/>
<look name="Aruco19" uv_mode="1"/>
<origin xyz="1.29 1.32144 0.25" rpy="-1.57 0.0 1.57"/>
</link>

<joint name="aruco_structure_01_joint" type="fixed">
<parent name="markers_frame"/>
<child name="aruco_structure_01"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<link name="aruco_structure_02" type="box">
<dimensions xyz="0.15 0.15 0.002"/>
<material name="Neutral"/>
<look name="Aruco28" uv_mode="1"/>
<origin xyz="0.05 0.975 0.65" rpy="1.57 0.0 1.57"/>
</link>

<joint name="aruco_structure_02_joint" type="fixed">
<parent name="markers_frame"/>
<child name="aruco_structure_02"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<world_transform
xyz="$(param structure_position)"
rpy="$(param structure_orientation)"/>
</robot>
</scenario>
9 changes: 5 additions & 4 deletions stonefish_sim/scenarios/structure.scn
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
<environment>
<ned latitude="63.446827" longitude="10.421906"/>
<ocean>
<water density="1031.0" jerlov="0.60"/>
<waves height="0.0"/>
<water density="1031.0" jerlov="$(param water_jerlow)"/>
Copy link

Copilot AI Sep 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The parameter name 'water_jerlow' appears to be misspelled. It should likely be 'water_jerlov' to match the jerlov attribute name.

Suggested change
<water density="1031.0" jerlov="$(param water_jerlow)"/>
<water density="1031.0" jerlov="$(param water_jerlov)"/>

Copilot uses AI. Check for mistakes.
<waves height="10.0"/>
Comment on lines +6 to +7
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Make waves height param too?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should be a separate issue, just copied the setup in the tacc.scn

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Set the height to 0 then probably, don't think tsunamies are common in Stavanger 😂

<particles enabled="true"/>
<current type="uniform">
<velocity xyz="0.1 0.1 0.0"/>
Expand All @@ -28,8 +28,9 @@

<include file="$(find stonefish_sim)/drones/orca.scn"/>

<include file="$(find stonefish_sim)/objects/tacc_seafloor.scn"/>

<include file="$(find stonefish_sim)/objects/structure.scn"/>

<include file="$(find stonefish_sim)/objects/default_seafloor.scn"/>

</scenario>
</scenario>