class OrbitaActuator(object):
def __init__(
self,
root_part,
name,
luos_disks_motor,
Pc_z,
Cp_z,
R,
R0,
pid,
reduction,
wheel_size,
encoder_res,
):
self.disk_bottom, self.disk_middle, self.disk_top = luos_disks_motor
self.model = OrbitaModel(Pc_z=Pc_z, Cp_z=Cp_z, R=R, R0=R0)
self._compliancy = False
self.setup(
pid=pid,
reduction=reduction,
wheel_size=wheel_size,
encoder_res=encoder_res,
)
@property
def disks(self):
return [self.disk_top, self.disk_middle, self.disk_bottom]
@property
def compliant(self):
return self._compliancy
@compliant.setter
def compliant(self, compliancy):
self._compliancy = compliancy
for d in self.disks:
d.compliant = compliancy
def goto(
self,
thetas,
duration,
wait,
interpolation_mode='linear',
):
if len(thetas) != len(self.disks):
raise ValueError(
f'Invalid thetas {thetas} (length should be {len(self.disks)}')
if interpolation_mode not in interpolation_modes.keys():
available = tuple(interpolation_modes.keys())
raise ValueError(
f'interpolation_mode should be one of {available}')
Traj = interpolation_modes[interpolation_mode]
trajs = [
Traj(
initial_position=disk.rot_position,
goal_position=angle,
duration=duration,
) for disk, angle in zip(self.disks, thetas)
]
for disk, traj in zip(self.disks, trajs):
traj.start(disk)
if wait:
for traj in trajs:
traj.wait()
return trajs
def point_at(self, vector, angle, duration, wait):
thetas = self.model.get_angles_from_vector(vector, angle)
# We used a reversed encoder so we need to inverse the angles
thetas = [-q for q in thetas]
self.goto(thetas,
duration=duration,
wait=wait,
interpolation_mode='minjerk')
def orient(self, quat, duration, wait):
thetas = self.model.get_angles_from_quaternion(quat.w, quat.x, quat.y,
quat.z)
# We used a reversed encoder so we need to inverse the angles
thetas = [-q for q in thetas]
self.goto(thetas,
duration=duration,
wait=wait,
interpolation_mode='minjerk')
def setup(self, pid, reduction, wheel_size, encoder_res):
for disk in self.disks:
disk.limit_current = 0.4
disk.encoder_res = encoder_res
disk.reduction = reduction
disk.wheel_size = wheel_size
disk.positionPid = pid
disk.rot_position_mode = True
disk.rot_speed_mode = False
disk.rot_position = True
disk.rot_speed = False
disk.setToZero()
def homing(self, limit_pos=-270, target_pos=102):
recent_speed = deque([], 10)
for d in self.disks:
d.setToZero()
time.sleep(0.1)
self.compliant = False
time.sleep(0.1)
trajs = self.goto(
[limit_pos] * 3,
duration=4,
interpolation_mode='minjerk',
wait=False,
)
for d in self.disks:
d.rot_speed = True
time.sleep(1)
while True:
recent_speed.append([d.rot_speed for d in self.disks])
avg_speed = np.mean(recent_speed, axis=0)
if np.all(avg_speed >= 0):
for traj in trajs:
traj.stop()
traj.wait()
break
time.sleep(0.01)
for d in self.disks:
d.setToZero()
for d in self.disks:
d.rot_speed = False
time.sleep(1)
self.goto(
[target_pos] * 3,
duration=2,
wait=True,
interpolation_mode='minjerk',
)
for d in self.disks:
d.setToZero()
time.sleep(0.1)
self.model.reset_last_angles()
self.orient(Quaternion(1, 0, 0, 0), duration=1, wait=True)
r.disk_bottom.wheel_size = 60.0
r.disk_middle.wheel_size = 60.0
r.disk_top.wheel_size = 60
r.disk_bottom.positionPid = [9, 0.02, 100]
r.disk_middle.positionPid = [9, 0.02, 100]
r.disk_top.positionPid = [9, 0.02, 100]
r.disk_bottom.rot_position_mode(True)
r.disk_middle.rot_position_mode(True)
r.disk_top.rot_position_mode(True)
# ##Motors not compliant###
r.disk_bottom.compliant = False
r.disk_middle.compliant = False
r.disk_top.compliant = False
# ##Control the actuator with an IMU###
a.reset_last_angles()
while (True):
imu_quat = r.Imu_mod.quaternion
q11, q12, q13 = a.from_quaternion_get_angles(imu_quat)
r.disk_top.target_rot_position = q11
r.disk_middle.target_rot_position = q12
r.disk_bottom.target_rot_position = q13
time.sleep(0.01)