def test_actuator_setup():
actuator = Actuator()
assert np.allclose(actuator.x0, np.array((1, 0, 0)))
assert np.allclose(actuator.y0, np.array((0, 1, 0)))
assert np.allclose(actuator.z0, np.array((0, 0, 1)))
assert actuator.x0_quat == Quaternion(0, 1, 0, 0)
assert actuator.y0_quat == Quaternion(0, 0, 1, 0)
assert actuator.z0_quat == Quaternion(0, 0, 0, 1)
R0 = np.dot(rot('z', 60), rot('y', 10))
actuator = Actuator(R0=R0)
assert np.allclose(actuator.x0, R0[0])
assert np.allclose(actuator.y0, R0[1])
assert np.allclose(actuator.z0, R0[2])
assert actuator.x0_quat == Quaternion(0, R0[0][0], R0[0][1], R0[0][2])
assert actuator.y0_quat == Quaternion(0, R0[1][0], R0[1][1], R0[1][2])
assert actuator.z0_quat == Quaternion(0, R0[2][0], R0[2][1], R0[2][2])
Pc_z = [0, 0, 89.4]
arr = np.array(Pc_z)
actuator = Actuator(Pc_z=Pc_z)
Pc_z[0] = 42
assert np.all(actuator.Pc_z == arr)
Cp_z = [0, 0, 89.4]
arr = np.array(Cp_z)
actuator = Actuator(Cp_z=Cp_z)
Cp_z[0] = 42
assert np.all(actuator.Cp_z == arr)
def test_get_angles_from_vector_big_angles():
a = Actuator()
x = 180.0 + np.random.rand() * 180
q11, q12, q13 = a.get_angles_from_vector([0, 0, 1], x)
assert isclose(q11, x - 360)
assert isclose(q12, x - 360)
assert isclose(q13, x - 360)
def test_last_angles():
a1 = Actuator()
a2 = Actuator()
assert np.allclose(a1.last_angles, [0, 2 * np.pi / 3, -2 * np.pi / 3])
assert np.allclose(a2.last_angles, [0, 2 * np.pi / 3, -2 * np.pi / 3])
a1.last_angles[1] = 0
assert np.allclose(a2.last_angles, [0, 2 * np.pi / 3, -2 * np.pi / 3])
def test_transform():
a = Actuator()
vo = np.random.rand(3)
q = a.find_quaternion_transform(vo, vo)
assert q == Quaternion(1, 0, 0, 0)
vo = [1, 0, 0]
alpha = np.random.rand() * 90
r = R.from_euler('z', alpha, degrees=True)
vt = np.dot(r.as_matrix(), vo)
q = a.find_quaternion_transform(vo, vt)
assert q == Quaternion(axis=[0, 0, 1], degrees=alpha)
def test_multi_turn():
a = Actuator()
for x in range(0, 1080, 10):
q11, q12, q13 = a.get_angles_from_vector([0, 0, 1], x)
assert isclose(q11, x, abs_tol=1e-9)
assert isclose(q12, x, abs_tol=1e-9)
assert isclose(q13, x, abs_tol=1e-9)
for x in range(1080, 0, -10):
q11, q12, q13 = a.get_angles_from_vector([0, 0, 1], x)
assert isclose(q11, x, abs_tol=1e-9)
assert isclose(q12, x, abs_tol=1e-9)
assert isclose(q13, x, abs_tol=1e-9)
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,
)
def test_get_angles_from_vector_with_last_angles():
a = Actuator()
last = np.random.randint(180, 360)
Q = []
for x in range(0, last + 1):
q = a.get_angles_from_vector([0, 0, 1], x)
Q.append(q)
q11, q12, q13 = Q[-1]
assert np.isclose(q11, last)
assert np.isclose(q12, last)
assert np.isclose(q13, last)
def __init__(self, root_part, name, disks_motor, Pc_z, Cp_z, R, R0,
hardware_zero):
"""Create a OrbitaActuator given its three disks controllers."""
self.disk_bottom, self.disk_middle, self.disk_top = disks_motor
self.model = OrbitaModel(Pc_z=Pc_z, Cp_z=Cp_z, R=R, R0=R0)
self._hardware_zero = hardware_zero
self._compliancy = False
self.setup()
def test_new_frame_from_quaternion():
a = Actuator()
q = Quaternion(axis=[0, 0, 1], degrees=90)
X, Y, Z = a.get_new_frame_from_quaternion(q.w, q.x, q.y, q.z)
assert np.allclose(X, a.y0)
assert np.allclose(Y, -a.x0)
assert np.allclose(Z, a.z0)
q1 = Quaternion(
axis=[np.random.rand(), np.random.rand(), np.random.rand()],
degrees=np.random.rand()*90,
)
q2 = q1.inverse
q = q1 * q2
X, Y, Z = a.get_new_frame_from_quaternion(q.w, q.x, q.y, q.z)
assert np.allclose(X, a.x0)
assert np.allclose(Y, a.y0)
assert np.allclose(Z, a.z0)
def test_new_frame_from_vector():
a = Actuator()
X, Y, Z = a.get_new_frame_from_vector([0, 0, 1], 90)
assert np.allclose(X, a.y0)
assert np.allclose(Y, -a.x0)
assert np.allclose(Z, a.z0)
a = Actuator(R0=rot('z', 60))
X, Y, Z = a.get_new_frame_from_vector([0, 0, 1], 90)
assert np.allclose(X, a.y0)
assert np.allclose(Y, -a.x0)
assert np.allclose(Z, a.z0)
def test_get_angles_from_vector():
a = Actuator()
for _ in range(5):
x = np.random.rand() * 180 - 90
q11, q12, q13 = a.get_angles_from_vector([0, 0, 1], x)
assert isclose(q11, x)
assert isclose(q12, x)
assert isclose(q13, x)
a = Actuator(R0=rot('z', 60))
for _ in range(5):
x = np.random.rand() * 180 - 90
q11, q12, q13 = a.get_angles_from_vector([0, 0, 1], x)
assert isclose(q11, x - 60)
assert isclose(q12, x - 60)
assert isclose(q13, x - 60)
def test_get_angles_from_quaternion():
a = Actuator()
x = np.random.rand() * 180 - 90
q = Quaternion(axis=[0, 0, 1], degrees=x)
q11, q12, q13 = a.get_angles_from_quaternion(q.w, q.x, q.y, q.z)
assert isclose(q11, x)
assert isclose(q12, x)
assert isclose(q13, x)
a = Actuator(R0=rot('z', 60))
x = np.random.rand() * 180 - 90
q = Quaternion(axis=[0, 0, 1], degrees=x)
q11, q12, q13 = a.get_angles_from_quaternion(q.w, q.x, q.y, q.z)
assert isclose(q11, x - 60)
assert isclose(q12, x - 60)
assert isclose(q13, x - 60)
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)
from pyluos import Robot
from orbita import Actuator
import time
a = Actuator()
r = Robot('/dev/cu.usbserial-DN05NM0L')
r.gate.delay = 10
r.disk_bottom.rot_position = False
r.disk_middle.rot_position = False
r.disk_top.rot_position = False
# ##########Luos Parameters############
r.disk_bottom.encoder_res = 5
r.disk_middle.encoder_res = 5
r.disk_top.encoder_res = 5
r.disk_bottom.setToZero()
r.disk_middle.setToZero()
r.disk_top.setToZero()
r.disk_bottom.reduction = 232
r.disk_middle.reduction = 232
r.disk_top.reduction = 232
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]
def test_domain_error():
a = Actuator()
with pytest.raises(ValueError, match=r"math domain error"):
X, Y, Z = a.get_angles_from_vector([1, 0, 0], np.random.rand() * 90)
class OrbitaActuator(object):
"""Orbita Actuator abstraction.
Args:
root_part (str): name of the part where the motor is attached to (eg 'head')
name (str): name of the actuator (eg. 'neck')
disks_motor (list of :py:class:`pyluos.motor_controller`): list of the three disks controllers
Pc_z (float, float, float): 3D coordinates of the center of the platform (in mm)
Cp_z (float, float, float): center of the disks rotation circle (in mm)
R (float): radius of the arms rotation circle around the platform (in mm)
R0 (:py:class:`~numpy.ndarray`): rotation matrix for the initial rotation
hardware_zero (float, float, float): absolute hardware zero position of the orbita disks
Wrap the three disk and the computation model of Orbita to expose higher-level functionalities such as:
* quaternion control
* compliancy mode
* goto
"""
def __init__(self, root_part, name, disks_motor, Pc_z, Cp_z, R, R0,
hardware_zero):
"""Create a OrbitaActuator given its three disks controllers."""
self.disk_bottom, self.disk_middle, self.disk_top = disks_motor
self.model = OrbitaModel(Pc_z=Pc_z, Cp_z=Cp_z, R=R, R0=R0)
self._hardware_zero = hardware_zero
self._compliancy = False
self.setup()
def __repr__(self):
"""Orbita representation."""
return (f'<Orbita '
f'"top disk": {self.disk_top.rot_position} '
f'"middle disk": {self.disk_middle.rot_position} '
f'"bottom disk": {self.disk_bottom.rot_position}>')
@property
def disks(self):
"""Get three disks [top, middle, bottom]."""
return [self.disk_top, self.disk_middle, self.disk_bottom]
@property
def compliant(self):
"""Check the disks compliancy."""
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',
):
"""Set trajectory goal for three disks.
Args:
thetas (float, float, float): target position (in degrees) for each disks (top, middle, bottom)
duration (float): duration of the movement (in seconds)
wait (bool): whether or not to wait for the end of the motion
interpolation_mode (str): interpolation technique used for computing the trajectory ('linear', 'minjerk')
Returns:
reachy.trajectory.TrajectoryPlayer: trajectory player that can be used to monitor the trajectory, stop it, etc
"""
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.target_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):
"""Make orbita point at the given vector.
Args:
vector (float, float, float): 3D vector indicating the poiting direction (in m)
angle (float): rotation around the vector (in degrees)
duration (float): move duration (in seconds)
wait (bool): whether or not to wait for the end of the motion
Returns:
reachy.trajectory.TrajectoryPlayer: trajectory player that can be used to monitor the trajectory, stop it, etc
"""
thetas = self.model.get_angles_from_vector(vector, angle)
# We used a reversed encoder so we need to inverse the angles
return self.goto(thetas,
duration=duration,
wait=wait,
interpolation_mode='minjerk')
def orient(self, quat, duration, wait):
"""Orient orbita given a quaternion.
Args:
quat (pyquaternion.Quaterion): quaternion goal orientation
duration (float): move duration (in seconds)
wait (bool): whether or not to wait for the end of the motion
Returns:
reachy.trajectory.TrajectoryPlayer: trajectory player that can be used to monitor the trajectory, stop it, etc
"""
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
return self.goto(thetas,
duration=duration,
wait=wait,
interpolation_mode='minjerk')
def setup(self):
"""Configure each of the three disks.
.. note:: automatically called at instantiation.
"""
for disk in self.disks:
disk.setup()
def _find_zero(disk, z):
A = 360 / (52 / 24)
p = disk.rot_position
zeros = [z, -(A - z), A + z]
distances = [abs(p - z) for z in zeros]
best = np.argmin(distances)
return zeros[best]
time.sleep(0.25)
for d, z in zip(self.disks, self._hardware_zero):
d.offset = _find_zero(d, z) + 60