def testGradientRandomValues(self):
with self.test_session():
us = [2, 3]
u = tf.reshape([0.854, -0.616, 0.767, 0.725, -0.927, 0.159], shape=us)
v = tf.reshape([-0.522, 0.755, 0.407, -0.652, 0.241, 0.247], shape=us)
s = tf.cross(u, v)
jacob_u, jacob_v = tf.test.compute_gradient([u, v], [us, us], s, us)
self.assertAllClose(jacob_u[0], jacob_u[1], rtol=1e-3, atol=1e-3)
self.assertAllClose(jacob_v[0], jacob_v[1], rtol=1e-3, atol=1e-3)
def _call(self, inputs):
x = inputs
norm_x = tf.nn.l2_normalize(x, axis=1)
norm_support = tf.nn.l2_normalize(self.support, axis=0)
norm_mix = tf.cross(norm_x, norm_support)
norm_mix = norm_mix*tf.inv(tf.reduce_sum(norm_mix))
sampledIndex = tf.multinomial(tf.log(norm_mix), self.rank)
new_support = dot(self.support,tf.diag(norm_mix),sparse=True)
# dropout
if self.sparse_inputs:
x = sparse_dropout(x, 1-self.dropout, self.num_features_nonzero)
else:
x = tf.nn.dropout(x, 1-self.dropout)
# convolve
# supports = list()
# for i in range(len(self.support)):
# if not self.featureless:
# pre_sup = dot(x, self.vars['weights_' + str(i)],
# sparse=self.sparse_inputs)
# else:
# pre_sup = self.vars['weights_' + str(i)]
# support = dot(self.support[i], pre_sup, sparse=True)
# supports.append(support)
# output = tf.add_n(supports)
if not self.featureless:
pre_sup = dot(x, self.vars['weights_0'],
sparse=self.sparse_inputs)
else:
pre_sup = self.vars['weights_0']
output = dot(new_support, pre_sup, sparse=True)
# bias
if self.bias:
output += self.vars['bias']
return self.act(output)
def get_ver_norm_bk(ver_xyz, ver_tri, tri, scope_name):
"""
Compute vertex normals and vertex contour mask(for 2d lmk loss).
:param:
ver_xyz: [batch, N, 3], vertex geometry
tri: [M, 3], mesh triangles definition
:return:
ver_normals: [batch, N, 3], vertex normals
ver_contour_mask: [batch, N, 1], vertex contour mask, indicating whether the vertex is on the contour
"""
batch_size, n_ver, n_channels = ver_xyz.get_shape().as_list()
n_ver_tri_pair = ver_tri.get_shape().as_list()[0]
n_tri = tri.get_shape().as_list()[0]
v1_idx, v2_idx, v3_idx = tf.unstack(tri, 3, axis=-1)
v1 = tf.gather(ver_xyz, v1_idx, axis=1, name="v1_tri")
v2 = tf.gather(ver_xyz, v2_idx, axis=1, name="v2_tri")
v3 = tf.gather(ver_xyz, v3_idx, axis=1, name="v3_tri")
EPS = 1e-8
tri_normals = tf.cross(v2 - v1, v3 - v1)
tri_visible = tf.cast(tf.greater(tri_normals[:, :, 2:], float(EPS)),
tf.float32)
with tf.variable_scope(scope_name, tf.AUTO_REUSE):
var_sum_of_tri_normals = tf.get_variable('ver_sum_of_tri_normals',
[batch_size, n_ver, 3],
tf.float32,
tf.zeros_initializer(),
trainable=False)
var_sum_of_tri_visible = tf.get_variable('ver_sum_of_tri_visible',
[batch_size, n_ver, 1],
tf.float32,
tf.zeros_initializer(),
trainable=False)
var_sum_of_tri_counts = tf.get_variable('ver_sum_of_counts',
[batch_size, n_ver, 1],
tf.float32,
tf.zeros_initializer(),
trainable=False)
init_sum_of_tri_normals = tf.zeros_like(var_sum_of_tri_normals)
init_sum_of_tri_visible = tf.zeros_like(var_sum_of_tri_visible)
init_sum_of_tri_counts = tf.zeros_like(var_sum_of_tri_counts)
assign_op = tf.group([
tf.assign(var_sum_of_tri_normals, init_sum_of_tri_normals),
tf.assign(var_sum_of_tri_visible, init_sum_of_tri_visible),
tf.assign(var_sum_of_tri_counts, init_sum_of_tri_counts)
],
name='assign_op')
with tf.control_dependencies([assign_op]):
to_ver_ids, from_tri_ids = tf.split(ver_tri, 2, axis=1)
tmp_ver_tri_normals = tf.gather(tri_normals,
tf.squeeze(from_tri_ids),
axis=1)
tmp_ver_tri_visible = tf.gather(tri_visible,
tf.squeeze(from_tri_ids),
axis=1)
tmp_ver_tri_counts = tf.ones([batch_size, n_ver_tri_pair, 1],
tf.float32)
print(tmp_ver_tri_normals.get_shape().as_list())
batch_indices = tf.reshape(tf.tile(
tf.expand_dims(tf.range(batch_size), axis=1),
[1, n_ver_tri_pair]), [batch_size, n_ver_tri_pair, 1],
name='batch_indices')
to_ver_ids = tf.tile(tf.expand_dims(to_ver_ids, axis=0),
[batch_size, 1, 1])
batch_to_ver_ids = tf.concat([batch_indices, to_ver_ids], axis=2)
var_sum_of_tri_normals = tf.scatter_nd_add(var_sum_of_tri_normals,
batch_to_ver_ids,
tmp_ver_tri_normals)
var_sum_of_tri_visible = tf.scatter_nd_add(var_sum_of_tri_visible,
batch_to_ver_ids,
tmp_ver_tri_visible)
var_sum_of_tri_counts = tf.scatter_nd_add(var_sum_of_tri_counts,
batch_to_ver_ids,
tmp_ver_tri_counts)
ver_normals = tf.div(var_sum_of_tri_normals,
var_sum_of_tri_counts + EPS)
ver_normals = tf.nn.l2_normalize(ver_normals)
ver_visible = tf.div(var_sum_of_tri_visible,
var_sum_of_tri_counts + EPS)
cond1 = tf.less(ver_visible, float(1.0))
cond2 = tf.greater(ver_visible, float(0.0))
ver_contour_mask = tf.cast(
tf.logical_and(cond1, cond2),
tf.float32,
name="ver_votes_final",
)
return ver_normals, ver_contour_mask
def timeflow(self, t=0.0):
self.setalpha(t)
for p in range(numLeg):
Qs = [tf.matmul(self.body.Q, self.leg[p].sub[0].Q)]
#List of rotation matrices of each sublegs in space frame
#Type : list of [3,3] Tensor
for i in range(1, numsubleg):
Qs.append(tf.matmul(Qs[i - 1], self.leg[p].sub[i].Q))
e = [
tf.matmul(Qs[i], tf.reshape(self.leg[p].sub[i].axis, [3, 1]))
for i in range(numsubleg)
]
#List of axes of each sublegs in space frame
#Type : list of [3,1] Tensor
Qalpha = [
tf.scalar_mul(self.leg[p].sub[i].alpha, e[i])
for i in range(numsubleg)
]
Qw = [
tf.scalar_mul(self.leg[p].sub[i].omega, e[i])
for i in range(numsubleg)
]
wbs = tf.matmul(self.body.Q, tf.reshape(self.body.wb,
[3, 1])) #[3,1] matrix
ws = [wbs + Qw[0]]
for i in range(1, numsubleg):
ws.append(ws[i - 1] + Qw[i])
w = [
tf.matmul(tf.transpose(Qs[i]), tf.reshape(ws[i], [3, 1]))
for i in range(numsubleg)
]
ls = [[
tf.matmul(Qs[i], tf.reshape(self.leg[p].sub[i].l[0], [3, 1])),
tf.matmul(Qs[i], tf.reshape(self.leg[p].sub[i].l[1], [3, 1]))
] for i in range(numsubleg)] #ls = 2Dtensor
lbtomotbs = tf.matmul(self.body.Q,
tf.reshape(self.body.lbtomot[p],
[3, 1])) # lbtomotbs = 2Dtensor
lbtomots = [tf.add(lbtomotbs, ls[0][0])] # lbtomots = 2Dtensor
for i in range(1, numsubleg):
lbtomots.append(
tf.add(lbtomots[i - 1], tf.add(ls[i - 1][1], ls[i][0])))
A = [
tf.add(
tf.cross(wbs, tf.cross(wbs, lbtomotbs)),
tf.add(tf.cross(Qalpha[0], lbtomots[0]),
tf.cross(ws[0], tf.cross(ws[0], lbtomots[0]))))
]
for i in range(1, numsubleg):
A.append(tf.add(tf.cross(Qalpha[i - 1], tf.add())))
assert sys.version_info >= (3, 5)
# Scikit-Learn ≥0.20 is required
assert sklearn.__version__ >= "0.20"
# 屏蔽警告：Your CPU supports instructions that this TensorFlow binary was not compiled to use: AVX2 FMA
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
# Open graph session
sess = tf.Session()
show_values(tf.div(3, 4), "tf.div(3,4) = 整数除")
show_values(tf.truediv(3, 4), "tf.truediv(3,4) = 浮点除")
show_values(tf.floordiv(3.0, 4.0), "tf.floordiv(3.0,4.0) = 浮点取整除")
show_values(tf.mod(22.0, 5.0), "tf.mod(22.0,5.0) = 取模")
# 张量点积--Compute the pairwise cross product
# 张量点积：即两个向量的叉乘，又叫向量积、外积、叉积，叉乘的运算结果是一个向量而不是一个标量。
# 两个向量的点积与这两个向量组成的坐标平面垂直。
show_values(tf.cross([1., 0., 0.], [0., 1., 0.]),
"tf.cross([1., 0., 0.], [0., 1., 0.]) = 张量点积")
# 张量点积必须是三维的
# show_values(tf.cross([1., 0., 0., 0.], [0., 1., 0., 0.]),
# "tf.cross([1., 0., 0.,0.], [0., 1., 0.,0.]) = 张量点积")
# ToSee：P11，数学函数列表
show_values(tf.div(tf.sin(3.1416 / 4.), tf.cos(3.1416 / 4.)),
"tan(pi/4) = 1 = tf.div(tf.sin(3.1416/4.),tf.cos(3.1416/4.))")
test_nums = range(15)
# What should we get with list comprehension
expected_output = [3 * x * x - x + 10 for x in test_nums]
print('-' * 50)
print("[3 * x ^ 2 - x + 10 for x in test_nums] = ")
def compute_camera_matrices(self, camera_position, camera_look_at,
camera_up):
"""Computes camera viewing matrices.
Functionality mimes gluLookAt (third_party/GL/glu/include/GLU/glu.h).
Args:
camera_position: 2-D float32 tensor with shape [batch_size, 3] containing the XYZ world
space position of the camera.
camera_look_at: 2-D float32 tensor with shape [batch_size, 3] containing a position
along the center of the camera's gaze.
camera_up: 2-D float32 tensor with shape [batch_size, 3] specifying the
world's up direction; the output camera will have no tilt with respect
to this direction.
Returns:
A [batch_size, 4, 4] float tensor containing a right-handed camera
extrinsics matrix that maps points from world space to points in eye space.
"""
batch_size = camera_look_at.shape[0].value
vector_degeneracy_cutoff = 1e-6
forward = camera_look_at - camera_position
forward_norm = tf.norm(forward, ord='euclidean', axis=1, keepdims=True)
tf.assert_greater(
forward_norm,
vector_degeneracy_cutoff,
message=
'Camera matrix is degenerate because eye and center are close.')
forward = tf.divide(forward, forward_norm)
to_side = tf.cross(forward, camera_up)
to_side_norm = tf.norm(to_side, ord='euclidean', axis=1, keepdims=True)
tf.assert_greater(
to_side_norm,
vector_degeneracy_cutoff,
message=
'Camera matrix is degenerate because up and gaze are close or because up is degenerate.'
)
to_side = tf.divide(to_side, to_side_norm)
cam_up = tf.cross(to_side, forward)
w_column = tf.constant(batch_size * [[0., 0., 0., 1.]],
dtype=tf.float32)
w_column = tf.reshape(w_column, [batch_size, 4, 1])
view_rotation = tf.stack([
to_side, cam_up, -forward,
tf.zeros_like(to_side, dtype=tf.float32)
],
axis=1)
view_rotation = tf.concat([view_rotation, w_column], axis=2)
identity_batch = tf.tile(tf.expand_dims(tf.eye(3), 0),
[batch_size, 1, 1])
view_translation = tf.concat(
[identity_batch,
tf.expand_dims(-camera_position, 2)], 2)
view_translation = tf.concat(
[view_translation,
tf.reshape(w_column, [batch_size, 1, 4])], 1)
camera_matrices = tf.matmul(view_rotation, view_translation)
return camera_matrices
def expSE3(x, batch):
one_6th = tf.constant(1.0 / 6.0)
one_20th = tf.constant(1.0 / 20.0)
w = x[:, 3:6]
theta_sq = tf.tensordot(w, w, axes=[[1], [1]])
#theta_sq = tf.matmul(w,w,transpose_a=False,transpose_b=True)
theta_sq = theta_sq[:, 0]
theta = tf.sqrt(theta_sq)
#w = tf.squeeze(w)
cross_ = tf.cross(w, x[:, 0:3])
A1 = 1.0 - one_6th * theta_sq
B1 = 0.5 * tf.ones_like(A1)
translation1 = (x[:, 0:3] + 0.5 * cross_)
C2 = one_6th * (1.0 - one_20th * theta_sq)
A2 = 1.0 - theta_sq * C2
B2 = 0.5 - 0.25 * one_6th * theta_sq
inv_theta = 1.0 / theta
A3 = tf.sin(theta) * inv_theta
B3 = (1 - tf.cos(theta)) * (inv_theta * inv_theta)
C3 = (1 - A3) * (inv_theta * inv_theta)
bool_tensor = tf.less(theta_sq, 1e-8)
bool_tensor2 = tf.less(theta_sq, 1e-6)
A_Alternate = tf.where(bool_tensor2, A2, A3)
B_Alternate = tf.where(bool_tensor2, B2, B3)
C = tf.where(bool_tensor2, C2, C3)
A = tf.where(bool_tensor, A1, A_Alternate)
B = tf.where(bool_tensor, B1, B_Alternate)
translation2 = (x[:, 0:3] + tf.expand_dims(B, -1) * cross_ +
tf.expand_dims(C, -1) * (tf.cross(w, cross_)))
translation = tf.where(bool_tensor, translation1, translation2)
wx2 = w[:, 0] * w[:, 0]
wy2 = w[:, 1] * w[:, 1]
wz2 = w[:, 2] * w[:, 2]
r00 = 1.0 - B * (wy2 + wz2)
r11 = 1.0 - B * (wx2 + wz2)
r22 = 1.0 - B * (wx2 + wz2)
a = A * w[:, 2]
b = B * (w[:, 0] * w[:, 1])
r01 = b - a
r10 = b + a
a = A * w[:, 1]
b = B * (w[:, 0] * w[:, 2])
r02 = b + a
r20 = b - a
a = A * w[:, 0]
b = B * (w[:, 1] * w[:, 2])
r12 = b - a
r21 = b + a
c0 = tf.stack([r00, r01, r02, translation[:, 0]])
c1 = tf.stack([r10, r11, r12, translation[:, 1]])
c2 = tf.stack([r20, r21, r22, translation[:, 2]])
c4 = tf.constant([0.0, 0.0, 0.0, 1.0], shape=[1, 4])
c4 = tf.transpose(tf.tile(c4, [batch, 1]))
SE3out = tf.transpose(tf.stack([c0, c1, c2, c4]), [2, 0, 1])
return SE3out
batch_size * npoint int32
'''
return sampling_module.farthest_point_sample(inp, npoint)
ops.NoGradient('FarthestPointSample')
if __name__=='__main__':
import numpy as np
np.random.seed(100)
triangles=np.random.rand(1,5,3,3).astype('float32')
with tf.device('/gpu:1'):
inp=tf.constant(triangles)
tria=inp[:,:,0,:]
trib=inp[:,:,1,:]
tric=inp[:,:,2,:]
areas=tf.sqrt(tf.reduce_sum(tf.cross(trib-tria,tric-tria)**2,2)+1e-9)
randomnumbers=tf.random_uniform((1,8192))
triids=prob_sample(areas,randomnumbers)
tria_sample=gather_point(tria,triids)
trib_sample=gather_point(trib,triids)
tric_sample=gather_point(tric,triids)
us=tf.random_uniform((1,8192))
vs=tf.random_uniform((1,8192))
uplusv=1-tf.abs(us+vs-1)
uminusv=us-vs
us=(uplusv+uminusv)*0.5
vs=(uplusv-uminusv)*0.5
pt_sample=tria_sample+(trib_sample-tria_sample)*tf.expand_dims(us,-1)+(tric_sample-tria_sample)*tf.expand_dims(vs,-1)
print 'pt_sample: ', pt_sample
reduced_sample=gather_point(pt_sample,farthest_point_sample(1024,pt_sample))
print reduced_sample
def batch_laplacian(v, f, return_sparse=True):
# v: B x N x 3
# f: M x 3
num_b = tf.shape(v)[0]
num_v = tf.shape(v)[1]
num_f = tf.shape(f)[0]
v_a = f[:, 0]
v_b = f[:, 1]
v_c = f[:, 2]
a = tf.gather(v, v_a, axis=1)
b = tf.gather(v, v_b, axis=1)
c = tf.gather(v, v_c, axis=1)
ab = a - b
bc = b - c
ca = c - a
cot_a = -1 * tf.reduce_sum(ab * ca, axis=2) / \
tf.sqrt(tf.reduce_sum(tf.cross(ab, ca) ** 2, axis=-1))
cot_b = -1 * tf.reduce_sum(bc * ab, axis=2) / \
tf.sqrt(tf.reduce_sum(tf.cross(bc, ab) ** 2, axis=-1))
cot_c = -1 * tf.reduce_sum(ca * bc, axis=2) / \
tf.sqrt(tf.reduce_sum(tf.cross(ca, bc) ** 2, axis=-1))
I = tf.tile(
tf.expand_dims(tf.concat((v_a, v_c, v_a, v_b, v_b, v_c), axis=0), 0),
(num_b, 1))
J = tf.tile(
tf.expand_dims(tf.concat((v_c, v_a, v_b, v_a, v_c, v_b), axis=0), 0),
(num_b, 1))
W = 0.5 * tf.concat((cot_b, cot_b, cot_c, cot_c, cot_a, cot_a), axis=1)
batch_dim = tf.tile(tf.expand_dims(tf.range(num_b), 1), (1, num_f * 6))
indices = tf.reshape(tf.stack((batch_dim, J, I), axis=2),
(num_b, 6, -1, 3))
W = tf.reshape(W, (num_b, 6, -1))
l_indices = [
tf.cast(tf.reshape(indices[:, i], (-1, 3)), tf.int64) for i in range(6)
]
shape = tf.cast(tf.stack((num_b, num_v, num_v)), tf.int64)
sp_L_raw = [
tf.sparse_reorder(
tf.SparseTensor(l_indices[i], tf.reshape(W[:, i], (-1, )), shape))
for i in range(6)
]
L = sp_L_raw[0]
for i in range(1, 6):
L = tf.sparse_add(L, sp_L_raw[i])
dia_values = tf.sparse_reduce_sum(L, axis=-1) * -1
I = tf.tile(tf.expand_dims(tf.range(num_v), 0), (num_b, 1))
batch_dim = tf.tile(tf.expand_dims(tf.range(num_b), 1), (1, num_v))
indices = tf.reshape(tf.stack((batch_dim, I, I), axis=2), (-1, 3))
dia = tf.sparse_reorder(
tf.SparseTensor(tf.cast(indices, tf.int64),
tf.reshape(dia_values, (-1, )), shape))
return tf.sparse_add(L, dia)
def test_Cross(self):
t = tf.cross(*self.random((4, 3), (4, 3)))
self.check(t)
def timeflow(self, t=0.0):
wbs = tf.matmul(self.body.wb, self.body.Q) #[1,3] matrix
tot_lbtomots = []
Feqc = tf.constant([0, 0, -Mtot * 9.81], dtype=tf.float32)
Teqc = tf.zeros([1, 3], dtype=tf.float32)
#List of External Forces
Flist = []
llist = []
for p in range(numLeg):
for i in range(numsubleg):
#print('alpha = ',self.leg[p].sub[i].alpha);
self.leg[p].sub[i].omega += self.leg[p].sub[
i].alpha * dtime #omega를 시간에 따라 갱신
#print('omega = ',self.leg[p].sub[i].omega);
self.leg[p].sub[i].theta += self.leg[p].sub[
i].omega * dtime #theta를 시간에 따라 갱신
#print('theta = ',self.leg[p].sub[i].theta);
self.leg[p].sub[i].Q = tf.scalar_mul(tf.cos(self.leg[p].sub[i].theta), tf.eye(3, dtype=tf.float32)) + \
tf.scalar_mul(1.-tf.cos(self.leg[p].sub[i].theta), tf.matmul(self.leg[p].sub[i].axis, self.leg[p].sub[i].axis, transpose_a = True)) + \
tf.scalar_mul(tf.sin(self.leg[p].sub[i].theta), tf.cross(tf.tile(self.leg[p].sub[i].axis,[3,1]), tf.eye(3, dtype=tf.float32)))
Qs = [tf.matmul(self.leg[p].sub[0].Q,
self.body.Q)] #Qs는 i번째 subleg에서 space로의 좌표변환
#List of rotation matrices of each sublegs in space frame
#Type : list of [3,3] Tensor
for i in range(1, numsubleg):
Qs.append(tf.matmul(self.leg[p].sub[i].Q, Qs[i - 1]))
e = [
tf.matmul(self.leg[p].sub[i].axis, Qs[i])
for i in range(numsubleg)
]
#List of axes of each sublegs in space frame
#Type : list of [None,3] Tensor
Qw = [
tf.scalar_mul(self.leg[p].sub[i].omega, e[i])
for i in range(numsubleg)
]
ws = [wbs + Qw[0]]
for i in range(1, numsubleg):
ws.append(ws[i - 1] + Qw[i])
ls = [[
tf.matmul(self.leg[p].sub[i].l[0], Qs[i]),
tf.matmul(self.leg[p].sub[i].l[1], Qs[i])
] for i in range(numsubleg)] #ls = 2Dtensor
lbtomotbs = tf.matmul(self.body.lbtomot[p],
self.body.Q) # lbtomotbs = 2Dtensor
lbtomots = [lbtomotbs + ls[0][0]] # lbtomots = 2Dtensor
for i in range(1, numsubleg):
lbtomots.append(lbtomots[i - 1] + ls[i - 1][1] + ls[i][0])
#Calculating External Forces
vstmp = self.body.vs + tf.cross(wbs, lbtomotbs)
NormalScale = 300.0
TanhConst = 100.0
Zfilter = tf.constant([[0., 0., 1.]])
negZfilter = tf.constant([[0., 0., -1.]])
XYfilter = tf.constant([[1., 1., 0.]])
for i in range(numsubleg):
Fz_primi = tf.multiply(negZfilter,
self.body.rs + lbtomots[i] + ls[i][1])
colbool = tf.reshape(
tf.matmul(Zfilter, tf.nn.relu(Fz_primi), transpose_b=True),
[])
Fz = tf.scalar_mul(NormalScale, tf.nn.relu(Fz_primi))
vstmp += tf.cross(ws[i], ls[i][0] + ls[i][1])
vstmp_z = tf.reshape(
tf.matmul(Zfilter, vstmp, transpose_b=True), [])
Vscale = 3. - tf.nn.tanh(TanhConst * vstmp_z)
Fnormal = tf.scalar_mul(Vscale, Fz)
Flist.append(Fnormal)
vstmp_xy = tf.multiply(XYfilter, vstmp)
Ffric = tf.scalar_mul(-Fricscale,
tf.scalar_mul(colbool, vstmp_xy))
Flist.append(Ffric)
Feqc += Fnormal
Feqc += Ffric
Teqc += tf.cross(lbtomots[i] + ls[i][1], Fnormal + Ffric)
llist.append(tf.norm(lbtomots[i] + ls[i][1]))
#Teqc+=
tot_lbtomots += lbtomots
asb = tf.scalar_mul(Mtotinv, Feqc)
alphab = tf.matmul(tf.matmul(Teqc, self.body.Q, transpose_b=True),
Ibinv)
self.body.wb += tf.scalar_mul(dtime, alphab)
self.body.Q += tf.scalar_mul(
dtime, tf.cross(tf.concat([wbs, wbs, wbs], axis=0), self.body.Q))
self.body.vs += tf.scalar_mul(dtime, asb)
self.body.rs += tf.scalar_mul(dtime, self.body.vs)
# Q to quaternion
'''
qw = tf.scalar_mul(0.5, tf.sqrt(tf.reduce_sum(tf.diag_part(self.body.Q))+1.))
qv = tf.reduce_sum(tf.cross(self.body.Q, tf.eye(3, dtype = tf.float32)), axis = 0)/tf.scalar_mul(4., qw)
# quaternion normalization
qvsquare = tf.reduce_sum(tf.square(qv))
qnorm = tf.sqrt(tf.square(qw)+qvsquare)
qw /= qnorm
qv /= qnorm
# quaternion to Q
self.body.Q = tf.scalar_mul(qw*qw-qvsquare,tf.eye(3, dtype = tf.float32))\
+ 2 * tf.matmul(tf.reshape(qv, [3, 1]), tf.reshape(qv, [1, 3]))\
- 2 * qw * tf.cross(tf.tile(tf.reshape(qv, [1,3]), [3,1]), tf.eye(3, dtype = tf.float32))
'''
return llist, Flist, asb, Qs, [self.body.rs] + [
x + self.body.rs for x in tot_lbtomots
] #, qnorm, qw
def draw_rendering_net(self,
input_params,
position,
rotate_theta,
variable_scope_name,
with_cos=True,
pd_ps_wanted="both"):
'''
input_params = (rendering parameters) shape = [self.fitting_batch_size,self.parameter_len] i.e.[24576,10]
position = (rendering positions) shape=[self.fitting_batch_size,3]
variable_scope_name = (for variable check a string like"rendering1")
rotate_theta = [self.fitting_batch_size,1]
return shape = (rendered results)[batch,lightnum,1] or [batch,lightnum,3]
with_cos: if True,lumitexl is computed with cos and dir
'''
with tf.variable_scope(variable_scope_name):
###[STEP 0]
#load constants
view_mat_for_normal_t, view_mat_model_t, light_normals, light_poses, cam_pos = self.__net_load_constants(
variable_scope_name)
#rotate object
rotate_axis = tf.constant(
np.repeat(np.array([0, 0, 1], np.float32).reshape([1, 3]),
self.fitting_batch_size,
axis=0))
view_mat_model = self.rotation_axis(rotate_theta,
rotate_axis) #[batch,4,4]
self.endPoints[variable_scope_name +
"view_mat_model"] = view_mat_model
view_mat_model_t = tf.matrix_transpose(view_mat_model)
view_mat_for_normal = tf.matrix_transpose(
tf.matrix_inverse(view_mat_model))
self.endPoints[variable_scope_name +
"view_mat_for_normal"] = view_mat_for_normal
view_mat_for_normal_t = tf.matrix_transpose(view_mat_for_normal)
# self.endPoints[variable_scope_name+"view_mat_for_normal_t"] = view_mat_for_normal
###[STEP 1]
##define input
with tf.variable_scope("fittinger"):
self.endPoints[variable_scope_name +
"input_parameters"] = input_params
self.endPoints[variable_scope_name + "positions"] = position
view_dir = cam_pos - position #shape=[batch,3]
view_dir = self.normalized(view_dir) #shape=[batch,3]
self.endPoints[variable_scope_name + "view_dir"] = view_dir
#build local frame
frame_t, frame_b = self.build_frame_f_z(
view_dir, None, with_theta=False) #[batch,3]
frame_n = view_dir
self.endPoints[variable_scope_name + "frame_t"] = frame_t
self.endPoints[variable_scope_name + "frame_b"] = frame_b
self.endPoints[variable_scope_name + "frame_n"] = frame_n
###[STEP 1.1]
###split input parameters into position and others
if self.if_grey_scale:
n_2d, theta, ax, ay, pd, ps = tf.split(input_params,
[2, 1, 1, 1, 1, 1],
axis=1)
self.endPoints[variable_scope_name + "pd"] = pd
pd = tf.tile(pd, [1, 3])
ps = tf.tile(ps, [1, 3])
else:
n_local, t_local, ax, ay, pd, ps = tf.split(input_params,
[2, 1, 1, 1, 3, 3],
axis=1)
#position shape=[bach,3]
# n_2d = tf.clip_by_value(n_2d,0.0,1.0)
n_local = self.back_hemi_octa_map(n_2d) #[batch,3]
self.endPoints[variable_scope_name + "normal_local"] = n_local
t_local, _ = self.build_frame_f_z(n_local, theta, with_theta=True)
n_local_x, n_local_y, n_local_z = tf.split(
n_local, [1, 1, 1], axis=1) #[batch,1],[batch,1],[batch,1]
n = n_local_x * frame_t + n_local_y * frame_b + n_local_z * frame_n #[batch,3]
self.endPoints[variable_scope_name + "normal"] = n
t_local_x, t_local_y, t_local_z = tf.split(
t_local, [1, 1, 1], axis=1) #[batch,1],[batch,1],[batch,1]
t = t_local_x * frame_t + t_local_y * frame_b + t_local_z * frame_n #[batch,3]
b = tf.cross(n, t) #[batch,3]
#rotate frame
pn = tf.expand_dims(tf.concat(
[n, tf.ones([n.shape[0], 1], tf.float32)], axis=1),
axis=1)
pt = tf.expand_dims(tf.concat(
[t, tf.ones([t.shape[0], 1], tf.float32)], axis=1),
axis=1)
pb = tf.expand_dims(tf.concat(
[b, tf.ones([b.shape[0], 1], tf.float32)], axis=1),
axis=1)
n = tf.squeeze(tf.matmul(pn, view_mat_for_normal_t), axis=1)
t = tf.squeeze(tf.matmul(pt, view_mat_for_normal_t), axis=1)
b = tf.squeeze(tf.matmul(pb, view_mat_for_normal_t), axis=1)
n, _ = tf.split(n, [3, 1], axis=1) #shape=[batch,3]
t, _ = tf.split(t, [3, 1], axis=1) #shape=[batch,3]
b, _ = tf.split(b, [3, 1], axis=1) #shape=[batch,3]
self.endPoints[variable_scope_name + "n"] = n
self.endPoints[variable_scope_name + "t"] = t
self.endPoints[variable_scope_name + "b"] = b
# n = tf.tile(tf.constant([0,0,1],dtype=tf.float32,shape=[1,3]),[self.fitting_batch_size,1])#self.normalized(n)
# t = tf.tile(tf.constant([0,1,0],dtype=tf.float32,shape=[1,3]),[self.fitting_batch_size,1])#self.normalized(t)
# ax = tf.clip_by_value(ax,0.006,0.503)
# ay = tf.clip_by_value(ay,0.006,0.503)
# pd = tf.clip_by_value(pd,0,1)
# ps = tf.clip_by_value(ps,0,10)
#regularizer of params
# regular_loss_n = self.regularizer_relu(n_2d,1e-3,1.0)+self.regularizer_relu(theta,0.0,math.pi)+self.regularizer_relu(ax,0.006,0.503)+self.regularizer_relu(ay,0.006,0.503)+self.regularizer_relu(pd,1e-3,1)+self.regularizer_relu(ps,1e-3,10)
# self.endPoints["regular_loss"] = regular_loss_n
self.endPoints[variable_scope_name + "render_params"] = tf.concat(
[n, t, b, ax, ay, pd, ps], axis=-1)
###[STEP 2]
##define rendering
with tf.variable_scope("rendering"):
self.endPoints[variable_scope_name +
"position_origin"] = position
position = tf.expand_dims(tf.concat(
[position,
tf.ones([position.shape[0], 1], tf.float32)],
axis=1),
axis=1)
position = tf.squeeze(tf.matmul(position, view_mat_model_t),
axis=1) #position@view_mat_model_t
position, _ = tf.split(position, [3, 1],
axis=1) #shape=[batch,3]
self.endPoints[variable_scope_name +
"position_rotated"] = position
#get real view dir
view_dir = cam_pos - position #shape=[batch,3]
view_dir = self.normalized(view_dir) #shape=[batch,3]
self.endPoints[variable_scope_name +
"view_dir_rotated"] = view_dir
light_poses_broaded = tf.tile(
tf.expand_dims(light_poses,
axis=0), [self.fitting_batch_size, 1, 1],
name="expand_light_poses") #shape is [batch,lightnum,3]
light_normals_broaded = tf.tile(
tf.expand_dims(light_normals,
axis=0), [self.fitting_batch_size, 1, 1],
name="expand_light_normals") #shape is [batch,lightnum,3]
position_broded = tf.tile(tf.expand_dims(position, axis=1),
[1, self.lumitexel_size, 1],
name="expand_position")
wi = light_poses_broaded - position_broded
wi = self.normalized_nd(wi) #shape is [batch,lightnum,3]
self.endPoints[variable_scope_name + "wi"] = wi
wi_local = tf.concat([
self.dot_ndm_vector(wi, tf.expand_dims(t, axis=1)),
self.dot_ndm_vector(wi, tf.expand_dims(b, axis=1)),
self.dot_ndm_vector(wi, tf.expand_dims(n, axis=1))
],
axis=-1) #shape is [batch,lightnum,3]
view_dir_broaded = tf.tile(
tf.expand_dims(view_dir, axis=1),
[1, self.lumitexel_size, 1]) #shape is [batch,lightnum,3]
wo_local = tf.concat([
self.dot_ndm_vector(view_dir_broaded,
tf.expand_dims(t, axis=1)),
self.dot_ndm_vector(view_dir_broaded,
tf.expand_dims(b, axis=1)),
self.dot_ndm_vector(view_dir_broaded,
tf.expand_dims(n, axis=1))
],
axis=-1) #shape is [batch,lightnum,3]
self.endPoints[variable_scope_name + "wi_local"] = wo_local
form_factors = self.compute_form_factors(
position, n, light_poses_broaded, light_normals_broaded,
variable_scope_name, with_cos) #[batch,lightnum,1]
self.endPoints[variable_scope_name +
"form_factors"] = form_factors
lumi = self.calc_light_brdf(wi_local, wo_local, ax, ay, pd, ps,
pd_ps_wanted) #[batch,lightnum,3]
self.endPoints[variable_scope_name +
"lumi_without_formfactor"] = lumi
lumi = lumi * form_factors * 1e4 * math.pi * 1e-2 #[batch,lightnum,3]
if self.if_grey_scale:
lumi = tf.reduce_mean(
lumi, axis=2, keepdims=True
) #[batch,lightnum,1] for greyscale#TODO should be depended by if is grey
wi_dot_n = self.dot_ndm_vector(wi, tf.expand_dims(
n, axis=1)) #[batch,lightnum,1]
lumi = lumi * ((tf.sign(wi_dot_n) + 1.0) * 0.5)
# judgements = tf.less(wi_dot_n,1e-5)
# lumi = tf.where(judgements,tf.zeros([self.fitting_batch_size,self.lumitexel_size,1]),lumi)
n_dot_view_dir = self.dot_ndm_vector(
view_dir_broaded,
tf.tile(tf.expand_dims(n, axis=1),
[1, self.lumitexel_size, 1])) #[batch,lightnum,1]
n_dot_views = tf.gather(n_dot_view_dir, 0, axis=1)
self.endPoints[variable_scope_name +
"n_dot_view_dir"] = n_dot_views
n_dot_view_penalty = self.regularizer_relu(
n_dot_views, 1e-6, 1.0)
self.endPoints[variable_scope_name +
"n_dot_view_penalty"] = n_dot_view_penalty
judgements = tf.less(n_dot_view_dir, 0.0)
if self.if_grey_scale:
rendered_results = tf.where(judgements,
tf.zeros([
self.fitting_batch_size,
self.lumitexel_size, 1
]), lumi) #[batch,lightnum]
else:
rendered_results = tf.where(tf.tile(judgements, [1, 1, 3]),
tf.zeros([
self.fitting_batch_size,
self.lumitexel_size, 3
]), lumi) #[batch,lightnum]
self.endPoints[variable_scope_name +
"rendered_results"] = rendered_results
return rendered_results
def calculateDistance(self, pA, pB, pC, pD):
vAB = pB - pA # u
vCD = pD - pC # v
vCA = pA - pC # r
if tf.tensordot(tf.cross(vAB, vCD), vCA, axes=1) != 0: # check determination in 3-dimension or planar
a = tf.tensordot( vAB, vAB, axes=1)
b = tf.tensordot(-vAB, vCD, axes=1)
c = tf.tensordot( vCD, vCD, axes=1)
d = tf.tensordot( vAB, vCA, axes=1)
e = tf.tensordot(-vCD, vCA, axes=1)
f = tf.tensordot( vCA, vCA, axes=1)
s = (b*e-c*d)/(a*c-b*b) # parameter in vector A (vector M, L1, AB, i-th robot link)
t = (b*d-a*e)/(a*c-b*b) # parameter in vector B (vector N, L2, CD, j-th line obstacle)
if (s < 0) and (t < 0): # AC
distance = tf.linalg.norm(vCA)
elif (s < 0) and (0 <= t) and (t <= 1): # AC + tCD
distance = tf.linalg.norm(-vCA + t*vCD)
elif (s < 0) and (1 < t): # AD
distance = tf.linalg.norm(pD-pA)
elif (0 <= s) and (s <= 1) and (t < 0): # CA + sAB
distance = tf.linalg.norm(vCA + s*vAB)
elif (0 <= s) and (s <= 1) and (0 <= t) and (t <= 1):
distance = tf.sqrt(a*s**2 + 2*b*s*t + c*t**2 + 2*d*s + 2*e*t + f)
elif (0 <= s) and (s <= 1) and (1 < t): # DA + sAB
distance = tf.linalg.norm(pA-pD + s*vAB)
elif (1 < s) and (t < 0): # BC
distance = tf.linalg.norm(pC-pB)
elif (1 < s) and (0 <= t) and (t <= 1): # BC + tCD
distance = tf.linalg.norm(pC-pB + t*vCD)
elif (1 < s) and (1 < t): # BD
distance = tf.linalg.norm(pD-pB)
else:
vAC = pC - pA
vAD = pD - pA
vCB = pB - pC
s1 = tf.tensordot(vAC, vAB, axes=1)/(tf.linalg.norm(vAB)*tf.linalg.norm(vAB))
s2 = tf.tensordot(vAD, vAB, axes=1)/(tf.linalg.norm(vAB)*tf.linalg.norm(vAB))
t1 = tf.tensordot(vCA, vCD, axes=1)/(tf.linalg.norm(vCD)*tf.linalg.norm(vCD))
t2 = tf.tensordot(vCB, vCD, axes=1)/(tf.linalg.norm(vCD)*tf.linalg.norm(vCD))
distance_planar = tf.constant([999, 999, 999, 999]) # s1 s2 t1 t2
distance_Temp = 999
if (0 <= s1) and (s1 <= 1):
distance_planar[0] = tf.linalg.norm(vCA + s1*vAB) # CA + s1*AB
if (0 <= s2) and (s2 <= 1):
distance_planar[1] = tf.linalg.norm(-vAD + s2*vAB) # DA + s2*AB
if (0 <= t1) and (t1 <= 1):
distance_planar[2] = tf.linalg.norm(vAC + t1*vCD) # AC + t1*CD
if (0 <= t2) and (t2 <= 1):
distance_planar[3] = tf.linalg.norm(pC-pB + t2*vCD) # BC + t2*CD
if (s1 < 0) and (s2 < 0) and (t1 < 0) and (t2 < 0): # AC
distance_planar[0] = tf.linalg.norm(vAC)
elif (s1 > 1) and (s2 > 1) and (t1 < 0) and (t2 < 0): # BC
distance_planar[1] = tf.linalg.norm(pC - pB)
elif (s1 < 0) and (s2 < 0) and (t1 > 1) and (t2 > 1): # AD
distance_planar[2] = tf.linalg.norm(vAD)
elif (s1 > 1) and (s2 > 1) and (t1 > 1) and (t2 > 1): # BD
distance_planar[3] = tf.linalg.norm(pD - pB)
for cnt in range(1,4):
if distance_Temp > distance_planar[cnt]:
distance_Temp = distance_planar[cnt]
# cnt_Temp = cnt
distance = distance_Temp
return distance
def cross(*args, **kwargs):
""" See https://www.tensorflow.org/api_docs/python/tf/cross .
"""
return tensorflow.cross(*args, **kwargs)
def get_ver_norm_bk(ver_xyz, tri):
"""
Compute vertex normals and vertex contour mask(for 2d lmk loss).
:param:
ver_xyz: [batch, N, 3], vertex geometry
tri: [M, 3], mesh triangles definition
:return:
ver_normals: [batch, N, 3], vertex normals
ver_contour_mask: [batch, N, 1], vertex contour mask, indicating whether the vertex is on the contour
"""
n_tri = tri.get_shape().as_list()[0]
v1_idx, v2_idx, v3_idx = tf.unstack(tri, 3, axis=-1)
v1 = tf.gather(ver_xyz, v1_idx, axis=1, name="v1_tri")
v2 = tf.gather(ver_xyz, v2_idx, axis=1, name="v2_tri")
v3 = tf.gather(ver_xyz, v3_idx, axis=1, name="v3_tri")
EPS = 1e-8
tri_normals = tf.cross(v2 - v1, v3 - v1)
#tri_normals = tf.div(
# tri_normals,
# (tf.norm(tri_normals, axis=-1, keep_dims=True) + EPS),
# name="norm_tri",
#)
#tri_normals = tf.nn.l2_normalize(tri_normals, dim=-1)
tmp = tf.tile(tf.expand_dims(tri_normals, 2), [1, 1, 3, 1],
name="tri_normals_tile") # per vertex attribute
# per_vertex: information for each vertex in triangle
tri_normals_per_vertex = tf.reshape(tmp, [-1, 3],
name="tri_normals_reshape")
tri_visible_per_vertex = tf.cast(
tf.greater(tri_normals_per_vertex[:, 2:], float(EPS)), tf.float32)
tri_one_per_vertex = tf.ones_like(tri_visible_per_vertex,
name="tri_cnts")
B = v1.get_shape().as_list()[0] # batch size
batch_indices = tf.reshape(
tf.tile(tf.expand_dims(tf.range(B), axis=1), [1, n_tri * 3]),
[-1],
name="batch_indices",
)
tri_inds = tf.stack(
[
batch_indices,
tf.concat([tf.reshape(tri, [n_tri * 3])] * B, axis=0)
],
axis=1,
name="tri_inds",
)
ver_shape = ver_xyz.get_shape().as_list()
ver_normals = tf.get_variable(
shape=ver_shape,
dtype=tf.float32,
initializer=tf.zeros_initializer(),
name="ver_norm",
trainable=False,
)
# refresh normal per iteration
init_normals = tf.zeros(shape=ver_shape,
dtype=tf.float32,
name="init_normals")
assign_op = tf.assign(ver_normals,
init_normals,
name="ver_normal_assign")
with tf.control_dependencies([assign_op]):
ver_normals = tf.scatter_nd_add(ver_normals,
tri_inds,
tri_normals_per_vertex,
name="ver_normal_scatter")
#ver_normals = ver_normals / (
# tf.norm(ver_normals, axis=2, keep_dims=True, name="ver_normal_norm")
# + EPS
#)
ver_normals = tf.nn.l2_normalize(ver_normals, dim=-1)
ver_visible = tf.get_variable(
shape=ver_shape[:-1] + [1],
dtype=tf.float32,
initializer=tf.zeros_initializer(),
name="ver_vote",
trainable=False,
)
ver_tri_cnt = tf.get_variable(
shape=ver_shape[:-1] + [1],
dtype=tf.float32,
initializer=tf.zeros_initializer(),
name="ver_cnt",
trainable=False,
)
init_values = tf.zeros(shape=ver_shape[:-1] + [1],
dtype=tf.float32,
name="init_votes")
# find the visible boundary
assign_op2 = tf.assign(ver_visible,
init_values,
name="ver_votes_assign")
assign_op3 = tf.assign(ver_tri_cnt,
init_values,
name="ver_cnts_assign")
with tf.control_dependencies([assign_op2, assign_op3]):
ver_visible = tf.scatter_nd_add(ver_visible,
tri_inds,
tri_visible_per_vertex,
name="ver_vote_scatter")
ver_tri_cnt = tf.scatter_nd_add(ver_tri_cnt,
tri_inds,
tri_one_per_vertex,
name="ver_cnt_scatter")
ver_visible_ratio = ver_visible / (ver_tri_cnt + EPS)
cond1 = tf.less(ver_visible_ratio, float(1.0))
cond2 = tf.greater(ver_visible_ratio, float(0.0))
ver_contour_mask = tf.cast(
tf.logical_and(cond1, cond2),
tf.float32,
name="ver_votes_final",
)
return ver_normals, ver_contour_mask
def cross(a, b):
return tf.cross(a, b)
def TriNormalsScaled(v, f):
edge_vec1 = tf.reshape(TriEdges(v, f, 1, 0), (-1, 3))
edge_vec2 = tf.reshape(TriEdges(v, f, 2, 0), (-1, 3))
return tf.cross(edge_vec1, edge_vec2)
def tf_prior_get_normal(u, v, w):
get_wu = lambda: tf.cross(w, u)
get_vw = lambda: tf.cross(v, w)
cond = tf.logical_and(tf.norm(w - v) < eps, tf.norm(w + v) < eps)
normal = tf.cond(cond, get_wu, get_vw)
return tf_unit_norm(normal)
def test_Cross(self):
t = tf.cross(*self.random((4, 3), (4, 3)))
self.check(t)
def step_graph_construct(self, Jinv_=None, observation_provided=False):
# import tensorflow as tf
self.observation_provided = observation_provided
with tf.variable_scope('MellingerControl'):
if not observation_provided:
#Here we will provide all components independently
self.xyz_tf = tf.placeholder(name='xyz',
dtype=tf.float32,
shape=(None, 3))
self.Vxyz_tf = tf.placeholder(name='Vxyz',
dtype=tf.float32,
shape=(None, 3))
self.Omega_tf = tf.placeholder(name='Omega',
dtype=tf.float32,
shape=(None, 3))
self.R_tf = tf.placeholder(name='R',
dtype=tf.float32,
shape=(None, 3, 3))
else:
#Here we will provide observations directly and split them
self.observation = tf.placeholder(name='obs',
dtype=tf.float32,
shape=(None, 3 + 3 + 9 + 3))
self.xyz_tf, self.Vxyz_tf, self.R_flat, self.Omega_tf = tf.split(
self.observation, [3, 3, 9, 3], axis=1)
self.R_tf = tf.reshape(self.R_flat, shape=[-1, 3, 3], name='R')
R = self.R_tf
# R_flat = tf.placeholder(name='R_flat', type=tf.float32, shape=(None, 9))
# R = tf.reshape(R_flat, shape=(-1, 3, 3), name='R')
#GOAL = [x,y,z, Vx, Vy, Vz]
self.goal_xyz_tf = tf.placeholder(name='goal_xyz',
dtype=tf.float32,
shape=(None, 3))
# goal_Vxyz = tf.placeholder(name='goal_Vxyz', type=tf.float32, shape=(None, 3))
# Learnable gains with static initialization
kp_p = tf.get_variable('kp_p',
shape=[],
initializer=tf.constant_initializer(4.5),
trainable=True) # 4.5
kd_p = tf.get_variable('kd_p',
shape=[],
initializer=tf.constant_initializer(3.5),
trainable=True) # 3.5
kp_a = tf.get_variable('kp_a',
shape=[],
initializer=tf.constant_initializer(200.0),
trainable=True) # 200.
kd_a = tf.get_variable('kd_a',
shape=[],
initializer=tf.constant_initializer(50.0),
trainable=True) # 50.
## IN case you want to optimize them from random values
# kp_p = tf.get_variable('kp_p', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=10.0), trainable=True) # 4.5
# kd_p = tf.get_variable('kd_p', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=10.0), trainable=True) # 3.5
# kp_a = tf.get_variable('kp_a', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=100.0), trainable=True) # 200.
# kd_a = tf.get_variable('kd_a', initializer=tf.random_uniform(shape=[1], minval=0.0, maxval=100.0), trainable=True) # 50.
to_goal = self.goal_xyz_tf - self.xyz_tf
e_p = -tf.clip_by_norm(to_goal, 4.0, name='e_p')
e_v = self.Vxyz_tf
acc_des = -kp_p * e_p - kd_p * e_v + tf.constant([0, 0, 9.81],
name='GRAV')
print('acc_des shape: ', acc_des.get_shape().as_list())
def project_xy(x, name='project_xy'):
# print('x_shape:', x.get_shape().as_list())
# x = tf.squeeze(x, axis=2)
return tf.multiply(x, tf.constant([1., 1., 0.]), name=name)
# goal_dist = tf.norm(to_goal, name='goal_xyz_dist')
xc_des = project_xy(tf.squeeze(tf.slice(R,
begin=[0, 0, 2],
size=[-1, 3, 1]),
axis=2),
name='xc_des')
print('xc_des shape: ', xc_des.get_shape().as_list())
# xc_des = project_xy(R[:, 0])
# rotation towards the ideal thrust direction
# see Mellinger and Kumar 2011
zb_des = tf.nn.l2_normalize(acc_des, axis=1, name='zb_dex')
yb_des = tf.nn.l2_normalize(tf.cross(zb_des, xc_des),
axis=1,
name='yb_des')
xb_des = tf.cross(yb_des, zb_des, name='xb_des')
R_des = tf.stack([xb_des, yb_des, zb_des], axis=2, name='R_des')
print('zb_des shape: ', zb_des.get_shape().as_list())
print('yb_des shape: ', yb_des.get_shape().as_list())
print('xb_des shape: ', xb_des.get_shape().as_list())
print('R_des shape: ', R_des.get_shape().as_list())
def transpose(x):
return tf.transpose(x, perm=[0, 2, 1])
# Rotational difference
Rdiff = tf.matmul(transpose(R_des), R) - tf.matmul(
transpose(R), R_des, name='Rdiff')
print('Rdiff shape: ', Rdiff.get_shape().as_list())
def tf_vee(R, name='vee'):
return tf.squeeze(tf.stack([
tf.squeeze(tf.slice(R, [0, 2, 1], [-1, 1, 1]), axis=2),
tf.squeeze(tf.slice(R, [0, 0, 2], [-1, 1, 1]), axis=2),
tf.squeeze(tf.slice(R, [0, 1, 0], [-1, 1, 1]), axis=2)
],
axis=1,
name=name),
axis=2)
# def vee(R):
# return np.array([R[2, 1], R[0, 2], R[1, 0]])
e_R = 0.5 * tf_vee(Rdiff, name='e_R')
print('e_R shape: ', e_R.get_shape().as_list())
# e_R[2] *= 0.2 # slow down yaw dynamics
e_w = self.Omega_tf
# Control orientation
dw_des = -kp_a * e_R - kd_a * e_w
print('dw_des shape: ', dw_des.get_shape().as_list())
# we want this acceleration, but we can only accelerate in one direction!
# thrust_mag = np.dot(acc_des, R[:, 2])
acc_cur = tf.squeeze(tf.slice(R, begin=[0, 0, 2], size=[-1, 3, 1]),
axis=2)
print('acc_cur shape: ', acc_cur.get_shape().as_list())
acc_dot = tf.multiply(acc_des, acc_cur)
print('acc_dot shape: ', acc_dot.get_shape().as_list())
thrust_mag = tf.reduce_sum(acc_dot,
axis=1,
keepdims=True,
name='thrust_mag')
print('thrust_mag shape: ', thrust_mag.get_shape().as_list())
# des = np.append(thrust_mag, dw_des)
des = tf.concat([thrust_mag, dw_des], axis=1, name='des')
print('des shape: ', des.get_shape().as_list())
if Jinv_ is None:
# Learn the jacobian inverse
Jinv = tf.get_variable('Jinv',
initializer=tf.random_normal(
shape=[4, 4], mean=0.0, stddev=0.1),
trainable=True)
else:
# Jacobian inverse is provided
Jinv = tf.constant(Jinv_.astype(np.float32), name='Jinv')
# Jinv = tf.get_variable('Jinv', shape=[4,4], initializer=tf.constant_initializer())
print('Jinv shape: ', Jinv.get_shape().as_list())
## Jacobian inverse for our quadrotor
# Jinv = np.array([[0.0509684, 0.0043685, -0.0043685, 0.02038736],
# [0.0509684, -0.0043685, -0.0043685, -0.02038736],
# [0.0509684, -0.0043685, 0.0043685, 0.02038736],
# [0.0509684, 0.0043685, 0.0043685, -0.02038736]])
# thrusts = np.matmul(self.Jinv, des)
thrusts = tf.matmul(des, tf.transpose(Jinv), name='thrust')
thrusts = tf.clip_by_value(thrusts,
clip_value_min=0.0,
clip_value_max=1.0,
name='thrust_clipped')
return thrusts
from tensorflow.python.framework import ops
ops.reset_default_graph()
# Open graph session
sess = tf.Session()
# div() vs truediv() vs floordiv()
print(sess.run(tf.div(3,4)))
print(sess.run(tf.truediv(3,4)))
print(sess.run(tf.floordiv(3.0,4.0)))
# Mod function
print(sess.run(tf.mod(22.0,5.0)))
# Cross Product
print(sess.run(tf.cross([1.,0.,0.],[0.,1.,0.])))
# Trig functions
print(sess.run(tf.sin(3.1416)))
print(sess.run(tf.cos(3.1416)))
# Tangemt
print(sess.run(tf.div(tf.sin(3.1416/4.), tf.cos(3.1416/4.))))
# Custom operation
test_nums = range(15)
#from tensorflow.python.ops import math_ops
#print(sess.run(tf.equal(test_num, 3)))
def custom_polynomial(x_val):
# Return 3x^2 - x + 10
return(tf.sub(3 * tf.square(x_val), x_val) + 10)
def getRotInvPatches(x, adj):
# The trick here is to compute a rotation from any face normal to a fixed axis (say, (0,0,1))
# We follow the procedure described here: https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d#476311
# Using the Levi-Civita tensor, as explained here https://math.stackexchange.com/questions/258775/from-a-vector-to-a-skew-symmetric-matrix
batch_size, num_points, in_channels = x.get_shape().as_list()
batch_size, num_points, K = adj.get_shape().as_list()
# Position
xp = tf.slice(x, [0, 0, 3], [-1, -1, -1])
# Normal
xn = tf.slice(x, [0, 0, 0], [-1, -1, 3])
ref_axis_t = tf.reshape(ref_axis, [1, 1, 3])
# [batch, N, 3]
#ref_axes = tf.tile(ref_axes,[batch_size,num_points,1])
#ref_axes = broadcast(ref_axes,x.shape)
ref_axes = tf.zeros_like(xn)
ref_axes = ref_axes + ref_axis_t
# [batch, N, 3]
ref_cross = tf.cross(xn, ref_axes)
# [batch, N, 1]
ref_sin = tf.norm(ref_cross)
# [batch, N, 1]
ref_cos = tensorDotProduct(ref_axis, xn)
# [batch, N, 3, 1]
ref_cross = tf.expand_dims(ref_cross, -1)
# [batch, N, 3, 3, 1]
ref_cross = tf.tile(tf.expand_dims(ref_cross, 2), [1, 1, 3, 1, 1])
# [1, 1, 3, 3, 3]
LC = tf.reshape(LC_tensor, [1, 1, 3, 3, 3])
# [batch, N, 3, 3, 1]
temp_zero = tf.zeros_like(ref_cross)
# [batch, N, 3, 3, 3]
temp_zero = tf.tile(temp_zero, [1, 1, 1, 1, 3])
# [batch, N, 3, 3, 3]
LC = LC + temp_zero
#LC = tf.tile(LC,[batch_size,num_points,1,1,1])
# [batch, N, 3, 3, 1]
ssm = tf.matmul(LC, ref_cross)
# [batch, N, 3, 3]
ssm = tf.squeeze(ssm)
# [batch, N, 1]
rot_coef = tf.divide(tf.subtract(1.0, ref_cos),
tf.multiply(ref_sin, ref_sin))
# [batch, N, 3, 3]
rot_coef = tf.tile(tf.reshape(rot_coef, [batch_size, -1, 1, 1]),
[1, 1, 3, 3])
# [1, 1, 3, 3]
Idmat = tf.reshape(Id_tensor, [1, 1, 3, 3])
# [batch, N, 3, 3]
Idmat = Idmat + tf.zeros_like(rot_coef)
#Idmat = tf.tile(Idmat,[batch_size,num_points,1,1])
# [batch, N, 3, 3]
rot = Idmat + ssm + tf.multiply(tf.matmul(ssm, ssm), rot_coef)
# [batch, N, K, 3, 3]
rot = tf.tile(tf.expand_dims(rot, axis=2), [1, 1, K, 1, 1])
# rot gives a (3,3) rotation matrix for every face
# [batch, N, K, ch]
patches = get_patches(x, adj)
# Normals part of patches
npatches = tf.slice(patches, [0, 0, 0, 0], [-1, -1, -1, 3])
# [batch, N, K, ch, 1]
npatches = tf.expand_dims(npatches, -1)
# [batch, N, K, ch, 1]
npatches = tf.matmul(rot, npatches)
# [batch, N, K, ch]
npatches = tf.reshape(npatches, [batch_size, -1, K, 3])
# Position part of patches
# [batch, N, K, 3]
ppatches = tf.slice(patches, [0, 0, 0, 3], [-1, -1, -1, -1])
# Compute displacement to current face
ppatches = tf.subtract(ppatches, tf.expand_dims(xp, axis=2))
# Rotate, just like the normals
ppatches = tf.expand_dims(ppatches, -1)
ppatches = tf.matmul(rot, ppatches)
ppatches = tf.reshape(ppatches, [batch_size, -1, K, 3])
# [batch, N, K, ch (6)]
patches = tf.concat([npatches, ppatches], axis=-1)
return patches
