def update_wings(self, sb_p, port_p):
sb_p = deg2rad(sb_p)
port_p = deg2rad(port_p)
a1 = -self.hinge1.getAngle()
a2 = -self.hinge2.getAngle()
d1 = limit(a1 - sb_p, -2, 2) * 10
d2 = limit(a2 - port_p, -2, 2) * 10
self.hinge1.getMotor1D().setSpeed(d1)
self.hinge2.getMotor1D().setSpeed(d2)
def pre(self, t):
"""
Args:
t:
"""
rot = self.rov.link1.getRotation()
current_depth = self.rov.getRigidBody('rovBody').getPosition()[2]
self.pid.compute(current_depth)
output_left = deg2rad(-self.pid.output) # -rot[0] * 100) # - self.pid_trim.output)
output_right = deg2rad(-self.pid.output) # -rot[0] * 100) # + self.pid_trim.output)
def update(self):
if self.time >= self.period:
self.dir = random.randint(0, 359)
self.time -= self.period
frame_span = self.clock.frame_span()
self.time += frame_span
delta_pos = frame_span * self.speed / 1000.
self.pos[0] += round(delta_pos * math.sin(deg2rad(self.dir)))
self.pos[1] += round(delta_pos * math.cos(deg2rad(self.dir)))
return self.pos
def __init__(self, pos, speed, params = {}):
"""
"""
Mover.__init__(self)
self._setattrs('dir, ang_speed', params)
self.pos = list(pos)
self.speed = speed
self.dir = deg2rad(self.dir)
self.ang_speed = deg2rad(self.ang_speed)
self.target = None
def main():
"""
main program
"""
#Collect user input
F1 = float(
input("What is the magnitude of the first force (in Newtons)? "))
theta1 = float(input("What is the angle of the first force (in deg)? "))
F2 = float(
input("What is the magnitude of the second force (in Newtons)? "))
theta2 = float(input("What is the angle of the second force (in deg)?"))
#Convert to radians
theta1_rad = functions.deg2rad(theta1)
theta2_rad = functions.deg2rad(theta2)
#x and y components of Fsum
F3x = F1 * functions.cos(theta1_rad) + F2 * functions.cos(theta2_rad)
F3y = F1 * functions.sin(theta1_rad) + F2 * functions.sin(theta2_rad)
print("Net force is: ", F3x, F3y)
def build_hinge(link,
part,
pos,
axis,
lock,
motor,
compliance,
range=None) -> agx.Hinge:
"""
Args:
range:
compliance:
lock:
motor:
link:
part:
pos:
axis:
Returns:
"""
hinge = demoutils.create_constraint(pos=pos,
axis=axis,
rb1=link,
rb2=part,
c=agx.Hinge) # type: agx.Hinge
hinge.setCompliance(compliance)
hinge.getLock1D().setEnable(lock)
hinge.getMotor1D().setEnable(motor)
if range:
hinge.getRange1D().setEnable(True)
hinge.getRange1D().setRange(deg2rad(range[0]), deg2rad(range[1]))
else:
hinge.getRange1D().setEnable(False)
return hinge
def __init__(self, pos, speed, params = {}):
"""
Create mover instance.
@type pos: tuple or array of two elements representing x- and y-coordinate
@param pos: object's initial position
@type speed: integer
@param speed: object's maximal linear speed
@type params: dict
@param params: 'speed' - max speed in pixels per second;
'dir' - direction in degrees
"""
Mover.__init__(self)
self.pos = list(pos)
self.speed = speed
self._setattrs(('dir'), params)
self.dir = deg2rad(self.dir)
schmale_data=schmale_data[schmale_data['time']<=max_time]
ground_data=ground_data[min_time<=ground_data['time']]
ground_data=ground_data[ground_data['time']<=max_time]
points = [(ross_pos_m[0],ross_pos_m[1]),(schmale_pos_m[0],schmale_pos_m[1]),(ground_m[0],ground_m[1])]
for t in range(ground_data.shape[0]):
if ground_data.iloc[t]['time'] != schmale_data.iloc[t]['time'] or ground_data.iloc[t]['time'] != ross_data.iloc[t]['time']:
print('ERROR: Sample Time Inequal @ {0}'.format(t))
break
values = [ross_data.iloc[t]['wind_speed'],schmale_data.iloc[t]['wind_speed'],ground_data.iloc[t]['wind_speed']]
wind_speed = griddata(points,values,(ross_pos_m[0],schmale_pos_m[1]),method='cubic')
values = [ross_data.iloc[t]['wind_dir'],schmale_data.iloc[t]['wind_dir'],ground_data.iloc[t]['wind_dir']]
wind_dir = griddata(points,values,(ross_pos_m[0],schmale_pos_m[1]),method='cubic')
u = -wind_speed*np.sin(f.deg2rad(wind_dir))
v = wind_speed*np.cos(f.deg2rad(wind_dir))
u_schmale = -schmale_data.iloc[t]['wind_speed']*np.sin(f.deg2rad(schmale_data.iloc[t]['wind_dir']))
v_schmale = schmale_data.iloc[t]['wind_speed']*np.cos(f.deg2rad(schmale_data.iloc[t]['wind_dir']))
u_ross = -ross_data.iloc[t]['wind_speed']*np.sin(f.deg2rad(ross_data.iloc[t]['wind_dir']))
v_ross = ross_data.iloc[t]['wind_speed']*np.cos(f.deg2rad(ross_data.iloc[t]['wind_dir']))
u_ground = -ground_data.iloc[t]['wind_speed']*np.sin(f.deg2rad(ground_data.iloc[t]['wind_dir']))
v_ground = ground_data.iloc[t]['wind_speed']*np.cos(f.deg2rad(ground_data.iloc[t]['wind_dir']))
dudx = (u-u_schmale)/dx
dudy = (u-u_ross)/dy
dvdx = (v-v_schmale)/dx
dvdy = (v-v_ross)/dy
def __init__(self, keyboard):
"""
Args:
keyboard:
seaFloor:
wing_scale:
depth:
"""
super().__init__()
self.keyboard = keyboard
self.plot_pitch = []
self.plot_wing_angle = []
self.plot_depth = []
self.plot_roll = []
self.plotted = False
print("initilaized master and vields")
# building models
rov_material = self.build_material('AluminumMaterial',
ROV_BODY_DENSITY)
wing_material = self.build_material('AluminumMaterial',
ROV_WING_DENSITY)
tank_material = self.build_material('AluminumMaterial',
ROV_TANK_DENSITY)
builder = rov_builder()
print("rov builder ready")
# rov body
self.link1 = builder.create_rov_body(rov_material,
name='rovBody',
scale=ROV_SCALE,
cm=(0.27511, -0.18095, 0.0494),
tank_material=tank_material)
print("buildt rov body")
print("volumebounds: ",
self.link1.getGeometry("Rov_body").getBoundingVolume().size())
# wing left
self.link2 = builder.create_wing_right(wing_material,
WING_SCALE,
"wing_r",
rot=(0, 0, 0))
link2rot = self.link2.getPosition()
print("buildt right wing", link2rot)
# wing right
self.link3 = builder.create_wing_left(wing_material,
WING_SCALE,
"wing_l",
rot=(0, 0, 0))
link3rot = self.link3.getPosition()
print("buildt left wing")
# disabling internal collisions
self.link1.getGeometry('Rov_body').setEnableCollisions(
self.link2.getGeometry('wing_r'), False)
self.link1.getGeometry('Rov_body').setEnableCollisions(
self.link3.getGeometry('wing_l'), False)
print("removed internal collitions")
# adding visualisation
demoutils.create_visual(self.link1)
demoutils.create_visual(self.link2)
demoutils.create_visual(self.link3)
print("created visuals")
# conecting models
# left wing
self.hinge1 = self.build_hinge(link=self.link1,
part=self.link2,
pos=link2rot,
axis=agx.Vec3(0, 1, 0),
lock=False,
motor=True,
range=(MIN_WING_ANGLE, MAX_WING_ANGLE),
compliance=1e-5)
# right wing
self.hinge2 = self.build_hinge(self.link1,
self.link3,
pos=link3rot,
axis=agx.Vec3(0, 1, 0),
lock=False,
motor=True,
range=(MIN_WING_ANGLE, MAX_WING_ANGLE),
compliance=1e-5)
# sonar
print("buildt joints")
# adding models to assembly
self.add(self.link1)
self.add(self.link2)
self.add(self.link3)
print("added models to assembly")
self.add(self.hinge1)
self.add(self.hinge2)
print("aded links to assembly")
self.left_wing_angle = lambda: self.hinge1.getAngle()
self.right_wing_angle = lambda: self.hinge2.getAngle()
print("set the wing controll functions")
self.wing_step_length = deg2rad(2)
print("added wing step lenght")
self.setName('rov')
print("set name")
Sec.postCallback(self.post)
ross_data.iloc[t]['wind_speed'],
schmale_data.iloc[t]['wind_speed'],
ground_data.iloc[t]['wind_speed']
]
wind_speed = griddata(points,
values, (ross_pos_m[0], schmale_pos_m[1]),
method='cubic')
values = [
ross_data.iloc[t]['wind_dir'], schmale_data.iloc[t]['wind_dir'],
ground_data.iloc[t]['wind_dir']
]
wind_dir = griddata(points,
values, (ross_pos_m[0], schmale_pos_m[1]),
method='cubic')
u = -wind_speed * np.sin(f.deg2rad(wind_dir))
v = wind_speed * np.cos(f.deg2rad(wind_dir))
u_schmale = -schmale_data.iloc[t]['wind_speed'] * np.sin(
f.deg2rad(schmale_data.iloc[t]['wind_dir']))
v_schmale = schmale_data.iloc[t]['wind_speed'] * np.cos(
f.deg2rad(schmale_data.iloc[t]['wind_dir']))
u_ross = -ross_data.iloc[t]['wind_speed'] * np.sin(
f.deg2rad(ross_data.iloc[t]['wind_dir']))
v_ross = ross_data.iloc[t]['wind_speed'] * np.cos(
f.deg2rad(ross_data.iloc[t]['wind_dir']))
u_ground = -ground_data.iloc[t]['wind_speed'] * np.sin(
f.deg2rad(ground_data.iloc[t]['wind_dir']))
v_ground = ground_data.iloc[t]['wind_speed'] * np.cos(
N = fc.calc_N(lats_rad[0]) paralelo1 = fc.comprimento_paralelo(lats_rad[0],longs_rad[0],longs_rad[1]) paralelo2 = fc.comprimento_paralelo(lats_rad[1],longs_rad[0],longs_rad[1]) #meridianos meridiano = fc.comprimento_meridiano(lats_rad[0],lats_rad[1]) # Area do quadrilatero dlambda = fc.delta_lambda(longs_rad[0],longs_rad[1]) dphi = fc.delta_phi(lats_rad[0],lats_rad[1]) phiMedio = fc.phi_medio(lats_rad[0],lats_rad[1]) areaQuadrilatero = fc.area_quadrilatero(lats_rad[0],lats_rad[1],longs_rad[0],longs_rad[1]) # Coordenadas ponto central latMedio = fc.phi_medio(lat1,lat2) lonMedio = fc.phi_medio(lon1,lon2) lats_rad.append(fc.deg2rad(latMedio)) #radianos lats_rad.append(fc.deg2rad(lonMedio)) # Raio da esfera que melhor se ajusta a regiao M = fc.calc_M(lats_rad[2]) N = fc.calc_N(lats_rad[2]) raioEsfera = fc.raio_esfera(M,N) # Raio do paralelo que contem o ponto indicado raioParalelo = N * math.cos(lats_rad[2]) # Raio de curvatura com azimute de 30 graus az = fc.deg2rad(30) raioCurvatura = fc.raio_curvatura(az,M,N) # Latitude Geocentrica
import numpy as np from functions import deg2rad, eulermethod from math import sqrt, pi initial_yaw = 40 initial_pitch = 30 initial_roll = 80 initial_angles = [initial_yaw, initial_pitch, initial_roll] initial_angles = deg2rad(initial_angles) tstart = 0.0 tstop = 42.0 step = 0.01 length = int(round(tstop-tstart)/step) time = np.linspace(tstart,tstop+step,length) omegas = (20*pi/180)*np.array([np.sin(0.1*time),np.full(np.shape(time), 0.01),np.cos(0.1*time)]) angles = np.zeros((len(initial_angles), np.size(time))) angles[:,0] = np.array(initial_angles) angles = eulermethod(angles, time, omegas) result = sqrt(angles[0,len(time)-1]**2 + angles[1,len(time)-1]**2 + angles[2,len(time)-1]**2) print(result)
