Esempio n. 1
0
def setup(world, body, mass, space, g_world, body_geom, i, c1, boat_model):
    world.setGravity((0, 0, -9.81))
    # this is the mass of the boat "boat_mass", this also sets the inertia based on a box with length_x, length_y, length_z
    mass.setBoxTotal(boat_mass, boat_length, boat_width, boat_height)
    body.setMass(mass)
    body.setPosition(initial_position)
    # the last unit is the initial rotation of the body position in radians
    body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), math.pi))
    #body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), 0))
    body_geom.setBody(body)
    lake_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'lake.obj'))
    lake_geom = ode.GeomTriMesh(lake_mesh.ode_trimeshdata, space)

    # TODO: Add obstacles here

    g_world.objs.append(lake_mesh)
    
    g_world.objs.append(boat_model)
    g_world.objs.append(srh.VectorField(get_water_vel))

    geoms = []
    buoy_list = [v(54.5, -33.9, 0), v(53.5, -37.4, 0), v(38.0, -29.6, 0), v(35.7, -33.5, 0)]
    for pos in buoy_list:
        newbuoy_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'green_buoy.obj'))
        newbuoy_mesh = newbuoy_mesh.translate(pos)
        g_world.objs.append(newbuoy_mesh)
        geoms.append(ode.GeomTriMesh(newbuoy_mesh.ode_trimeshdata, space))

    i.init(g_world)
Esempio n. 2
0
def setup(world, body, mass, space, g_world, body_geom, i, c1, boat_model):
    world.setGravity((0, 0, -9.81))
    # this is the mass of the boat "boat_mass", this also sets the inertia based on a box with length_x, length_y, length_z
    mass.setBoxTotal(boat_mass, boat_length, boat_width, boat_height)
    body.setMass(mass)
    body.setPosition(initial_position)
    # the last unit is the initial rotation of the body position in radians
    body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), math.pi))
    #body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), 0))
    body_geom.setBody(body)
    lake_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'lake.obj'))
    lake_geom = ode.GeomTriMesh(lake_mesh.ode_trimeshdata, space)

    # TODO: Add obstacles here

    g_world.objs.append(lake_mesh)

    g_world.objs.append(boat_model)
    g_world.objs.append(srh.VectorField(get_water_vel))

    geoms = []
    buoy_list = [v(54.5, -33.9, 0), v(53.5, -37.4, 0), v(38.0, -29.6, 0), v(35.7, -33.5, 0)]
    for pos in buoy_list:
        newbuoy_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'green_buoy.obj'))
        newbuoy_mesh = newbuoy_mesh.translate(pos)
        g_world.objs.append(newbuoy_mesh)
        geoms.append(ode.GeomTriMesh(newbuoy_mesh.ode_trimeshdata, space))

    i.init(g_world)
Esempio n. 3
0
 def get_water_vel(self, pos):
     return v(0, 0, 0)
     return (pos % v(0, 0, 1))*math.e**(-pos.mag()/3)
