Skip to content

Ekf positioning #40

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
aircad wants to merge 18 commits into
base: dev
Choose a base branch
from
Open

Ekf positioning #40

Show file tree
Hide file tree
Changes from 13 commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
fc808f0
march 12 work progress
aircad Mar 12, 2020
05211d0
added listener classes for ekf
aircad Mar 17, 2020
938ffb7
added world positioning ekf for sub and 1 object
aircad Mar 19, 2020
603f8a4
Merge branch 'dev' into ekf_positioning
aircad Mar 19, 2020
ffcb0aa
made changes suggested by Amaar
aircad Mar 20, 2020
0e20da1
Merge branch 'ekf_positioning' of https://github.com/Waterloo-Aquadro…
aircad Mar 20, 2020
17d1d57
git glitch, had to re-clone
aircad Apr 5, 2020
3cc5fd1
github glitch, had to re-clone, also did the changes request in pr re…
aircad Apr 5, 2020
a0bfd4e
Merge branch 'ekf_positioning' of https://github.com/Waterloo-Aquadro…
aircad Apr 5, 2020
76111fa
fixed pr requested changes
aircad Apr 5, 2020
364fe78
made x absolute and fixed h function as per pr. Also needed to fix gi…
aircad Apr 11, 2020
670dfee
Merge branch 'ekf_positioning' of https://github.com/Waterloo-Aquadro…
aircad Apr 11, 2020
4b55869
made x absolute position and fixed h function as per pr and needed to…
aircad Apr 11, 2020
e087fcd
Merge branch 'ekf_positioning' of https://github.com/Waterloo-Aquadro…
aircad May 26, 2020
17b3ebd
removed vision to align with old ekf_positioning branch
aircad May 26, 2020
e4f0c7d
made node.py and made the code work (with vision node that is also un…
aircad May 26, 2020
1878c9c
Initial commit
aircad May 26, 2020
badaf1b
remove temp.txt
aircad May 26, 2020
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
9 changes: 1 addition & 8 deletions aquadrone_description/models/v2.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,6 @@
<!-- Input debug flag -->
<xacro:arg name="debug" default="0"/>


<!-- Describing the dimensions of the vehicle's bounding box -->
<xacro:property name="strut_thickness" value="0.02"/>
<xacro:property name="length" value="0.51"/>
<xacro:property name="width" value="0.46"/>
<xacro:property name="height" value="0.2"/>


<!-- Include the ROV macro file -->
<xacro:include filename="$(find aquadrone_description)/urdf/aquadrone_v2.urdf.xacro"/>
Expand Down Expand Up @@ -64,4 +57,4 @@
</xacro:thruster_macro>


</robot>
</robot>
29 changes: 10 additions & 19 deletions aquadrone_description/models/v28.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,6 @@
<xacro:arg name="debug" default="0"/>


<xacro:property name="strut_thickness" value="0.02"/>

<!-- Describing the dimensions of the vehicle's bounding box -->
<xacro:property name="length" value="0.51"/>
<xacro:property name="width" value="0.46"/>
<xacro:property name="height" value="0.2"/>


<!-- Include the ROV macro file -->
<xacro:include filename="$(find aquadrone_description)/urdf/aquadrone_v2.urdf.xacro"/>
<xacro:include filename="$(find aquadrone_description)/urdf/thruster.urdf.xacro"/>
Expand All @@ -26,8 +18,7 @@
<!-- Pressure -->
<xacro:include filename="$(find uuv_sensor_ros_plugins)/urdf/pressure_snippets.xacro"/>
<xacro:default_pressure namespace="aquadrone/out/" parent_link="base_link">
<origin xyz="0 0 ${0.11 + 0.2/2.0}" rpy="0 ${pi/2.0} 0"/>

<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:default_pressure>

<!-- IMU -->
Expand All @@ -39,38 +30,38 @@

<xacro:include filename="$(find aquadrone_description)/urdf/simple_camera.urdf.xacro"/>
<xacro:simple_camera_macro name="front_cam">
<origin xyz="${0.91/2.0 - 0.08 - 0.22/2.0} 0 ${0.11 + 0.2/2.0}" rpy="0 0 0"/>
<origin xyz="0.5 0 0.2" rpy="0 0 0"/>
</xacro:simple_camera_macro>

<!-- UP/DOWN-->
<xacro:thruster_macro thruster_id="0">
<origin xyz="0.2 0.285 0.1" rpy="0 ${pi/2} 0"/>
<origin xyz="0.4 0.4 -0.2" rpy="0 ${pi/2} 0"/>
</xacro:thruster_macro>
<xacro:thruster_macro thruster_id="1">
<origin xyz="0.2 -0.285 0.1" rpy="0 ${pi/2} 0"/>
<origin xyz="0.4 -0.4 -0.2" rpy="0 ${pi/2} 0"/>
</xacro:thruster_macro>

<xacro:thruster_macro thruster_id="2">
<origin xyz="-0.2 0.285 0.1" rpy="0 ${pi/2} 0"/>
<origin xyz="-0.4 0.4 -0.2" rpy="0 ${pi/2} 0"/>
</xacro:thruster_macro>
<xacro:thruster_macro thruster_id="3">
<origin xyz="-0.2 -0.285 0.1" rpy="0 ${pi/2} 0"/>
<origin xyz="-0.4 -0.4 -0.2" rpy="0 ${pi/2} 0"/>
</xacro:thruster_macro>

<!-- FORWARD/BACKWARD-->
<xacro:thruster_macro thruster_id="4">
<origin xyz="0 0.285 0.1" rpy="0 0 0"/>
<origin xyz="0 0.4 0" rpy="0 0 0"/>
</xacro:thruster_macro>
<xacro:thruster_macro thruster_id="5">
<origin xyz="0 -0.285 0.1" rpy="0 0 0"/>
<origin xyz="0 -0.4 0" rpy="0 0 0"/>
</xacro:thruster_macro>

<!-- LEFT/RIGHT-->
<xacro:thruster_macro thruster_id="6">
<origin xyz="0 0.17 0" rpy="0 0 ${-pi/2}"/>
<origin xyz="0.4 0 0.05" rpy="0 0 ${-pi/2}"/>
</xacro:thruster_macro>
<xacro:thruster_macro thruster_id="7">
<origin xyz="0 -0.17 0" rpy="0 0 ${-pi/2}"/>
<origin xyz="-0.4 0 0.05" rpy="0 0 ${-pi/2}"/>
</xacro:thruster_macro>


Expand Down
7 changes: 1 addition & 6 deletions aquadrone_description/models/v2_wobbly.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,6 @@

<!-- Input debug flag -->
<xacro:arg name="debug" default="0"/>

<xacro:property name="strut_thickness" value="0.02"/>
<xacro:property name="length" value="0.51"/>
<xacro:property name="width" value="0.46"/>
<xacro:property name="height" value="0.2"/>


<!-- Include the ROV macro file -->
Expand Down Expand Up @@ -62,4 +57,4 @@
</xacro:thruster_macro>


</robot>
</robot>
136 changes: 26 additions & 110 deletions aquadrone_description/urdf/aquadrone_v2.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,45 +21,16 @@
<!-- https://github.com/uuvsimulator/uuv_simulator/wiki/Templates-to-create-a-new-thruster-actuated-vehicle-model -->



<!--


Mass = 50.21 pounds
Volume = 1009.04 cubic inches
Surface area = 9076.65 square inches
Center of mass: ( inches )
X = -0.30
Y = -0.06
Z = 3.71
Principal axes of inertia and principal moments of inertia: ( pounds * square inches )
Taken at the center of mass.
Ix = ( 0.97, -0.07, 0.24) Px = 2940.79
Iy = ( 0.07, 1.00, 0.04) Py = 3445.89
Iz = (-0.24, -0.02, 0.97) Pz = 4069.04
Moments of inertia: ( pounds * square inches )
Taken at the center of mass and aligned with the output coordinate system.
Lxx = 3008.06 Lxy = -39.13 Lxz = 261.66
Lyx = -39.13 Lyy = 3443.46 Lyz = 4.14
Lzx = 261.66 Lzy = 4.14 Lzz = 4004.21
Moments of inertia: ( pounds * square inches )
Taken at the output coordinate system.
Ixx = 3699.83 Ixy = -38.18 Ixz = 206.63
Iyx = -38.18 Iyy = 4139.40 Iyz = -7.85
Izx = 206.63 Izy = -7.85 Izz = 4008.79


-->


<!-- Fluid density -->
<xacro:property name="rho" value="1028"/>

<xacro:property name="displaced_volume" value="${0.02074192626}"/>
<xacro:property name="bouyancy_fudge_factor" value="1.08"/>

<xacro:property name="mass" value="22.75"/>
<!-- Describing the dimensions of the vehicle's bounding box -->
<xacro:property name="length" value="0.9144"/>
<xacro:property name="width" value="0.3048"/>
<xacro:property name="height" value="0.2"/>

<xacro:property name="volume" value="${width * length * height}"/>
<xacro:property name="mass" value="${width * length * height * rho}"/>

<!-- Vehicle macro -->
<xacro:macro name="aquadrone_base" params="debug cog cob">
Expand All @@ -68,72 +39,26 @@ Taken at the output coordinate system.
<link name="base_link">
<inertial>
<mass value="${mass}" />
<origin xyz="-0.0076 -0.0015 0.094" rpy="0 0 0"/>
<!-- taken at the center of mass and aligned with the output coordinate system (Lxx) -->
<inertia ixx="3008" ixy="-39.13" ixz="261.66"
iyy="3443.46" iyz="4.14"
izz="4004" />
<origin xyz="${cog}" rpy="0 0 0"/>
<inertia ixx="1000" ixy="0" ixz="0"
iyy="1000" iyz="0"
izz="1" />
</inertial>

<!-- Top Plate -->
<visual>
<origin xyz="0 0 ${(height - strut_thickness)/2.0}" rpy="0 0 0"/>
<geometry>
<box size="${length} ${width} ${strut_thickness}"/>
</geometry>
</visual>

<!-- Bottom Plate -->
<visual>
<origin xyz="0 0 ${-(height - strut_thickness)/2.0}" rpy="0 0 0"/>
<geometry>
<box size="${length} ${width} 0.02"/>
</geometry>
</visual>

<!-- Verticals -->
<visual>
<origin xyz="${(length - strut_thickness)/2.0} ${(width - strut_thickness)/2.0} 0" rpy="0 0 0"/>
<geometry>
<box size="${strut_thickness} ${strut_thickness} ${height}"/>
</geometry>
</visual>
<visual>
<origin xyz="${-(length - strut_thickness)/2.0} ${(width - strut_thickness)/2.0} 0" rpy="0 0 0"/>
<geometry>
<box size="${strut_thickness} ${strut_thickness} ${height}"/>
</geometry>
</visual>
<visual>
<origin xyz="${(length - strut_thickness)/2.0} ${-(width - strut_thickness)/2.0} 0" rpy="0 0 0"/>
<geometry>
<box size="${strut_thickness} ${strut_thickness} ${height}"/>
</geometry>
</visual>
<visual>
<origin xyz="${-(length - strut_thickness)/2.0} ${-(width - strut_thickness)/2.0} 0" rpy="0 0 0"/>
<geometry>
<box size="${strut_thickness} ${strut_thickness} ${height}"/>
</geometry>
</visual>

<!-- Back Block -->
<visual>
<origin xyz="${-(length - 0.13)/2.0} 0 ${(height + 0.08)/2.0}" rpy="0 0 0"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.13 0.4 0.08"/>
<box size="${length} ${width} ${height}"/>
</geometry>
</visual>

<!-- Cylinder -->
<visual>
<origin xyz="${length/2.0 - 0.08 - 0.23/2.0} 0 ${0.11 + height/2.0}" rpy="0 ${pi/2} 0"/>
<origin xyz="${length/2} 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.11" length="0.23"/>
<sphere radius="${width/2}"/>
</geometry>
</visual>


<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
Expand All @@ -158,14 +83,10 @@ Taken at the output coordinate system.

<link name="base_link">
<!-- This flag will make the link neutrally buoyant -->

<neutrally_buoyant>0</neutrally_buoyant>

<center_of_buoyancy>${cob}</center_of_buoyancy>

<!-- Treat our bouyant area as a box -->
<!-- Width and length match the sub's box part-->
<!-- Height adapts to match the proper vloume -->
<volume>${displaced_volume*bouyancy_fudge_factor}</volume>
<volume>${volume*1.005}</volume>
<box>
<width>${width}</width>
<length>${length}</length>
Expand All @@ -184,23 +105,13 @@ Taken at the output coordinate system.
0 0 0 0 0 0
0 0 0 0 0 0
</added_mass>


<!--
The linear damping coefficients can be provided as a diagonal (6 elements)
or a full matrix (36 coefficients), like the added-mass coefficients above
-->
<!--
The quadratic damping coefficients can be provided as a diagonal (6 elements)
or a full matrix (36 coefficients), like the added-mass coefficients above
-->

<!-- From UUV_SIM rex_rov model: Linear damping: see p.31 in Berg2012 -->
<linear_damping>-74.82 -69.48 -728.4 -268.8 -309.77 -105</linear_damping>
<!-- From UUV_SIM rex_rov model: Non-linear damping: see p.30 in Berg2012 -->
<quadratic_damping>-748.22 -992.53 -1821.01 -672 -774.44 -523.27</quadratic_damping>


<linear_damping>
-3 -3 -3 -1 -1 -1
</linear_damping>
<!--
The linear damping coefficients proportional to the forward speed
can be provided as a diagonal (6 elements) or a full matrix (36 coefficients),
Expand All @@ -210,8 +121,13 @@ Taken at the output coordinate system.
<linear_damping_forward_speed>
0 0 0 0 0 0
</linear_damping_forward_speed>


<!--
The quadratic damping coefficients can be provided as a diagonal (6 elements)
or a full matrix (36 coefficients), like the added-mass coefficients above
-->
<quadratic_damping>
-1 -1 -1 -1 -1 -1
</quadratic_damping>

<!-- Link's bounding box, it is used to recalculate the immersed
volume when close to the surface.
Expand Down
6 changes: 3 additions & 3 deletions aquadrone_description/urdf/thruster.urdf.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,16 +5,16 @@
<link name="thruster_${thruster_id}">

<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<origin xyz="0.025 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.06 0.01 0.08"/>
<box size="0.01 0.01 0.1"/>
</geometry>
</visual>

<visual>
<origin xyz="0 0 0" rpy="0 ${pi/2} 0"/>
<geometry>
<cylinder radius="0.05" length="0.01"/>
<cylinder radius="0.01" length="0.05"/>
</geometry>
</visual>

Expand Down
Loading