Skip to content

Commit

Permalink
added the 3D model to gazebo and adapted the mission so it works once…
Browse files Browse the repository at this point in the history 
… simulated
  • Loading branch information
@tjcanro
tjcanro committed Apr 9, 2024
1 parent 9f3144c commit f2b211d
Show file tree
Hide file tree
Showing 2 changed files with 114 additions and 9 deletions.
21 changes: 12 additions & 9 deletions SubjuGator/command/subjugator_missions/subjugator_missions/hydrothermal_vent.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,21 +10,24 @@
# Computer vision aspect still required to detect the red buoy. For now, random position used
class HydrothermalVent(SubjuGatorMission):
async def run(self, args):
self.buoy_pos = np.array([3, 2, 4]) # Convert buoy position to numpy array
self.buoy_pos = np.array([3, 4, -2]) # Convert buoy position to numpy array

# First, move down the necessary depth to reach the buoy

self.send_feedback("Submerging to buoy depth")
await self.go(self.move().down(self.buoy_pos[1]))
yaw_angle = np.arctan(self.buoy_pos[2]/self.buoy_pos[0])
if (self.buoy_pos[2] < 0):
await self.go(self.move().down(abs(self.buoy_pos[2] + 0.3)))
else:
await self.go(self.move().up(self.buoy_pos[2] - 0.3))
yaw_angle = np.arctan(self.buoy_pos[1]/self.buoy_pos[0])
self.send_feedback(f"Rotating towards Buoy with yaw {math.degrees(yaw_angle)}")
rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle)
rotate = self.move().set_roll_pitch_yaw(0, 0, yaw_angle + 0.1)
await self.go(rotate)

self.send_feedback(f"Traveling forward to buoy")
await self.go(
self.move()
.forward(self.buoy_pos[2] - 1) # don't reach the buoy, remain 1 meter away
.forward(np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 0.7) # don't reach the buoy, remain 0.5 meter away
)
yaw_angle2 = np.deg2rad(90)
# rotate 90 degrees:
Expand All @@ -33,18 +36,18 @@ async def run(self, args):
await self.go(rotate)

self.send_feedback("Circumnaviganting the buoy")
await self.go(self.move().forward(0.5))
await self.go(self.move().forward(0.7))
for i in range(0, 3):
rotate = self.move().yaw_right(yaw_angle2)
await self.go(rotate)
await self.go(self.move().forward(1))
await self.go(self.move().forward(1.4))
rotate = self.move().yaw_right(yaw_angle2)
await self.go(rotate)
await self.go(self.move().forward(0.5))
await self.go(self.move().forward(0.7))

self.send_feedback("Returning to origin")
await self.go(self.move().yaw_left(yaw_angle2))
await self.go(self.move().forward(self.buoy_pos[2] - 1))
await self.go(self.move().forward(np.sqrt(np.square(self.buoy_pos[0]) + np.square(self.buoy_pos[1])) - 1))

await self.go(self.move().set_roll_pitch_yaw(0, 0, -yaw_angle))

Expand Down
102 changes: 102 additions & 0 deletions SubjuGator/simulation/subjugator_gazebo/worlds/robosub_2022.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -937,5 +937,107 @@
</link>
<pose>-0.957126 1.98002 0 0 -0 0</pose>
</model>
<model name='Red_buoy'>
<link name='link_0'>
<inertial>
<mass>0.0131332</mass>
<inertia>
<ixx>7.311e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>7.311e-05</iyy>
<iyz>0</iyz>
<izz>7.311e-05</izz>
</inertia>
<pose>0 0 0 0 -0 0</pose>
</inertial>
<pose>0 0 0 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name='visual'>
<pose>3 4 -2 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.15</radius>
</sphere>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<shader type='pixel'>
<normal_map>__default__</normal_map>
</shader>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
<emissive>1 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.117968</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
</link>
<static>1</static>
<allow_auto_disable>1</allow_auto_disable>
</model>
</world>
</sdf>

0 comments on commit f2b211d

Please sign in to comment.