def test_to_from_euler(self):
"""2-way conversion starting from quaternions."""
np.random.seed(0)
angles_euler = np.pi * np.random.rand(100, 3)
conventions_euler = ["xzx", "xyx", "yxy", "yzy", "zyz", "zxz"]
# For Tait-Bryan angles the second angle must be between -pi/2 and pi/2
angles_tb = angles_euler.copy()
angles_tb[:, 1] -= np.pi / 2
conventions_tb = ["xzy", "xyz", "yxz", "yzx", "zyx", "zxy"]
axis_types = ["extrinsic", "intrinsic"]
for convention in conventions_euler:
for axis_type in axis_types:
out = rowan.to_euler(
rowan.from_euler(
angles_euler[..., 0],
angles_euler[..., 1],
angles_euler[..., 2],
convention,
axis_type,
),
convention,
axis_type,
)
self.assertTrue(
np.all(
np.logical_or(
np.isclose(out - angles_euler, 0),
np.isclose(out + angles_euler, 0),
)),
msg="Failed for convention {}, axis type {}".format(
convention, axis_type),
)
for convention in conventions_tb:
for axis_type in axis_types:
out = rowan.to_euler(
rowan.from_euler(
angles_tb[..., 0],
angles_tb[..., 1],
angles_tb[..., 2],
convention,
axis_type,
),
convention,
axis_type,
)
self.assertTrue(
np.all(
np.logical_or(
np.isclose(out - angles_tb, 0),
np.isclose(out + angles_tb, 0),
)),
msg="Failed for convention {}, axis type {}".format(
convention, axis_type),
)
def test_from_to_euler(self):
"""2-way conversion starting from Euler angles."""
np.random.seed(0)
quats = rowan.normalize(np.random.rand(25, 4))
conventions = [
"xzx",
"xyx",
"yxy",
"yzy",
"zyz",
"zxz",
"xzy",
"xyz",
"yxz",
"yzx",
"zyx",
"zxy",
]
axis_types = ["extrinsic", "intrinsic"]
for convention in conventions:
for axis_type in axis_types:
euler = rowan.to_euler(quats, convention, axis_type)
out = rowan.from_euler(euler[..., 0], euler[..., 1],
euler[..., 2], convention, axis_type)
self.assertTrue(
np.all(
np.logical_or(np.isclose(out - quats, 0),
np.isclose(out + quats, 0))),
msg="Failed for convention {}, axis type {}".format(
convention, axis_type),
)
def render(self, rotation=(1, 0, 0, 0), translation=(0, 0, 0), **kwargs):
(positions, orientations, colors) = mesh.unfoldProperties(
[self.positions, self.orientations, self.colors])
positions = rowan.rotate(rotation, positions)
positions += translation
orientations = rowan.multiply(rotation, rowan.normalize(orientations))
rotations = np.degrees(rowan.to_euler(orientations))
lines = []
for (pos, rot, col, alpha) in zip(positions, rotations, colors[:, :3],
1 - colors[:, 3]):
lines.append(
'sphere {{ '
'0, 1 scale<{a}, {b}, {c}> '
'rotate <{rot[2]}, {rot[1]}, {rot[0]}> '
'translate <{pos[0]}, {pos[1]}, {pos[2]}> '
'pigment {{ color <{col[0]}, {col[1]}, {col[2]}> transmit {alpha} }} '
'}}'.format(a=self.a,
b=self.b,
c=self.c,
pos=pos,
rot=rot,
col=col,
alpha=alpha))
return lines
def quaternion2z(quaternion: np.ndarray) -> np.ndarray:
"""Convert a rotation about the z axis to a quaternion.
This is a helper for 2D simulations, taking the rotation of a particle about the z axis and
converting it to a quaternion. The input angle `theta` is assumed to be in radians.
"""
return rowan.to_euler(quaternion)[:, 0].astype(np.float32)
def policy(self, state):
# set state
self.state.position.x = state[0]
self.state.position.y = state[1]
self.state.position.z = state[2]
self.state.velocity.x = state[3]
self.state.velocity.y = state[4]
self.state.velocity.z = state[5]
rpy = np.degrees(rowan.to_euler(state[6:10], 'xyz'))
self.state.attitude.roll = rpy[0]
self.state.attitude.pitch = -rpy[1] # inverted coordinate system!
self.state.attitude.yaw = rpy[2]
self.sensors.gyro.x = np.degrees(state[10])
self.sensors.gyro.y = np.degrees(state[11])
self.sensors.gyro.z = np.degrees(state[12])
firm.controllerSJC(self.control, self.setpoint, self.sensors,
self.state, self.tick)
self.tick += 10
# power distribution
thrust = self.control.thrustSI
torqueArr = firm.floatArray_frompointer(self.control.torque)
torque = np.array([torqueArr[0], torqueArr[1], torqueArr[2]])
thrust_to_torque = 0.006
arm_length = 0.046
thrustpart = 0.25 * thrust
yawpart = -0.25 * torque[2] / thrust_to_torque
arm = 0.707106781 * arm_length
rollpart = 0.25 / arm * torque[0]
pitchpart = 0.25 / arm * torque[1]
motorForce = np.array([
thrustpart - rollpart - pitchpart + yawpart,
thrustpart - rollpart + pitchpart - yawpart,
thrustpart + rollpart + pitchpart + yawpart,
thrustpart + rollpart - pitchpart - yawpart
])
motorForce = np.clip(motorForce, self.a_min, self.a_max)
v = firm.controllerSJCGetq()
self.q.append(np.array([v.x, v.y, v.z]))
v = firm.controllerSJCGetqr()
self.qr.append(np.array([v.x, v.y, v.z]))
v = firm.controllerSJCGetomega()
self.omega.append(np.array([v.x, v.y, v.z]))
v = firm.controllerSJCGetomegar()
self.omegar.append(np.array([v.x, v.y, v.z]))
# return np.array([0.0, 0.01, 0.01, 0.0]) * 9.81
return motorForce
def add(self, position: np.ndarray, orientation: Optional[np.ndarray]):
"""Update the state of the dynamics calculations by adding the next values.
This updates the motion of the particles, comparing the positions and
orientations of the current frame with the previous frame, adding the difference
to the total displacement. This approach allows for tracking particles over
periodic boundaries, or through larger rotations assuming that there are
sufficient frames to capture the information. Each single displacement obeys the
minimum image convention, so for large time intervals it is still possible to
have missing information.
Args:
position: The current positions of the particles
orientation: The current orientations of the particles represented as a quaternion
"""
self.delta_translation -= self.box.wrap(self.previous_position - position)
if self.previous_orientation is not None and orientation is not None:
self.delta_rotation += rowan.to_euler(
rowan.divide(orientation, self.previous_orientation)
)
self.previous_position = position
self.previous_orientation = orientation
def deduce_state(self, s):
rpy = np.degrees(rowan.to_euler(rowan.normalize(s[6:10]), 'xyz'))
return rpy
def test_to_euler(self):
"""Test conversion to Euler angles"""
v = one
self.assertTrue(np.all(rowan.to_euler(v) == np.array([0, 0, 0])))
v = np.array([0.5, 0.5, 0.5, 0.5])
self.assertTrue(
np.all(rowan.to_euler(v) == np.array([np.pi / 2, 0, np.pi / 2])))
# More complicated test, checks 2d arrays
# and more complex angles
self.assertTrue(
np.allclose(rowan.to_euler(euler_quaternions, 'zyz', 'intrinsic'),
euler_angles))
# Ensure proper errors are raised
with self.assertRaises(ValueError):
rowan.to_euler(euler_quaternions, 'foo', 'intrinsic')
with self.assertRaises(ValueError):
rowan.to_euler(euler_quaternions, 'foo', 'extrinsic')
with self.assertRaises(ValueError):
rowan.to_euler(euler_quaternions, 'zyz', 'bar')
with self.assertRaises(ValueError):
rowan.to_euler(2 * one)
with self.assertRaises(ValueError):
rowan.to_euler(zero)
def test_zero_beta(self):
"""Check cases where beta is 0."""
# Since the Euler calculations are all done using matrices, it's easier
# to construct the test cases by directly using matrices as well. We
# assume gamma is 0 since, due to gimbal lock, only either alpha+gamma
# or alpha-gamma is a relevant parameter, and we just scan the other
# possible values. The actual function is defined such that gamma will
# always be zero in those cases. We define the matrices using lambda
# functions to support sweeping a range of values for alpha and beta,
# specifically to test cases where signs flip e.g. cos(0) vs cos(pi).
# These sign flips lead to changes in the rotation angles that must be
# tested.
mats_euler_intrinsic = [
('xzx', 'intrinsic', lambda alpha, beta:
[[np.cos(beta), 0, 0],
[0, np.cos(beta) * np.cos(alpha), -np.sin(alpha)],
[0, np.cos(beta) * np.sin(alpha),
np.cos(alpha)]]),
('xyx', 'intrinsic', lambda alpha, beta:
[[np.cos(beta), 0, 0],
[0, np.cos(alpha), -np.cos(beta) * np.sin(alpha)],
[0, np.sin(alpha),
np.cos(beta) * np.cos(alpha)]]),
('yxy', 'intrinsic', lambda alpha, beta:
[[np.cos(alpha), 0,
np.cos(beta) * np.sin(alpha)], [0, np.cos(beta), 0],
[-np.sin(alpha), 0,
np.cos(beta) * np.cos(alpha)]]),
('yzy', 'intrinsic', lambda alpha, beta:
[[np.cos(beta) * np.cos(alpha), 0,
np.sin(alpha)], [0, np.cos(beta), 0],
[-np.cos(beta) * np.sin(alpha), 0,
np.cos(alpha)]]),
('zyz', 'intrinsic', lambda alpha, beta: [[
np.cos(beta) * np.cos(alpha), -np.sin(alpha), 0
], [np.cos(beta) * np.sin(alpha),
np.cos(beta), 0], [0, 0, np.cos(beta)]]),
('zxz', 'intrinsic', lambda alpha, beta:
[[np.cos(alpha), -np.cos(beta) * np.sin(alpha), 0],
[np.sin(alpha), np.cos(beta) * np.cos(beta), 0],
[0, 0, np.cos(beta)]]),
]
mats_tb_intrinsic = [
('xzy', 'intrinsic', lambda alpha, beta: [[0, -np.sin(
beta), 0], [np.sin(beta) * np.cos(alpha), 0, -np.sin(
alpha)], [np.sin(beta) * np.sin(alpha), 0,
np.cos(alpha)]]),
('xyz', 'intrinsic',
lambda alpha, beta: [[0, 0, np.sin(
beta)], [np.sin(beta) * np.sin(alpha),
np.cos(alpha), 0
], [-np.sin(beta) * np.cos(alpha),
np.sin(alpha), 0]]),
('yxz', 'intrinsic', lambda alpha, beta:
[[np.cos(alpha), np.sin(beta) * np.sin(alpha), 0],
[0, 0, -np.sin(beta)],
[-np.sin(alpha), np.sin(beta) * np.cos(alpha), 0]]),
('yzx', 'intrinsic', lambda alpha, beta:
[[0, -np.sin(beta) * np.cos(alpha),
np.sin(alpha)], [np.sin(beta), 0, 0],
[0, np.sin(beta) * np.sin(alpha),
np.cos(alpha)]]),
('zyx', 'intrinsic', lambda alpha, beta: [[
0, -np.sin(alpha),
np.sin(beta) * np.cos(alpha)
], [0, np.cos(alpha),
np.sin(beta) * np.sin(alpha)], [-np.sin(beta), 0, 0]]),
('zxy', 'intrinsic', lambda alpha, beta: [[
np.cos(alpha), 0,
np.sin(beta) * np.sin(alpha)
], [np.sin(alpha), 0, -np.sin(beta) * np.cos(alpha)], [0, -1, 0]]),
]
# Extrinsic rotations can be tested identically to intrinsic rotations
# in the case of proper Euler angles.
mats_euler_extrinsic = [(m[0], 'extrinsic', m[2])
for m in mats_euler_intrinsic]
# For Tait-Bryan angles, extrinsic rotations axis order must be
# reversed (since axes 1 and 3 are not identical), but more
# importantly, due to the sum/difference of alpha and gamma that
# arises, we need to test the negative of alpha to catch the dangerous
# cases. In practice we get the same results since we're sweeping alpha
# values in the tests below, but it's useful to set this up precisely.
mats_tb_extrinsic = [(m[0][::-1], 'extrinsic', lambda alpha, beta: m[2]
(-alpha, beta)) for m in mats_tb_intrinsic]
# Since angle representations may not be unique, checking that
# quaternions are equal may not work. Instead we perform rotations and
# check that they are identical. For simplicity, we rotate the
# simplest vector with all 3 components (otherwise tests won't catch
# the problem because there's no component to rotate).
test_vector = [1, 1, 1]
mats_intrinsic = (mats_euler_intrinsic, mats_tb_intrinsic)
mats_extrinsic = (mats_euler_extrinsic, mats_tb_extrinsic)
# The beta angles are different for proper Euler angles and Tait-Bryan
# angles because the relevant beta terms will be sines and cosines,
# respectively.
all_betas = ((0, np.pi), (np.pi / 2, -np.pi / 2))
alphas = (0, np.pi / 2, np.pi, 3 * np.pi / 2)
for mats in (mats_intrinsic, mats_extrinsic):
for betas, mat_set in zip(all_betas, mats):
for convention, axis_type, mat_func in mat_set:
quaternions = []
for beta in betas:
for alpha in alphas:
mat = mat_func(alpha, beta)
if np.linalg.det(mat) == -1:
# Some of these will be improper rotations.
continue
quat = rowan.from_matrix(mat)
quaternions.append(quat)
euler = rowan.to_euler(quat, convention, axis_type)
converted = rowan.from_euler(*euler,
convention=convention,
axis_type=axis_type)
correct_rotation = rowan.rotate(quat, test_vector)
test_rotation = rowan.rotate(
converted, test_vector)
self.assertTrue(np.allclose(correct_rotation,
test_rotation,
atol=1e-6),
msg="""
Failed for convention {},
axis type {},
alpha = {},
beta = {}.
Expected quaternion: {}.
Calculated: {}.
Expected vector: {}.
Calculated vector: {}.""".format(
convention, axis_type, alpha,
beta, quat, converted,
correct_rotation,
test_rotation))
# For completeness, also test with broadcasting.
quaternions = np.asarray(quaternions).reshape(-1, 4)
all_euler = rowan.to_euler(quaternions, convention,
axis_type)
converted = rowan.from_euler(all_euler[...,
0], all_euler[...,
1],
all_euler[..., 2], convention,
axis_type)
self.assertTrue(
np.allclose(rowan.rotate(quaternions, test_vector),
rowan.rotate(converted, test_vector),
atol=1e-6))
def render(self):
"""Render all the shapes in this scene.
:returns: HTML string contents to be displayed
"""
canvas_id = 'zdog_{}'.format(self.CANVAS_INDEX)
illo_id = 'illo_{}'.format(self.CANVAS_INDEX)
Scene.CANVAS_INDEX += 1
html_lines = []
js_lines = []
euler = -rowan.to_euler(
self.rotation, convention='xyz', axis_type='intrinsic')
translation = self.translation*(1, -1, 1)
pan_cfg = self.get_feature_config('pan')
pan = pan_cfg.get('value', True) if pan_cfg is not None else False
js_lines.append("""
let {illo_id} = new Zdog.Illustration({{
element: '#{canvas_id}',
zoom: {zoom},
dragRotate: {rotation_enabled},
rotate: {{x: {angle[0]}, y: {angle[1]}, z: {angle[2]}}},
translate: {{x: {pos[0]}, y: {pos[1]}, z: {pos[2]}}},
}});
""".format(
illo_id=illo_id, canvas_id=canvas_id, zoom=self.zoom*self.pixel_scale,
angle=euler, pos=translation,
rotation_enabled=('false' if pan else 'true')))
config = self.get_feature_config('ambient_light')
ambient_light = 0 if config is None else config.get('value', .4)
config = self.get_feature_config('directional_light')
directional_light = ([(0, 0, 0)] if config is None else
config.get('value', [(0, 0, 0)]))
directional_light = np.atleast_2d(directional_light)
shapeIndex = 0
for i, prim in enumerate(self._primitives):
js_lines.extend(prim.render(
rotation=self.rotation, illo_id=illo_id,
name_suffix=i, ambient_light=ambient_light,
directional_light=directional_light))
(width, height) = map(int, self.size_pixels)
html_lines.append("""
<canvas id="{canvas_id}" width="{width}" height="{height}"></canvas>
""".format(canvas_id=canvas_id, width=width, height=height))
html_lines.append("""<script>
var fill_{canvas_id} = function() {{
""".format(canvas_id=canvas_id))
html_lines.append(LOCAL_HELPER_SCRIPT)
html_lines.extend(js_lines)
pan_snippet = """
new Zdog.Dragger({{
startElement: {illo_id}.element,
onDragStart: function( pointer, moveX, moveY) {{
this.lastX = 0;
this.lastY = 0;
}},
onDragMove: function( pointer, moveX, moveY ) {{
let deltax = moveX - this.lastX;
let deltay = moveY - this.lastY;
let scale = 1.0/{illo_id}.zoom;
{illo_id}.translate.x += deltax*scale;
{illo_id}.translate.y += deltay*scale;
this.lastX = moveX;
this.lastY = moveY;
}}
}});""".format(illo_id=illo_id)
if pan:
html_lines.append(pan_snippet)
html_lines.append("""
let this_canvas = document.querySelector("#{canvas_id}");
""".format(canvas_id=canvas_id))
html_lines.append("""
let animate_{canvas_id} = function() {{
if(is_in_view(this_canvas))
{{
{illo_id}.updateRenderGraph();
}}
if(document.contains(this_canvas))
{{
requestAnimationFrame(animate_{canvas_id});
}}
}};
animate_{canvas_id}();""".format(canvas_id=canvas_id, illo_id=illo_id))
# remove the global reference to this function after using it
html_lines.append('fill_{canvas_id} = null;'.format(canvas_id=canvas_id))
html_lines.append('};') # end of fill_{canvas_id}
# now call fill_{canvas_id}, possibly after loading zdog
html_lines.append("""
if (typeof Zdog == 'undefined')
{{
var script = document.createElement('script');
script.addEventListener('load', fill_{canvas_id}, false);
script.src = 'https://unpkg.com/zdog@1/dist/zdog.dist.min.js';
document.getElementsByTagName('head')[0].appendChild(script);
}}
else
fill_{canvas_id}();
""".format(canvas_id=canvas_id))
html_lines.append('</script>')
return '\n'.join(html_lines)
def f(self, X, Z, A, t, logentry=None):
# x = X[0]
# y = X[1]
# z = X[2]
# xdot = X[3]
# ydot = X[4]
# zdot = X[5]
# qw = X[6]
# qx = X[7]
# qy = X[8]
# qz = X[9]
# wx = X[10]
# wy = X[11]
# wz = X[12]
a = Z**2
# a = A
J = self.J
dxdt = np.zeros(13)
q = X[6:10]
q = rowan.normalize(q)
omega = X[10:]
B0 = self.B0 * self.params['C_T']
# B0 = self.B0
eta = np.dot(B0, a)
f_u = np.array([0, 0, eta[0]])
tau_u = np.array([eta[1], eta[2], eta[3]])
dxdt[0:3] = X[3:6]
# dxdt[3:6] = -9.81 + rowan.rotate(q, f_u) / self.params['m']
dxdt[3:6] = np.array([0., 0., -9.81
]) + rowan.rotate(q, f_u) / self.params['m']
Vinf = self.params['Vwind_mean'] - np.linalg.norm(X[3:6])
Vinf_B = rowan.rotate(rowan.inverse(q), Vinf)
Vz_B = np.array([0.0, 0.0, Vinf_B[2]])
Vs_B = Vinf_B - Vz_B
alpha = np.arcsin(np.linalg.norm(Vz_B) / np.linalg.norm(Vinf_B))
n = np.sqrt(
np.multiply(a, self.B0[0, :]) /
(self.params['C_T'] * self.params['rho'] * self.params['D']**4))
Fs_B = (Vs_B / np.linalg.norm(Vs_B)
) * self.params['C_s'] * self.params['rho'] * sum(
n**self.params['k1']) * (np.linalg.norm(Vinf)**(
2 - self.params['k1'])) * (self.params['D']**(
2 + self.params['k1'])) * (
(np.pi / 2)**2 -
alpha**2) * (alpha + self.params['k2'])
# Fs_B = [0,0,0]
dxdt[3:6] += rowan.rotate(q, Fs_B) / self.mass
qnew = rowan.calculus.integrate(q, omega, self.ave_dt)
if qnew[0] < 0:
qnew = -qnew
dxdt[6:10] = (qnew - q) / self.ave_dt
dxdt[10:] = 1 / J * (np.cross(J * omega, omega) + tau_u)
dxdt[10:] += 1 / J * np.cross(
np.array([0.0, 0.0, self.params['D'] / 4]), Fs_B)
euler_o = rowan.to_euler(q, 'xyz')
if logentry is not None:
logentry['f_u'] = f_u
logentry['tau'] = tau_u
logentry['euler_o'] = euler_o
return dxdt.reshape((len(dxdt), 1))
def write(self, trajectory, file=sys.stdout):
"""Serialize a trajectory into pos-format and write it to file.
:param trajectory: The trajectory to serialize
:type trajectory: :class:`~garnett.trajectory.Trajectory`
:param file: A file-like object."""
def _write(msg, end='\n'):
file.write(msg + end)
for i, frame in enumerate(trajectory):
# data section
if frame.data is not None:
header_keys = frame.data_keys
_write('#[data] ', end='')
_write(' '.join(header_keys))
columns = list()
for key in header_keys:
columns.append(frame.data[key])
rows = np.array(columns).transpose()
for row in rows:
_write(' '.join(row))
_write('#[done]')
# boxMatrix and rotation
box_matrix = np.array(frame.box.get_box_matrix())
if self._rotate and frame.view_rotation is not None:
for i in range(3):
box_matrix[:, i] = rowan.rotate(frame.view_rotation,
box_matrix[:, i])
if frame.view_rotation is not None and not self._rotate:
angles = rowan.to_euler(frame.view_rotation,
axis_type='extrinsic',
convention='xyz') * 180 / math.pi
_write('rotation ' + ' '.join((str(_num(_)) for _ in angles)))
_write('boxMatrix ', end='')
_write(' '.join((str(_num(v)) for v in box_matrix.flatten())))
# shape defs
try:
if len(frame.types) != len(frame.type_shapes):
raise ValueError(
"Unequal number of types and type_shapes.")
for name, type_shape in zip(frame.types, frame.type_shapes):
_write('def {} "{}"'.format(name, type_shape.pos_string))
except AttributeError:
# If AttributeError is raised because the frame does not contain
# shape information, fill them all with the default shape
for name in frame.types:
logger.info("No shape defined for '{}'. "
"Using fallback definition.".format(name))
_write('def {} "{}"'.format(
name, DEFAULT_SHAPE_DEFINITION.pos_string))
# Orientations must be provided for all particles
# If the frame does not have orientations, identity quaternions are used
orientation = getattr(frame, 'orientation',
np.array([[1, 0, 0, 0]] * frame.N))
for typeid, pos, rot in zip(frame.typeid, frame.position,
orientation):
name = frame.types[typeid]
_write(name, end=' ')
try:
shapedef = frame.shapedef.get(name)
except AttributeError:
shapedef = DEFAULT_SHAPE_DEFINITION
if self._rotate and frame.view_rotation is not None:
pos = rowan.rotate(frame.view_rotation, pos)
rot = rowan.multiply(frame.view_rotation, rot)
if isinstance(shapedef, SphereShape):
_write(' '.join((str(_num(v)) for v in pos)))
elif isinstance(shapedef, ArrowShape):
# The arrow shape actually has two position vectors of
# three elements since it has start.{x,y,z} and end.{x,y,z}.
# That is, "rot" is not an accurate variable name, since it
# does not represent a quaternion.
_write(' '.join(
(str(_num(v)) for v in chain(pos, rot[:3]))))
else:
_write(' '.join((str(_num(v)) for v in chain(pos, rot))))
_write('eof')
logger.debug("Wrote frame {}.".format(i + 1))
logger.info("Wrote {} frames.".format(i + 1))
def policy(self, state): # current state p = state[0:3] v = state[3:6] q = state[6:10] omega = state[10:13] R = rowan.to_matrix(q) # position controller # p_tilde = p - self.p_d # v_tilde = v - self.v_d # v_r = self.v_d - self.lambda_p * p_tilde # v_r_dot = self.a_d - self.lambda_p * v_tilde # s_v = v - v_r # f_r = self.mass * v_r_dot \ # - self.Kv * s_v \ # - self.Kp * p_tilde # thrust = np.linalg.norm(f_r) # qr = mkvec( # asinf((F_d.x * sinf(yaw) - F_d.y * cosf(yaw)) / control->thrustSI), # atanf((F_d.x * cosf(yaw) + F_d.y * sinf(yaw)) / F_d.z), # desiredYaw); # position controller pos_e = self.p_d - p vel_e = self.v_d - v F_d = self.mass * (self.a_d + self.Kpos_D * vel_e + self.Kpos_P * pos_e) print(F_d) thrust = np.linalg.norm(F_d) yaw = 0 rpy_d = np.array([ np.arcsin((F_d[0] * np.sin(yaw) - F_d[1] * np.cos(yaw)) / thrust), np.arctan((F_d[0] * np.cos(yaw) + F_d[1] * np.sin(yaw)) / F_d[2]), yaw]) R_d = rowan.to_matrix(rowan.from_euler(*rpy_d)) print(rpy_d) print(R_d) # attitude controller # rotation error Rtilde = self.R_d.T @ R qtilde_0 = 1/2 * np.sqrt(1 + np.trace(Rtilde)) qtilde_v = 1 / (4 * qtilde_0) * veemap(Rtilde - Rtilde.T) omega_r = Rtilde.T @ self.omega_d - 2 * self.lambda_a * qtilde_v print(omega_r) if self.omega_r_last is not None: omega_r_dot = (omega_r - self.omega_r_last) / 0.01 else: omega_r_dot = np.zeros(3) self.omega_r_last = omega_r s_omega = omega - omega_r torque = self.J * omega_r_dot \ - np.cross(self.J * omega, omega_r) \ - self.K_att * s_omega \ - self.kq * qtilde_v # power distribution thrust_to_torque = 0.006 arm_length = 0.046 thrustpart = 0.25 * thrust yawpart = -0.25 * torque[2] / thrust_to_torque arm = 0.707106781 * arm_length rollpart = 0.25 / arm * torque[0] pitchpart = 0.25 / arm * torque[1] motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) motorForce = np.clip(motorForce, self.a_min, self.a_max) # logging self.qr.append(np.degrees(rpy_d)) self.q.append(np.degrees(rowan.to_euler(q, 'xyz'))) self.omegar.append(omega_r) self.omega.append(omega) return motorForce
def policy(self, state): # current state p = state[0:3] v = state[3:6] q = state[6:10] omega = state[10:13] R = rowan.to_matrix(q) # position controller pos_e = self.p_d - p vel_e = self.v_d - v F_d = self.mass * (self.a_d + self.Kpos_D * vel_e + self.Kpos_P * pos_e) print(F_d) thrust = np.linalg.norm(F_d) yaw = 0 q_d = np.array([ np.arcsin((F_d[0] * np.sin(yaw) - F_d[1] * np.cos(yaw)) / thrust), np.arctan((F_d[0] * np.cos(yaw) + F_d[1] * np.sin(yaw)) / F_d[2]), yaw]) if self.q_d_last is not None: q_d_dot = (q_d - self.q_d_last) / 0.01 else: q_d_dot = np.zeros(3) self.q_d_last = q_d # attitude controller q = rowan.to_euler(q, 'xyz') omega_r = Zinv(q) @ (q_d_dot +self.lambda_att * (q_d -q)) if self.omega_r_last is not None: omega_r_dot = (omega_r - self.omega_r_last) / 0.01 else: omega_r_dot = np.zeros(3) self.omega_r_last = omega_r torque = self.J * omega_r_dot \ - np.cross(self.J * omega, omega_r) \ - self.K_att * (omega - omega_r) # power distribution thrust_to_torque = 0.006 arm_length = 0.046 thrustpart = 0.25 * thrust yawpart = -0.25 * torque[2] / thrust_to_torque arm = 0.707106781 * arm_length rollpart = 0.25 / arm * torque[0] pitchpart = 0.25 / arm * torque[1] motorForce = np.array([ thrustpart - rollpart - pitchpart + yawpart, thrustpart - rollpart + pitchpart - yawpart, thrustpart + rollpart + pitchpart + yawpart, thrustpart + rollpart - pitchpart - yawpart ]) motorForce = np.clip(motorForce, self.a_min, self.a_max) # logging self.qr.append(np.degrees(q_d)) self.q.append(np.degrees(q)) self.omegar.append(omega_r) self.omega.append(omega) return motorForce
def render(self,
rotation=(1, 0, 0, 0),
name_suffix='',
illo_id='illo',
ambient_light=0.4,
directional_light=[],
stroke=False,
outline=False,
**kwargs):
# in the zdog coordinate system, x is to the right, y is down,
# and z is toward you
lines = []
stroke = stroke or 'false'
(vertices, faces) = geometry.convexHull(self.vertices)
face_normals = []
face_paths = []
for face in faces:
r01 = vertices[face[1]] - vertices[face[0]]
r12 = vertices[face[2]] - vertices[face[1]]
normal = np.cross(r01, r12)
normal /= np.linalg.norm(normal)
face_normals.append(normal)
path = ', '.join('{{x: {}, y: {}, z: {}}}'.format(*v)
for v in vertices[face] * (1, -1, 1))
face_paths.append(path)
face_normals = np.array(face_normals, dtype=np.float32)
orientations_euler = rowan.to_euler(self.orientations,
convention='xyz',
axis_type='intrinsic')
particles = zip(*mesh.unfoldProperties([
self.positions *
(1, -1, 1), self.orientations, -orientations_euler, self.colors *
255
]))
for i, (position, orientation, eulers, color) in enumerate(particles):
group_index = 'convexPoly_{}_{}'.format(name_suffix, i)
# full rotation to apply to vectors from base orientation
# to final scene orientation
full_rotation = rowan.multiply(rotation, orientation)
lines.append("""
let {group_index} = new Zdog.Group({{
addTo: {illo_id},
rotate: {{x: {angle[0]}, y: {angle[1]}, z: {angle[2]}}},
translate: {{x: {pos[0]}, y: {pos[1]}, z: {pos[2]}}},
updateSort: true,
}});""".format(group_index=group_index,
illo_id=illo_id,
angle=eulers,
pos=position))
for (face_path, normal) in zip(face_paths, face_normals):
rotated_normal = rowan.rotate(full_rotation, normal)
light = ambient_light
for direction in directional_light:
light += max(0, -np.dot(rotated_normal, direction))
light = np.clip(light, 0, 1)
(r, g, b) = map(int, light * color[:3])
# RGB components are 0-255, A component is a float 0-1
face_color = '"rgba({}, {}, {}, {})"'.format(
r, g, b, color[3] / 255)
lines.append("""
new Zdog.Shape({{
addTo: {group_index},
color: {face_color},
path: [{path}],
fill: true,
backface: true,
stroke: {stroke},
}});
""".format(group_index=group_index,
face_color=face_color,
path=face_path,
stroke=stroke))
if outline:
outline_color = '"rgba(0, 0, 0, {})"'.format(color[3] /
255)
lines.append("""
new Zdog.Shape({{
addTo: {group_index},
color: {color},
path: [{path}],
fill: false,
stroke: {stroke},
}});
""".format(group_index=group_index,
color=outline_color,
path=face_path,
stroke=outline))
return lines
def policy(self,
X,
pd=np.zeros(3),
vd=np.zeros(3),
ad=np.zeros(3),
jd=np.zeros(3),
sd=np.zeros(3),
logentry=None,
time=None,
K_i=0,
K_p=0,
K_d=0):
# print('baseline: X = ', X)
p_e = pd - X[:3]
v_e = vd - X[3:6]
print(X)
int_p_e = self.integrate_error(p_e, time)
F_d = (self.K_i * int_p_e + self.K_p * p_e +
self.K_d * v_e) * self.mass # TODO: add integral term
F_d[2] += self.g * self.mass
T_d = np.linalg.norm(F_d)
yaw_d = 0
roll_d = np.arcsin(
(F_d[0] * np.sin(yaw_d) - F_d[1] * np.cos(yaw_d)) / T_d)
pitch_d = np.arctan(
(F_d[0] * np.cos(yaw_d) + F_d[1] * np.sin(yaw_d)) / F_d[2])
euler_d = np.array([roll_d, pitch_d, yaw_d])
q = rowan.normalize(X[6:10])
euler = rowan.to_euler(q, 'xyz')
att_e = -(euler - euler_d)
th_r = rowan.normalize(
rowan.from_euler(att_e[0], att_e[1], att_e[2], 'xyz'))
th_d = rowan.normalize(
rowan.from_euler(euler_d[0], euler_d[1], euler_d[2], 'xyz'))
att_v_e = -X[10:]
torque = self.Att_p * att_e + self.Att_d * att_v_e
torque[0] *= self.J[0]
torque[1] *= self.J[1]
torque[2] *= self.J[2]
Jomega = np.array(
[self.J[0] * X[10], self.J[1] * X[11], self.J[2] * X[12]])
torque -= np.cross(Jomega, X[10:])
yawpart = -0.25 * torque[2] / self.t2t
rollpart = 0.25 / self.arm * torque[0] #/ self.t2t
pitchpart = 0.25 / self.arm * torque[1] #/ self.t2t
thrustpart = 0.25 * T_d
motorForce = np.array([
thrustpart - rollpart - pitchpart + yawpart,
thrustpart - rollpart + pitchpart - yawpart,
thrustpart + rollpart + pitchpart + yawpart,
thrustpart + rollpart - pitchpart - yawpart
])
Fc = motorForce / self.params['C_T']
omega = np.sqrt(np.maximum(Fc, self.params['motor_min_speed'])
) # Ensure each prop has a positive thrust
omega = np.minimum(
omega, self.params['motor_max_speed']) # Maximum rotation speed
# print('baseline: omega = ', omega)
# print('T', T_r)
# print('tau',tau_r)
# x_error = pd[0] - X[0]
# y_error = pd[1] - X[1]
# z_error = pd[2] - X[2]
#
# vx_error = vd[0] - X[3]
# vy_error = vd[1] - X[4]
# vz_error = vd[2] - X[5]
#
# # print('baseline: x_error = ', x_error)
# # print('baseline: y_error = ', y_error)
#
# ax_d = ad[0]
# ay_d = ad[1]
# az_d = ad[2]
#
# self.int_x_error += self.params.dt_posctrl * x_error
# self.int_y_error += self.params.dt_posctrl * y_error
# self.int_z_error += self.params.dt_posctrl * z_error
#
# ax_r = self.params['K_p_x'] * x_error + self.params['K_d_x'] * vx_error + self.params[
# 'K_i_x'] * self.int_x_error + ax_d
# ay_r = self.params['K_p_y'] * y_error + self.params['K_d_y'] * vy_error + self.params[
# 'K_i_y'] * self.int_y_error + ay_d
# az_r = self.params['K_p_z'] * z_error + self.params['K_d_z'] * vz_error + self.params[
# 'K_i_z'] * self.int_z_error + az_d
# # print('baseline: x PDI terms:',K_p_x * x_error, K_d_x * vx_error, K_i_x * intx_error, ax_d)
# # print('baseline: y PDI terms:',K_p_y * y_error, K_d_y * vy_error, K_i_y * inty_error, ay_d)
#
# jx_d = jd[0]
# jy_d = jd[1]
# jz_d = jd[2]
# # jx_r = K_p_x * vx_error + K_d_x * (ax_d - ax_r) + K_i_x * x_error + jx_d
# # jy_r = K_p_y * vy_error + K_d_y * (ay_d - ay_r) + K_i_y * y_error + jy_d
#
# sx_d = sd[0]
# sy_d = sd[1]
# sz_d = sd[2]
# # sx_r = K_p_x * (ax_d - ax_r) + K_d_x * (jx_d - jx_r) + K_i_x * vx_error + sx_d
# # sy_r = K_p_y * (ay_d - ay_r) + K_d_y * (jy_d - jy_r) + K_i_y * vy_error + sy_d
#
# # print('baseline: ay_r = ', ay_r)
#
# Fx_r = ax_r * self.params['m']
# Fy_r = (ay_r + self.params['g']) * self.params['m']
# Fz_r = az_r * self.params['m']
#
# Fy_r = np.maximum(Fy_r, 0.10 * self.params['g'] * self.params['m'])
#
#
# # print('baseline: Fx_r, Fy_r = ', Fx_r, Fy_r)
#
# T_r = np.minimum(np.sqrt(Fy_r ** 2 + Fx_r ** 2 + Fz_r ** 2), 125.) # / np.cos(X[2])
# if T_r > 124.:
# warn('thrust gets too high')
# # print('T_r = ', T_r)
# T_d = self.params['m'] * np.sqrt(ax_d ** 2 + (ay_d + self.params['g']) ** 2 + az_d ** 2)
#
# # th = X[2]
# th_r =
# th_d =
#
# mat = np.array([[-np.sin(th_d), -T_d * np.cos(th_d)], #TODO: use th_d, T_d
# [np.cos(th_d), -T_d * np.sin(th_d)]])
# b = self.params['m'] * np.array([jx_d, jy_d, jz_d])#
# v = np.linalg.solve(mat, b)
# T_d_dot = v[0]
# w_d = v[1]
# # w = X[5]
#
# mat = np.array([[-np.sin(th_d), -T_d * np.cos(th_d)],
# [np.cos(th_d), -T_d * np.sin(th_d)]])
# b = self.params['m'] * np.array([sx_d, sy_d]) - np.array(
# [-2. * T_d_dot * np.cos(th_d) * w_d + T_d * np.sin(th_d) * (w_d ** 2),
# -2. * T_d_dot * np.sin(th_d) * w_d - T_d * np.cos(th_d) * (w_d ** 2)])
#
# v = np.linalg.solve(mat, b)
# # T_d_ddot = v[0]
# alpha_d = v[1]
# # print('baseline_pos: Tdot, w_d, alpha_d', Tdot, w_d, alpha_d)
w_d = 0
alpha_d = 0
if logentry is not None:
logentry['th_d'] = th_d
logentry['th_r'] = th_r
logentry['T_d'] = T_d
logentry['euler'] = euler
logentry['att_e'] = att_e
# logentry['Fx_r'] = Fx_r
# logentry['Fy_r'] = Fy_r
# logentry['Fz_r'] = Fz_r
# logentry['int_x_error'] = self.int_x_error
# logentry['int_y_error'] = self.int_y_error
# logentry['int_y_error'] = self.int_z_error
return T_d, th_d, w_d, alpha_d, omega, motorForce