Esempio n. 4
0
    def world_tick(self):
        
        #if random.randrange(10) == 0:
          #boat_lidar.get_lidar_range() # Use and publish whoa!

        water_vel = self.get_water_vel(V(body.getPosition()))
        
        body.addForceAtRelPos((0, 0, self.buoyancy_force(-body.getPosition()[2], 0.22728849402137372)), (0, 0, .1))
        # the following frictional forces are from darsen's models
        #frictional force opposite to the velocity of the boat
        body_velocity=body.vectorFromWorld(body.getLinearVel())
        # adds a resistance force for the water proportional to the velocity (where [0] is x, [1] is y, and [2] is z). units in N/(m/s)
        friction_force_forward=self.friction_coefficient_forward*body_velocity[0]-self.friction_coefficient_forward_reduction*(-1.0*pow(body_velocity[0],2) if body_velocity[0]<0.0 else pow(body_velocity[0],2))
        friction_force_lateral=self.friction_coefficient_lateral*body_velocity[1]-self.friction_coefficient_lateral_reduction*(-1.0*pow(body_velocity[1],2) if body_velocity[1]<0.0 else pow(body_velocity[1],2))
        body.addRelForce([-friction_force_forward,-friction_force_lateral,body_velocity[2]*-40])
        # adds a angular resistance force for the water proportional to the velocity
        # angular_velocity is a 3-tuple ([0]=roll,[1]=pitch,[2]=yaw)
        angular_velocity=body.vectorFromWorld(body.getAngularVel())
        friction_force_roll=self.friction_coefficient_rotational*angular_velocity[0]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[0],2) if angular_velocity[0]<0.0 else pow(angular_velocity[0],2))
        friction_force_pitch=self.friction_coefficient_rotational*angular_velocity[1]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[1],2) if angular_velocity[1]<0.0 else pow(angular_velocity[1],2))
        friction_force_yaw=self.friction_coefficient_rotational*angular_velocity[2]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[2],2) if angular_velocity[2]<0.0 else pow(angular_velocity[2],2))
        body.addRelTorque([-friction_force_roll,-friction_force_pitch,-friction_force_yaw])
        #print thrusters
        # map the thrusters position on the boat
        self.boat_model.vectors = []
        # id2=starboard, id3=port
        if not self.killed:
            for thruster_id in [2, 3]:
                # set the thrusters position on the boat
                relpos = v(self.starboard_servo_x_offset, self.starboard_servo_y_offset, 0) if thruster_id == 2 else v(self.port_servo_x_offset, self.port_servo_y_offset, 0)
                angle = 0
                reldir = v(math.cos(angle), math.sin(angle), 0)
                force = self.thrusts[thruster_id]
                body.addRelForceAtRelPos(reldir*force, relpos)
                self.boat_model.vectors.append((relpos, relpos - .02*reldir*force))
        
        keys = pygame.key.get_pressed()
        for keycode, force in [
            (pygame.K_k, v(-50, 0, 0)),
            (pygame.K_i, v(+50, 0, 0)),
            (pygame.K_j, v(0, +50, 0)),
            (pygame.K_l, v(0, -50, 0)),
            (pygame.K_o, v(0, 0, +50)),
            (pygame.K_m, v(0, 0, -50)),
        ]:
            if keys[keycode]:
                body.addRelForce(force*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1))
        for keycode, torque in [
            (pygame.K_COMMA, v(-20, 0, 0)),
            (pygame.K_u, v(+20, 0, 0)),
            (pygame.K_h, v(0, +20, 0)),
            (pygame.K_SEMICOLON, v(0, -20, 0)),
            (pygame.K_0, v(0, 0, +20)),
            (pygame.K_n, v(0, 0, -20)),
        ]:
            if keys[keycode]:
                body.addRelTorque(torque*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1))
        
        
        if keys[pygame.K_1]:
            self.killed = True
        if keys[pygame.K_2]:
            self.killed = False
        global locked
        if keys[pygame.K_3]:
            locked = True
        if keys[pygame.K_4]:
            locked = False
        
        contactgroup = ode.JointGroup()
        
        if self.locked:
            j = ode.FixedJoint(world, contactgroup)
            j.attach(body, None)
            j.setFixed()
        
        near_pairs = []
        space.collide(None, lambda _, geom1, geom2: near_pairs.append((geom1, geom2)))
        for geom1, geom2 in near_pairs:
            for contact in ode.collide(geom1, geom2):
                contact.setBounce(0.2)
                contact.setMu(5000)
                j = ode.ContactJoint(world, contactgroup, contact)
                j.attach(geom1.getBody(), geom2.getBody())
        
        dt = 1/30
        self.world_time += dt

        world.step(dt)
        
        contactgroup.empty()

        pos = body.getPosition()
        q = V(body.getQuaternion())

        # Publish tf /enu
        self.enu_tf_br.sendTransform(
            translation = (pos[0], pos[1], pos[2]),
            rotation = (q[1], q[2], q[3], q[0]),
            time = rospy.Time(self.world_time),
            child = '/base_link',
            parent = '/enu')
        
        # Publish odom
        msg = Odometry()
        msg.header.stamp = rospy.Time(self.world_time)
        msg.header.frame_id = '/enu'
        msg.child_frame_id = '/base_link'
        msg.pose.pose.position = Point(*pos)
        msg.pose.pose.orientation = Quaternion(q[1], q[2], q[3], q[0])
       
        msg.twist.twist.linear = Vector3(*q.conj().quat_rot(body.getLinearVel()))
        msg.twist.twist.angular = Vector3(*q.conj().quat_rot(body.getAngularVel()))
        self.odom_pub.publish(msg)
        
        # XXX
        msg = Odometry()
        msg.header.stamp = rospy.Time(self.world_time)
        msg.header.frame_id = '/ecef'
        msg.child_frame_id = '/base_link'
        # ecef_loc = gps.ecef_from_latlongheight(math.radians(36.802002), math.radians(-76.191019), 7)
        enu_loc = numpy.array([53.6686215007, -20.8502282916, -0.0733864689281])
        # msg.pose.pose.position = Point(*gps.ecef_from_enu(enu_v=body.getPosition() - enu_loc, ecef_pos=ecef_loc) + ecef_loc)
        self.abs_odom_pub.publish(msg)
        
        reactor.callLater(max(0, self.world_time + dt - reactor.seconds()), self.world_tick)
