Tensegrity Geometry

geom.py

#!/usr/bin/env python

import numpy as np
from collections import OrderedDict
from scipy.optimize import LinearConstraint, minimize, BFGS


def normlen(v):
    ''' length of norm of a vector '''
    return np.sqrt(np.sum(np.square(v)))


# FREE PARAMETERS
L = 13  # Length of upper triangle side
H = 5  # Height of structure
G = 7  # Length of lower triangle side
T = 0  # Angle (degrees) of top triangle (should probably be 0)
U = 200  # Angle (degrees) of bottom triangle (should probably be 200)

# GEOMETRY CALCULATION
X, Y, Z = np.eye(3)  # Unit vectors
J = L * np.sqrt(3) / 3  # Radius of upper triangle
K = G * np.sqrt(3) / 3  # Radius of lower triangle
# Top triangle points
a, b, c = [np.deg2rad(T + x) for x in range(0, 360, 120)]  # Angles in radians
A, B, C = [np.array([np.sin(x) * J, np.cos(x) * J, H]) for x in (a, b, c)]
# Bottom triangle points
d, e, f = [np.deg2rad(U + x) for x in range(0, 360, 120)]  # Angles in radians
D, E, F = [np.array([np.sin(x) * K, np.cos(x) * K, 0]) for x in (d, e, f)]

print('Points')
for i, j in zip('ABCDEF', (A, B, C, D, E, F)):
    print(i, j)

def normvec(v):
    ''' return a normalized vector '''
    return np.array(v) / normlen(v)


# structural force vectors
structure = OrderedDict([
    ('rod AD', normvec(A - D)),
    ('rope AB', normvec(B - A)),
    ('rope AC', normvec(C - A)),
    ('rope AE', normvec(E - A)),
])
SV = np.array(list(structure.values())).transpose()


# hammock force vectors
hammocks = OrderedDict([
    ('hammock AB', normvec(B - A - Z * L / 2)),
    ('hammock AC', normvec(C - A - Z * L / 2)),
])
HV = np.array(list(hammocks.values())).transpose()


def error(hf, sf):
    ''' Calculate error in total forces '''
    return normlen(np.dot(SV, sf) + np.dot(HV, hf))


def solve_sf(hf):
    ''' Solve for structural forces (sf) given hammock forces (hf) '''
    # Convert to array
    hf = np.array(hf)
    # Total forces at corner must equal zero
    #   SV * sf + HV * hf = 0, therefore: -HV * hf <= SV * sf <= -HV * hf
    equality_constraint = LinearConstraint(SV, -np.dot(HV, hf), -np.dot(HV, hf))
    # Also forces must all be nonnegative
    n = len(structure)
    positive_constraint = LinearConstraint(np.eye(n), np.zeros(n), np.ones(n) * np.inf)
    # We will use both constraints for optimization
    constraints = [equality_constraint, positive_constraint]

    # Calculate structure forces as minimum total forces which satisfy constraints
    sf = minimize(np.sum,  # function to minimize -- want the smallest sum of total forces
                  np.zeros(n),  # initial guess
                  method='trust-constr',  # For when we have LinearConstraint's
                  constraints=constraints,  # Constraints on our solution space
                  # These two are defaults, but need to be included because of a bug.
                  jac='2-point', hess=BFGS())  # https://github.com/scipy/scipy/issues/8867
    return sf.x


def relative_angles(q, r, s, t):
    '''
    Calculate azimuth and elevation angles for the ropes relative to pole.
        Given pole vector q and rope vectors r, s, t.  r is used for 0 degrees azimuth.
    '''

    # Uncomment to see visual axes
    # from vpython import arrow, color, vector
    # qv = arrow(radius=0.3, axis=vector(*q), color=color.white, emissive=True)
    # rv = arrow(radius=0.3, axis=vector(*r), color=color.red, emissive=True)
    # sv = arrow(radius=0.3, axis=vector(*s), color=color.blue, emissive=True)
    # tv = arrow(radius=0.3, axis=vector(*t), color=color.green, emissive=True)

    q, r, s, t = (normvec(i) for i in (q, r, s, t))
    r_elev = 180 / np.pi * np.arccos(np.dot(q, r))
    s_elev = 180 / np.pi * np.arccos(np.dot(q, s))
    t_elev = 180 / np.pi * np.arccos(np.dot(q, t))
    plane_qr = normvec(np.cross(q, r))
    plane_qs = normvec(np.cross(q, s))
    plane_qt = normvec(np.cross(q, t))
    r_azim = 180 / np.pi * np.arccos(np.dot(plane_qr, plane_qr))
    s_azim = 180 / np.pi * np.arccos(np.dot(plane_qr, plane_qs))
    t_azim = 180 / np.pi * np.arccos(np.dot(plane_qr, plane_qt))
    print('rel elev', r_elev, s_elev, t_elev)
    print('rel azim', r_azim, s_azim, 360 - t_azim)


def main():
    # Check geometry
    print('Top triangle sides', normlen(A - B), normlen(B - C), normlen(C - A))
    print('Bottom triangle sides', normlen(D - E), normlen(E - F), normlen(F - D))
    print('Short vertical sides', normlen(A - E), normlen(B - F), normlen(C - D))
    print('Long distances empty', normlen(A - F), normlen(B - D), normlen(C - E))
    print('Rod lengths', normlen(A - D), normlen(B - E), normlen(C - F))
    # Check angles at rod ends
    # print('Relative angles A', relative_angles(D - A, B - A, C - A, E - A))
    # print('Relative angles B', relative_angles(E - B, C - B, A - B, F - B))
    # print('Relative angles C', relative_angles(F - C, A - C, B - C, D - C))
    # print('Relative angles D', relative_angles(A - D, E - D, F - D, C - D))
    # print('Relative angles E', relative_angles(B - E, F - E, D - E, A - E))
    # print('Relative angles F', relative_angles(C - F, D - F, E - F, B - F))
    # Print out structure forces for different hammock loads
    # for hf in [[200, 200], [200, 0], [0, 200]]:
    #     sf = solve_sf(hf)
    #     print('hammock forces:', list(zip(hammocks.keys(), hf)))
    #     print('error', error(hf, sf))
    #     print('structure forces:')
    #     for name, force in zip(structure.keys(), sf):
    #         print(f'    {name} {force:.2f}')


if __name__ == '__main__':
    main()