Beispiel #1
0
    def default_action(self):
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Integrated version
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)

        self.local_data['x'] = current_pos.x
        self.local_data['y'] = current_pos.y
        self.local_data['z'] = current_pos.z
        self.local_data['yaw'] = current_pos.yaw
        self.local_data['pitch'] = current_pos.pitch
        self.local_data['roll'] = current_pos.roll

        # speed in the sensor frame, related to the robot pose
        lin_vel = linear_velocities(self.previous_pos, current_pos, self.frequency)
        ang_vel = angular_velocities(self.previous_pos, current_pos, 1/self.frequency)

        self.local_data['vx'] = lin_vel[0]
        self.local_data['vy'] = lin_vel[1]
        self.local_data['vz'] = lin_vel[2]
        self.local_data['wx'] = ang_vel[0]
        self.local_data['wy'] = ang_vel[1]
        self.local_data['wz'] = ang_vel[2]

        # Store the 'new' previous data
        self.previous_pos = current_pos
Beispiel #2
0
    def sim_imu_simple(self):
        """
        Simulate angular velocity and linear acceleration measurements via simple differences.
        """
        # linear and angular velocities
        lin_vel = linear_velocities(self.pp, self.position_3d,
                                    1 / self.frequency)
        ang_vel = angular_velocities(self.pp, self.position_3d,
                                     1 / self.frequency)

        # linear acceleration in imu frame
        dv_imu = self.rot_i2w.transposed() * (lin_vel -
                                              self.plv) * self.frequency

        # measurement includes gravity and acceleration
        accel_meas = dv_imu + self.rot_i2w.transposed() * self.gravity

        # save current position and attitude for next step
        self.pp = copy(self.position_3d)

        # save velocity for next step
        self.plv = lin_vel
        self.pav = ang_vel

        return ang_vel, accel_meas
Beispiel #3
0
    def default_action(self):
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Integrated version
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)

        self.local_data['x'] = current_pos.x
        self.local_data['y'] = current_pos.y
        self.local_data['z'] = current_pos.z
        self.local_data['yaw'] = current_pos.yaw
        self.local_data['pitch'] = current_pos.pitch
        self.local_data['roll'] = current_pos.roll

        # speed in the sensor frame, related to the robot pose
        lin_vel = linear_velocities(self.previous_pos, current_pos, 1/self.frequency)
        ang_vel = angular_velocities(self.previous_pos, current_pos,1/self.frequency)

        self.local_data['vx'] = lin_vel[0]
        self.local_data['vy'] = lin_vel[1]
        self.local_data['vz'] = lin_vel[2]
        self.local_data['wx'] = ang_vel[0]
        self.local_data['wy'] = ang_vel[1]
        self.local_data['wz'] = ang_vel[2]

        # Store the 'new' previous data
        self.previous_pos = current_pos
Beispiel #4
0
    def default_action(self):
        if self._type == 'Velocity':
            vel = self.rot_b2a * self.robot_vel_body
        else:
            vel = linear_velocities(self.pp, self.position_3d, 1 / self.frequency)
            self.pp = copy(self.position_3d)

        v_x = vel[0]
        self.local_data['diff_pressure'] = 0.5 * AIR_DENSITY * v_x * v_x
Beispiel #5
0
    def default_action(self):
        if self._type == 'Velocity':
            vel = self.rot_b2a * self.robot_vel_body
        else:
            vel = linear_velocities(self.pp, self.position_3d,
                                    1 / self.frequency)
            self.pp = copy(self.position_3d)

        v_x = vel[0]
        self.local_data['diff_pressure'] = 0.5 * AIR_DENSITY * v_x * v_x
Beispiel #6
0
    def _sim_simple(self):
        v = linear_velocities(self.position_3d, self.pp, self.dt)
        w = angular_velocities(self.position_3d, self.pp, self.dt)
        a = (v - self.plv) / self.dt
        aw = (w - self.paw) / self.dt

        # Update the data for the velocity
        self.plv = v.copy()
        self.pav = w.copy()

        # Store the important data
        w2a = self.position_3d.rotation_matrix.transposed()
        self.local_data['velocity'] = w2a * v
        self.local_data['acceleration'] = w2a * a
        self.local_data['angular_acceleration'] = w2a * aw
Beispiel #7
0
    def _sim_simple(self):
        self.dt = self.robot_parent.gettime() - self.pt
        self.pt = self.robot_parent.gettime()

        if self.dt < 1e-6:
            return

        v = linear_velocities(self.pp, self.position_3d, self.dt)
        w = angular_velocities(self.pp, self.position_3d, self.dt)

        self.pp = copy(self.position_3d)

        w2a = self.position_3d.rotation_matrix.transposed()

        self.local_data['linear_velocity'] = w2a * v
        self.local_data['angular_velocity'] =  w
        self.local_data['world_linear_velocity'] = v
Beispiel #8
0
    def sim_imu_simple(self):
        """
        Simulate angular velocity and linear acceleration measurements via simple differences.
        """
        # linear and angular velocities
        lin_vel = linear_velocities(self.pp, self.position_3d, 1 / self.frequency)
        ang_vel = angular_velocities(self.pp, self.position_3d, 1 / self.frequency)

        # linear acceleration in imu frame
        dv_imu = self.rot_i2w.transposed() * (lin_vel - self.plv) * self.frequency

        # measurement includes gravity and acceleration
        accel_meas = dv_imu + self.rot_i2w.transposed() * self.gravity

        # save current position and attitude for next step
        self.pp = copy(self.position_3d)

        # save velocity for next step
        self.plv = lin_vel
        self.pav = ang_vel

        return ang_vel, accel_meas