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
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
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
def sim_attitude_simple(self):
"""
Simulate angular velocity measurements via simple differences.
"""
# linear and angular velocities
rates = angular_velocities(self.pp, self.position_3d, 1 / self.frequency)
self.pp = copy(self.position_3d)
return rates
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
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
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
