Example #1
0
 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)
Example #2
0
    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)
Example #3
0
  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
Example #4
0
  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
Example #5
0
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)
Example #6
0
    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
Example #7
0
  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)
Example #8
0
    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
Example #9
0
    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
Example #12
0
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)