def calc_vel(AA, q, Yd, Conn, Prop):
"""
Returns the linear velocity of all body centroids and the angular velocity
of all body (eqs. 3.8-3.9 and 3.12-3.13, figure 3.5).
vv = [vv_0, vv_1, ... ] (3, num_b)
ww = [ww_0, ww_1, ... ] (3, num_b)
"""
BB, j_type = Conn[2], Conn[3]
cc = Prop[2]
v0, w0, qd = Yd[0:3], Yd[3:6], Yd[6:]
num_b = len(q) + 1 # Number of bodies
vv = np.zeros((3, num_b))
ww = np.zeros((3, num_b))
# Base
vv[:, 0] = v0
ww[:, 0] = w0
# i = current link, k = lower link connection
for i in range(1, num_b):
idxi = i - 1 # Index link/joint <i> in BB, j_type, q, qd
k = BB[idxi] # Index lower link connection
# Rotation matrices
A_I_i = AA[:, 3 * i:3 * (i + 1)]
A_I_k = AA[:, 3 * k:3 * (k + 1)]
# Relative positions
cc_I_i = A_I_i @ cc[:, i, i]
cc_I_k = A_I_k @ cc[:, k, i]
# Joint values
q_I_i = A_I_i @ (Ez * q[idxi])
qd_I_i = A_I_i @ (Ez * qd[idxi])
# Rotational joint (eqs. 3.8-3.9)
if (j_type[idxi] == 'R'):
ww[:, i] = ww[:, k] + qd_I_i
vv[:, i] = vv[:, k] + cross(ww[:, k], cc_I_k) \
- cross(ww[:, i], cc_I_i)
# Prismatic joint (eqs. 3.12-3.13)
elif (j_type[idxi] == 'P'):
ww[:, i] = ww[:, k]
vv[:, i] = vv[:, k] + cross(ww[:, k], cc_I_k) \
- cross(ww[:, i], cc_I_i) \
+ cross(ww[:, i], q_I_i) \
+ qd_I_i
return vv, ww
def side(self, v0, v1, v2, origin, direction):
v0v1 = sub(v1, v0)
v0v2 = sub(v2, v0)
N = cross(v0v1, v0v2)
raydirection = dot(N, direction)
if abs(raydirection) < 0.0001:
return None
d = dot(N, v0)
t = (dot(N, origin) + d) / raydirection
if t < 0:
return None
P = sum(origin, mul(direction, t))
U, V, W = barycentric(v0, v1, v2, P)
if U < 0 or V < 0 or W < 0:
return None
else:
return Intersect(distance = d,
point = P,
normal = norm(N))
def __init__(self, point_item):
self.vertices = set()
if not point_item.is_bounded():
return
# Compute segments edges
for other in point_item.others:
v1, v2 = other.vertices()
for v in [v1, v2]:
EPSILON = 1e-4
distances = list(utils.distance2(v, i) for i in self.vertices)
if len(distances) == 0 or min(distances) > EPSILON:
self.vertices |= {v}
# Sort vertices to make the shape convex
self.vertices = list(self.vertices)
OA = utils.vector(point_item.point, self.vertices[0])
len_OA = utils.length(OA)
t = {(self.vertices[0], 0)}
for vertex in self.vertices[1:]:
OB = utils.vector(point_item.point, vertex)
len_OB = utils.length(OB)
cos_angle = utils.dot(OA, OB) / (len_OA * len_OB)
if cos_angle < -1:
cos_angle = -1
elif cos_angle > 1:
cos_angle = 1
angle = math.acos(cos_angle)
if utils.cross(OA, OB) < 0: # sign of sin
angle = 2 * math.pi - angle
t |= {(vertex, angle)}
self.vertices = list(vertex
for vertex, _ in sorted(t, key=lambda x: x[1]))
def take_RK4_step(self, b_rand):
o, c = self.options, self.constants
# Get the current position
p = self.current_position()
# Calculate partial Runge Kutta steps
k1 = self.calculate_function_value(p)
k2 = self.calculate_function_value(
p + k1 * o['dt'] / 2) # Needs B_eff with spins p+k1
k3 = self.calculate_function_value(p + k2 * o['dt'] / 2)
k4 = self.calculate_function_value(p + k3 * o['dt'])
# Calculate the total difference in the spin
d_spin = (k1 + 2 * k2 + 2 * k3 + k4) * o['dt'] / 6
# Calculate the random energy added
# This is based on temperature
d_spin_rand = c['gamma'] * cross(p, b_rand)
# Calculate new position and normalise the vector
p += d_spin + d_spin_rand
p = p / math.sqrt(dot(p, p))
# Save the data to the atom
self.set_position(p)
return self.id, p
def generate_submit_constrained_parameters_grid(self, dict_parameters_range, filtering_function=None, filtering_function_parameters=None, pbs_submission_infos=None, submit_jobs=True):
'''
Takes a dictionary of parameters, with their list of values, and generates a list of all the combinations.
Filter them with a specific function if provided.
if pbs_submission_infos is provided, will create a script and submit it to PBS when an acceptable set of parameters if found
'''
candidate_parameters = []
# Get all cross combinations of parameters
cross_comb = cross([dict_parameters_range[param]['range'].tolist() for param in dict_parameters_range])
# Convert them back into dictionaries
candidate_parameters = [dict(zip(dict_parameters_range.keys(), x)) for x in cross_comb]
# Now filter them
constrained_parameters = []
for new_parameters in progress.ProgressDisplay(candidate_parameters, display=progress.SINGLE_LINE):
if (filtering_function and filtering_function(new_parameters, dict_parameters_range, filtering_function_parameters)) or (filtering_function is None):
constrained_parameters.append(new_parameters)
# Submit to PBS if required
if pbs_submission_infos:
self.create_submit_job_parameters(pbs_submission_infos, new_parameters, submit=submit_jobs)
return constrained_parameters
def take_RK2_step(self, b_rand):
o, c = self.options, self.constants
# Get the current position
p = self.current_position()
# Calculate partial Runge Kutta steps
k1 = self.calculate_function_value(p)
self.set_position(p + k1 * o['dt'])
# self.combine_neighbours # We need to call this, but then we have to call from particles
k2 = self.calculate_function_value(self.pos)
# Calculate the total difference in the spin
# d_spin = (k1 + k2)/2*o['dt']
d_spin = k2 * o['dt']
# Calculate the random energy added
# This is based on temperature
d_spin_rand = c['gamma'] * cross(p, b_rand)
# Calculate new position and normalise the vector
p += d_spin + d_spin_rand
p = p / math.sqrt(dot(p, p))
# Save the data to the atom
self.set_position(p)
return self.id, p
def get_mean_activity(self,
axes=(0, 1),
precision=100,
specific_neurons=None,
return_axes_vect=False):
'''
Returns the mean activity of the network.
'''
coverage_1D = self.init_feature_space(precision)
possible_stimuli = np.array(utils.cross(self.R * [coverage_1D.tolist()]))
activity = np.empty((possible_stimuli.shape[0], self.M))
for stimulus_i, stimulus in enumerate(possible_stimuli):
activity[stimulus_i] = self.get_network_response(
stimulus, specific_neurons=specific_neurons)
# Reshape
activity.shape = self.R * (precision, ) + (self.M, )
mean_activity = activity
for dim_to_avg in (set(range(len(activity.shape))) - set(axes)):
mean_activity = np.mean(mean_activity, axis=dim_to_avg, keepdims=True)
mean_activity = np.squeeze(mean_activity)
if return_axes_vect:
return (mean_activity, coverage_1D, coverage_1D)
else:
return mean_activity
def take_ad_bs_step(self, b_rand):
# Grab the constants
o, c = self.options, self.constants
pos = self.current_position()
# Gradually increase steps in the Adams Bashforth method
if self.spos4 is None:
d_spos = self.first_ad_bs_step(pos)
d_fpos = d_spos
elif self.spos3 is None:
d_spos, d_fpos = self.second_ad_bs_step(pos)
elif self.spos2 is None:
d_spos, d_fpos = self.third_ad_bs_step(pos)
elif self.spos1 is None:
d_spos, d_fpos = self.fourth_ad_bs_step(pos)
else:
d_spos, d_fpos = self.fifth_ad_bs_step(pos)
# Move the values along so we keep continuing
self.spos4, self.spos3, self.spos2, self.spos1 = d_fpos, self.spos4, self.spos3, self.spos2
# Calculate the random energy added
# This is based on temperature
d_spin_rand = c['gamma'] * cross(pos, b_rand)
# Take the step and calculate the final position and normalise the vector
pos = pos + d_spos * o['dt'] + d_spin_rand
pos = pos / math.sqrt(dot(pos, pos))
# Save the data to the atom
self.set_position(pos)
return self.id, pos
def calc_ang_mom1(self, P_ref=np.zeros(3), V_ref=np.zeros(3)):
"""
Returns the derivative of the angular momentum for the entire system
and for each body with respect to point P_ref and velocity V_ref.
"""
mass = self.Prop[0]
inertia = self.Prop[1]
HM = np.zeros((3, self.num_b))
for i in range(self.num_b):
AA = self.AA[:, 3 * i:3 * (i + 1)]
In = AA @ inertia[:, 3 * i:3 * (i + 1)] @ AA.T
HM[:, i] = In @ self.wd[:, i] \
+ cross(self.ww[:, i], In @ self.ww[:, i]) \
+ cross(self.RR[:, i] - P_ref, mass[i] * self.vd[:, i]) \
+ cross(self.vv[:, i] - V_ref, mass[i] * self.vv[:, i])
return HM.sum(axis=1), HM
def __init__(self):
"""Constructor of the Algorithm Norvig that initialize the variables
used in the resolution of a Sudoku
"""
self.digit_list = '123456789'
self.row_list = 'ABCDEFGHI'
self.column_list = self.digit_list
self.list_squares = utils.cross(self.row_list, self.column_list)
self.unit_list = (
[utils.cross(self.row_list, column) for column in
self.column_list] +
[utils.cross(row, self.column_list) for row in self.row_list] +
[utils.cross(rows, columns) for rows in ('ABC', 'DEF', 'GHI')
for columns in ('123', '456', '789')])
self.unit_dict_squares = dict(
(square, [unit for unit in self.unit_list if square in unit]) for square in
self.list_squares)
self.peers_set_squares = dict((square, set(
sum(self.unit_dict_squares[square], [])) - set([square])) for square in
self.list_squares)
def __init__(self):
"""Constructor of the Algorithm Norvig that initialize the variables
used in the resolution of a Sudoku
"""
self.digit_list = '123456789'
self.row_list = 'ABCDEFGHI'
self.column_list = self.digit_list
self.list_squares = utils.cross(self.row_list, self.column_list)
self.unit_list = ([
utils.cross(self.row_list, column) for column in self.column_list
] + [utils.cross(row, self.column_list) for row in self.row_list] + [
utils.cross(rows, columns) for rows in ('ABC', 'DEF', 'GHI')
for columns in ('123', '456', '789')
])
self.unit_dict_squares = dict(
(square, [unit for unit in self.unit_list if square in unit])
for square in self.list_squares)
self.peers_set_squares = dict(
(square,
set(sum(self.unit_dict_squares[square], [])) - set([square]))
for square in self.list_squares)
def test_verify_that_the_cross_method_generate_the_peers(self):
self.cols = '123456789'
self.rows = 'ABCDEFGHI'
expected_peers = ['A1', 'A2', 'A3', 'A4', 'A5', 'A6', 'A7', 'A8', 'A9',
'B1', 'B2', 'B3', 'B4', 'B5', 'B6', 'B7', 'B8', 'B9',
'C1', 'C2', 'C3', 'C4', 'C5', 'C6', 'C7', 'C8', 'C9',
'D1', 'D2', 'D3', 'D4', 'D5', 'D6', 'D7', 'D8', 'D9',
'E1', 'E2', 'E3', 'E4', 'E5', 'E6', 'E7', 'E8', 'E9',
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', 'F7', 'F8', 'F9',
'G1', 'G2', 'G3', 'G4', 'G5', 'G6', 'G7', 'G8', 'G9',
'H1', 'H2', 'H3', 'H4', 'H5', 'H6', 'H7', 'H8', 'H9',
'I1', 'I2', 'I3', 'I4', 'I5', 'I6', 'I7', 'I8', 'I9']
self.assertEqual(expected_peers, utils.cross(self.rows, self.cols))
def test_verify_that_the_cross_method_generate_the_peers(self):
self.cols = '123456789'
self.rows = 'ABCDEFGHI'
expected_peers = [
'A1', 'A2', 'A3', 'A4', 'A5', 'A6', 'A7', 'A8', 'A9', 'B1', 'B2',
'B3', 'B4', 'B5', 'B6', 'B7', 'B8', 'B9', 'C1', 'C2', 'C3', 'C4',
'C5', 'C6', 'C7', 'C8', 'C9', 'D1', 'D2', 'D3', 'D4', 'D5', 'D6',
'D7', 'D8', 'D9', 'E1', 'E2', 'E3', 'E4', 'E5', 'E6', 'E7', 'E8',
'E9', 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', 'F7', 'F8', 'F9', 'G1',
'G2', 'G3', 'G4', 'G5', 'G6', 'G7', 'G8', 'G9', 'H1', 'H2', 'H3',
'H4', 'H5', 'H6', 'H7', 'H8', 'H9', 'I1', 'I2', 'I3', 'I4', 'I5',
'I6', 'I7', 'I8', 'I9'
]
self.assertEqual(expected_peers, utils.cross(self.rows, self.cols))
def forward(self, input, logdet=0, reverse=False):
if not reverse:
x1, x2 = utils.split(input)
h = self.Net(x1)
t, h_scale = utils.cross(h)
scale = torch.nn.functional.softplus(h_scale)
logscale = torch.log(scale)
y2 = (x2 + t) * scale
y1 = x1
y = torch.cat((y1, y2), 1)
logdet += utils.flatten_sum(logscale)
return y, logdet
else:
y1, y2 = utils.split(input)
h = self.Net(y1)
t, h_scale = utils.cross(h)
scale = torch.nn.functional.softplus(h_scale)
logscale = torch.log(scale)
x2 = (y2 / scale) - t
x1 = y1
x = torch.cat((x1, x2), 1)
logdet += utils.flatten_sum(logscale)
return x, logdet
def create_euler_from_vectors(v1, v2):
# q = Quaternion.create_quaternion_from_vector_to_vector(v1, v2)
v1 = v1.reshape(-1)
v2 = v2.reshape(-1)
direction = cross(v1, v2)
x = direction[0]
y = direction[1]
z = direction[2]
w = np.sqrt((v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2]) * (v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2])) + \
(v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2])
# explicit_q = [q.x, q.y, q.z, q.w]
euler = euler_from_quaternion([x, y, z, w])
euler = np.array(euler).reshape(3, 1)
return euler
def assign_prefered_stimuli_conjunctive(self, neurons_indices):
'''
Tile conjunctive neurons along the space of possible angles.
'''
# Cover the space uniformly
N_sqrt = np.floor(np.power(neurons_indices.size, 1./self.R))
# coverage_1D = np.linspace(-np.pi, np.pi, N_sqrt, endpoint=False)
coverage_1D = np.linspace(-np.pi + np.pi/N_sqrt, np.pi + np.pi/N_sqrt, N_sqrt, endpoint=False)
new_stim = np.array(utils.cross(self.R*[coverage_1D.tolist()]))
# Assign the preferred stimuli
# Unintialized neurons will get masked out down there.
self.neurons_preferred_stimulus[neurons_indices[:new_stim.shape[0]]] = new_stim
def calc_je(ie, RR, AA, q, Conn, Prop):
"""
Returns the Jacobian (6 x num_j) of the endpoint <ie> (eqs 3.25 and 3.26).
"""
SE, BB, j_type = Conn[1], Conn[2], Conn[3]
cc, ce = Prop[2], Prop[3]
# Link sequence from the base (excluded) to the endpoint
seq_link = []
i = SE[0, ie]
while (i > 0):
seq_link.insert(0, i)
i = BB[i - 1]
# Position of the endpoint
j = seq_link[-1]
A_I_j = AA[:, 3 * j:3 * (j + 1)]
POS_e = RR[:, j] + A_I_j @ ce[:, ie]
# Position for all joints in the link sequence
n_links = len(seq_link)
POS_jnt = np.zeros((3, n_links))
Jacobian = np.zeros((6, len(q)))
for i in range(n_links):
j = seq_link[i] # Joint/link number in the sequence
idxj = j - 1 # Index link/joint <j> in q, j_type
A_I_j = AA[:, 3 * j:3 * (j + 1)]
Ez_I_j = A_I_j @ Ez
# Position for a rotational joint
if (j_type[idxj] == 'R'):
POS_jnt[:, i] = RR[:, j] + A_I_j @ cc[:, j, j]
JJ_te = cross(Ez_I_j, POS_e - POS_jnt[:, i])
JJ_re = Ez_I_j
# Prismatic joint (rotational component already set to zero)
elif (j_type[idxj] == 'P'):
POS_jnt[:, i] = RR[:, j] + A_I_j @ (cc[:, j, j] - Ez * q[idxj])
JJ_te = Ez_I_j
JJ_re = np.zeros(3)
# Assemble the endpoint Jacobian in the global Jacobian
Jacobian[0:3, j - 1] = JJ_te
Jacobian[3:6, j - 1] = JJ_re
return Jacobian
def create_quaternion_from_vector_to_vector(cls, v1, v2):
""" from two direction vectors, create the quaternion
formulas from https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
verified by create axis_angle then convert to quaternion
Params:
v1, v2: direction vectors, expected 3 by 1
Return:
q: Quaternion class
"""
q = cls()
direction = cross(v1.reshape([-1]), v2.reshape([-1]))
q.x = direction[0]
q.y = direction[1]
q.z = direction[2]
q.w = np.sqrt((np.linalg.norm(v1)**2) *
(np.linalg.norm(v2)**2)) + np.matmul(v1.T, v2).item()
q.normalize()
return q
def triangle_kernel(points, verts, faces, kernel_height=1.0):
n_p = points.shape[0]
n_f = faces.shape[0]
points_expand = points.unsqueeze(1).expand(-1, n_f, -1)
face_normals = utils.face_normals(verts, faces)
# Longest edge in each triangle
# longest_edge = torch.zeros(n_f, dtype=verts.dtype, device=verts.device)
# True if point projects inside of face in plane
min_edge_dist = torch.ones(
(n_p, n_f), dtype=verts.dtype, device=points.device) * float('inf')
for i in range(3):
lineA = verts[faces[:, i]]
lineB = verts[faces[:, (i + 1) % 3]]
# Update longest edge
# longest_edge = torch.max(longest_edge, torch.norm(lineA - lineB))
# Edge perp vec
e_perp = utils.normalize(utils.cross(face_normals, lineB - lineA))
edge_inside_dist = utils.dot(
e_perp.unsqueeze(0).expand(n_p, -1, -1),
points_expand - lineA.unsqueeze(0).expand(n_p, -1, -1))
# edge_inside_dist = torch.max(dge_inside_dist, torch.tensor(0., device=verts.device))
min_edge_dist = torch.min(min_edge_dist, edge_inside_dist)
# normal distance
point_in_face = verts[faces[:, 0]].unsqueeze(0).expand(n_p, -1, -1)
normal_face_dist = torch.abs(
utils.dot(
face_normals.unsqueeze(0).expand(n_p, -1, -1),
points_expand - point_in_face))
EPS = 1e-8
k_val = torch.max(
torch.tensor(0., device=verts.device),
1. - (normal_face_dist / (min_edge_dist * kernel_height + EPS)))
k_val = torch.where(min_edge_dist < 0., torch.zeros_like(k_val), k_val)
return k_val
def generate_submit_constrained_parameters_grid(
self,
dict_parameters_range,
filtering_function=None,
filtering_function_parameters=None,
pbs_submission_infos=None,
submit_jobs=True):
'''
Takes a dictionary of parameters, with their list of values, and generates a list of all the combinations.
Filter them with a specific function if provided.
if pbs_submission_infos is provided, will create a script and submit it to PBS when an acceptable set of parameters if found
'''
candidate_parameters = []
# Get all cross combinations of parameters
cross_comb = cross([
dict_parameters_range[param]['range'].tolist()
for param in dict_parameters_range
])
# Convert them back into dictionaries
candidate_parameters = [
dict(zip(dict_parameters_range.keys(), x)) for x in cross_comb
]
# Now filter them
constrained_parameters = []
for new_parameters in progress.ProgressDisplay(
candidate_parameters, display=progress.SINGLE_LINE):
if (filtering_function and filtering_function(
new_parameters, dict_parameters_range,
filtering_function_parameters)) or (filtering_function is
None):
constrained_parameters.append(new_parameters)
# Submit to PBS if required
if pbs_submission_infos:
self.create_submit_job_parameters(pbs_submission_infos,
new_parameters,
submit=submit_jobs)
return constrained_parameters
def calc_jac(RR, AA, Conn, Prop):
"""
Returns the translational Jacobians (3 x num_j x num_j) of all link
centroids (equation 3.25-3.26).
"""
BB, j_type = Conn[2], Conn[3]
cc = Prop[2]
num_j = len(j_type) # Number of joints/links
JJ_t = np.zeros((3, num_j * num_j))
JJ_r = np.zeros((3, num_j * num_j))
# Loop over all links
for i in range(1, num_j + 1):
j = i # Initial index from link/joint i to base (itself)
# Follow the branch until the base (j = 0) is reached
while (j > 0):
idxj = j - 1 # Index link/joint <j> in BB, j_type
A_I_j = AA[:, 3 * j:3 * (j + 1)]
Ez_I_j = A_I_j @ Ez
cc_I_j = A_I_j @ cc[:, j, j]
col = (i - 1) * num_j + (j - 1)
# Rotational joint
if (j_type[idxj] == 'R'):
JJ_t[:, col] = \
cross(Ez_I_j, RR[:, i] - (RR[:, j] + cc_I_j))
JJ_r[:, col] = Ez_I_j
# Prismatic joint
elif (j_type[idxj] == 'P'):
JJ_t[:, col] = Ez_I_j
JJ_r[:, col] = np.zeros(3)
j = BB[idxj] # Previous link/joint along the branch
return JJ_t, JJ_r
def get_neuron_activity_full(self, neuron_index, precision=100, axes=None):
"""Returns the activity of a specific neuron over the entire space.
"""
coverage_1D = utils.init_feature_space(precision)
possible_stimuli = np.array(utils.cross(self.R * [coverage_1D.tolist()]))
activity = np.empty(possible_stimuli.shape[0])
# Compute the activity of that neuron over the whole space
for stimulus_i, stimulus in enumerate(possible_stimuli):
activity[stimulus_i] = self.get_neuron_response(neuron_index, stimulus)
# Reshape
activity.shape = self.R * (precision, )
if axes is not None:
for dim_to_avg in set(range(len(activity.shape))) - set(axes):
activity = np.mean(activity, axis=dim_to_avg, keepdims=True)
activity = np.squeeze(activity)
return activity
def turn_left(self):
self.body.angular_velocity = 0
t = cross(self.left_turn_point, self.turn_force)
self.body.apply_torque(t)
from utils import dot, vecadd, cross
a = [2, -1, -2]
b = [5, -3, 3]
c = [3, -1, -3]
if __name__ == "__main__":
print("a ", dot(a, cross(b, c)))
print("b ", dot(a, vecadd(b, c)))
print("cde ", cross(a, vecadd(b, c)))
def calculate_function_value(self, p):
o, c, B = self.options, self.constants, self.B_eff
return c['gamma'] * (cross(p, B) + o['l'] *
(B * dot(p, p) - p * dot(p, B)))
def plots_fitting_experiments_random(data_pbs, generator_module=None):
'''
Reload 2D volume runs from PBS and plot them
'''
#### SETUP
#
savefigs = True
savedata = True
plots_per_T = False
plots_interpolate = False
# do_relaunch_bestparams_pbs = True
colormap = None # or 'cubehelix'
plt.rcParams['font.size'] = 16
#
#### /SETUP
print "Order parameters: ", generator_module.dict_parameters_range.keys()
# parameters: ratio_conj, sigmax, T
# Extract data
result_fitexperiments_flat = np.array(data_pbs.dict_arrays['result_fitexperiments']['results_flat'])
result_fitexperiments_all_flat = np.array(data_pbs.dict_arrays['result_fitexperiments_all']['results_flat'])
result_parameters_flat = np.array(data_pbs.dict_arrays['result_fitexperiments']['parameters_flat'])
# Extract order of datasets
experiment_ids = data_pbs.loaded_data['datasets_list'][0]['fitexperiment_parameters']['experiment_ids']
dataio = DataIO(output_folder=generator_module.pbs_submission_infos['simul_out_dir'] + '/outputs/', label='global_' + dataset_infos['save_output_filename'])
# Compute some stuff
result_fitexperiments_bic_avg = utils.nanmean(result_fitexperiments_flat[:, 0, 0], axis=-1)
T_space = np.unique(result_parameters_flat[:, 2])
if plots_per_T:
for T in T_space:
currT_indices = result_parameters_flat[:, 2] == T
utils.contourf_interpolate_data_interactive_maxvalue(result_parameters_flat[currT_indices][..., :2], result_fitexperiments_bic_avg[currT_indices], xlabel='Ratio_conj', ylabel='sigma x', title='BIC, T %d' % T, interpolation_numpoints=200, interpolation_method='nearest', log_scale=False)
# Interpolate
if plots_interpolate:
rcscale_target = 5.0
rbf_interpolator = spint.Rbf(result_parameters_flat[:,0], result_parameters_flat[:,1], result_parameters_flat[:,2], result_parameters_flat[:,3], result_fitexperiments_bic_avg, smooth = 0.0)
param1_space = np.linspace(0.01, 1.0, 50)
param2_space = np.linspace(0.01, 1.0, 50)
param3_space = np.array([rcscale_target])
param4_space = np.array([rcscale_target])
params_crossspace = np.array(utils.cross(param1_space, param2_space, param3_space, param4_space))
interpolated_data = rbf_interpolator(params_crossspace[:, 0], params_crossspace[:, 1], params_crossspace[:, 2], params_crossspace[:, 3]).reshape((param1_space.size, param2_space.size))
# utils.pcolor_2d_data(interpolated_data, param1_space, param2_space, 'ratio', 'sigmax', 'interpolated, fixing rcscales= %.2f' % rcscale_target)
points_closeby = ((result_parameters_flat[:, 2] - rcscale_target)**2 + (result_parameters_flat[:, 3] - rcscale_target)**2) < 0.5
plt.figure()
plt.imshow(interpolated_data, extent=(param1_space.min(), param1_space.max(), param2_space.min(), param2_space.max()))
plt.scatter(result_parameters_flat[points_closeby, 0], result_parameters_flat[points_closeby, 1], s=100, c=result_fitexperiments_bic_avg[points_closeby], marker='o')
# if plot_per_ratio:
# # Plot the evolution of loglike as a function of sigmax, with std shown
# for ratio_conj_i, ratio_conj in enumerate(ratio_space):
# ax = utils.plot_mean_std_area(sigmax_space, result_log_posterior_mean[ratio_conj_i], result_log_posterior_std[ratio_conj_i])
# ax.get_figure().canvas.draw()
# if savefigs:
# dataio.save_current_figure('results_fitexp_%s_loglike_ratioconj%.2f_{label}_global_{unique_id}.pdf' % (exp_dataset, ratio_conj))
all_args = data_pbs.loaded_data['args_list']
variables_to_save = ['experiment_ids']
if savedata:
dataio.save_variables_default(locals(), variables_to_save)
dataio.make_link_output_to_dropbox(dropbox_current_experiment_folder='fitting_experiments')
plt.show()
return locals()
for s in st.get_skeleton(1):
e = s[0]
if len(e) == 2:
fle_edges.append(Y[[e[0],e[1]]])
plt.scatter(Y[:,0], Y[:,1],c =t, cmap = plt.cm.Spectral)
lc = LineCollection(segments = fle_edges, linewidths=0.3)
plt.gca().add_collection(lc)
plt.xticks([], [])
plt.yticks([], [])
if save_fig:
plt.savefig(fig_dir + data_choice + '_FLE_1step.pdf', bbox_inches='tight')
plt.show()
#%%
crosses = cross(fle_edges, edge_list, findall=True)
if len(crosses) != 0:
print('{} cross found!'.format(len(crosses)))
else:
print('No cross found!')
#%% Second-step Fixed-point LE
if twostepFLE and (not use_boundary):
# detect boundary again because we haven't done so if use_boundary == False
boundary_edge, boundary_point_idx = detect_boundary(st, edge_list)
# if there is no boundary, we raise an error (note error msg different)
assert len(boundary_point_idx) > 0, "No boundary detected, one-step FLE is sufficient"
def keyboard_control(args_, pygame_clock, save_screenshot_path, vehicles_,
walkers_, world_, client_):
"""Process all the keyboard event since last tick."""
# the rgb camera and seg camera are in the args
# since we may need to change the fov for them
# by rebuild the camera actor
# return True to exit
ms_since_last_tick = pygame_clock.get_time()
# TOdo: change the following. dont get the transform at each time to avoid
# stuttering due to the frame lag
# set a global variable of the rotation instead
prev_rotation = args_.spectator.get_transform().rotation
prev_location = args_.spectator.get_transform().location
global_up_vector = carla.Vector3D(x=0, z=1, y=0)
# a normalized x,y,z, between 0~1
forward_vector = prev_rotation.get_forward_vector()
left_vector = cross(forward_vector, global_up_vector)
global_forward_vector = cross(global_up_vector, left_vector)
# up_vector = cross(left_vector, forward_vector)
# get all event from event queue
# empty out the event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.MOUSEBUTTONUP:
# get the depth map in meters
depth_in_meters = parse_carla_depth(args_.depth_camera.rgb_image)
pos_x, pos_y = pygame.mouse.get_pos()
click_point = np.array([pos_x, pos_y, 1])
intrinsic = args_.rgb_camera.camera_actor.intrinsic
# 3d point in the camera coordinates
click_point_3d = np.dot(np.linalg.inv(intrinsic), click_point)
click_point_3d *= depth_in_meters[pos_y, pos_x]
# why? this is because unreal transform is (y, -z , x)???
y, z, x = click_point_3d
z = -z
click_point_3d = np.array([x, y, z, 1])
click_point_3d.reshape([4, 1])
# transform to the world origin
camera_rt = compute_extrinsic_from_transform(
args_.rgb_camera.camera_actor.get_transform())
click_point_world_3d = np.dot(camera_rt, click_point_3d)
x, y, z = click_point_world_3d.tolist(
)[0][:3] # since it is np.matrix
red = carla.Color(r=255, g=0, b=0)
green = carla.Color(r=0, g=255, b=0)
blue = carla.Color(r=0, g=0, b=255)
if args_.last_click_point is None:
args_.last_click_point = carla.Location(x=x, y=y, z=z)
print("Click origin location xyz: %s %s %s" % (x, y, z))
world.debug.draw_point(args_.last_click_point,
color=red,
size=0.3,
life_time=30)
else:
this_click_point = carla.Location(x=x, y=y, z=z)
world.debug.draw_arrow(args_.last_click_point,
this_click_point,
thickness=0.2,
arrow_size=0.2,
color=red,
life_time=30)
# draw the (x=1.0, y=0.0) and (x=0.0, y=1.0) point translated to the
# origin point, and compute the rotation degree needed
ref_point = (1.0 + args_.last_click_point.x,
0 + args_.last_click_point.y)
ref_vec = (ref_point[0] - args_.last_click_point.x,
ref_point[1] - args_.last_click_point.y)
this_vec = (x - args_.last_click_point.x,
y - args_.last_click_point.y)
angle_degree = get_degree_of_two_vectors(this_vec, ref_vec)
# notice here the degree is negative.
print("Click second location xyz: %s, Degree between (1, 0) "
"and this vector: %s" % ([x, y, z], -angle_degree))
world.debug.draw_arrow(args_.last_click_point,
carla.Location(x=ref_point[0],
y=ref_point[1],
z=z),
thickness=0.1,
arrow_size=0.1,
color=green,
life_time=30)
ref_point = (0.0 + args_.last_click_point.x,
1.0 + args_.last_click_point.y)
ref_vec = (ref_point[0] - args_.last_click_point.x,
ref_point[1] - args_.last_click_point.y)
world.debug.draw_arrow(args_.last_click_point,
carla.Location(x=ref_point[0],
y=ref_point[1],
z=z),
thickness=0.1,
arrow_size=0.1,
color=blue,
life_time=30)
args_.last_click_point = None
elif event.type == pygame.KEYUP:
if event.key == pygame.K_r:
# reset rotation
args_.spectator.set_transform(
carla.Transform(rotation=carla.Rotation(pitch=0.0,
yaw=0.0,
roll=0.0),
location=prev_location))
elif event.key == pygame.K_t:
# print out the camera transform
print(args_.spectator.get_transform())
print("camera FOV: %s" % \
args_.rgb_camera.camera_actor.attributes["fov"])
# back to preset location
elif event.key == pygame.K_y:
if args_.preset is not None:
args_.spectator.set_transform(args_.preset)
# save the image in camera_to_save to save_path
elif event.key == pygame.K_p:
# in asyn mode, the frame_number might be different for the two
# cameras
save_rgb_image(
args_.seg_camera.rgb_image,
os.path.join(
save_screenshot_path, "%08d.seg.jpg" %
args_.seg_camera.last_image_frame_num))
save_rgb_image(
args_.rgb_camera.rgb_image,
os.path.join(
save_screenshot_path, "%08d.rgb.jpg" %
args_.rgb_camera.last_image_frame_num))
print(
"Saved seg and rgb image to %s, framenum %s, timestamp %s, "
% (save_screenshot_path,
args_.rgb_camera.last_image_frame_num,
args_.rgb_camera.last_image_seconds))
# toggle saving to videos
elif event.key == pygame.K_b:
if not args_.rgb_camera.recording:
print("Start saving video frames to %s" %
args_.rgb_camera.save_path)
args_.rgb_camera.recording = True
print("\tScene segmentation to %s" %
args_.seg_camera.save_path)
args_.seg_camera.recording = True
else:
print("stop saving video frame & seg feature.")
args_.rgb_camera.recording = False
args_.seg_camera.recording = False
elif event.key == pygame.K_x:
# before recording the video, remember to hit this button to get all
# actor bboxes
# update the actors in the scene, like after running build moment
# reset all the vehicle and walker actor list
# the actual box will be computed in client loop
vehicles_[:] = []
walkers_[:] = []
if args_.show_actor_box:
args_.show_actor_box = False
print("stop showing box.")
else:
args_.show_actor_box = True
for v in world_.get_actors().filter('vehicle.*'):
vehicles_.append(v)
for w in world.get_actors().filter('walker.*'):
walkers_.append(w)
print(
"Start showing actor boxes: got %s vehicles, %s walkers."
% (len(vehicles_), len(walkers_)))
# an ugly way to change the camera fov
elif (event.key == pygame.K_n) or (event.key == pygame.K_m):
if event.key == pygame.K_n:
new_fov = args.prev_camera_fov - 5.0
else:
new_fov = args.prev_camera_fov + 5.0
new_fov = new_fov if new_fov >= 5.0 else 5.0
new_fov = new_fov if new_fov <= 175.0 else 175.0
prev_recording = args_.rgb_camera.recording
args_.camera_bp.set_attribute("fov", "%s" % new_fov)
args_.camera_seg_bp.set_attribute("fov", "%s" % new_fov)
args_.camera_depth_bp.set_attribute("fov", "%s" % new_fov)
# destroy the original actor and make a new camera object
args_.rgb_camera.camera_actor.stop()
args_.seg_camera.camera_actor.stop()
args_.depth_camera.camera_actor.stop()
commands_ = [
# destroy the previous actor first
carla.command.DestroyActor(
args_.depth_camera.camera_actor.id),
carla.command.DestroyActor(
args_.seg_camera.camera_actor.id),
carla.command.DestroyActor(
args_.rgb_camera.camera_actor.id),
# spawn the new actor
carla.command.SpawnActor(args_.camera_bp,
carla.Transform(),
args_.spectator),
carla.command.SpawnActor(args_.camera_seg_bp,
carla.Transform(),
args_.spectator),
carla.command.SpawnActor(args_.camera_depth_bp,
carla.Transform(),
args_.spectator),
]
response_ = client_.apply_batch_sync(commands_)
camera_actor_ids_ = [r.actor_id for r in response_[-3:]]
camera_, camera_seg_, camera_depth_ = world.get_actors(
camera_actor_ids_)
args_.rgb_camera = Camera(camera_,
width=args_.width,
height=args_.height,
recording=prev_recording,
fov=new_fov,
save_path=args_.save_video_path,
camera_type="rgb")
args_.seg_camera = Camera(
camera_seg_,
save_path=args_.save_seg_path,
camera_type="seg",
seg_save_img=args_.save_seg_as_img,
recording=prev_recording,
image_type=carla.ColorConverter.CityScapesPalette)
args_.depth_camera = Camera(camera_depth_,
save_path=None,
camera_type="depth",
recording=prev_recording)
args_.prev_camera_fov = new_fov
# get a big dict of what key is pressed now, so to avoid hitting forward
# multiple times to go forward for a distance
step = 0.1 * ms_since_last_tick # this is from experimenting
keys = pygame.key.get_pressed()
if keys[pygame.K_w]:
args_.spectator.set_location(prev_location +
step * global_forward_vector)
if keys[pygame.K_s]:
args_.spectator.set_location(prev_location -
step * global_forward_vector)
if keys[pygame.K_a]:
args_.spectator.set_location(prev_location + step * left_vector)
if keys[pygame.K_d]:
args_.spectator.set_location(prev_location - step * left_vector)
if keys[pygame.K_u]:
args_.spectator.set_location(prev_location +
step * 0.5 * global_up_vector)
if keys[pygame.K_i]:
args_.spectator.set_location(prev_location -
step * 0.5 * global_up_vector)
if keys[pygame.K_UP]:
args_.spectator.set_transform(
carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch +
1.0,
yaw=prev_rotation.yaw,
roll=prev_rotation.roll),
location=prev_location))
if keys[pygame.K_DOWN]:
args_.spectator.set_transform(
carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch -
1.0,
yaw=prev_rotation.yaw,
roll=prev_rotation.roll),
location=prev_location))
if keys[pygame.K_LEFT]:
args_.spectator.set_transform(
carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch,
yaw=prev_rotation.yaw -
1.0,
roll=prev_rotation.roll),
location=prev_location))
if keys[pygame.K_RIGHT]:
args_.spectator.set_transform(
carla.Transform(rotation=carla.Rotation(pitch=prev_rotation.pitch,
yaw=prev_rotation.yaw +
1.0,
roll=prev_rotation.roll),
location=prev_location))
return False
def point_triangle_distance(points, verts, faces):
n_p = points.shape[0]
n_f = faces.shape[0]
# make sure everything is contiguous
points = points.contiguous()
verts = verts.contiguous()
faces = faces.contiguous()
points_expand = points.unsqueeze(1).expand(-1, n_f, -1)
face_normals = utils.face_normals(verts, faces)
# Accumulate distances
dists2 = float('inf') * torch.ones(
(n_p, n_f), dtype=points.dtype, device=points.device)
# True if point projects inside of face in plane
inside_face = torch.ones((n_p, n_f),
dtype=torch.bool,
device=points.device)
for i in range(3):
# Distance to each of the three edges
lineA = verts[faces[:, i]]
lineB = verts[faces[:, (i + 1) % 3]]
dists2 = torch.min(dists2,
point_line_segment_distances2(points, lineA, lineB))
# Edge perp vec (not normalized)
e_perp = utils.cross(face_normals, lineB - lineA)
inside_edge = utils.dot(
e_perp.unsqueeze(0).expand(n_p, -1, -1),
points_expand - lineA.unsqueeze(0).expand(n_p, -1, -1)) > 0
inside_face = inside_face & inside_edge
dists = torch.sqrt(dists2)
# For points inside, distance is just normal distance
point_in_face = verts[faces[:, 0]].unsqueeze(0).expand(n_p, -1, -1)
inside_face_dist = torch.abs(
utils.dot(
face_normals.unsqueeze(0).expand(n_p, -1, -1)[inside_face],
points_expand[inside_face] - point_in_face[inside_face]))
# dists[inside_face] = inside_face_dist
inside_face_dist_full = torch.zeros_like(dists)
inside_face_dist_full[inside_face] = inside_face_dist
dists = torch.where(inside_face, inside_face_dist_full, dists)
if False:
polyscope.remove_all_structures()
samp = polyscope.register_point_cloud("points", toNP(points))
samp.add_scalar_quantity("dist", toNP(dists[:, 0]))
samp.add_scalar_quantity("inside face", toNP(inside_face[:,
0].float()))
tri = polyscope.register_surface_mesh("tri", toNP(verts),
toNP(faces[0, :].unsqueeze(0)))
tri.add_face_vector_quantity("e perp", toNP(e_perp[0, :].unsqueeze(0)))
tri.add_face_vector_quantity("e vec",
toNP((lineB - lineA)[0, :].unsqueeze(0)))
tri.add_face_vector_quantity("N",
toNP((face_normals)[0, :].unsqueeze(0)))
polyscope.show()
return dists
def showEdges():
fig = plt.figure()
fig.canvas.mpl_connect('button_press_event', onclick)
#calculate smoothed normalmap
smoothing = 3
maxdepth = 0
width = utils.getWidth()
height = utils.getHeight()
normalmap = np.zeros((width, height, 3))
for x in range(1 + smoothing, width - 1 - smoothing):
for y in range(1 + smoothing, height - 1 - smoothing):
maxdepth = max(maxdepth, utils.parseDepth(x, y))
cmx = utils.convert2Dto3D(
calibration[1], x - 1, y,
utils.parseDepthSmoothed(x - 1, y, smoothing))
cmy = utils.convert2Dto3D(
calibration[1], x, y - 1,
utils.parseDepthSmoothed(x, y - 1, smoothing))
cpx = utils.convert2Dto3D(
calibration[1], x + 1, y,
utils.parseDepthSmoothed(x + 1, y, smoothing))
cpy = utils.convert2Dto3D(
calibration[1], x, y + 1,
utils.parseDepthSmoothed(x, y + 1, smoothing))
n = utils.normalize(
utils.cross(utils.sub(cpx, cmx), utils.sub(cpy, cmy)))
normalmap[x][height - y - 1][0] = min(abs(n[0]), 1)
normalmap[x][height - y - 1][1] = min(abs(n[1]), 1)
normalmap[x][height - y - 1][2] = min(abs(n[2]), 1)
#fade normalmap
#for x in range(1, width):
# for y in range(1, height):
# factor = utils.parseDepth(x, y) / maxdepth
# normalmap[x][height - y - 1][0] *= (1 - factor)
# normalmap[x][height - y - 1][1] *= (1 - factor)
# normalmap[x][height - y - 1][2] *= (1 - factor)
#calculate edgemap
edgemap = np.zeros((width, height, 3))
for x in range(2, width - 2):
for y in range(2, height - 2):
#calculate edge from depthmap
d = utils.convert2Dto3D(calibration[1], x, y,
utils.parseDepth(x, y))
mx = utils.convert2Dto3D(calibration[1], x - 1, y,
utils.parseDepth(x - 1, y))
my = utils.convert2Dto3D(calibration[1], x, y - 1,
utils.parseDepth(x, y - 1))
px = utils.convert2Dto3D(calibration[1], x + 1, y,
utils.parseDepth(x + 1, y))
py = utils.convert2Dto3D(calibration[1], x, y + 1,
utils.parseDepth(x, y + 1))
edge = max(max(utils.length(d, mx), utils.length(d, my)),
max(utils.length(d, px), utils.length(d, py)))
if edge > 0.1: # difference of depth in meters
edgemap[x][height - y - 1][1] = 1
#calculate edge from normalmap
d = normalmap[x][height - y - 1]
mx = normalmap[x - 1][height - y - 1]
my = normalmap[x][height - y - 1 - 1]
px = normalmap[x + 1][height - y - 1]
py = normalmap[x][height - y - 1 + 1]
edge = max(max(utils.dot(d, mx), utils.dot(d, my)),
max(utils.dot(d, px), utils.dot(d, py)))
if edge > 0.9: # normal matching in percentage
edgemap[x][height - y - 1][0] = 1
#calculate output
output = np.zeros((width, height, 3))
for x in range(2, width - 2):
for y in range(2, height - 2):
if edgemap[x][height - y - 1][1] == 1:
output[x][height - y - 1][2] = 1
else:
if edgemap[x][height - y - 1][0] == 0:
if edgemap[x - 1][height - y - 1][0] == 1:
output[x][height - y - 1][1] = 1
if edgemap[x][height - y - 1 - 1][0] == 1:
output[x][height - y - 1][1] = 1
if edgemap[x + 1][height - y - 1][0] == 1:
output[x][height - y - 1][1] = 1
if edgemap[x][height - y - 1 + 1][0] == 1:
output[x][height - y - 1][1] = 1
#plt.imshow(normalmap, extent=[0, height, 0, width])
#plt.imshow(edgemap, extent=[0, height, 0, width])
plt.imshow(output, extent=[0, height, 0, width])
import utils as ut
row_units = [ut.cross(r, ut.cols) for r in ut.rows]
column_units = [ut.cross(ut.rows, c) for c in ut.cols]
square_units = [ut.cross(rs, cs) for rs in ('ABC', 'DEF', 'GHI') for cs in ('123', '456', '789')]
unitlist = row_units + column_units + square_units
# Update the unit list to add the new diagonal units
unitlist = unitlist + [[ut.rows[i] + ut.cols[i] for i in range(9)]] + [[ut.rows[::-1][i] + ut.cols[i] for i in range(9)]]
# Must be called after all units (including diagonals) are added to the unitlist
units = ut.extract_units(unitlist, ut.boxes)
peers = ut.extract_peers(units, ut.boxes)
def naked_twins(values):
"""Eliminate values using the naked twins strategy.
The naked twins strategy says that if you have two or more unallocated boxes
in a unit and there are only two digits that can go in those two boxes, then
those two digits can be eliminated from the possible assignments of all other
boxes in the same unit.
Parameters
----------
values(dict)
a dictionary of the form {'box_name': '123456789', ...}
Returns
-------
dict
from utils import rows, cols, cross, extract_units, extract_peers, \
boxes, grid2values, display, history, assign_value
row_units = [cross(r, cols) for r in rows]
column_units = [cross(rows, c) for c in cols]
square_units = [
cross(rs, cs) for rs in ('ABC', 'DEF', 'GHI')
for cs in ('123', '456', '789')
]
unitlist = row_units + column_units + square_units
diagonal_1 = [x[0] + x[1] for x in zip(rows, cols)]
diagonal_2 = [x[0] + x[1] for x in zip(rows, cols[::-1])]
diagonal_units = [diagonal_1] + [diagonal_2]
unitlist = unitlist + diagonal_units
# Must be called after all units (including diagonals) are added to the unitlist
units = extract_units(unitlist, boxes)
peers = extract_peers(units, boxes)
def naked_twins(values):
"""Eliminate values using the naked twins strategy.
The naked twins strategy says that if you have two or more unallocated boxes
in a unit and there are only two digits that can go in those two boxes, then
those two digits can be eliminated from the possible assignments of all other
boxes in the same unit.
Parameters
----------
def calc_acc(AA, ww, q, qd, Ydd, Conn, Prop):
"""
Returns the linear acceleration of all body centroids and the angular
acceleration of all body (eqs. 3.10-3.11 and 3.14-3.15, figure 3.5).
vd = [vd_0, vd_1, ... ] (3, num_b)
wd = [wd_0, wd_1, ... ] (3, num_b)
"""
BB, j_type = Conn[2], Conn[3]
cc = Prop[2]
vd0, wd0, qdd = Ydd[0:3], Ydd[3:6], Ydd[6:]
num_b = len(q) + 1 # Number of bodies
vd = np.zeros((3, num_b))
wd = np.zeros((3, num_b))
# Base
vd[:, 0] = vd0
wd[:, 0] = wd0
# i = current link, k = lower link connection
for i in range(1, num_b):
idxi = i - 1 # Index link/joint <i> in BB, j_type, q, qd, qdd
k = BB[idxi] # Index lower link connection
# Rotation matrices
A_I_i = AA[:, 3 * i:3 * (i + 1)]
A_I_k = AA[:, 3 * k:3 * (k + 1)]
# Relative positions
cc_I_i = A_I_i @ cc[:, i, i]
cc_I_k = A_I_k @ cc[:, k, i]
# Joint values
q_I_i = A_I_i @ (Ez * q[idxi])
qd_I_i = A_I_i @ (Ez * qd[idxi])
qdd_I_i = A_I_i @ (Ez * qdd[idxi])
# Rotational joint (eqs. 3.10-3.11)
if (j_type[idxi] == 'R'):
wd[:, i] = wd[:, k] + cross(ww[:, i], qd_I_i) + qdd_I_i
vd[:, i] = vd[:, k] + cross(wd[:, k], cc_I_k) \
+ cross(ww[:, k], cross(ww[:, k], cc_I_k)) \
- cross(wd[:, i], cc_I_i) \
- cross(ww[:, i], cross(ww[:, i], cc_I_i))
# Prismatic joint (eqs. 3.14-3.15)
elif (j_type[idxi] == 'P'):
wd[:, i] = wd[:, k]
vd[:, i] = vd[:, k] + cross(wd[:, k], cc_I_k) \
+ cross(ww[:, k], cross(ww[:, k], cc_I_k)) \
- cross(wd[:, i], cc_I_i) \
- cross(ww[:, i], cross(ww[:, i], cc_I_i)) \
+ cross(wd[:, i], q_I_i) \
+ cross(ww[:, i], cross(ww[:, i], q_I_i)) \
+ 2.0 * cross(ww[:, i], qd_I_i) \
+ qdd_I_i
return vd, wd
def _cross(cls, v1, v2):
return utils.cross(v1, v2)
def r_ne(RR, AA, q, Yd, Ydd, Fe, Te, Conn, Prop):
"""
Inverse dynamics computation by the recursive Newton-Euler method
(eqs. 3.30-3.39).
Given:
state RR, AA, v0, w0, q, qd
accelerations vd0, wd0, qdd
external forces and moments Fe, Te
Returns:
reaction forces and moments F0, T0, tau
i.e. the system reaction force <F0> and moment <T0> on the base, and the
torque/forces <tau> on the joints.
"""
SS, SE, j_type = Conn[0], Conn[1], Conn[3]
mass, inertia, cc, ce, Qe, gravity = \
Prop[0], Prop[1], Prop[2], Prop[3], Prop[5], Prop[6]
qd = Yd[6:]
num_j = len(q) # Number of joints/links
num_b = num_j + 1 # Number of bodies
num_e = SE.shape[1] # Number of endpoints
# Linear and angular velocities of all bodies
vv, ww = calc_vel(AA, q, Yd, Conn, Prop)
# Linear and angular accelerations of all bodies
vd, wd = calc_acc(AA, ww, q, qd, Ydd, Conn, Prop)
# Inertial forces and moments on the body centroids (for convenience the
# the gravitational force is also included here) - eqs. 3.30-3.31
F_in = np.zeros((3, num_b))
T_in = np.zeros((3, num_b))
for i in range(num_b):
A_I_i = AA[:, 3*i:3*(i+1)]
In_I_i = A_I_i @ inertia[:, 3*i:3*(i+1)] @ A_I_i.T
# Eq. 3.30 and 3.31
F_in[:, i] = mass[i] * (vd[:, i] - gravity)
T_in[:, i] = In_I_i @ wd[:, i] + cross(ww[:, i], (In_I_i @ ww[:, i]))
# Forces and moments on the joints (eqs. 3.32-3.35)
F_jnt = np.zeros((3, num_j))
T_jnt = np.zeros((3, num_j))
# Start from the last link
for i in range(num_j, 0, -1):
idxi = i - 1 # Index joint <i> in Fjnt, Tjnt, j_type, q
A_I_i = AA[:, 3*i:3*(i+1)]
is_P = float(j_type[idxi] == 'P') # = 1 if joint is prismatic
# Vector centroid <i> to joint <i>
L_ii = cc[:, i, i] - is_P * Ez * q[idxi]
# Add inertial force and moment on the link centroid (the cross-product
# is negative because the arm should go from the joint to the centroid)
F_jnt[:, idxi] = F_in[:, i]
T_jnt[:, idxi] = T_in[:, i] - cross(A_I_i @ L_ii, F_in[:, i])
# Add force and moment due to connected upper links (note that a link
# may have more than one upper connection)
for j in range(i+1, num_j+1):
idxj = j - 1 # Index joint <j> in j_type, q, F_jnt, T_jnt
# Add contribution if link <j> is an upper connection of link <i>
if (SS[i, j]):
# Vector joint <i> to joint <j>
L_ij = cc[:, i, j] - cc[:, i, i] + is_P * Ez * q[idxi]
# Add joint force and moment (note that Fj and Tj are calculated
# as joint force and moment on the j-th link, thus the force and
# moment passed to the i-th link are equal and opposite)
F_jnt[:, idxi] += F_jnt[:, idxj]
T_jnt[:, idxi] += cross(A_I_i @ L_ij, F_jnt[:, idxj]) \
+ T_jnt[:, idxj]
# Add external forces and moments
for ie in range(num_e):
# Add contribution if endpoint <ie> is connected to link <i>
if (SE[0, ie] == i):
# Vector centroid <i> to endpoint <ie>
A_i_ie = rpy2dc(Qe[:, ie]).T # Endpoint to link/joint
A_I_ie = A_I_i @ A_i_ie # Endpoint to inertial
# If the external load is given wrt the local frame
if (SE[1, ie] == 0):
L_i_ie = A_i_ie.T @ (ce[:, ie] - cc[:, i, i]
+ is_P * Ez * q[idxi])
F_jnt[:, idxi] -= A_I_ie @ Fe[:, ie]
T_jnt[:, idxi] -= A_I_ie @ (tilde(L_i_ie) @ Fe[:, ie]
+ Te[:, ie])
# If the external load is given wrt the inertial frame
else:
L_i_ie = A_I_ie @ (ce[:, ie] - cc[:, i, i]
+ is_P * Ez * q[idxi])
F_jnt[:, idxi] -= Fe[:, ie]
T_jnt[:, idxi] -= (tilde(L_i_ie) @ Fe[:, ie] + Te[:, ie])
# Reaction force and moment on the base centroid (Eqs. 3.38 and 3.39)
F0 = np.zeros(3)
T0 = np.zeros(3)
# Add inertial force and moment of the base
F0 += F_in[:, 0]
T0 += T_in[:, 0]
# Add forces and moments from the links connected to the base
for i in range(1, num_j+1):
# Add if link <i> is connected
if (SS[0, i] > 0):
idxi = i - 1 # Index link/joint <i> in Fjnt, Tjnt
F0 += F_jnt[:, idxi]
T0 += cross(AA[:, 0:3] @ cc[:, 0, i], F_jnt[:, idxi]) \
+ T_jnt[:, idxi]
# Add external forces and moments
for ie in range(num_e):
# Add contribution if endpoint <ie> is connected to the base
if (SE[0, ie] == 0):
A_0_ie = rpy2dc(Qe[:, ie]).T # Endpoint to base
A_I_ie = AA[:, 0:3] @ A_0_ie # Endpoint to inertial
# If the external load is given wrt the local frame
if (SE[1, ie] == 0):
R_0_ie = A_0_ie.T @ ce[:, ie]
F0 -= A_I_ie @ Fe[:, ie]
T0 -= A_I_ie @ (tilde(R_0_ie) @ Fe[:, ie] + Te[:, ie])
# If the external load is given wrt the inertial frame
else:
R_0_ie = A_0_ie @ ce[:, ie]
F0 -= Fe[:, ie]
T0 -= (tilde(R_0_ie) @ Fe[:, ie] + Te[:, ie])
# Calculation of joint torques/forces (eq. 3.36 and 3.37)
tau = np.zeros(num_j)
for i in range(1, num_j+1):
idxi = i - 1 # Index link/joint <i> in Fjnt, Tjnt, j_type, tau
Ez_I_i = AA[:, 3*i:3*(i+1)] @ Ez
# If revolute joint (eq. 3.36)
if (j_type[idxi] == 'R'):
tau[idxi] = T_jnt[:, idxi] @ Ez_I_i
# If prismatic joint (eq. 3.37)
elif (j_type[idxi] == 'P'):
tau[idxi] = F_jnt[:, idxi] @ Ez_I_i
# Compose generalized forces
Force = np.block([F0, T0, tau])
return Force
def testFunc(**kwargs):
print(kwargs) # prints the dictionary of keyword arguments
mlp = alg( **kwargs )
scores = cross_val_score(mlp, X, Y, cv=folds)
print(scores)
print("Accuracy: %0.2f (+/- %0.2f)" % (scores.mean(), scores.std() * 2))
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../core")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../configs")
import utils
#utils.cross(params_collection, testFunc)
import data
import output
X,Y = data.get_full_data()
import mlp
configurations = mlp.getconfig()
folds = configurations['folds']
configs = configurations['configurations']
for config in configs:
alg = config['algorithm_name']
utils.cross(config['parameters'], testFunc)
if i % 100 == 0:
print('Epoch {}: {:.4f}'.format(i, loss))
AE.eval()
with torch.no_grad():
data_transformed = AE.encode(data_torch).detach().numpy()
else:
raise Exception('Please specify a valid solver!')
edges2d = []
for e in edge_list:
edges2d.append(data_transformed[[e[0], e[1]]])
# check crosses
crosses = cross(edges2d, edge_list, findall=True)
print('{}: {} crosses'.format(method, len(crosses)))
plt.scatter(data_transformed[:, 0],
data_transformed[:, 1],
c=t,
cmap=plt.cm.Spectral)
lc = LineCollection(segments=edges2d, linewidths=0.3)
plt.gca().add_collection(lc)
plt.xticks([], [])
plt.yticks([], [])
if save_fig:
filels = glob.glob(fig_dir + '*{}*.pdf'.format(method))
if len(filels) != 0:
for file in filels:
