You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
179 lines
9.1 KiB
Python
179 lines
9.1 KiB
Python
7 months ago
|
from agent.Base_Agent import Base_Agent as Agent
|
||
|
|
from math_ops.Matrix_3x3 import Matrix_3x3
|
||
|
|
from math_ops.Matrix_4x4 import Matrix_4x4
|
||
|
|
from scripts.commons.Script import Script
|
||
|
|
from world.commons.Draw import Draw
|
||
|
|
from world.Robot import Robot
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
'''
|
||
|
|
Objective:
|
||
|
|
----------
|
||
|
|
Demonstrate the accuracy of the IMU
|
||
|
|
Robot.imu_(...) variables are based on the visual localizer algorithm and the IMU when no visual data is available.
|
||
|
|
If visual data is not available for longer than 0.2 seconds, the robot's position is frozen and the velocity decays to zero.
|
||
|
|
The rotation computed by the IMU is so accurate that it is never frozen, no matter how long the robot goes without visual data.
|
||
|
|
It is almost always safe to use IMU data for rotation.
|
||
|
|
Known issues: the accelerometer is not reliable in the presence of "instant" acceleration peaks, due to its low sample rate (50Hz)
|
||
|
|
this limitation impacts the translation estimation during crashes (e.g. falling, crashing against other players)
|
||
|
|
'''
|
||
|
|
|
||
|
|
class IMU():
|
||
|
|
|
||
|
|
def __init__(self,script:Script) -> None:
|
||
|
|
self.script = script
|
||
|
|
self.player : Agent = None
|
||
|
|
self.cycle = 0
|
||
|
|
|
||
|
|
self.imu_torso_to_field_rotation = [Matrix_3x3() for _ in range(3)]
|
||
|
|
self.imu_torso_to_field_transform = [Matrix_4x4() for _ in range(3)]
|
||
|
|
self.imu_head_to_field_transform = [Matrix_4x4() for _ in range(3)]
|
||
|
|
self.imu_torso_position = np.zeros((3,3))
|
||
|
|
self.imu_torso_velocity = np.zeros((3,3))
|
||
|
|
self.imu_torso_acceleration = np.zeros((3,3))
|
||
|
|
self.imu_torso_next_position = np.zeros((3,3))
|
||
|
|
self.imu_torso_next_velocity = np.zeros((3,3))
|
||
|
|
self.imu_CoM_position = np.zeros((3,3))
|
||
|
|
self.colors = [Draw.Color.green_light, Draw.Color.yellow, Draw.Color.red]
|
||
|
|
|
||
|
|
def act(self):
|
||
|
|
r = self.player.world.robot
|
||
|
|
joint_indices = [r.J_LLEG_PITCH,
|
||
|
|
r.J_LKNEE,
|
||
|
|
r.J_LFOOT_PITCH,
|
||
|
|
r.J_LARM_ROLL,
|
||
|
|
r.J_RLEG_PITCH,
|
||
|
|
r.J_RKNEE,
|
||
|
|
r.J_RFOOT_PITCH,
|
||
|
|
r.J_RARM_ROLL]
|
||
|
|
|
||
|
|
amplitude = [1,0.93,1,1,1][r.type]
|
||
|
|
|
||
|
|
self.cycle += 1
|
||
|
|
if self.cycle < 50:
|
||
|
|
r.set_joints_target_position_direct(joint_indices, np.array([32+10,-64,32, 45, 40+10,-80,40, 0])*amplitude)
|
||
|
|
elif self.cycle < 100:
|
||
|
|
r.set_joints_target_position_direct(joint_indices, np.array([ -10, 0, 0, 0, -10, 0, 0, 0])*amplitude)
|
||
|
|
elif self.cycle < 150:
|
||
|
|
r.set_joints_target_position_direct(joint_indices, np.array([40+10,-80,40, 0, 32+10,-64,32, 45])*amplitude)
|
||
|
|
elif self.cycle < 200:
|
||
|
|
r.set_joints_target_position_direct(joint_indices, np.array([ -10, 0, 0, 0, -10, 0, 0, 0])*amplitude)
|
||
|
|
else:
|
||
|
|
self.cycle = 0
|
||
|
|
|
||
|
|
self.player.scom.commit_and_send( r.get_command() )
|
||
|
|
self.player.scom.receive()
|
||
|
|
|
||
|
|
def act2(self):
|
||
|
|
r = self.player.world.robot
|
||
|
|
self.player.behavior.execute("Walk", (0.2,0), False, 5, False, None ) # Args: target, is_target_abs, ori, is_ori_abs, distance
|
||
|
|
self.player.scom.commit_and_send( r.get_command() )
|
||
|
|
self.player.scom.receive()
|
||
|
|
|
||
|
|
def draw_player_reference_frame(self,i):
|
||
|
|
pos = self.imu_torso_position[i]
|
||
|
|
xvec = self.imu_torso_to_field_rotation[i].multiply((1,0,0)) + pos
|
||
|
|
yvec = self.imu_torso_to_field_rotation[i].multiply((0,1,0)) + pos
|
||
|
|
zvec = self.imu_torso_to_field_rotation[i].multiply((0,0,1)) + pos
|
||
|
|
self.player.world.draw.arrow(pos, xvec, 0.2, 2, self.colors[i], "IMU"+str(i), False)
|
||
|
|
self.player.world.draw.arrow(pos, yvec, 0.2, 2, self.colors[i], "IMU"+str(i), False)
|
||
|
|
self.player.world.draw.arrow(pos, zvec, 0.2, 2, self.colors[i], "IMU"+str(i), False)
|
||
|
|
self.player.world.draw.annotation(xvec, "x", Draw.Color.white, "IMU"+str(i), False)
|
||
|
|
self.player.world.draw.annotation(yvec, "y", Draw.Color.white, "IMU"+str(i), False)
|
||
|
|
self.player.world.draw.annotation(zvec, "z", Draw.Color.white, "IMU"+str(i), False)
|
||
|
|
self.player.world.draw.sphere(self.imu_CoM_position[i],0.04,self.colors[i],"IMU"+str(i), True)
|
||
|
|
|
||
|
|
|
||
|
|
def compute_local_IMU(self):
|
||
|
|
r = self.player.world.robot
|
||
|
|
g = r.gyro / 50 # convert degrees per second to degrees per step
|
||
|
|
self.imu_torso_to_field_rotation[2].multiply( Matrix_3x3.from_rotation_deg(g), in_place=True, reverse_order=True)
|
||
|
|
self.imu_torso_position[2][:] = self.imu_torso_next_position[2]
|
||
|
|
if self.imu_torso_position[2][2] < 0: self.imu_torso_position[2][2] = 0 #limit z coordinate to positive values
|
||
|
|
self.imu_torso_velocity[2][:] = self.imu_torso_next_velocity[2]
|
||
|
|
|
||
|
|
# convert proper acceleration to coordinate acceleration and fix rounding bias
|
||
|
|
self.imu_torso_acceleration[2] = self.imu_torso_to_field_rotation[2].multiply(r.acc) + Robot.GRAVITY
|
||
|
|
self.imu_torso_to_field_transform[2] = Matrix_4x4.from_3x3_and_translation(self.imu_torso_to_field_rotation[2],self.imu_torso_position[2])
|
||
|
|
self.imu_head_to_field_transform[2] = self.imu_torso_to_field_transform[2].multiply(r.body_parts["torso"].transform.invert())
|
||
|
|
self.imu_CoM_position[2][:] = self.imu_head_to_field_transform[2](r.rel_cart_CoM_position)
|
||
|
|
|
||
|
|
# Next Position = x0 + v0*t + 0.5*a*t^2, Next velocity = v0 + a*t
|
||
|
|
self.imu_torso_next_position[2] = self.imu_torso_position[2] + self.imu_torso_velocity[2] * 0.02 + self.imu_torso_acceleration[2] * 0.0002
|
||
|
|
self.imu_torso_next_velocity[2] = self.imu_torso_velocity[2] + self.imu_torso_acceleration[2] * 0.02
|
||
|
|
self.imu_torso_next_velocity[2] *= Robot.IMU_DECAY #stability tradeoff
|
||
|
|
|
||
|
|
def compute_local_IMU_rotation_only(self):
|
||
|
|
r = self.player.world.robot
|
||
|
|
g = r.gyro / 50 # convert degrees per second to degrees per step
|
||
|
|
self.imu_torso_to_field_rotation[1].multiply( Matrix_3x3.from_rotation_deg(g), in_place=True, reverse_order=True)
|
||
|
|
self.imu_torso_position[1][:] = r.loc_torso_position
|
||
|
|
self.imu_torso_to_field_transform[1] = Matrix_4x4.from_3x3_and_translation(self.imu_torso_to_field_rotation[1],self.imu_torso_position[1])
|
||
|
|
self.imu_head_to_field_transform[1] = self.imu_torso_to_field_transform[1].multiply(r.body_parts["torso"].transform.invert())
|
||
|
|
self.imu_CoM_position[1][:] = self.imu_head_to_field_transform[1](r.rel_cart_CoM_position)
|
||
|
|
|
||
|
|
|
||
|
|
def update_local_IMU(self, i):
|
||
|
|
r = self.player.world.robot
|
||
|
|
self.imu_torso_to_field_rotation[i].m[:] = r.imu_torso_to_field_rotation.m
|
||
|
|
self.imu_torso_to_field_transform[i].m[:] = r.imu_weak_torso_to_field_transform.m
|
||
|
|
self.imu_head_to_field_transform[i].m[:] = r.imu_weak_head_to_field_transform.m
|
||
|
|
self.imu_torso_position[i][:] = r.imu_weak_torso_position
|
||
|
|
self.imu_torso_velocity[i][:] = r.imu_weak_torso_velocity
|
||
|
|
self.imu_torso_acceleration[i][:] = r.imu_weak_torso_acceleration
|
||
|
|
self.imu_torso_next_position[i] = self.imu_torso_position[i] + self.imu_torso_velocity[i] * 0.02 + self.imu_torso_acceleration[i] * 0.0002
|
||
|
|
self.imu_torso_next_velocity[i] = self.imu_torso_velocity[i] + self.imu_torso_acceleration[i] * 0.02
|
||
|
|
self.imu_CoM_position[i][:] = r.imu_weak_CoM_position
|
||
|
|
|
||
|
|
def execute(self):
|
||
|
|
|
||
|
|
a = self.script.args
|
||
|
|
self.player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name
|
||
|
|
|
||
|
|
self.player.scom.unofficial_beam((-3,0,self.player.world.robot.beam_height),15)
|
||
|
|
|
||
|
|
for _ in range(10): #beam to place
|
||
|
|
self.player.scom.commit_and_send()
|
||
|
|
self.player.scom.receive()
|
||
|
|
|
||
|
|
self.player.world.draw.annotation((-3,1,1.1), "IMU + Localizer", self.colors[0], "note_IMU_1", True)
|
||
|
|
|
||
|
|
for _ in range(150):
|
||
|
|
self.act()
|
||
|
|
self.update_local_IMU(0)
|
||
|
|
self.draw_player_reference_frame(0)
|
||
|
|
|
||
|
|
self.player.world.draw.annotation((-3,1,0.9), "IMU for rotation", self.colors[1], "note_IMU_2", True)
|
||
|
|
self.update_local_IMU(1)
|
||
|
|
|
||
|
|
for _ in range(200):
|
||
|
|
self.act()
|
||
|
|
self.update_local_IMU(0)
|
||
|
|
self.draw_player_reference_frame(0)
|
||
|
|
self.compute_local_IMU_rotation_only()
|
||
|
|
self.draw_player_reference_frame(1)
|
||
|
|
|
||
|
|
self.player.world.draw.annotation((-3,1,0.7), "IMU for rotation & position", self.colors[2], "note_IMU_3", True)
|
||
|
|
self.update_local_IMU(2)
|
||
|
|
|
||
|
|
for _ in range(200):
|
||
|
|
self.act()
|
||
|
|
self.update_local_IMU(0)
|
||
|
|
self.draw_player_reference_frame(0)
|
||
|
|
self.compute_local_IMU_rotation_only()
|
||
|
|
self.draw_player_reference_frame(1)
|
||
|
|
self.compute_local_IMU()
|
||
|
|
self.draw_player_reference_frame(2)
|
||
|
|
|
||
|
|
print("\nPress ctrl+c to return.")
|
||
|
|
|
||
|
|
# Still "IMU for rotation & position" but now start walking
|
||
|
|
self.update_local_IMU(2)
|
||
|
|
while True:
|
||
|
|
self.act2()
|
||
|
|
self.update_local_IMU(0)
|
||
|
|
self.draw_player_reference_frame(0)
|
||
|
|
self.compute_local_IMU_rotation_only()
|
||
|
|
self.draw_player_reference_frame(1)
|
||
|
|
self.compute_local_IMU()
|
||
|
|
self.draw_player_reference_frame(2)
|