def create(cls, a, b, c, nel, ori):
'''
Creates a chain of ellipsoids
ori : (nel, 4) ndarray
Orientations of all ellipsoids
'''
el_oris = ori
el_coms = np.zeros((nel, 3))
joints = {}
for k in range(1, nel):
xi = np.array([a, 0, 0])
zeta = np.array([-a, 0, 0])
nu = tr.shift_vector_quat(xi, el_oris[k - 1, :], forward=False)
mu = tr.shift_vector_quat(zeta, el_oris[k, :], forward=False)
el_coms[k, :] = el_coms[k - 1, :] + nu - mu
joints[k + 1] = {
'from': xi,
'to': zeta,
'pf': k,
'sf': k + 1,
}
ec = cls(a, b, c, el_coms, el_oris, joints)
return ec
def set_com(self, loc):
delta = loc - self.com
for k in range(self.nel):
self.el_coms[k, :] += delta
self.com = np.mean(self.el_coms, axis=0)
#Update the root joint
q = self.el_oris[0, :]
to = tr.shift_vector_quat(-self.el_coms[0, :], q, forward=True)
self._joints[1]['to'] = to
def set_orientation(self, ori):
'''
ori: (4,) ndarray representing a unit quaternion
'''
q_ini = np.copy(self.orientation)
q_fin = ori
#Vector rotation
q = tr.get_quat_prod(q_fin, tr.invert_quat(q_ini))
self.verts = tr.rotate_vector_quat(self.verts, q)
#Update the director, codirector, and bidirector
#Transform the axes to the world frame using the orientation
self.director = tr.shift_vector_quat(np.array([1,0,0]), q_fin,
forward=False)
self.codirector = tr.shift_vector_quat(np.array([0,1,0]), q_fin,
forward=False)
self.bidirector = tr.shift_vector_quat(np.array([0,0,1]), q_fin,
forward=False)
#Update the orientation
self.orientation = ori
def __init__(self, a, b, c, el_coms, el_oris, joints):
'''
Parameters
----------
a : float/ndarray
Ellipsoid a
b : float/ndarray
Ellipsoid b
c : float/ndarray
Ellipsoid c
el_coms : (N,3) ndarray, dtype float
Ellipsoid c.o.m. in world frame.
el_oris : (N,4) ndarray, dtype float
Ellipsoid orientations in world frame.
joints : dict
The values of the joint variables will be overwritten.
Returns
-------
An Ellipsoid_chain instance
'''
self.a = a
self.b = b
self.c = c
self.el_coms = el_coms
self.el_oris = el_oris
self._joints = joints
self.nel = self.el_coms.shape[0] #ellipsoids
self.com = np.mean(self.el_coms, axis=0)
#Root joint
q = self.el_oris[0, :]
to = tr.shift_vector_quat(-self.el_coms[0, :], q, forward=True)
self._joints[1] = {
'joint_type': 'root',
'to': to,
'orientation': {
'repr': 'quat',
'quat': q
}
}
#Joint variables for non-root joints
for jid in range(2, self.nel + 1):
#Joint `jid` connects body `jid-1` with body `jid`. The array
#indices for accessing com and ori for body `k` is `k-1`.
inv = tr.get_inverted_quat(self.el_oris[jid - 2, :])
quat = tr.get_quat_prod(inv, self.el_oris[jid - 1, :])
self._joints[jid]['orientation'] = {'repr': 'quat', 'quat': quat}
def from_df_mbs(cls, df_mbs, modelspec):
'''
Creates a MiuraSheet instance from df_mbs and modelspec. The c.o.m.
position and orientation is as calculated from df_mbs.
Parameters
----------
df_mbs : dict
keywords: 'body_coms', 'body_orientations'
modelspec : dict
Returns
-------
ms : An instance of MiuraSheet
'''
#Extract from df_mbs
body_coms = df_mbs['body_coms']
body_oris = df_mbs['body_orientations']
#Extract from modelspec
bodies = modelspec['bodies']
joints = modelspec['joints']
internal = modelspec['{internal}']
faces = np.asarray(internal['faces'], dtype='i4')
joint_edges = internal['joint_edges']
nverts_z = internal['nverts_z']
nverts_x = internal['nverts_x']
vert_map = np.asarray(internal['vert_map'], dtype='i4')
nverts = nverts_z*nverts_x
verts = np.zeros((nverts,3))
for k in range(nverts):
iface = vert_map[k,0]
ivert = vert_map[k,1]
ibody = iface + 1
body_vert = np.asarray(bodies[ibody]['parallelogram']['vertices'][ivert])
body_com = body_coms[iface]
body_ori = body_oris[iface]
vert = tr.shift_vector_quat(body_vert, body_ori, forward=False)
verts[k,:] = vert + body_com
ms = cls(nverts_z, nverts_x, verts, faces, joint_edges)
return ms