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 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 _instantiate_node(surfaces, model, node, transform, texcache):
rot = node['rot']
transform = tf.concatenate_matrices(transform,
tf.translation_matrix(node['offset']),
_rotation_matrix(rot))
if node['mesh'] != -1:
mesh_transform = tf.concatenate_matrices(
transform, tf.translation_matrix(node['pivot']))
mesh = model.meshes[node['mesh']]
for k, surface in mesh.items():
try:
material_name = model.materials[surface['material']]
except:
continue # if there's no material, don't render the surface
surfaces.append({
'vertices':
_transform_vertices(mesh_transform, surface['vertices']),
'material':
texcache.load(material_name)
})
for child in node['children']:
_instantiate_node(surfaces, model, child, transform, texcache)
def make_next_step(self, prev_step: TrajectoryPoint) -> TrajectoryPoint:
point = TrajectoryPoint()
point.timestamp = prev_step.timestamp + 1 / SERVO_FREQ
phase = prev_step.phase + 1 / (SERVO_FREQ * GAIT_PERIOD)
z_lift = self.leg_lift * sin(phase * pi)
current_lift = tf.translation_matrix((0.0, 0.0, z_lift))
if phase > 1.0:
phase = 0
point.left_pose = prev_step.next_left_stand
point.right_pose = prev_step.next_right_stand
# TODO after sending to stm, publish first element as frontPoint
next_stand = self.path_proxy.get_point(point.timestamp)
if prev_step.left_moving:
point.prev_left_stand = prev_step.next_left_stand
point.next_left_stand = prev_step.next_left_stand
point.prev_right_stand = prev_step.next_right_stand
point.next_right_stand = next_stand
else:
point.prev_right_stand = prev_step.next_right_stand
point.next_right_stand = prev_step.next_right_stand
point.prev_left_stand = prev_step.next_left_stand
point.next_left_stand = next_stand
point.left_moving = not prev_step.left_moving
else:
if prev_step.left_moving:
left_shift = interpolate(prev_step.prev_left_stand,
prev_step.next_left_stand,
phase)
point.left_pose = tf.concatenate_matrices(
# prev_step.prev_left_stand,
left_shift,
current_lift)
point.right_pose = prev_step.right_pose
else:
right_shift = interpolate(prev_step.prev_right_stand,
prev_step.next_right_stand,
phase)
point.right_pose = tf.concatenate_matrices(
# prev_step.prev_right_stand,
right_shift,
current_lift)
point.left_pose = prev_step.left_pose
point.next_left_stand = prev_step.next_left_stand
point.next_right_stand = prev_step.next_right_stand
point.prev_left_stand = prev_step.prev_left_stand
point.prev_right_stand = prev_step.prev_right_stand
point.left_moving = prev_step.left_moving
point.phase = phase
point.base_link_pose = midpoint(point.left_pose, point.right_pose)
point.base_link_pose[2, 3] = self.base_height
return point
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 to_homogeneous(vec):
d = [float(x) for x in vec]
if len(d) != 7 and len(d) != 3:
raise ValueError("Only poses with 3 or 7 elements supported! this one has %d" % len(d))
if len(d) == 7:
return concatenate_matrices( translation_matrix(d[0:3]), quaternion_matrix([d[6]]+d[3:6]) )
return concatenate_matrices( translation_matrix(d[0:2]+[0]), rotation_matrix(d[2], [0,0,1]) )
def triangle(self, center: ndarray,
left_side: bool) -> Tuple[ndarray, ndarray, ndarray]:
mult = 1 if left_side else -1
x_shift = 0.15
side_y_shift = mult * 0.17
mid_y_shift = mult * -0.2
z = 0.0
front_point = tf.concatenate_matrices(
center, tf.translation_matrix((x_shift, side_y_shift, z)))
mid_point = tf.concatenate_matrices(
center, tf.translation_matrix((0.0, mid_y_shift, z)))
rear_point = tf.concatenate_matrices(
center, tf.translation_matrix((-x_shift, side_y_shift, z)))
return front_point, mid_point, rear_point
def _unpack(x):
trans = x[:3]
eulxyz = x[3:]
T = tf.concatenate_matrices(tf.translation_matrix(trans),
tf.euler_matrix(eulxyz[0], eulxyz[1],
eulxyz[2], 'sxyz'))
return T
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 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 new_trajectory(self, msg: Path):
# if len(self.curr_path) == 0:
# self.curr_path.append(self.transform('odom', 'front_point'))
self.curr_path = [self.transform('odom', 'front_point')]
# print(self.curr_path)
projection_to_odom = self.transform('odom', 'base_link')
projection_to_odom[2, 3] = 0.0
dense_path = [
tf.concatenate_matrices(pose_to_matrix(pose_stamped.pose),
projection_to_odom)
for pose_stamped in msg.poses
]
dense_path = [dense_path[0]]
first_new = dense_path[0]
last_idx, last_point = self.get_last_closest(first_new, self.curr_path)
# offset = 2
# last_idx = last_idx - offset if last_idx - offset >= 0 else last_idx
last_point = self.curr_path[last_idx]
# print(f'Last: {last_point}')
# print(f'New: {first_new}')
inter_path = self.interpolate_path(last_point, first_new)
print(f'Distance: {matrix_dist(last_point, first_new)}')
print(f'Interpolated: {len(inter_path)}')
sparse_path = self.make_sparse(inter_path[-1], dense_path)
# fixed_orientation = self.fix_orientation(
# self.curr_path[last_idx - 1], inter_path + sparse_path
# ) # TODO don't
self.curr_path = self.curr_path[:last_idx] + inter_path + sparse_path
front_point = self.transform('odom', 'front_point')
front_point_idx, _ = self.get_last_closest(front_point, self.curr_path)
self.publish_path(self.curr_path[front_point_idx + 1:])
def coil_transform_pos(pos):
pos[1] = -pos[1]
a, b, g = np.radians(pos[3:])
r_ref = tf.euler_matrix(a, b, g, 'sxyz')
t_ref = tf.translation_matrix(pos[:3])
m_img = tf.concatenate_matrices(t_ref, r_ref)
return m_img
def pose_to_matrix(pose: Pose) -> ndarray:
rot = pose.orientation
trans = pose.position
rot_matrix = tf.quaternion_matrix((rot.w, rot.x, rot.y, rot.z))
trans_matrix = tf.translation_matrix((trans.x, trans.y, trans.z))
return tf.concatenate_matrices(trans_matrix, rot_matrix)
def _x2transform(self, x):
trans, eulxyz, scale = self._unpack(x)
origin = [0, 0, 0]
T = tf.concatenate_matrices(tf.translation_matrix(trans),
tf.euler_matrix(eulxyz[0], eulxyz[1],
eulxyz[2], 'sxyz'),
tf.scale_matrix(scale, origin))
return T
def transform_pad(padcoords, pad_length, pad_separation, pad_thickness,
drum_separation, drum_radius, totalAlpha):
TranG = trn.translation_matrix((padcoords[0], padcoords[1], padcoords[2]))
if padcoords[4] != 0:
RotG = trn.rotation_matrix(padcoords[4], [0, 1, 0])
else:
RotG = trn.identity_matrix()
TranJ = trn.translation_matrix(((pad_length + pad_separation), 0, 0))
if (padcoords[0] + pad_separation + pad_length) > (drum_separation / 2):
TranJ_Rot = trn.translation_matrix(
(-(pad_length + pad_separation) / 2, 0, 0))
alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0] < -math.pi:
alpha -= (totalAlpha[0] - math.pi)
totalAlpha[0] = -math.pi
RotJ = trn.rotation_matrix(
alpha, [0, 1, 0],
[TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
elif (padcoords[0] - pad_separation - pad_length) < -(drum_separation / 2):
TranJ_Rot = trn.translation_matrix(
(-(pad_length + pad_separation) / 2, 0, 0))
alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0] < -2 * math.pi:
alpha -= (totalAlpha[0] - 2 * math.pi)
totalAlpha[0] = -2 * math.pi
RotJ = trn.rotation_matrix(
alpha, [0, 1, 0],
[TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
else:
Final = trn.concatenate_matrices(TranG, RotG, TranJ)
angle, direction, point = trn.rotation_from_matrix(Final)
padcoords = [Final[0][3], Final[1][3], Final[2][3], 0, angle, 0]
return padcoords
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 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 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 relative_to_reference(reference, target):
if len(reference) != len(target):
raise ValueError("reference and target should be of same length! len(reference): %d, len(target): %d" % (len(reference), len(target)) )
F_ref = to_homogeneous(reference)
F_tar = to_homogeneous(target)
F_ref_inv = inverse_matrix(F_ref)
T_tar_ref = concatenate_matrices(F_ref_inv, F_tar)
return from_homogeneous(T_tar_ref, len(reference))
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 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 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 process_april_tag(pose):
## Aquí jugar
## pose es tag con respecto al robot
## T_a es la transformación del april tag con respecto al mapa
T_a = tf.translation_matrix([-X * tile_size, -tag_size * 3/4, y*tile_size])
R_a = tf.euler_matrix(0, angle, 0)
T_m_a = tf.concatenate_matrices(T_a, R_a)
## Aquí dando vuelta el robot, por cuenta del cambio de los angulos
T_r_a = np.dot(pose, tf.euler_matrix(0, np.pi, 0))
##print(T_r_a)
T_a_r = np.linalg.inv(T_r_a)
T_m_r = np.dot(T_m_a, T_a_r)
print(pose @ T_m_r)
def compound(reference, diff, inv_diff = False):
if len(reference) != len(diff):
raise ValueError("reference and diff should be of same length! len(reference): %d, len(diff): %d" % (len(reference), len(diff)) )
F_ref = to_homogeneous(reference)
T_d = to_homogeneous(diff)
if inv_diff:
T_d = inverse_matrix(T_d)
F_d = concatenate_matrices( F_ref, T_d )
return from_homogeneous(F_d, len(reference))
def fromString(s):
"""assume s looks like: 'o 0.3 0.5 -2 q 1 2 3 4'
nb: we can't represent skew and scale but for our purposes
this is fine.
"""
vals = s.split(" ")
assert (len(vals) == 9)
assert (vals[0] == "o")
assert (vals[4] == "q")
x = [float(vals[1]), float(vals[2]), float(vals[3])]
q = [float(vals[5]), float(vals[6]), float(vals[7]), float(vals[8])]
om = xform.translation_matrix(x)
qm = xform.quaternion_matrix(q)
result = xform.concatenate_matrices(om, qm)
return Affine3(result)
def interpolate(start: np.ndarray, stop: np.ndarray, phase: float) -> np.ndarray:
start_trans = tf.translation_from_matrix(start)
stop_trans = tf.translation_from_matrix(stop)
shift_trans = phase * (stop_trans - start_trans)
inter_trans = start_trans + shift_trans
trans_matrix = tf.translation_matrix(inter_trans)
start_rot = np.array(tf.euler_from_matrix(start, axes='sxyz'))
stop_rot = np.array(tf.euler_from_matrix(stop, axes='sxyz'))
shift_rot = phase * (stop_rot - start_rot)
inter_rot = start_rot + shift_rot
rot_matrix = tf.euler_matrix(*inter_rot, axes='sxyz')
result = tf.concatenate_matrices(trans_matrix, rot_matrix)
return result
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 errors(reference, target):
if len(reference) != len(target):
raise ValueError("reference and target should be of same length! len(reference): %d, len(target): %d" % (len(reference), len(target)) )
F_ref = to_homogeneous(reference)
F_tar = to_homogeneous(target)
F_ref_inv = inverse_matrix(F_ref)
T_tar_ref = concatenate_matrices(F_ref_inv, F_tar)
trans = translation_from_matrix(T_tar_ref)
ang, foo, bar = rotation_from_matrix(T_tar_ref)
return (numpy.sum( numpy.square(trans) ), ang*ang)
def transform_pad(padcoords,pad_length,pad_separation,pad_thickness,drum_separation,drum_radius,totalAlpha):
TranG = trn.translation_matrix(( padcoords[0],padcoords[1],padcoords[2] ))
if padcoords[4]!=0:
RotG = trn.rotation_matrix(padcoords[4],[0,1,0])
else:
RotG = trn.identity_matrix()
TranJ = trn.translation_matrix(( (pad_length+pad_separation),0,0))
if (padcoords[0]+pad_separation+pad_length) >(drum_separation/2):
TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0]<-math.pi:
alpha -= (totalAlpha[0] - math.pi)
totalAlpha[0] = -math.pi
RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
elif (padcoords[0]-pad_separation-pad_length)<-(drum_separation/2):
TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
totalAlpha[0] += alpha
if totalAlpha[0]<-2*math.pi:
alpha -= (totalAlpha[0] - 2*math.pi)
totalAlpha[0] = -2*math.pi
RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
angle, direction, point = trn.rotation_from_matrix(Final)
else :
Final = trn.concatenate_matrices(TranG,RotG,TranJ)
angle, direction, point = trn.rotation_from_matrix(Final)
padcoords = [Final[0][3],Final[1][3],Final[2][3],0,angle,0]
return padcoords
def transform_cloud(cloud_in, trans=None, quat=None, mat=None):
'''
Tranform point cloud np array
'''
if trans is not None and quat is not None:
R = transformations.quaternion_matrix(quat)
T = transformations.translation_matrix(trans)
total_transform = transformations.concatenate_matrices(T, R)
elif mat is not None:
total_transform = mat
cloud_in = np.hstack((cloud_in, np.ones((cloud_in.shape[0], 1))))
cloud_out = np.matmul(total_transform, np.transpose(cloud_in))
cloud_out = np.transpose(cloud_out)[:, :3]
cloud_out = np.array(cloud_out, dtype=np.float32)
return cloud_out
def point_cloud_gen(camera_info, rawImage):
max_depth = 600 # Default Max Distance Field in UE4
# Intrinsic Matrix for 512x512 resolution
K = np.array([[64., 0., 64.], [0., 64., 64.], [0., 0., 1.]])
K = np.matmul(K, np.array([[0, 0, -1], [0, 1, 0],
[1, 0, 0]])) # match the coordinate system
K_inv = np.linalg.inv(K) # reflect the image back to the frame
pointsU = np.zeros((128, 128))
pointsV = np.zeros((128, 128))
points1 = np.ones((128, 128))
for i in range(pointsU.shape[0]):
for j in range(pointsU.shape[1]):
pointsU[i, j] = i
pointsV[i, j] = j
pointsUV = np.stack((pointsU, pointsV), axis=-1)
pointsUV1 = np.stack((pointsU, pointsV, points1), axis=-1)
w_val = camera_info.pose.orientation.w_val
x_val = camera_info.pose.orientation.x_val
y_val = camera_info.pose.orientation.y_val
z_val = camera_info.pose.orientation.z_val
x_cor = camera_info.pose.position.x_val
y_cor = camera_info.pose.position.y_val
z_cor = camera_info.pose.position.z_val * (-1)
array = np.asarray(rawImage.image_data_float)
depth = array.reshape((-1))
depth3 = np.stack((depth, depth, depth), axis=-1)
proj = np.matmul(K_inv, pointsUV1.reshape((-1, 3)).T).T
drone3d = np.multiply(proj,
depth3) # point cloud without the extrinsic matrix
drone3d1 = np.append(drone3d, np.ones((drone3d.shape[0], 1)), 1)
trans = transformations.translation_matrix([x_cor, y_cor, z_cor])
rot = transformations.quaternion_matrix([w_val, x_val, y_val, z_val])
transform = transformations.concatenate_matrices(trans, rot)
T = transform
drone3d1world = np.matmul(T, drone3d1.T).T
points = drone3d1world[:, :3] # point cloud
return points
def get_coxa_position(self, body_xyz, body_angle):
"""find x, y, z of coxa joint in mm given body position"""
a, b, c = body_angle
u, v, w = body_xyz
rotation = tf.euler_matrix(a, b, c, 'rxyz')
translation = tf.translation_matrix([u, v, w])
transform = tf.concatenate_matrices(rotation, translation)
position = transform.dot([self.rel_x, self.rel_y, self.rel_z, 1])
reference1 = transform.dot([100, 100, 0,
1]) # another vector on the body plane
reference2 = transform.dot([-100, 100, 0,
1]) # another vector on the body plane
#print("\tBody position xyz: "+string_point([u,v,w]))
#print("\tCoxa joint xyz: "+string_point(position))
#print("\tLeg xyz: "+string_point([self.x, self.y, self.z]))
return (position, reference1, reference2) # have extra indices
def draw(self, surface, 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)
start = tr.translation_from_matrix(baseTransform)
end = tr.translation_from_matrix(transform)
# Only use the y and z coords for a quick and dirty orthogonal projection
pygame.draw.line(surface, (0, 0, 0), start[::2], end[::2], 5)
# Draw angle constraints
#pygame.draw.aaline(surface, (100, 0, 0), position, position + to_vector(self.pitch.absolute_min(base_angle), 30))
#pygame.draw.aaline(surface, (100, 0, 0), position, position + to_vector(self.pitch.absolute_max(base_angle), 30))
if self.after is not None:
self.after.draw(surface, transform, parameters[2:])
def poseGlobal(matrix,x_ob,y_ob,angulo):
tag_size = 0.18
tile_size = 0.585
T_a = tf.translation_matrix([
-x_ob, -tag_size*3/4, y_ob]) # esto ya viene multiplicado por el tile_size
R_a = tf.euler_matrix(0,angulo,0)
T_m_a = tf.concatenate_matrices(T_a, R_a)
# pose tag con respecto al robot
T_r_a = np.dot(matrix, tf.euler_matrix(0, np.pi, 0))
# pose tag con respecto al mapa
T_a_r = np.linalg.inv(T_r_a) # T_r_a-1
T_m_r = np.dot(T_m_a, T_a_r)
return T_m_r
def global_pose(matrix,x_ob,y_ob,angle): # matrix es la pose del apriltag x_ob e y_ob es el x e y del apriltag
tag_size = 0.18
tile_size = 0.585
T_a = tf.translation_matrix([
-x_ob, -tag_size*3/4, y_ob]) # esto ya viene multiplicado por el tile_size
R_a = tf.euler_matrix(0,angle,0)
T_m_a = tf.concatenate_matrices(T_a, R_a)
# pose tag con respecto al robot
T_r_a = np.dot(matrix, tf.euler_matrix(0, np.pi, 0))
# pose tag con respecto al mapa
T_a_r = np.linalg.inv(T_r_a) # T_r_a-1
T_m_r = np.dot(T_m_a, T_a_r)
return T_m_r
def scale_and_translate(Tlistgroundtruth, Tlistartoolkit):
ground_truth_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistgroundtruth])
artoolkit_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
for T in Tlistartoolkit])
Tscale = tf.affine_matrix_from_points(artoolkit_pointcloud.T,
ground_truth_pointcloud.T,
shear=False,
scale=True)
#print("Scaling by {0}".format(tf.scale_from_matrix(Tscale)[0]))
try:
print("translation by {0}".format(tf.translation_from_matrix(Tscale)))
print("rotation by {0}".format(tf.rotation_from_matrix(Tscale)[0]))
except ValueError:
pass
Tlistartoolkit = [tf.concatenate_matrices(Tscale, T) for T in Tlistartoolkit]
return Tlistartoolkit
def add_orientation(self, prev_point:Tuple[float, float],
path: List[Tuple[float, float]]) -> List[Matrix]:
new_path = []
for point in path:
x_prev, y_prev = prev_point
x_curr, y_curr = point
x, y, = x_curr - x_prev, y_curr - y_prev
rot = np.arctan2(y, x)
oriented_point = tf.concatenate_matrices(
tf.translation_matrix((x_curr, y_curr, 0)),
tf.rotation_matrix(rot, (0, 0, 1))
)
new_path.append(oriented_point)
prev_point = point
return new_path
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 update3DPoints(self, newPoints):
center = sum([np.array(roundPoint(x))
for x in newPoints]) / 2 - self.minor
diff = newPoints[0] - newPoints[1]
radius = ((diff[1]**2 + diff[0]**2)**(0.5)) / 2
scaled = np.concatenate((self.primitivePoints,
np.ones(
(1, np.shape(self.primitivePoints)[1]))),
axis=0)
theta = np.arctan2(diff[1], diff[0])
zaxis = [0, 1, 0]
rotation = trans.rotation_matrix(theta, zaxis)
scale = trans.scale_matrix(radius)
translation = trans.translation_matrix([center[0], 0, -center[1]])
complete = trans.concatenate_matrices(translation, rotation, scale)
affineTrans = np.matmul(complete, scaled)
if (self.objectPoints.any()):
self.objectPoints = np.concatenate(
(self.objectPoints, np.transpose(affineTrans)), axis=0)
else:
self.objectPoints = np.transpose(affineTrans)
def create_pair_with_orientation(self):
# pair is constructed with p_i at the origin and p_j translated along
# the x-axis by distance of r. z serves as the orientation reference
# axis.
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)
mat_p_i = setup_particle_orientation(0, self.theta_i, self.phi_i)
mat_p_j = setup_particle_orientation(1, self.theta_j, self.phi_j)
# rotate p_j about separation axis (x-axis) create target omega
omega_mat = transformations.rotation_matrix(self.omega, (1, 0, 0))
mat_p_j = transformations.concatenate_matrices(omega_mat, mat_p_j)
return Pair(Particle((0,0,0), mat_p_i),
Particle((self.r,0,0), mat_p_j))
def transform(R, t):
Tr = tf.identity_matrix()
Tr[:3, :3] = R
return tf.concatenate_matrices(Tr, tf.translation_matrix(t))
def transform_from_quat(pos, quat):
T = tf.concatenate_matrices(tf.translation_matrix(pos),
tf.quaternion_matrix(quat))
return T
def transform(pos, eulxyz):
ex, ey, ez = eulxyz * np.pi / 180
return tf.concatenate_matrices(tf.translation_matrix(pos),
tf.euler_matrix(ex, ey, ez, 'sxyz'))