Esempio n. 5
0
def get_water_vel(pos):
    return v(0, 0, 0)
    return (pos % v(0, 0, 1))*math.e**(-pos.mag()/3)
Esempio n. 6
0
    def world_tick(self):

        #if random.randrange(10) == 0:
          #boat_lidar.get_lidar_range() # Use and publish whoa!

        water_vel = self.get_water_vel(V(body.getPosition()))

        body.addForceAtRelPos((0, 0, self.buoyancy_force(-body.getPosition()[2], 0.5)), (0, 0, .1))
        # the following frictional forces are from darsen's models
        #frictional force opposite to the velocity of the boat
        body_velocity=body.vectorFromWorld(body.getLinearVel())
        # adds a resistance force for the water proportional to the velocity (where [0] is x, [1] is y, and [2] is z). units in N/(m/s)
        friction_force_forward=self.friction_coefficient_forward*body_velocity[0]-self.friction_coefficient_forward_reduction*(-1.0*pow(body_velocity[0],2) if body_velocity[0]<0.0 else pow(body_velocity[0],2))
        friction_force_lateral=self.friction_coefficient_lateral*body_velocity[1]-self.friction_coefficient_lateral_reduction*(-1.0*pow(body_velocity[1],2) if body_velocity[1]<0.0 else pow(body_velocity[1],2))
        body.addRelForce([-friction_force_forward,-friction_force_lateral,body_velocity[2]*-450])
        # adds a angular resistance force for the water proportional to the velocity
        # angular_velocity is a 3-tuple ([0]=roll,[1]=pitch,[2]=yaw)
        angular_velocity=body.vectorFromWorld(body.getAngularVel())
        friction_force_roll=self.friction_coefficient_rotational*angular_velocity[0]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[0],2) if angular_velocity[0]<0.0 else pow(angular_velocity[0],2))
        friction_force_pitch=self.friction_coefficient_rotational*angular_velocity[1]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[1],2) if angular_velocity[1]<0.0 else pow(angular_velocity[1],2))
        friction_force_yaw=self.friction_coefficient_rotational*angular_velocity[2]-self.friction_coefficient_rotational_reduction*(-1.0*pow(angular_velocity[2],2) if angular_velocity[2]<0.0 else pow(angular_velocity[2],2))
        body.addRelTorque([-friction_force_roll,-friction_force_pitch,-friction_force_yaw])
        #print thrusters
        # map the thrusters position on the boat
        self.boat_model.vectors = []
        # id2=starboard, id3=port
        if not self.killed:
            for thruster_id in [0, 1, 2, 3]:
                # set the thrusters position on the boat
                if thruster_id == 0:
                    relpos = v(self.BL_offset[0], self.BL_offset[1], 0)
                    reldir = v(1, 1, 0)
                if thruster_id == 1:
                    relpos = v(self.BR_offset[0], self.BR_offset[1], 0)
                    reldir = v(1, -1, 0)
                if thruster_id == 2:
                    relpos = v(self.FL_offset[0], self.FL_offset[1], 0)
                    reldir = v(1, -1, 0)
                if thruster_id == 3:
                    relpos = v(self.FR_offset[0], self.FR_offset[1], 0)
                    reldir = v(1, 1, 0)

                force = self.thrusts[thruster_id]
                body.addRelForceAtRelPos(reldir*force, relpos)
                self.boat_model.vectors.append((relpos, relpos + 0.05*reldir*force))

        keys = pygame.key.get_pressed()
        for keycode, force in [
            (pygame.K_k, v(-50, 0, 0)),
            (pygame.K_i, v(+50, 0, 0)),
            (pygame.K_j, v(0, +50, 0)),
            (pygame.K_l, v(0, -50, 0)),
            (pygame.K_o, v(0, 0, +50)),
            (pygame.K_m, v(0, 0, -50)),
        ]:
            if keys[keycode]:
                body.addRelForce(force*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1))
        for keycode, torque in [
            (pygame.K_COMMA, v(-20, 0, 0)),
            (pygame.K_u, v(+20, 0, 0)),
            (pygame.K_h, v(0, +20, 0)),
            (pygame.K_SEMICOLON, v(0, -20, 0)),
            (pygame.K_0, v(0, 0, +20)),
            (pygame.K_n, v(0, 0, -20)),
        ]:
            if keys[keycode]:
                body.addRelTorque(torque*(10 if keys[pygame.K_RSHIFT] else 1)*(.1 if keys[pygame.K_RCTRL] else 1))


        if keys[pygame.K_1]:
            self.killed = True
        if keys[pygame.K_2]:
            self.killed = False
        global locked
        if keys[pygame.K_3]:
            locked = True
        if keys[pygame.K_4]:
            locked = False

        contactgroup = ode.JointGroup()

        if self.locked:
            j = ode.FixedJoint(world, contactgroup)
            j.attach(body, None)
            j.setFixed()

        near_pairs = []
        space.collide(None, lambda _, geom1, geom2: near_pairs.append((geom1, geom2)))
        for geom1, geom2 in near_pairs:
            for contact in ode.collide(geom1, geom2):
                contact.setBounce(0.2)
                contact.setMu(5000)
                j = ode.ContactJoint(world, contactgroup, contact)
                j.attach(geom1.getBody(), geom2.getBody())

        dt = 1/50
        self.world_time += dt

        world.step(dt)

        contactgroup.empty()

        pos = body.getPosition()
        q = V(body.getQuaternion())

        # Avoid stupid [0, 0, 0, 0] quaternion initialization
        if not numpy.any(q):
            q = [1, 0, 0, 0]

        # Publish tf /enu
        self.enu_tf_br.sendTransform(
            translation = (pos[0], pos[1], pos[2]),
            rotation = (q[1], q[2], q[3], q[0]),
            time = rospy.Time(self.world_time),
            child = '/base_link',
            parent = '/enu')

        # Publish odom
        msg = Odometry()
        msg.header.stamp = rospy.Time(self.world_time)
        msg.header.frame_id = '/enu'
        msg.child_frame_id = '/base_link'
        msg.pose.pose.position = Point(*pos)
        msg.pose.pose.orientation = Quaternion(q[1], q[2], q[3], q[0])

        msg.twist.twist.linear = Vector3(*q.conj().quat_rot(body.getLinearVel()))
        msg.twist.twist.angular = Vector3(*q.conj().quat_rot(body.getAngularVel()))
        self.odom_pub.publish(msg)

        # XXX
        msg = Odometry()
        msg.header.stamp = rospy.Time(self.world_time)
        msg.header.frame_id = '/ecef'
        msg.child_frame_id = '/base_link'
        # ecef_loc = gps.ecef_from_latlongheight(math.radians(36.802002), math.radians(-76.191019), 7)
        enu_loc = numpy.array([53.6686215007, -20.8502282916, -0.0733864689281])
        # msg.pose.pose.position = Point(*gps.ecef_from_enu(enu_v=body.getPosition() - enu_loc, ecef_pos=ecef_loc) + ecef_loc)
        self.abs_odom_pub.publish(msg)

        reactor.callLater(max(0, self.world_time + dt - reactor.seconds()), self.world_tick)