def dalk_path_generation(path_filename, calib_filename):
data = np.loadtxt(path_filename)
oct2robot = np.loadtxt(calib_filename)[0, :].reshape((4, 4))
robot2oct = np.linalg.inv(oct2robot)
robot_target = []
xyzrpy = []
# dalk_frame = (so3.from_rpy([0, 0, -np.pi/2]), [52.5 * 0.0254, 6.5 * 0.0251, 0.75 * 0.0254])
dalk_frame = (so3.from_rpy([0, np.pi, 0]), [0.0, 0.0, 0.0])
for row in data[::1]:
x_target = row[12 + 6:12 + 6 + 6]
# xyzrpy.append(x_target)
T = np.eye(4)
T = rotation_matrix(x_target[3], [1, 0, 0]).dot(
rotation_matrix(x_target[4], [0, 1, 0])).dot(
rotation_matrix(x_target[5], [0, 0, 1]))
T[:3, 3] = x_target[0:3]
T = robot2oct.dot(T)
T[:3, 3] = T[:3, 3] * 1
T = oct2robot.dot(T)
robot_target.append(T)
T2rpyxyz = list(se3.from_homogeneous(T)[1]) + list(
so3.rpy(se3.from_homogeneous(T)[0]))
print(T2rpyxyz)
xyzrpy.append(T2rpyxyz)
# T_m = se3.mul(se3.inv(dalk_frame), se3.from_homogeneous(T))
# T_h = se3.homogeneous(T_m)
# T2rpyxyz = list(T_m[1]) + list(so3.rpy(T_m[0]))
# xyzrpy.append(T2rpyxyz)
# robot_target.append(T_h)
return robot_target, xyzrpy
def createAnimationTrack(self, jointsOrder=None, name="BVHMotion"):
"""
Create an animation track from the motion stored in this BHV file.
"""
if jointsOrder == None:
jointsData = [
joint.matrixPoses for joint in self.getJoints()
if not joint.isEndConnector()
]
# We leave out end effectors as they should not have animation data
else:
nFrames = self.frameCount
import re
# Remove the tail from duplicate bone names
for idx, jName in enumerate(jointsOrder):
# Joint mappings can contain a rotation compensation
if isinstance(jName, tuple):
jName, _ = jName
if not jName:
continue
r = re.search("(.*)_\d+$", jName)
if r:
jointsOrder[idx] = r.group(1)
jointsData = []
for jointName in jointsOrder:
if isinstance(jointName, tuple):
jointName, angle = jointName
else:
angle = 0.0
if jointName:
poseMats = self.getJointByCanonicalName(
jointName).matrixPoses.copy()
if isinstance(angle, float):
if angle != 0.0:
# Rotate around global Z axis
rot = tm.rotation_matrix(-angle * D, [0, 0, 1])
# Roll around global Y axis (this is a limitation)
roll = tm.rotation_matrix(angle * D, [0, 1, 0])
for i in xrange(nFrames):
# TODO make into numpy loop
poseMats[i] = np.dot(poseMats[i], rot)
poseMats[i] = np.dot(poseMats[i], roll)
else: # Compensation (angle) is a transformation matrix
# Compensate animation frames
for i in xrange(nFrames):
# TODO make into numpy loop
poseMats[i] = np.mat(poseMats[i]) * np.mat(angle)
#poseMats[i] = np.mat(angle) # Test compensated rest pose
jointsData.append(poseMats)
else:
jointsData.append(animation.emptyTrack(nFrames))
nJoints = len(jointsData)
nFrames = len(jointsData[0])
# Interweave joints animation data, per frame with joints in breadth-first order
animData = np.hstack(jointsData).reshape(nJoints * nFrames, 4, 4)
framerate = 1.0 / self.frameTime
return animation.AnimationTrack(name, animData, nFrames, framerate)
def rule_branch(self): ''' @brief Create a branch ''' # Translate frame = self.current_node.frame frame = np.dot(frame, tr.translation_matrix([0, 0, self.current_node.size])) # Add noise if self.noise != 0: self.current_phi_angle += random.uniform(-self.noise, self.noise) self.current_rho_angle += random.uniform(-self.noise, self.noise) # Phi Rotation phi_matrix = tr.rotation_matrix(self.current_phi_angle, [0, 0, 1]) frame[:3,:3] = np.dot(frame[:3,:3], phi_matrix[:3,:3]) # Rho rotation rho_matrix = tr.rotation_matrix(self.current_rho_angle, [1, 0, 0]) frame[:3,:3] = np.dot(frame[:3,:3], rho_matrix[:3,:3]) # Create a new node node = Node(frame, self.current_node.size * self.scale_factor, self.current_node.diameter * self.current_subdivision_ratio * self.scale_factor) self.current_node.add_node(node) self.current_node = node self.current_rho_angle = 0 self.current_phi_angle = 0 self.current_subdivision_ratio = 1
def getMatrix(head, tail, roll):
"""
Calculate an orientation (rest) matrix for a bone between specified head
and tail positions with given bone roll angle.
Returns length of the bone and rest orientation matrix in global coordinates.
"""
vector = toZisUp3(tail - head)
length = math.sqrt(np.dot(vector, vector))
if length == 0:
vector = [0,0,1]
else:
vector = vector/length
yproj = np.dot(vector, YUnit)
if yproj > 1-1e-6:
axis = YUnit
angle = 0
elif yproj < -1+1e-6:
axis = YUnit
angle = pi
else:
axis = np.cross(YUnit, vector)
axis = axis / math.sqrt(np.dot(axis,axis))
angle = math.acos(yproj)
mat = tm.rotation_matrix(angle, axis)
if roll:
mat = np.dot(mat, tm.rotation_matrix(roll, YUnit))
mat = fromZisUp4(mat)
mat[:3,3] = head
return length, mat
def setup_particle_orientation(invert, theta, phi):
# spin about z-axis vector to create phi spin
phi_mat = transformations.rotation_matrix(phi + (pi if invert else 0), vz)
# rotate z-x axis to create orientation vector
axis_rotation = transformations.rotation_matrix((-1 if invert else 1) * -(theta - pi/2), vy)
return transformations.concatenate_matrices(axis_rotation, phi_mat)
def set_block(self, s, d):
aoy = m.atan2(s[2], s[0])
aoz = m.atan2(s[1], m.sqrt(s[0]**2 + s[2]**2))
rot = tr.rotation_matrix(aoy, (0, 1, 0))
rot = np.dot(tr.rotation_matrix(-aoz, (0, 0, 1)), rot)
rot = np.dot(tr.rotation_matrix(m.pi / 2.0, (0, 0, 1)), rot)
v, n, t = self.gen_v(1, 1, s)
for x in range(v.shape[0]):
for y in range(v.shape[1]):
for z in range(v.shape[2]):
v[x, y, z] = np.dot(rot, v[x, y, z])
n[x, y,
z] = np.resize(np.dot(rot, np.resize(n[x, y, z], 4)), 3)
Mesh.__init__(self, buffers=(v, n, t))
self.x = np.array(
((0, 0, 0, 1), (s[0], 0, 0, 1), (0, 0, s[2], 1),
(s[0], 0, s[2], 1), (0, s[1], 0, 1), (s[0], s[1], 0, 1),
(0, s[1], s[2], 1), (s[0], s[1], s[2], 1)), np.float64)
for i in range(len(self.x)):
self.x[i] = np.dot(rot, self.x[i])
self.r = np.resize(
np.dot(rot,
np.array((s[0], s[1], s[2], 0), np.float64) / 2.0), 3)
self.m = np.array([d * s[0] * s[1] * s[2] / 8.0] * len(self.x),
np.float64)
self.M = self.calc_m(self.x, self.m)
self.Mi = np.linalg.inv(self.M)
def _getMatrix(head, tail, roll):
"""
Calculate an orientation (rest) matrix for a bone between specified head
and tail positions with given bone roll angle.
Returns length of the bone and rest orientation matrix in global coordinates.
"""
# TODO validate, or replace
vector = toZisUp3(tail - head)
length = math.sqrt(np.dot(vector, vector))
if length == 0:
vector = [0,0,1]
else:
vector = vector/length
yproj = np.dot(vector, YUnit)
if yproj > 1-1e-6:
axis = YUnit
angle = 0
elif yproj < -1+1e-6:
axis = YUnit
angle = pi
else:
axis = np.cross(YUnit, vector)
axis = axis / math.sqrt(np.dot(axis,axis))
angle = math.acos(yproj)
mat = tm.rotation_matrix(angle, axis)
if roll:
mat = np.dot(mat, tm.rotation_matrix(roll, YUnit))
mat = fromZisUp4(mat)
mat[:3,3] = head
return length, mat
def make_spin_pattern_sagimeridian(sagimeridian_pole, angular_velocity,
frame_rate):
#spin the world around a pole on the mid-sagital meridian.
from scipy.misc import imresize
unit_vector = np.array([1., 0., 0.])
zp_offset = (2 * pi) / 96 * 4
orientation_vector = dot(
tran.rotation_matrix(sagimeridian_pole, array([0, 1, 0]))[:3, :3],
unit_vector)
orientation_vector = dot(
tran.rotation_matrix(zp_offset, array([0, 0, 1]))[:3, :3],
orientation_vector) * angular_velocity
xyzp = integrate_frame(xyz,
rotation_vect=orientation_vector,
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate)
proj = arena_project(xyzp)
imgs = digitize_points(*proj, display_shape=display_shape)
imgs = imgs[:, :, newaxis]
frames_per_cycle = int(((2 * pi) / angular_velocity) * frame_rate)
for i in range(frames_per_cycle):
xyzp = integrate_frame(xyzp,
rotation_vect=orientation_vector,
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate)
proj = arena_project(xyzp)
img = digitize_points(*proj, display_shape=display_shape)
imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
from scipy import io
io.savemat('sagimeridian_%03d' % (around(rad2deg(sagimeridian_pole))),
{'imgs': imgs})
def make_spin_pattern_coromeridian(coromeridian_pole,
angular_velocity,
frame_rate,
max_sensory_radius=2,
star_density=50):
#Spin the world around a pole around a coronal maridian pole
from scipy.misc import imresize
unit_vector = np.array([0., 1., 0.])
zp_offset = (2 * pi) / 96 * 4
orientation_vector = dot(
tran.rotation_matrix(coromeridian_pole, array([1, 0, 0]))[:3, :3],
unit_vector)
orientation_vector = dot(
tran.rotation_matrix(zp_offset, array([0, 0, 1]))[:3, :3],
orientation_vector) * angular_velocity
xyz = make_universe(star_density=star_density,
max_sensory_radius=max_sensory_radius)
xyzp = integrate_frame(xyz,
rotation_vect=orientation_vector,
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate)
proj = arena_project(xyzp)
imgs = digitize_points(*proj, display_shape=display_shape)
imgs = imgs[:, :, newaxis]
frames_per_cycle = int(((2 * pi) / angular_velocity) * frame_rate)
for i in range(frames_per_cycle):
xyzp = integrate_frame(xyzp,
rotation_vect=orientation_vector,
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate)
proj = arena_project(xyzp)
img = digitize_points(*proj, display_shape=display_shape)
imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
from scipy import io
return imgs
def inverse_kinematics( self , pos , normal ) : l1 , l2 , l3 = .91 , .81 , .33 dy , dz = .27 , .26 normal = normal / la.norm( normal ) pos1 = pos + normal * l3 e = m.sqrt(pos1[2]*pos1[2]+pos1[0]*pos1[0]-dz*dz) a1 = m.atan2(pos1[2], -pos1[0]) + m.atan2(dz, e) pos2 = np.array( [ e , pos1[1]-dy , .0 ] ); a3 = -m.acos(min(1.0,(pos2[0]*pos2[0]+pos2[1]*pos2[1]-l1*l1-l2*l2)/(2.0*l1*l2))) k = l1 + l2 * m.cos(a3) l = l2 * m.sin(a3) a2 = -m.atan2(pos2[1],m.sqrt(pos2[0]*pos2[0]+pos2[2]*pos2[2])) - m.atan2(l,k); rotmat = tr.rotation_matrix( -a1 , (0,1,0) ) normal1 = np.resize( np.dot( rotmat , np.resize( normal , 4 ) ) , 3 ) rotmat = tr.rotation_matrix( -a2-a3 , (0,0,1) ) normal1 = np.resize( np.dot( rotmat , np.resize( normal1, 4 ) ) , 3 ) a5 = m.acos( normal1[0] ) a4 = m.atan2(normal1[2], normal1[1]) return a1 , a2 , a3 , a4 , a5
def check_points_in_range(current_rob_pos, other_rob_pos):
# calculate bottom right corner of the bounding rectangle
rx, ry, rth = current_rob_pos
rth = (rth / 100) + (pi / 2)
zx = round(rx - (m * sin(rth)), 3)
zy = round(ry + (m * cos(rth)), 3)
# calculating the transformation matrix of the new frame at that vertex
alpha, beta, gamma = 0, 0, (-(pi / 2 - rth))
origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
Rx = transformations.rotation_matrix(alpha, xaxis)
Ry = transformations.rotation_matrix(beta, yaxis)
Rz = transformations.rotation_matrix(gamma, zaxis)
T = transformations.translation_matrix([zx, zy, 0])
R = transformations.concatenate_matrices(Rx, Ry, Rz)
Z = transformations.shear_matrix(beta, xaxis, origin, zaxis)
S = transformations.scale_matrix(1, origin)
M = transformations.concatenate_matrices(T, R, Z, S)
M = np.linalg.inv(M)
# calculating the new point in that frame
other_x = other_rob_pos[0]
other_y = other_rob_pos[1]
other_z = 0
other_h = 1
other_point_homogenous = np.array([other_x, other_y, other_z, other_h])
new_point = np.dot(M, other_point_homogenous)
new_point = np.round(new_point, 3)
# checking that the point lies within the boundaries
px, py, pz, ph = new_point
if px <= (2 * m) and px >= 0 and py <= m and py >= 0:
return 1
else:
return 0
def gen_meas_3d(func, N, scale=1):
""" Generate a set of measurements according to a height function.
"""
# Use 3-D dirichlet distribution to sample 3-simplex then project to 2-d
# Gives a nicely uniform spread of points
m_a, m_b = np.random.dirichlet((1, 1, 1), N)[:, :2].T
m_g = func(m_a, m_b)
m_z = np.array([m_a, m_b, m_g]).T.reshape(N, 3, 1)
std_a = 0.01 * scale
std_b = 0.01 * scale
std_g = 0.02 * scale
std_z = np.diag([std_a, std_b, std_g])
alpha_rot = np.pi * np.random.random_sample(N) - np.pi / 2
beta_rot = np.pi * np.random.random_sample(N) - np.pi / 2
gamma_rot = np.pi * np.random.random_sample(N) - np.pi / 2
xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
C_z = np.empty((N, 3, 3), dtype=float)
for i in trange(N):
# Rotate Covariance Matrix
Rx = rotation_matrix(alpha_rot[i], xaxis)
Ry = rotation_matrix(beta_rot[i], yaxis)
Rz = rotation_matrix(gamma_rot[i], zaxis)
R = concatenate_matrices(Rx, Ry, Rz)[:-1, :-1]
C = R.dot(std_z * std_z).dot(R.T)
e = np.random.multivariate_normal(np.zeros(3), C, 1).T
m_z[i, :, :] += e
C_z[i] = C
return m_z, C_z
def __predict_nominal_state(self, imu_measurement: np.array):
p = self.nominal_state[:3].reshape(-1, 1)
q = self.nominal_state[3:7]
v = self.nominal_state[7:10].reshape(-1, 1)
a_b = self.nominal_state[10:13].reshape(-1, 1)
w_b = self.nominal_state[13:16]
g = self.nominal_state[16:19].reshape(-1, 1)
w_m = imu_measurement[1:4].copy()
a_m = imu_measurement[4:7].reshape(-1, 1).copy()
dt = imu_measurement[0] - self.last_predict_time
"""
dp/dt = v
dv/dt = R(a_m - a_b) + g
dq/dt = 0.5 * q x(quaternion product) (w_m - w_b)
a_m and w_m are the measurements of IMU.
a_b and w_b are biases of acc and gyro, respectively.
R = R{q}, which bring the point from local coordinate to global coordinate.
"""
w_m -= w_b
a_m -= a_b
# use the zero-order integration to integrate the quaternion.
# q_{n+1} = q_n x q{(w_m - w_b) * dt}
angle = la.norm(w_m)
axis = w_m / angle
R_w = tr.rotation_matrix(0.5 * dt * angle, axis)
q_w = tr.quaternion_from_matrix(R_w, True)
q_half_next = tr.quaternion_multiply(q, q_w)
R_w = tr.rotation_matrix(dt * angle, axis)
q_w = tr.quaternion_from_matrix(R_w, True)
q_next = tr.quaternion_multiply(q, q_w)
if q_next[0] < 0: # force the quaternion has a positive real part.
q_next *= -1
# use RK4 method to integration velocity and position.
# integrate velocity first.
R = tr.quaternion_matrix(q)[:3, :3]
R_half_next = tr.quaternion_matrix(q_half_next)[:3, :3]
R_next = tr.quaternion_matrix(q_next)[:3, :3]
v_k1 = R @ a_m + g
v_k2 = R_half_next @ a_m + g
# v_k3 = R_half_next @ a_m + g # yes. v_k2 = v_k3.
v_k3 = v_k2
v_k4 = R_next @ a_m + g
v_next = v + dt * (v_k1 + 2 * v_k2 + 2 * v_k3 + v_k4) / 6
# integrate position
p_k1 = v
p_k2 = v + 0.5 * dt * v_k1 # k2 = v_next_half = v + 0.5 * dt * v' = v + 0.5 * dt * v_k1(evaluate at t0)
p_k3 = v + 0.5 * dt * v_k2 # v_k2 is evaluated at t0 + 0.5*delta
p_k4 = v + dt * v_k3
p_next = p + dt * (p_k1 + 2 * p_k2 + 2 * p_k3 + p_k4) / 6
self.nominal_state[:3] = p_next.reshape(3, )
self.nominal_state[3:7] = q_next
self.nominal_state[7:10] = v_next.reshape(3, )
def compute_transform(rx, rz, tx, ty, tz): origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] Rx = tf.rotation_matrix(rx, xaxis) Rz = tf.rotation_matrix(rz, zaxis) T = tf.translation_matrix([tx, ty, tz]) return T.dot(Rz).dot(Rx)
def createAnimationTrack(self, jointsOrder = None, name="BVHMotion"):
"""
Create an animation track from the motion stored in this BHV file.
"""
if jointsOrder == None:
jointsData = [joint.matrixPoses for joint in self.getJoints() if not joint.isEndConnector()]
# We leave out end effectors as they should not have animation data
else:
nFrames = self.frameCount
import re
# Remove the tail from duplicate bone names
for idx,jName in enumerate(jointsOrder):
# Joint mappings can contain a rotation compensation
if isinstance(jName, tuple):
jName, _ = jName
if not jName:
continue
r = re.search("(.*)_\d+$",jName)
if r:
jointsOrder[idx] = r.group(1)
jointsData = []
for jointName in jointsOrder:
if isinstance(jointName, tuple):
jointName, angle = jointName
else:
angle = 0.0
if jointName:
poseMats = self.getJointByCanonicalName(jointName).matrixPoses.copy()
if isinstance(angle, float):
if angle != 0.0:
# Rotate around global Z axis
rot = tm.rotation_matrix(-angle*D, [0,0,1])
# Roll around global Y axis (this is a limitation)
roll = tm.rotation_matrix(angle*D, [0,1,0])
for i in range(nFrames):
# TODO make into numpy loop
poseMats[i] = np.dot(poseMats[i], rot)
poseMats[i] = np.dot(poseMats[i], roll)
else: # Compensation (angle) is a transformation matrix
# Compensate animation frames
for i in range(nFrames):
# TODO make into numpy loop
poseMats[i] = np.mat(poseMats[i]) * np.mat(angle)
#poseMats[i] = np.mat(angle) # Test compensated rest pose
jointsData.append(poseMats)
else:
jointsData.append(animation.emptyTrack(nFrames))
nJoints = len(jointsData)
nFrames = len(jointsData[0])
# Interweave joints animation data, per frame with joints in breadth-first order
animData = np.hstack(jointsData).reshape(nJoints*nFrames,4,4)
framerate = 1.0/self.frameTime
return animation.AnimationTrack(name, animData, nFrames, framerate)
def transform(self, pitch, yaw):
"""Gives the translation matrix associated with this arm with the given pitch
and yaw.
"""
tLength = tr.translation_matrix(np.array([self.length, 0, 0]))
rPitch = tr.rotation_matrix(pitch, yaxis)
rYaw = tr.rotation_matrix(yaw, zaxis)
#return armLength * rPitch * rYaw
return tr.concatenate_matrices(rYaw, rPitch, tLength)
def error(self, target_pos, baseTransform, parameters):
armLength = tr.translation_matrix([self.length, 0, 0])
rPitch = tr.rotation_matrix(parameters[0], yaxis)
rYaw = tr.rotation_matrix(parameters[1], zaxis)
transform = tr.concatenate_matrices(baseTransform, rYaw, rPitch,
armLength)
if self.after is None:
end = tr.translation_from_matrix(transform)
return distance(target_pos, end)
else:
return self.after.error(target_pos, transform, parameters[2:])
def rotation_matrix_angles(alpha, beta, gamma):
""" Returns the rotation matrix that applies a rotation by the given Euler angles.
It applies Rz(gamma)*Rx(beta)*Rz(alpha).
"""
R1 = trans.rotation_matrix(gamma, z)
R2 = trans.rotation_matrix(beta, x)
R3 = trans.rotation_matrix(alpha, z)
M = trans.concatenate_matrices(R1, R2, R3)
return M
def update_sagital_rot(self, robot):
# ok we assume a symetric configuration with both foot on the ground
self.LAnkleRotY = tf.rotation_matrix(
radians(robot.l_ankle_y.present_position), self.yaxis)
self.RAnkleRotY = tf.rotation_matrix(
radians(robot.r_ankle_y.present_position), self.yaxis)
self.LKneeRotY = tf.rotation_matrix(
radians(- robot.l_knee_y.present_position), self.yaxis)
self.RKneeRotY = tf.rotation_matrix(
radians(- robot.r_knee_y.present_position), self.yaxis)
self.LHipRotY = tf.rotation_matrix(
radians(- robot.l_hip_y.present_position), self.yaxis)
self.RHipRotY = tf.rotation_matrix(
radians(- robot.r_hip_y.present_position), self.yaxis)
self.AbsRotY = tf.rotation_matrix(
radians(robot.abs_y.present_position), self.yaxis)
self.BustRotY = tf.rotation_matrix(
radians(robot.bust_y.present_position), self.yaxis)
self.roty = array([tf.rotation_matrix(0, self.yaxis), (self.LAnkleRotY + self.RAnkleRotY) / 2.0,
(self.LKneeRotY + self.RKneeRotY) / 2.0, (self.LHipRotY + self.RHipRotY) / 2.0, self.AbsRotY, self.BustRotY])
# self.transformy =array([tf.concatenate_matrices(self.roty[i], self.SegCom[i]) for i in range(len(self.roty))])
# self.transformy =array([self.roty[i].dot( self.SegCom[i]) for i in range(len(self.roty))])
self.transformy = array([identity(4),
self.FootT,
tf.concatenate_matrices(
(self.LAnkleRotY + self.RAnkleRotY) / 2.0, self.ThighT),
tf.concatenate_matrices(
(self.LKneeRotY + self.RKneeRotY) / 2.0, self.LegT),
tf.concatenate_matrices(
(self.LHipRotY + self.RHipRotY) / 2.0, self.HipT),
tf.concatenate_matrices(
self.AbsRotY, self.AbsT),
tf.concatenate_matrices(
self.BustRotY, self.BustT)
])
a = dot(self.transformy[1], self.transformy[2])
b = dot(a, self.transformy[3])
c = dot(b, self.transformy[4])
d = dot(c, self.transformy[5])
e = dot(d, self.transformy[6])
self.comtrans = array([self.transformy[1],
a,
b,
c,
d,
e
])
def generate_leg(leg: str, params: list):
front_signs = {'front': '', 'rear': '-'}
side_signs = {'left': '', 'right': '-'}
lower_limits = {'left': '0', 'right': str(-pi)}
upper_limits = {'left': str(pi), 'right': '0'}
with open('../urdf/leg.urdf.template', 'r') as file:
leg_template = Template(file.read())
mappings = {'l_width': 0.02, 'leg': leg}
front, side = leg.split('_')
mappings['front_sign'] = front_signs[front]
mappings['side_sign'] = side_signs[side]
mappings['lower_limit'] = lower_limits[side]
mappings['upper_limit'] = upper_limits[side]
for param in params:
try:
d = float(param['d'])
except ValueError:
d = 0.0
try:
th = float(param['th'])
except ValueError:
th = 0.0
try:
a = float(param['a'])
except ValueError:
a = 0.0
try:
al = float(param['al'])
except ValueError:
al = 0.0
tz = translation_matrix((0, 0, d))
rz = rotation_matrix(th, zaxis)
tx = translation_matrix((a, 0, 0))
rx = rotation_matrix(al, xaxis)
matrix = concatenate_matrices(tz, rz, tx, rx)
rpy = euler_from_matrix(matrix)
xyz = translation_from_matrix(matrix)
name = param['joint']
mappings[f'{name}_j_xyz'] = '{} {} {}'.format(*xyz)
mappings[f'{name}_j_rpy'] = '{} {} {}'.format(*rpy)
mappings[f'{name}_l_xyz'] = '{} 0 0'.format(xyz[0] / 2.)
mappings[f'{name}_l_rpy'] = '0 0 0'
mappings[f'{name}_l_len'] = str(a)
return leg_template.substitute(mappings)
def rotated_vector_mat(axisFrom, axisTo):
v0 = Vector((axisFrom[0], axisFrom[1], axisFrom[2]))
v1 = Vector((axisTo[0], axisTo[1], axisTo[2]))
# rotation
rot = v1.rotation_difference(v0).to_euler()
trace([degrees(a) for a in rot])
matrix1 = rotation_matrix(rot[0], [1, 0, 0])
matrix11 = rotation_matrix(rot[1], [0, 1, 0])
matrix111 = rotation_matrix(rot[2], [0, 0, 1])
# combine transformations
mat_out = matrix1 * matrix11 * matrix111
print(mat_out)
return mat_out
def make_translation_pattern_coromeridian(coromeridian_pole,
translation_velocity,
frame_rate,
epoch_duration=2.25,
max_sensory_radius=2,
star_density=50):
#translate in a direction on the equator.
from scipy.misc import imresize
frame_period = 1 / frame_rate
frame_distance = frame_period * translation_velocity
frames_per_cycle = around(epoch_duration * frame_rate).astype(int)
####################
xyz = make_universe(star_density=star_density,
max_sensory_radius=max_sensory_radius + frame_distance)
###align the orientation vector with arena coordinates
unit_vector = np.array([0., 1., 0.])
zp_offset = (2 * pi) / 96 * 4
orientation_vector = dot(
tran.rotation_matrix(coromeridian_pole, array([1, 0, 0]))[:3, :3],
unit_vector)
orientation_vector = dot(
tran.rotation_matrix(zp_offset, array([0, 0, 1]))[:3, :3],
orientation_vector) * translation_velocity
xyzp = integrate_frame(xyz,
rotation_vect=np.array([0.1, 0.1, 0.1]),
translation_vect=orientation_vector,
frame_period=1 / frame_rate)
###crop to max_sensory_radius
xyzp = crop_universe(xyzp, max_sensory_radius)
proj = arena_project(xyzp)
imgs = digitize_points(*proj, display_shape=display_shape)
imgs = imgs[:, :, newaxis]
xyzp = refill_universe(xyzp, star_density, max_sensory_radius,
frame_distance)
#####################
for i in range(frames_per_cycle):
xyzp = integrate_frame(xyzp,
rotation_vect=np.array([1e-10, 1e-10, 1e-10]),
translation_vect=orientation_vector,
frame_period=1 / frame_rate)
xyzp = crop_universe(xyzp, max_sensory_radius)
proj = arena_project(xyzp)
img = digitize_points(*proj, display_shape=display_shape)
imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
xyzp = refill_universe(xyzp, star_density, max_sensory_radius,
frame_distance)
from scipy import io
return imgs
def dp_2_pc(self, depth_img, xmap, ymap, trans, rot):
offset = 50
def convert_to_zdepth(img):
return ((img[:, :, 0] * (750 / 255)) + offset) * 1.03
alpha_thresh = 100
flattened_alpha = np.ravel(depth_img[:,:,3].T)
id = np.arange(flattened_alpha.shape[0])
cond_idx = id[flattened_alpha > 100]
xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1]
cloud = np.zeros((424, 512, 3))
dp_im = convert_to_zdepth(depth_img)
cloud[:, :, 2] = dp_im[:, :]
cloud[:, :, 0] = xmap * cloud[:, :, 2]
cloud[:, :, 1] = ymap * cloud[:, :, 2]
cloud = np.reshape(cloud, (-1, 3))
# cloud = cloud[(cloud[:, 2] > offset + 0.2)]
if (rot[0] > 0):
rotyx = 180.0 - rot[0]
else:
rotyx = (-rot[0])
rotyy = rot[1]
if (trans[0] * trans[2]) > 0:
if rotyy < 0:
rotyy = -rotyy
else:
rotyy = -180 + rotyy
else:
if rotyy < 0:
rotyy = -180 + rotyy
else:
rotyy = -rotyy
rot_mat = np.dot(rotation_matrix(0, zaxis), np.dot(rotation_matrix(math.radians(rotyy), yaxis), rotation_matrix(math.radians(rotyx), xaxis)))
cloud = np.dot(cloud, rot_mat[:3, :3].T)
cloud += trans
cloud = cloud[cond_idx, :]
return cloud
def rotmat3(alpha,beta,gamma,xyzreftuple = ([1, 0, 0], [0, 1, 0], [0, 0, 1]),angtype='deg'):
xaxis, yaxis, zaxis = xyzreftuple
if angtype == 'deg':
alpha = np.deg2rad(alpha)
beta = np.deg2rad(beta)
gamma = np.deg2rad(gamma)
Rx = trans.rotation_matrix(alpha, xaxis)
Ry = trans.rotation_matrix(beta, yaxis)
Rz = trans.rotation_matrix(gamma, zaxis)
R = trans.concatenate_matrices(Rz, Ry, Rx)[:3,:3]
return R
def _orientation_distance(self, joint_orientation):
joint_euler_angles = quaternion_to_euler(joint_orientation)
rotmat_constraint = np.eye(4)
rotmat_target = np.eye(4)
for i in range(3):
if self.orientation[i] is not None:
tmp_constraint = rotation_matrix(
np.deg2rad(self.orientation[i]), self.ROTATION_AXIS[i])
rotmat_constraint = np.dot(tmp_constraint, rotmat_constraint)
tmp_target = rotation_matrix(np.deg2rad(joint_euler_angles[i]),
self.ROTATION_AXIS[i])
rotmat_target = np.dot(tmp_target, rotmat_target)
rotation_distance = GlobalTransformConstraint._vector_distance(
np.ravel(rotmat_constraint), np.ravel(rotmat_target), 16)
return rotation_distance
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None):
rospy.logdebug("joint: %s"%jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0,0,0]
if not joint.origin.rotation: joint.origin.rotation = [0,0,0]
parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
def __init__( self , fovy , ratio , near , far , robot_files ) : self.fovy = fovy self.near = near self.far = far self.ratio = ratio self.camera = None self.plane = Plane( (2,2) ) self.wall = Plane( (20,10) ) self.mw = tr.rotation_matrix( -m.pi / 2.0 , (1,0,0) ) self.mw = np.dot( self.mw , tr.translation_matrix( (0,3,0) ) ) self.robot = Robot( robot_files ) self.x = 0.0 self.last_time = timer() self.plane_alpha = 65.0 / 180.0 * m.pi self.lpos = [ 1 ,-1 , 0 ] self._make_plane_matrix() self.draw_robot = True self.draw_sparks = True self.draw_front = False self.draw_back = False
def Rotation(self, angle, dim, axis):
self.size = dim
mat = tm.rotation_matrix(angle, Matrix.Axis[axis])
if dim == 3:
self.matrix = mat[:3,:3]
else:
self.matrix = mat
def make_pts( self , corners ) : dx = (corners[3] - corners[0]) dx = dx / la.norm(dx) dy = (corners[1] - corners[0]) / (self.size[1]+3-1) dz = np.cross( dx , dy ) dz = dz / la.norm(dz) ax = np.cross( dx , dz ) ax = ax / la.norm(ax) an = 2.0 * m.pi / float(self.size[0]) rot = tr.rotation_matrix( an , ax ) ox = (corners[3] + corners[0]) / 2.0 rx = np.resize( ox - corners[0] , 4 ) self.axis = ax self.center = ox del self.pts[:] for y in range(self.size[1]+3) : drx = copy(rx) for x in range(self.size[0]) : pt = ox + np.resize(drx,3) self.pts.append( pt ) drx = np.dot( drx , rot ) self.pts.append( self.pts[-self.size[0]] ) self.pts.append( self.pts[-self.size[0]] ) self.pts.append( self.pts[-self.size[0]] ) ox += dy
def update_local_transformation(self):
"""
update local transformation w.r.t element's parent
.. seealso:: VRML transform calculation http://www.web3d.org/x3d/specifications/vrml/ISO-IEC-14772-VRML97/part1/nodesRef.html#Transform
"""
if self.jointType in ["free", "freeflyer"]:
self.localTransformation = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])
scale = [1,1,1]
if self.parent:
scale = self.cumul_scale()
scale_translation = [self.translation[i]*scale[i] for i in range(3)]
self.localTransformation[0:3,3]=numpy.array(scale_translation)+\
numpy.dot(numpy.eye(3)-self.localTransformation[0:3,:3],numpy.array(self.center))
elif ( type(self) == Joint and self.jointType in [ "rotate", "rotation", "revolute"]
and self.jointId >= 0):
if self.jointAxis in ["x","X"]:
axis = [1, 0, 0]
elif self.jointAxis in ["y","Y"]:
axis = [0, 1, 0]
elif self.jointAxis in ["z","Z"]:
axis = [0, 0, 1]
else:
raise RuntimeError("Invalid joint axis {0}".format(self.jointAxis))
self.localR2= tf.rotation_matrix(self.angle, axis)[:3,:3]
self.localTransformation[0:3,0:3] = numpy.dot(self.localR1, self.localR2)
def Rotation(self, angle, dim, axis):
self.size = dim
mat = tm.rotation_matrix(angle, Matrix.Axis[axis])
if dim == 3:
self.matrix = mat[:3, :3]
else:
self.matrix = mat
def plot_fingertip_contacts(self):
self._plot_handler = []
colors = [
np.array((1, 0, 0)),
np.array((0, 1, 0)),
np.array((0, 0, 1))
]
tip_link_ids = self.get_fingertip_links()
point_size = 0.005
for i in range(len(tip_link_ids)):
link = self._or_hand.GetLink(tip_link_ids[i])
T = link.GetGlobalMassFrame()
local_frame_rot = transformations.rotation_matrix(
math.pi / 6., [0, 0, 1])[:3, :3]
T[:3, :3] = T[:3, :3].dot(local_frame_rot)
offset = T[0:3, 0:3].dot(self.get_tip_offsets())
T[0:3, 3] = T[0:3, 3] + offset
position = T[:3, -1]
self._plot_handler.append(
self._or_env.plot3(points=position,
pointsize=point_size,
colors=colors[i],
drawstyle=1))
for j in range(3):
normal = T[:3, j]
self._plot_handler.append(
self._or_env.drawarrow(p1=position,
p2=position + 0.05 * normal,
linewidth=0.001,
color=colors[j]))
def stretchTo(self, goal, doStretch):
length, self.matrixGlobal = getMatrix(self.getHead(), goal, 0)
if doStretch:
factor = length / self.length
self.matrixGlobal[:3, 1] *= factor
pose = self.getPoseFromGlobal()
if 0 and self.name in ["DfmKneeBack_L", "DfmLoLeg_L"]:
log.debug("Stretch %s", self.name)
log.debug("G %s", goal)
log.debug("M1 %s", self.matrixGlobal)
log.debug("P1 %s", pose)
az, ay, ax = tm.euler_from_matrix(pose, axes='szyx')
rot = tm.rotation_matrix(-ay + self.roll, CBone.Axes[1])
self.matrixGlobal[:3, :3] = np.dot(self.matrixGlobal[:3, :3],
rot[:3, :3])
pose2 = self.getPoseFromGlobal()
if 0 and self.name in ["DfmKneeBack_L", "DfmLoLeg_L"]:
log.debug("A %s %s %s", ax, ay, az)
log.debug("R %s", rot)
log.debug("M2 %s", self.matrixGlobal)
log.debug("P2 %s", pose)
log.debug("")
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None):
rospy.logdebug("joint: %s" % jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0, 0, 0]
if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0]
parentFromChild = conversions.trans_rot_to_hmat(
joint.origin.position,
transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(
valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(
np.array(joint.axis) * valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,
joint.child,
ns=ns,
valuedict=valuedict)
def make_pts(self, corners):
dx = (corners[3] - corners[0])
dx = dx / la.norm(dx)
dy = (corners[1] - corners[0]) / (self.size[1] + 3 - 1)
dz = np.cross(dx, dy)
dz = dz / la.norm(dz)
ax = np.cross(dx, dz)
ax = ax / la.norm(ax)
an = 2.0 * m.pi / float(self.size[0])
rot = tr.rotation_matrix(an, ax)
ox = (corners[3] + corners[0]) / 2.0
rx = np.resize(ox - corners[0], 4)
self.axis = ax
self.center = ox
del self.pts[:]
for y in range(self.size[1] + 3):
drx = copy(rx)
for x in range(self.size[0]):
pt = ox + np.resize(drx, 3)
self.pts.append(pt)
drx = np.dot(drx, rot)
self.pts.append(self.pts[-self.size[0]])
self.pts.append(self.pts[-self.size[0]])
self.pts.append(self.pts[-self.size[0]])
ox += dy
def poleTargetCorrect(self, head, goal, pole, angle):
yvec = goal-head
xvec = pole-head
xy = dot(xvec, yvec)/dot(yvec,yvec)
xvec = xvec - xy * yvec
xlen = math.sqrt(dot(xvec,xvec))
if xlen > 1e-6:
xvec = xvec / xlen
zvec = self.matrixGlobal[:3,2]
zlen = math.sqrt(dot(zvec,zvec))
zvec = zvec / zlen
angle0 = math.asin( dot(xvec,zvec) )
rot = tm.rotation_matrix(angle - angle0, CBone.Axes[1])
#m0 = self.matrixGlobal.copy()
self.matrixGlobal[:3,:3] = dot(self.matrixGlobal[:3,:3], rot[:3,:3])
if 0 and self.name == "DfmUpArm2_L":
log.debug("")
log.debug("IK %s", self.name)
log.debug("X %s", xvec)
log.debug("Y %s", yvec)
log.debug("Z %s", zvec)
log.debug("A0 %s", angle0)
log.debug("A %s", angle)
log.debug("R %s", rot)
log.debug("M0 %s", m0)
log.debug("M %s", self.matrixGlobal)
def align(atoms, init_vec, align_vec):
import transformations
import numpy as np
if len(init_vec) == 2:
orig_atom_ind = init_vec[0]
final_atom_ind = init_vec[1]
init_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position
if len(align_vec) == 2:
orig_atom_ind = align_vec[0]
final_atom_ind = align_vec[1]
align_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position
rot_axis = np.cross(init_vec, align_vec)
nrot_axis = rot_axis/np.linalg.norm(rot_axis)
angle = transformations.angle_between_vectors(init_vec, align_vec)
rot_M = transformations.rotation_matrix(angle, nrot_axis)[:3,:3]
for a in atoms:
a.position = rot_M.dot(a.position)
return atoms
def get_new_point(
reference_point, start_point, angle, axis
): # where angle is radians of the stem length (edge to opposite vertex)
M = rotation_matrix(angle, axis)
new_point = np.dot(start_point, M[:3, :3].T)
return new_point + reference_point
def ur2np(ur_pose):
# ROS pose
ros_pose = []
# ROS position
ros_pose.append(ur_pose[0])
ros_pose.append(ur_pose[1])
ros_pose.append(ur_pose[2])
# Ros orientation
angle = sqrt(ur_pose[3]**2 + ur_pose[4]**2 + ur_pose[5]**2)
direction = [i / angle for i in ur_pose[3:6]]
np_T = tf.rotation_matrix(angle, direction)
np_q = tf.quaternion_from_matrix(np_T)
ros_pose.append(np_q[0])
ros_pose.append(np_q[1])
ros_pose.append(np_q[2])
ros_pose.append(np_q[3])
# orientation
np_pose = tf.quaternion_matrix(
[ros_pose[3], ros_pose[4], ros_pose[5], ros_pose[6]])
# position
np_pose[0][3] = ros_pose[0]
np_pose[1][3] = ros_pose[1]
np_pose[2][3] = ros_pose[2]
return np_pose
def poleTargetCorrect(self, head, goal, pole, angle):
yvec = goal-head
xvec = pole-head
xy = np.dot(xvec, yvec)/np.dot(yvec,yvec)
xvec = xvec - xy * yvec
xlen = math.sqrt(np.dot(xvec,xvec))
if xlen > 1e-6:
xvec = xvec / xlen
zvec = self.matrixGlobal[:3,2]
zlen = math.sqrt(np.dot(zvec,zvec))
zvec = zvec / zlen
angle0 = math.asin( np.dot(xvec,zvec) )
rot = tm.rotation_matrix(angle - angle0, CBone.Axes[1])
#m0 = self.matrixGlobal.copy()
self.matrixGlobal[:3,:3] = np.dot(self.matrixGlobal[:3,:3], rot[:3,:3])
if 0 and self.name == "DfmUpArm2_L":
log.debug("")
log.debug("IK %s", self.name)
log.debug("X %s", xvec)
log.debug("Y %s", yvec)
log.debug("Z %s", zvec)
log.debug("A0 %s", angle0)
log.debug("A %s", angle)
log.debug("R %s", rot)
log.debug("M0 %s", m0)
log.debug("M %s", self.matrixGlobal)
def send_msg(self):
if self.msg_sent:
return
print('Sending msg')
self.msg_sent = True
front_point = self.transform('odom', 'front_point')
inc = tf.concatenate_matrices(
tf.rotation_matrix(-5 * pi / 180, (0, 0, 1)),
tf.translation_matrix((0.03, 0.01, 0)),
)
path = [tf.concatenate_matrices(front_point, inc)]
for i in range(179):
path.append(tf.concatenate_matrices(path[-1], inc))
msg = Path()
msg.header.frame_id = 'odom'
msg.header.stamp.sec = int(self.get_clock().now().nanoseconds / 10**9)
msg.header.stamp.nanosec = self.get_clock().now().nanoseconds % 10**9
poses = [matrix_to_pose(point) for point in path]
for pose in poses:
pose_stamped = PoseStamped()
pose_stamped.header = msg.header
pose_stamped.pose = pose
msg.poses.append(pose_stamped)
self.pub.publish(msg)
print('Sent')
def writeBindPose(fp, meshes, skel, config):
id,key = getId("Pose::" + skel.name)
nBones = skel.getBoneCount()
nMeshes = len(meshes)
fp.write(
' Pose: %d, "%s", "BindPose" {\n' % (id, key)+
' Type: "BindPose"\n' +
' Version: 100\n' +
' NbPoseNodes: %d\n' % (1+nMeshes+nBones))
startLinking()
# Skeleton bind matrix
skelbindmat = tm.rotation_matrix(math.pi/2, (1,0,0))
poseNode(fp, "Model::%s" % skel.name, skelbindmat)
for mesh in meshes:
poseNode(fp, "Model::%sMesh" % mesh.name, skelbindmat)
for bone in skel.getBones():
bindmat,_ = bone.getBindMatrix(config.offset)
poseNode(fp, "Model::%s" % bone.name, bindmat)
stopLinking()
fp.write(' }\n')
def init_local_transformation(self):
"""
compute local transformation w.r.t for the first time (compute everything)
"""
# rotation part
self.localTransformation = numpy.eye(4)
# print type(self), id(self), self.rotation
try:
angle = self.rotation[3]
except IndexError:
logger.exception("Failed on {0}, rotation={1}".format(type(self),self.rotation))
raise
direction = self.rotation[:3]
self.localTransformation[0:3,0:3] = tf.rotation_matrix(angle, direction)[:3,:3]
self.rpy = tf.euler_from_matrix(self.localTransformation)
# last column
scale = [1,1,1]
if self.parent:
scale = self.cumul_scale()
scale_translation = [self.translation[i]*scale[i] for i in range(3)]
self.localTransformation[0:3,3] = numpy.array(scale_translation)+\
numpy.dot(numpy.eye(3)-self.localTransformation[:3,:3],
numpy.array(self.center))
# last line
self.localTransformation[3,0:4]=[0,0,0,1]
def init_local_transformation(self):
"""
compute local transformation w.r.t for the first time (compute everything)
"""
# rotation part
self.localTransformation = numpy.eye(4)
# print type(self), id(self), self.rotation
try:
angle = self.rotation[3]
except IndexError:
logger.exception("Failed on {0}, rotation={1}".format(
type(self), self.rotation))
raise
direction = self.rotation[:3]
self.localTransformation[0:3,
0:3] = tf.rotation_matrix(angle,
direction)[:3, :3]
self.rpy = tf.euler_from_matrix(self.localTransformation)
# last column
scale = [1, 1, 1]
if self.parent:
scale = self.cumul_scale()
scale_translation = [self.translation[i] * scale[i] for i in range(3)]
self.localTransformation[0:3,3] = numpy.array(scale_translation)+\
numpy.dot(numpy.eye(3)-self.localTransformation[:3,:3],
numpy.array(self.center))
# last line
self.localTransformation[3, 0:4] = [0, 0, 0, 1]
def step( self , dt ) : rot = tr.rotation_matrix( dt * self.w , (0,0,1) ) self.v1 = np.resize( np.dot( rot , np.resize( self.v1 , 4 ) ) , 2 ) R = self.v1 * self.l1 self.v2[1] = -R[1] L = self.l2 + rand.normalvariate(self.mu,self.sg) self.v2[0] = m.sqrt(L**2-self.v2[1]**2)
def fixCSysBone(self, bname, infix, mat, index, axis, angle):
csys = csysBoneName(bname, infix)
bone = self.armature.bones[csys]
rot = tm.rotation_matrix(angle, axis)
cmat = np.dot(mat, rot)
bone.tail = bone.head + self.armature.bones[bname].length * cmat[:3,1]
normal = getUnitVector(mat[:3,index])
bone.roll = computeRoll(bone.head, bone.tail, normal)
def _make_plane_matrix( self ) : r = tr.rotation_matrix( self.plane_alpha , (0,0,1) ) s = tr.scale_matrix( 1 ) t = tr.translation_matrix( (-1.25,.7,.05) ) self.m = np.dot( np.dot( t , s ) , r ) self.im = la.inv( self.m ) self.im[3] = [ 0 , 0 , 0 , 1 ]
def rotate(self, angle, axis, rotWorld):
mat = tm.rotation_matrix(angle*D, CBone.Axes[axis])
if rotWorld:
mat = dot(mat, self.matrixGlobal)
self.matrixGlobal[:3,:3] = mat[:3,:3]
self.matrixPose = self.getPoseFromGlobal()
else:
mat = dot(mat, self.matrixPose)
self.matrixPose[:3,:3] = mat[:3,:3]
def __init__(self,dis=[0,0,0],angle=0,axis=[0,0,1],unit='deg'):
"""initialization of object, dis = displacement, angle is the angle of
rotation around the vector axis, unit is the unit of the angle."""
self.__dis = np.array(dis).reshape((3,1))
if unit == 'deg': angle = angle*np.pi/180.
# otherwise, angle is assumed to be in radians
self.__rot = tf.rotation_matrix(angle,axis)[0:3,0:3]
self.__hom = np.vstack( (np.hstack((self.__rot,self.__dis)), np.array([0,0,0,1])))
self.__hom_inv = np.vstack( (np.hstack((self.__rot.T,-np.dot(self.__rot.T,self.__dis))), np.array([0,0,0,1])))
def calc_error(args, debug=False):
# angle_x, angle_y, o_x, o_y, o_z = args
angle_x, angle_y = args
x = np.array([1, 0, 0, 1])
R_x = transformations.rotation_matrix(angle_x, [1, 0, 0], eye_centre)
R_y = transformations.rotation_matrix(angle_y, [0, 1, 0], eye_centre)
S = transformations.scale_matrix(6)
T = transformations.translation_matrix(eye_centre)
# T = transformations.translation_matrix(eye_centre + np.array([o_x*100, o_y*100, o_z*100]))
T2 = transformations.translation_matrix([0,0,-12])
if debug:
trans = transformations.concatenate_matrices(*reversed([T, T2, R_x, R_y]))
pt = dehomo(trans.dot([0,0,-50,1]))
cv2.line(img,
tuple(dehomo(cam_mat.dot(coord_swap.dot(true_iris_centre_3d))).astype(int)),
tuple(dehomo(cam_mat.dot(coord_swap.dot(pt))).astype(int)),
(255, 255, 0))
est_pts = []
for t in np.linspace(0, np.pi*2, accuracy):
R = transformations.rotation_matrix(t, [0, 0, 1])
trans = transformations.concatenate_matrices(*reversed([R, S, T, T2, R_x, R_y]))
threeD_pt = coord_swap.dot(dehomo(trans.dot(x)))
pt = cam_mat.dot(threeD_pt)
if point_in_poly(dehomo(pt).astype(int), data["ldmks_lids_2d"]):
est_pts.append(dehomo(pt).astype(int))
if debug: cv2.circle(img, tuple(dehomo(pt).astype(int)), 1, (255, 0, 0), -1)
try:
D = cdist(est_pts, visible_pts, 'euclidean')
H1 = np.max(np.min(D, axis=1))
H2 = np.max(np.min(D, axis=0))
return (H1 + H2) / 2.0
except ValueError:
return 20
def get_standard_matrixes():
#90 degree clockwise around center of normalized square
R90CW = tr.rotation_matrix(-math.pi/2,[0,0,1],[0.5,0.5,0.5])
#R90CCW = tr.rotation_matrix(math.pi/2,[0,0,1],[0.5,0.5,0.5])
#horizontal axis flip
FH = tr.reflection_matrix([0.5,0.5,0],[0,1,0])
#vertical axis flip
FV = tr.reflection_matrix([0.5,0.5,0],[1,0,0])
R90CW_FH = tr.concatenate_matrices(FH,R90CW)
R90CW_FV = tr.concatenate_matrices(FV,R90CW)
R180CW = tr.rotation_matrix(-math.pi,[0,0,1],[0.5,0.5,0])
#R180CCW = tr.rotation_matrix(math.pi,[0,0,1],[0.5,0.5,0])
#R270CCW = tr.rotation_matrix(2*math.pi,[0,0,1],[0.5,0.5,0])
return R90CW,FH,FV,R90CW_FH,R90CW_FV,R180CW
def test_obj_pose():
from transformations import rotation_matrix, concatenate_matrices, euler_from_matrix
from utils import _axis_rot_matrices, _ypr_from_rot_matrix
alpha, beta, gamma = 0.123, -1.234, 2.345
origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
# I = identity_matrix()
Rx_tf = rotation_matrix(gamma, xaxis)
Ry_tf = rotation_matrix(beta, yaxis)
Rz_tf = rotation_matrix(alpha, zaxis)
obj = Obj("obj")
Rz, Ry, Rx = _axis_rot_matrices([alpha, beta, gamma, 0., 0., 0.])
assert np.allclose(Rx_tf[:3,:3], Rx)
assert np.allclose(Ry_tf[:3,:3], Ry)
assert np.allclose(Rz_tf[:3,:3], Rz)
R = concatenate_matrices(Rz_tf, Ry_tf, Rx_tf)
rot_mat = np.dot(Rz, np.dot(Ry, Rx))
euler = euler_from_matrix(R, 'sxyz')
assert np.allclose(R[:3,:3], rot_mat)
assert np.allclose([gamma, beta, alpha], euler)
assert np.allclose([alpha, beta, gamma], _ypr_from_rot_matrix(R[:3,:3]))
def writeBindPose(fp, meshes, skel, config):
id, key = getId("Pose::" + skel.name)
nBones = skel.getBoneCount()
nMeshes = len(meshes)
# Skeleton bind matrix
skelbindmat = tm.rotation_matrix(math.pi / 2, (1, 0, 0))
count = 1 + nMeshes + nBones
if config.binary:
from . import fbx_binary
elem = fbx_binary.get_child_element(fp, "Objects")
pelem = fbx_binary.fbx_data_bindpose_element(elem, key, id, count)
else:
fp.write(
' Pose: %d, "%s", "BindPose" {\n' % (id, key)
+ ' Type: "BindPose"\n'
+ " Version: 100\n"
+ " NbPoseNodes: %d\n" % count
)
startLinking()
key = "Model::%s" % skel.name
if config.binary:
id, _ = getId(key)
fbx_binary.fbx_data_pose_node_element(pelem, key, id, skelbindmat)
else:
poseNode(fp, key, skelbindmat)
for mesh in meshes:
key = "Model::%sMesh" % mesh.name
if config.binary:
id, _ = getId(key)
fbx_binary.fbx_data_pose_node_element(pelem, key, id, skelbindmat)
else:
poseNode(fp, key, skelbindmat)
for bone in skel.getBones():
key = "Model::%s" % bone.name
bindmat, _ = bone.getBindMatrix(config.offset)
if config.binary:
id, _ = getId(key)
fbx_binary.fbx_data_pose_node_element(pelem, key, id, bindmat)
else:
poseNode(fp, key, bindmat)
stopLinking()
if not config.binary:
fp.write(" }\n")
def cone_transf_m(apex,L,r_y):
import numpy as np
import transformations
"""a matrix that rotates and translates coordinates. A cone defined by the coordinates of it's apex,
the vector L between the centre of its base and the apex, and a vector r_y between the centre of its base and a point of maximum radius (the cone can be elliptical)
is translated such that the apex is at the origin, the cone lies along the z axis and the radius of maximum width lies along the y-axis"""
trans = -np.array(apex).reshape(3,1)
angle1 = transformations.angle_between_vectors(L, [0,0,1])
if angle1:
rot_axis1 = np.cross(L,[0,0,1])
nrot_axis1 = rot_axis1/np.linalg.norm(rot_axis1)
rot1 = transformations.rotation_matrix(angle1, nrot_axis1)[:3,:3]
else:
rot1 = np.identity(3)
r_y = rot1.dot(r_y)
angle2 = transformations.angle_between_vectors(r_y, [0,1,0])
if angle2:
rot_axis2 = np.cross([0,1,0],r_y)
nrot_axis2 = rot_axis2/np.linalg.norm(rot_axis2)
rot2 = transformations.rotation_matrix(angle2, nrot_axis2)[:3,:3]
else:
rot2 = np.identity(3)
#combine rot matrices
rot = np.dot(rot1,rot2)
#make rotation matrix into an affine matrix
final_rot = np.identity(4)
final_rot[:3,:3] = rot
#make translation into an affine matrix
final_trans = np.identity(4)
final_trans[:3,3:] = trans
#combine translation and rotation
final = final_rot.dot(final_trans)
return final
def canonical_rot_mat(start_on_unit_sphere, end_on_unit_sphere):
theta = np.arccos(np.dot(start_on_unit_sphere, end_on_unit_sphere))
if epsilon_eq(theta, 0, theta_epsilon):
mat = identity_rotation.copy()
elif epsilon_eq(theta, np.pi, theta_epsilon):
mat = identity_rotation.copy()
mat[2,2] = -1.0
else:
v = np.cross(start_on_unit_sphere, end_on_unit_sphere)
mat = np.matrix(transformations.rotation_matrix(theta, v)[:3,:3])
end_test = times_column3(mat, start_on_unit_sphere)
assert sum(abs(end_on_unit_sphere - end_test)) < 1e-2, '%s -> %s' % (end_on_unit_sphere, end_test)
return mat
def set_block( self , s , d ) : aoy = m.atan2( s[2] , s[0] ) aoz = m.atan2( s[1] , m.sqrt(s[0]**2+s[2]**2) ) rot = tr.rotation_matrix( aoy , (0,1,0) ) rot = np.dot( tr.rotation_matrix( -aoz , (0,0,1) ) , rot ) rot = np.dot( tr.rotation_matrix( m.pi/2.0 , (0,0,1) ) , rot ) v , n , t = self.gen_v( 1 , 1 , s ) for x in range(v.shape[0]) : for y in range(v.shape[1]) : for z in range(v.shape[2]) : v[x,y,z] = np.dot(rot,v[x,y,z]) n[x,y,z] = np.resize(np.dot(rot,np.resize(n[x,y,z],4)),3) Mesh.__init__( self , buffers = (v,n,t) ) self.x = np.array( ((0,0,0,1),(s[0],0,0,1),(0,0,s[2],1),(s[0],0,s[2],1),(0,s[1],0,1),(s[0],s[1],0,1),(0,s[1],s[2],1),(s[0],s[1],s[2],1)) , np.float64 ) for i in range(len(self.x)) : self.x[i] = np.dot(rot,self.x[i]) self.r = np.resize( np.dot( rot , np.array((s[0],s[1],s[2],0) , np.float64 )/2.0 ) , 3 ) self.m = np.array( [ d*s[0]*s[1]*s[2] / 8.0 ] * len(self.x) , np.float64 ) self.M = self.calc_m( self.x , self.m ) self.Mi = np.linalg.inv( self.M )
def getMatrix(head, tail, roll):
vector = toBlender3(tail - head)
length = math.sqrt(dot(vector, vector))
vector = vector/length
yproj = dot(vector, YUnit)
if yproj > 1-1e-6:
axis = YUnit
angle = 0
elif yproj < -1+1e-6:
axis = YUnit
angle = pi
else:
axis = numpy.cross(YUnit, vector)
axis = axis / math.sqrt(dot(axis,axis))
angle = math.acos(yproj)
mat = tm.rotation_matrix(angle, axis)
if roll:
mat = dot(mat, tm.rotation_matrix(roll, YUnit))
mat = fromBlender4(mat)
mat[:3,3] = head
return length, mat
def stretchTo(self, goal, doStretch):
"""
Orient bone to point to goal position. Set doStretch to true to
position the tail joint at goal, false to maintain length of this bone.
"""
length, self.matPoseGlobal = _getMatrix(self.getHead(), goal, 0)
if doStretch:
factor = length/self.length
self.matPoseGlobal[:3,1] *= factor
pose = self.getPoseFromGlobal()
az,ay,ax = tm.euler_from_matrix(pose, axes='szyx')
rot = tm.rotation_matrix(-ay + self.roll_angle, Bone.Axes[1])
self.matPoseGlobal[:3,:3] = np.dot(self.matPoseGlobal[:3,:3], rot[:3,:3])
def sketch(robot):
res = ""
res += "def robot {\n"
oldT = numpy.eye(4)
for j in robot.moving_joint_list[:20]:
T = j.globalTransformation
T1 = numpy.eye(4)
if j.jointAxis in ['y','Y']:
T1 = tf.rotation_matrix(math.pi/2, [0,0,1])
if j.jointAxis in ['z','Z']:
T1 = tf.rotation_matrix(math.pi/2, [0,1,0])
T = numpy.dot(T,T1)
relT = numpy.dot(numpy.linalg.inv(oldT),T)
angle, direc, point = tf.rotation_from_matrix(relT)
print T
p = relT[:3,3]
p = [w*10 for w in p]
res += """put {rotate (%f,(%f, %f, %f),[%f, %f, %f]) then translate ([%f, %f, %f])} {joint}
"""%(angle*180/math.pi,0,0,0, direc[0], direc[1], direc[2], p[0],p[1],p[2])
oldT = T
res += "}\n"
return res
