def test_filterreg_registration_pt2pl(self):
res = filterreg.registration_filterreg(self._source, self._target, self._target_normals)
res_rot = trans.identity_matrix()
res_rot[:3, :3] = res.transformation.rot
ref_rot = trans.identity_matrix()
ref_rot[:3, :3] = self._tf.rot
self.assertTrue(np.allclose(trans.euler_from_matrix(res_rot),
trans.euler_from_matrix(ref_rot), atol=2.0e-1, rtol=1.0e-1))
self.assertTrue(np.allclose(res.transformation.t, self._tf.t, atol=1.0e-2, rtol=1.0e-3))
def test_svr_registration(self):
res = l2dist_regs.registration_svr(self._source, self._target)
res_rot = trans.identity_matrix()
res_rot[:3, :3] = res.rot
ref_rot = trans.identity_matrix()
ref_rot[:3, :3] = self._tf.rot
self.assertTrue(np.allclose(trans.euler_from_matrix(res_rot),
trans.euler_from_matrix(ref_rot), atol=1.0e-1, rtol=1.0e-1))
self.assertTrue(np.allclose(res.t, self._tf.t, atol=1.0e-2, rtol=1.0e-3))
def test_cpd_registration(self):
res = cpd.registration_cpd(self._source, self._target)
res_rot = trans.identity_matrix()
res_rot[:3, :3] = res.transformation.rot
ref_rot = trans.identity_matrix()
ref_rot[:3, :3] = self._tf.rot
self.assertTrue(np.allclose(trans.euler_from_matrix(res_rot),
trans.euler_from_matrix(ref_rot), atol=1.0e-2, rtol=1.0e-2))
self.assertTrue(np.allclose(res.transformation.t, self._tf.t, atol=1.0e-4, rtol=1.0e-4))
def __init__(self, human, options):
self.human = human
self.posebones = OrderedDict()
self.modifier = None
self.restPosition = False
self.dirty = True
self.frames = []
self.controls = []
self.deforms = []
amt = self.armature = Armature("Armature", options)
amt.parser = Parser(amt, human)
debugCoords("Pose1")
amt.setup()
log.debug("Head %s" % amt.bones["head"].head)
amt.normalizeVertexWeights(human)
self.matrixGlobal = tm.identity_matrix()
self.deforms = []
for bone in amt.bones.values():
pb = self.posebones[bone.name] = PoseBone(self, bone)
pb.build()
if pb.bone.deform:
self.deforms.append(pb)
self.storeCoords()
nVerts = len(human.meshData.coord)
self.restCoords = np.zeros((nVerts,4), float)
self.restCoords[:,3] = 1
self.syncRestVerts("rest")
debugCoords("Pose2")
def __init__(self, human, config):
self.name = "Armature"
self.config = config
self.human = human
self.modifier = None
self.restPosition = False
self.dirty = True
self.frames = []
self.bones = {}
self.boneList = []
self.roots = []
self.controls = []
self.deforms = []
if config.rigtype == 'mhx':
self.visible = VISIBLE_LAYERS
self.last = 32
else:
self.visible = 1
self.last = 1
self.matrixGlobal = tm.identity_matrix()
self.restCoords = None
self.boneWeights = {}
if config.vertexWeights:
self.vertexgroups = config.vertexWeights
elif config.rigtype == "mhx":
self.vertexgroups = {}
for name in ["head", "bones", "palm"]:
mhx.mhx_main.getVertexGroups(name, self.vertexgroups)
def store(self):
shadowBones = {}
for bone in self.boneList:
shadowBones[bone.name] = bone.matrixPose
bone.matrixPose = tm.identity_matrix()
#self.listPose()
return shadowBones
def gradient_descent(armature, initial_parameters, target_pos, iterations):
parameters = np.array(initial_parameters, copy=True, dtype=np.float32)
parameters_min = armature.min_parameters()
parameters_max = armature.max_parameters()
automatic_parameters = armature.auto_parameters()
delta = .0001 # For calculating the derivative
tBase = tr.identity_matrix()
for iter in range(iterations):
base_error = armature.error(target_pos, tBase, parameters)
# Calculate derivative for each axis
derivative = np.empty(len(parameters))
for i in range(len(parameters)):
if automatic_parameters[i]:
test_parameters = np.copy(parameters)
test_parameters[i] += delta
new_error = armature.error(target_pos, tBase, test_parameters)
derivative[i] = (new_error - base_error) / delta
else:
derivative[i] = 0
# Step in the direction of the derivative to a given amount
# Cap the size of the step
parameter_step = min(base_error, 30) * .00005
parameters -= derivative * parameter_step
parameters = np.clip(parameters, parameters_min, parameters_max)
return parameters
def hand_obj_transform(self, hand_points, obj_points):
# We align the hand with the object by matching a frame at the grasp center
frame_hand = self.get_tri_frame(
hand_points) # [x; y; z] of this frame in the hand frame
frame_obj = self.get_tri_frame(
obj_points) # [x; y; z] of this frame in the object frame
# Let's build a transformation matrix from this
T = transformations.identity_matrix()
# frame_hand is a rotation matrix that rotates the hand frame to our helper frame at the grasp center
T[0:3, 0:3] = np.dot(frame_obj, np.transpose(
frame_hand)) # transpose == inverse for rotation matrices
# rotate the hand points to a frame that is aligned with the object frame, but located at the grasp center
# we call this frame rotated hand frame
new_hand_points = np.transpose(
np.dot(T[0:3, 0:3], np.transpose(hand_points)))
# use this to compute the translation from object to hand frame
obj_c = np.sum(
obj_points,
axis=0) / 3. # the position of the grasp center in object frame
new_hand_c = np.sum(
new_hand_points, axis=0
) / 3. # the position of the grasp center in the rotated hand frame
# Finally, the translation is from origin to obj_c and then from there in the opposite direction of new_hand_c
T[:3, -1] = np.transpose(obj_c - new_hand_c)
return T
def write_rotation(tf, scene_num):
#
# rot =
# | rot[00] rot[01] rot[02] t[0] |
# | rot[10] rot[11] rot[12] t[1] |
# | rot[20] rot[21] rot[22] t[2] |
# | 0 0 0 1 |
#
rot = trans.identity_matrix()
rot[:3, :3] = tf.rot
rot[0][3] = tf.t[0]
rot[1][3] = tf.t[1]
rot[2][3] = tf.t[2]
print(rot)
print("result: ", np.rad2deg(trans.euler_from_matrix(rot)), tf.scale, tf.t)
path = '/mnt/container-data/rotation/' + scene_num + ".txt"
with open(path, mode='w') as f:
for i in range(4):
for j in range(4):
f.write(np.array2string(rot[j][i]) + "\n")
def gradient_descent(armature, initial_parameters, automatic_parameters,
target_pos, iterations):
parameters = np.copy(initial_parameters)
parameters_min = armature.min_parameters()
parameters_max = armature.max_parameters()
parameter_step = np.full(len(parameters), .0005)
delta = .0001
tBase = tr.identity_matrix()
for iter in range(iterations):
base_error = armature.error(target_pos, tBase, parameters)
# Calculate derivative for each axis
derivative = np.empty(len(parameters))
for i in range(len(parameters)):
if automatic_parameters[i]:
test_parameters = np.copy(parameters)
test_parameters[i] += delta
new_error = armature.error(target_pos, tBase, test_parameters)
derivative[i] = (new_error - base_error) / delta
else:
derivative[i] = 0
# Step in the direction of the derivative to a given amount
parameter_step *= .9
parameters -= derivative * parameter_step
parameters = np.clip(parameters, parameters_min, parameters_max)
return parameters
def __init__(self, matrix_aff=None, transformation=None):
if matrix_aff is not None:
self.matrix_aff = np.copy(matrix_aff)
elif transformation is not None:
self.matrix_aff = np.copy(transformation.matrix_aff)
else:
self.matrix.aff = tf_aff.identity_matrix()
def __init__(self, human, config):
CArmature.__init__(self, "Armature", human, config)
self.modifier = None
self.restPosition = False
self.dirty = True
self.frames = []
self.bones = {}
self.boneList = []
self.roots = []
self.controls = []
self.deforms = []
"""
if self.rigtype == 'mhx':
self.visible = VISIBLE_LAYERS
self.last = 32
else:
self.visible = 1
self.last = 1
"""
self.setup()
self.matrixGlobal = tm.identity_matrix()
self.restCoords = None
self.boneWeights = {}
def store(self):
shadowBones = {}
for pb in self.posebones.values():
shadowBones[pb.name] = pb.matrixPose
pb.matrixPose = tm.identity_matrix()
#self.listPose()
return shadowBones
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2
block.'''
self.pub_imu = True
#self.imu_msg = self.imu_pub.initProto()
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']),
radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
w, x, y, z = convert_quat((w, x, y, z), o['frame'])
roll, pitch, yaw = euler_from_quaternion((w, x, y, z))
self.imu_msg.angleRoll = roll
self.imu_msg.anglePitch = pitch
self.imu_msg.angleYaw = yaw
def store(self):
shadowBones = {}
for pb in self.posebones.values():
shadowBones[pb.name] = pb.matrixPose
pb.matrixPose = tm.identity_matrix()
#self.listPose()
return shadowBones
def build_tree(self, nb_iterations, size, diameter):
'''
@brief Build a 3D tree from L-System parameters
@param nb_iterations number of iterations to apply to the base word
@param size trunk size
@param diameter trunk diameter
@return The 3D tree
'''
logger.info("Building Word with " + str(nb_iterations) + " iterations")
self.build_word(nb_iterations)
logger.info("==== Done ====")
word_length = len(self.word)
root = Node(tr.identity_matrix(), size, diameter)
self.current_node = root
self.current_rho_angle = 0
self.current_phi_angle = 0
self.current_index = 0
word_length = len(self.word)
# For each letter of the word
# Apply the L-system rules
while self.current_index < word_length:
letter = self.word[self.current_index]
self.current_index += 1
self.interpretation[letter](self)
return Tree(root)
def __init__(self, human, config):
CArmature.__init__(self, "Armature", human, config)
self.modifier = None
self.restPosition = False
self.dirty = True
self.frames = []
self.bones = {}
self.boneList = []
self.roots = []
self.controls = []
self.deforms = []
"""
if self.rigtype == 'mhx':
self.visible = VISIBLE_LAYERS
self.last = 32
else:
self.visible = 1
self.last = 1
"""
self.setup()
self.matrixGlobal = tm.identity_matrix()
self.restCoords = None
self.boneWeights = {}
def __init__(self, human, options):
self.human = human
self.posebones = OrderedDict()
self.modifier = None
self.restPosition = False
self.dirty = True
self.frames = []
self.controls = []
self.deforms = []
amt = self.armature = Armature("Armature", options)
amt.parser = Parser(amt, human)
debugCoords("Pose1")
amt.setup()
log.debug("Head %s" % amt.bones["head"].head)
amt.normalizeVertexWeights(human)
self.matrixGlobal = tm.identity_matrix()
self.deforms = []
for bone in amt.bones.values():
pb = self.posebones[bone.name] = PoseBone(self, bone)
pb.build()
if pb.bone.deform:
self.deforms.append(pb)
self.storeCoords()
nVerts = len(human.meshData.coord)
self.restCoords = np.zeros((nVerts,4), float)
self.restCoords[:,3] = 1
self.syncRestVerts("rest")
debugCoords("Pose2")
def store(self):
shadowBones = {}
for bone in self.boneList:
shadowBones[bone.name] = bone.matrixPose
bone.matrixPose = tm.identity_matrix()
#self.listPose()
return shadowBones
def __init__(self, human, config):
self.name = "Armature"
self.config = config
self.human = human
self.modifier = None
self.restPosition = False
self.dirty = True
self.frames = []
self.bones = {}
self.boneList = []
self.roots = []
self.controls = []
self.deforms = []
if config.rigtype == 'mhx':
self.visible = VISIBLE_LAYERS
self.last = 32
else:
self.visible = 1
self.last = 1
self.matrixGlobal = tm.identity_matrix()
self.restCoords = None
self.boneWeights = {}
if config.vertexWeights:
self.vertexgroups = config.vertexWeights
elif config.rigtype == "mhx":
self.vertexgroups = {}
for name in ["head", "bones", "palm"]:
mhx.mhx_main.getVertexGroups(name, self.vertexgroups)
def clear(self, update=False):
log.message("Clear armature")
for bone in self.boneList:
bone.matrixPose = tm.identity_matrix()
if update:
halt
self.update()
self.removeModifier()
def clear(self, update=False):
log.message("Clear armature")
for pb in self.posebones.values():
pb.matrixPose = tm.identity_matrix()
if update:
halt
self.update()
self.removeModifier()
def clear(self, update=False):
log.message("Clear armature")
for pb in self.posebones.values():
pb.matrixPose = tm.identity_matrix()
if update:
halt
self.update()
self.removeModifier()
def clear(self, update=False):
log.message("Clear armature")
for bone in self.boneList:
bone.matrixPose = tm.identity_matrix()
if update:
halt
self.update()
self.removeModifier()
def joints(self, parameters):
"""Returns the points representing the location of each joint in the arm
(convienant for drawing). You may have to do transformations on these points
to suit your graphical environment. One reccomendation is to intrepret the x
and z coordinates as x and y (using something like point[::2])
"""
return [np.zeros(3)] + self._joints_impl(tr.identity_matrix(),
parameters)
def reset_robot(self):
shift = transformations.identity_matrix()
shift[0, -1] = 0.2
self._robot.SetTransform(shift)
# Set hand to default (mean) configuration
mean_values = map(lambda min_v, max_v: (min_v + max_v) / 2.0,
self._robot.GetDOFLimits()[0],
self._robot.GetDOFLimits()[1])
self._robot.SetDOFValues(mean_values, range(len(mean_values)))
def get_transform_to_world(self):
mat_w_2_mocap = transformations.identity_matrix()
exist_mat_w_2_mocap = False
mat_mocap_2_link = transformations.identity_matrix()
exists_mat_mocap_2_link = False
mat_link_2_rgb = transformations.identity_matrix()
exists_mat_link_2_rgb = False
mat_optical = transformations.identity_matrix()
exists_mat_optical = False
with open(self.filename) as csv_file:
csv_reader = csv.reader(csv_file, delimiter=',')
for row in csv_reader:
if (not exist_mat_w_2_mocap) and (row[3] == "world" and row[4]
== "kinect2_mocap"):
exist_mat_w_2_mocap = True
mat_w_2_mocap = matrix_from_row(row)
if (not exists_mat_mocap_2_link) and (
row[3] == "/kinect2_mocap"
and row[4] == "/kinect2_link"):
exists_mat_mocap_2_link = True
mat_mocap_2_link = matrix_from_row(row)
if (not exists_mat_link_2_rgb) and (
row[3] == "kinect2_link"
and row[4] == "kinect2_rgb_optical_frame"):
exists_mat_link_2_rgb = True
mat_link_2_rgb = matrix_from_row(row)
if (not exists_mat_link_2_rgb) and (
row[3] == "kinect2_rgb_optical_frame"
and row[4] == "kinect2_ir_optical_frame"):
exists_mat_optical = True
mat_optical = matrix_from_row(row)
if exist_mat_w_2_mocap and exists_mat_mocap_2_link and exists_mat_link_2_rgb and exists_mat_optical:
break
else:
warnings.warn("Warning! No transformation in world found")
#print(mat_w_2_mocap, mat_mocap_2_link, mat_link_2_rgb)
# return mat_mocap_2_link.dot(transformations.inverse_matrix(mat_link_2_rgb.dot(mat_w_2_mocap)))
return mat_w_2_mocap.dot(
mat_mocap_2_link.dot(mat_link_2_rgb.dot(mat_optical)))
def load_hand(self, hand_file, hand_cache_file):
if not self._hand_loaded:
# TODO make this Robotiq hand independent (external hand loader)
self._robot = RobotiqHand(hand_cache_file=hand_cache_file,
env=self._orEnv, hand_file=hand_file)
self._hand_manifold = self._robot.get_hand_manifold()
self._hand_manifold.load()
self._num_contacts = self._robot.get_contact_number()
shift = transformations.identity_matrix()
shift[0, -1] = 0.2
self._robot.SetTransform(shift)
rospy.loginfo('Hand loaded in OpenRAVE environment')
self._hand_loaded = True
def look_at(eye, center, up):
f = unit_vector(center - eye)
u = unit_vector(up)
s = unit_vector(np.cross(f, u))
u = np.cross(s, f)
result = transformations.identity_matrix()
result[:3, 0] = s
result[:3, 1] = u
result[:3, 2] = -f
result[3, 0] = -np.dot(s, eye)
result[3, 1] = -np.dot(u, eye)
result[3, 2] = np.dot(f, eye)
return result.T
def fromAxes(xtgt, ytgt, ztgt):
"""return Affine3 representing rotation of axes to targets
NB: all targets should be normalized (unit length).
"""
m = xform.identity_matrix()
m[0][0] = xtgt[0]
m[1][0] = xtgt[1]
m[2][0] = xtgt[2]
m[0][1] = ytgt[0]
m[1][1] = ytgt[1]
m[2][1] = ytgt[2]
m[0][2] = ztgt[0]
m[1][2] = ztgt[1]
m[2][2] = ztgt[2]
return Affine3(m)
def read_rotation(model_num):
rot_in = trans.identity_matrix()
rote_path = '/mnt/container-data/rotation/' + model_num + ".txt"
with open(rote_path, mode='r') as f:
l_strip = [s.strip() for s in f.readlines()]
k = 0
for i in range(4):
for j in range(4):
rot_in[j][i] = np.array(l_strip[k])
k = k + 1
return rot_in
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 __init__(self, frame = tr.identity_matrix(), size = 10, diameter = 12): ''' @brief Constructor @param frame 4x4 transformation matrix. Contains the branch origin and orientation @param size branch size @param diameter branch diameter ''' ## 4x4 transformation matrix. Contains the branch origin and orientation self.frame = frame ## Branch size self.size = size ## Branch diameter self.diameter = diameter ## parent, parent node in the tree structure self.parent = None ## children, node children in the tree structure self.children = []
def build(self):
self.matrixPose = tm.identity_matrix()
x,y,z = self.bone.head
self.head4 = np.array((x,y,z,1.0))
x,y,z = self.bone.tail
self.tail4 = np.array((x,y,z,1.0))
self.vector4 = self.tail4 - self.head4
self.yvector4 = np.array((0, self.bone.length, 0, 1))
self.bone.calcRestMatrix()
if self.parent:
self.matrixGlobal = np.dot(self.parent.matrixGlobal, self.bone.matrixRelative)
else:
self.matrixGlobal = self.bone.matrixRest
try:
self.matrixVerts = np.dot(self.matrixGlobal, la.inv(self.bone.matrixRest))
except:
log.debug("%s\n %s\n %s", self.name, self.head4, self.tail4)
log.debug("%s", self.bone.matrixRest)
log.debug("%s", self.matrixPose)
log.debug("%s", self.matrixGlobal)
halt
def build(self):
self.matrixPose = tm.identity_matrix()
x,y,z = self.bone.head
self.head4 = np.array((x,y,z,1.0))
x,y,z = self.bone.tail
self.tail4 = np.array((x,y,z,1.0))
self.vector4 = self.tail4 - self.head4
self.yvector4 = np.array((0, self.bone.length, 0, 1))
self.bone.calcRestMatrix()
if self.parent:
self.matrixGlobal = np.dot(self.parent.matrixGlobal, self.bone.matrixRelative)
else:
self.matrixGlobal = self.bone.matrixRest
try:
self.matrixVerts = np.dot(self.matrixGlobal, la.inv(self.bone.matrixRest))
except:
log.debug("%s\n %s\n %s", self.name, self.head4, self.tail4)
log.debug("%s", self.bone.matrixRest)
log.debug("%s", self.matrixPose)
log.debug("%s", self.matrixGlobal)
halt
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.
'''
self.pub_imu = True
#self.imu_msg = self.imu_pub.initProto()
if 'quaternion' in orient_data:
w, x, y, z = orient_data['quaternion']
elif 'roll' in orient_data:
x, y, z, w = quaternion_from_euler(
radians(orient_data['roll']),
radians(orient_data['pitch']), radians(orient_data['yaw']))
elif 'matrix' in orient_data:
m = identity_matrix()
m[:3, :3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
w, x, y, z = convert_quat((w, x, y, z), o['frame'])
roll, pitch, yaw = euler_from_quaternion((w, x, y, z))
self.imu_msg.angleRoll = roll
self.imu_msg.anglePitch = pitch
self.imu_msg.angleYaw = yaw
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(R, t):
Tr = tf.identity_matrix()
Tr[:3, :3] = R
return tf.concatenate_matrices(Tr, tf.translation_matrix(t))
cz = color or 'blue'
def point(p):
return '({},{},{})'.format(*p)
return '''
\draw[thin,dashed,color=gray] {m} -- {m0};
\draw[thick,->,color={cx}] {m} -- {px} node[anchor=south]{{$x$}};
\draw[thick,->,color={cy}] {m} -- {py} node[anchor=south]{{$y$}};
\draw[thick,->,color={cz}] {m} -- {pz} node[anchor=south]{{$z$}};
'''.format(
m=point(m), m0=point((m[0], m[1], 0)), px=point(px), py=point(py), pz=point(pz),
cx=cx, cy=cy, cz=cz
)
T_i = t.identity_matrix()
T_ab = [[math.sqrt(3)/2, -0.5, 0, 2],
[0.5, math.sqrt(3)/2, 0, 1],
[0, 0, 1, 0],
[0, 0, 0, 1]]
T_bc = [[1/math.sqrt(2), 1/math.sqrt(2), 0, 1],
[-1/math.sqrt(2), 1/math.sqrt(2), 0, 1],
[0, 0, 1, 0],
[0, 0, 0, 1]]
T_ac = np.dot(T_ab, T_bc)
print(latex_axes(T_i, length=2, color="black"))
print(latex_axes(T_ab, color="red"))
def ros_publisher(self):
global odometry_x_
global odometry_y_
global odometry_yaw_
global right_encoder_prev
global left_encoder_prev
global current_time
global data_publish_flag
ser.write('S')
print ser.inWaiting()
ser_data=''
if ser.inWaiting()==27:
ser_data=ser.read(27)
data="hello"
#print type(data), len(data)
#print type(ser_data), len(ser_data)
encoder1_data=ord(ser_data[2])*255+ord(ser_data[3])*255+ord(ser_data[4])*255+ord(ser_data[5])
encoder2_data=ord(ser_data[6])*255+ord(ser_data[7])*255+ord(ser_data[8])*255+ord(ser_data[9])
#print 'raw'
#print encoder1_data
#print encoder2_data
encoder1='0x{0:08X}.format(encoder1_data)'
encoder2='0x{0:08X}.format(encoder2_data)'
signed_encoder1=~(0xffffffff - int(encoder1,16))+1
signed_encoder2=~(0xffffffff - int(encoder2,16))+1
print 'raw'
print signed_encoder1
print signed_encoder2
msg1 = {'op': 'publish', 'topic': '/encoder1', 'msg':{'data' : encoder1_data}}
msg2 = {'op': 'publish', 'topic': '/encoder2', 'msg':{'data' : encoder2_data}}
msg3 = {'op': 'publish', 'topic': '/sonar1', 'msg':{'range': ord(ser_data[10])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar1'}}}
msg4 = {'op': 'publish', 'topic': '/sonar2', 'msg':{'range': ord(ser_data[11])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar'}}}
msg5 = {'op': 'publish', 'topic': '/sonar3', 'msg':{'range': ord(ser_data[12])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar3'}}}
msg6 = {'op': 'publish', 'topic': '/sonar4', 'msg':{'range': ord(ser_data[13])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar4'}}}
msg7 = {'op': 'publish', 'topic': '/sonar5', 'msg':{'range': ord(ser_data[14])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar5'}}}
msg8 = {'op': 'publish', 'topic': '/sonar6', 'msg':{'range': ord(ser_data[15])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar6'}}}
msg9 = {'op': 'publish', 'topic': '/sonar7', 'msg':{'range': ord(ser_data[16])/10.0, 'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar7'}}}
msg10= {'op': 'publish', 'topic': '/sonar8', 'msg':{'range': ord(ser_data[17])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar8'}}}
self.send(dumps(msg1))
self.send(dumps(msg2))
self.send(dumps(msg3))
self.send(dumps(msg4))
self.send(dumps(msg5))
self.send(dumps(msg6))
self.send(dumps(msg7))
self.send(dumps(msg8))
self.send(dumps(msg9))
self.send(dumps(msg10))
last_x=odometry_x_
last_y=odometry_y_
last_yaw=odometry_yaw_
right_enc_dif=(encoder1_data-right_encoder_prev)*0.0008888
left_enc_dif=(encoder2_data-left_encoder_prev)*0.0008888
#print 'enc dif'
#print right_encoder_prev
#print encoder1_data
#print left_encoder_prev
#print encoder2_data
#print right_enc_dif
#print left_enc_dif
#calculate odometry
dist=(right_enc_dif+left_enc_dif)/2.0
ang=(right_enc_dif-left_enc_dif)/0.28
#print 'ang'
#print ang
#print dist
right_encoder_prev=encoder1_data
left_encoder_prev=encoder2_data
odometry_yaw_ = math.atan2(math.sin(odometry_yaw_+ang), math.cos(odometry_yaw_+ang))
odometry_x_=odometry_x_+dist*math.cos(odometry_yaw_)
odometry_y_=odometry_y_+dist*math.sin(odometry_yaw_)
#print 'odom'
#print odometry_x_
#print odometry_y_
#print odometry_yaw_
last_time=current_time
current_time=time.time()
#print current_time
#print 'odom'
dt=(current_time-last_time)
vel=dist/dt
vel_x=(odometry_x_-last_x)/dt
vel_y=(odometry_y_-last_y)/dt
vel_yaw=(odometry_yaw_-last_yaw)/dt
#print 'time'
#print (odometry_x_ - last_x)
#print dt
#print 'twist msgs'
#print vel
#print vel_x
#print vel_yaw
orient=tf.quaternion_from_euler(0,0,odometry_yaw_)
msg11= {'op': 'publish', 'topic': '/odom', 'msg': {'pose': {'pose':{'position':{'x':odometry_x_, 'y':odometry_y_, 'z':0.0},'orientation': {'x':orient.item(1), 'y':orient.item(2), 'z':orient.item(3), 'w':orient.item(0) }}},'child_frame_id': 'base_link','twist' : {'twist':{'linear':{'x':vel_x, 'y':vel_y }, 'angular':{'z':vel_yaw} }},'header':{'frame_id':'odom'}}}
self.send(dumps(msg11))
I=transformations.identity_matrix()
#print I
else:
ser.flushInput()
#data="hello"
#print type(data), len(data)
#print type(ser_data), len(ser_data)
#msg = {'op': 'publish', 'topic': '/rosbridge_example', 'msg':{'data' : data}}
#self.send(dumps(msg))
threading.Timer(0.08,self.ros_publisher).start()
def __init__( self , size ) : Drawable.__init__( self ) self.size = map( lambda x : x*.5 , size ) self.m = tr.identity_matrix()
def gen_contact(center = np.array([0,0,0]),R = identity_matrix()): c_4 = np.array(center.tolist()+[0]) p_rot = [R.dot(p_i + c_4)[0:3] for p_i in p ] n_rot = [R.dot(z)[0:3] for _ in range(4) ] return np.array(p_rot), np.array(n_rot)
#reference rectangle contact p = [np.array([x,y,0,1]) for x in [-0.05,0.05] for y in [-0.1,0.1]] z = np.array([0,0,1,1]) z_axis = np.array([0.,0.,1.]) y_axis = np.array([0.,1.,0.]) x_axis = np.array([1.,0.,0.]) z = np.array([0,0,1,1]) g = np.array([0.,0.,-9.81]) def gen_contact(center = np.array([0,0,0]),R = identity_matrix()): c_4 = np.array(center.tolist()+[0]) p_rot = [R.dot(p_i + c_4)[0:3] for p_i in p ] n_rot = [R.dot(z)[0:3] for _ in range(4) ] return np.array(p_rot), np.array(n_rot) P0, N0 = gen_contact(center = np.array([0,0,0]),R = identity_matrix()) P1, N1 = gen_contact(center = np.array([1,0,0]),R = rotation_matrix(math.pi/8., y_axis)) P2, N2 = gen_contact(center = np.array([4,0,0]),R = identity_matrix()) def gen_phase(p_a, n_a, p_b, n_b): phase_p = np.zeros((p_a.shape[0] + p_b.shape[0],3)) phase_n = np.zeros((n_a.shape[0] + n_b.shape[0],3)) phase_p[0:4,:] = p_a[:]; phase_n[0:4,:] = n_a[:]; phase_p[4:8,:] = p_b[:]; phase_n[4:8,:] = n_b[:]; return phase_p, phase_n phase_p_1, phase_n_1 = gen_phase(P0, N0, P1, N1) phase_p_2, phase_n_2 = P1[:], N1[:] phase_p_3, phase_n_3 = gen_phase(P1, N1, P2, N2)
def __init__(self):
self.button = {}
self.move = [0,0,0]
self.dirskeys = ( ( ['w'] , ['s'] ) , ( ['a'] , ['d'] ) , ( ['e'] , ['q'] ) )
for d in self.dirskeys :
for e in d :
for i in range(len(e)) : e[i] = ( gtk.gdk.unicode_to_keyval(ord(e[i])) , False )
self.near = 1
self.far = 1000
self.fov = 60
builder = gtk.Builder()
builder.add_from_file(ui_file)
glconfig = self.init_glext()
self.drawing_area = GLDrawingArea(glconfig)
self.drawing_area.set_events( gtk.gdk.BUTTON_PRESS_MASK | gtk.gdk.BUTTON_RELEASE_MASK | gtk.gdk.BUTTON1_MOTION_MASK | gtk.gdk.BUTTON2_MOTION_MASK |gtk.gdk.BUTTON3_MOTION_MASK )
self.drawing_area.set_size_request(320,240)
builder.get_object("vbox1").pack_start(self.drawing_area)
win_main = builder.get_object("win_main")
self.save_diag = builder.get_object("save_dialog")
self.open_diag = builder.get_object("open_dialog")
self.leuler = builder.get_object("leuler")
self.lquaternion = builder.get_object("lquaternion")
win_main.set_events( gtk.gdk.KEY_PRESS_MASK | gtk.gdk.KEY_RELEASE_MASK )
win_main.connect('key-press-event' , self._on_key_pressed )
win_main.connect('key-release-event', self._on_key_released )
self.qscene = QuaternionsScene( 10.0 , self.fov , .01 , self.near , self.far )
self.escene = EulersScene( 10.0 , self.fov , .01 , self.near , self.far )
self.drawing_area.add( self.escene , (0, 0,1,.5) )
self.drawing_area.add( self.qscene , (0,.5,1,.5) )
self.qscene.set_matrix( tr.identity_matrix() , tr.identity_matrix() )
self.escene.set_matrix( tr.identity_matrix() , tr.identity_matrix() )
print 'Scene added'
win_main.show_all()
width = self.drawing_area.allocation.width
height = self.drawing_area.allocation.height / 2.0
ratio = float(width)/float(height)
self.qscene.set_ratio( ratio )
self.escene.set_ratio( ratio )
builder.connect_signals(self)
# self.statbar = builder.get_object('statbar')
self.drawing_area.connect('motion_notify_event',self._on_mouse_motion)
self.drawing_area.connect('button_press_event',self._on_button_pressed)
self.drawing_area.connect('button_release_event',self._on_button_released)
self.drawing_area.connect('configure_event',self._on_reshape)
self.drawing_area.connect_after('expose_event',self._after_draw)
self.anim = False
gtk.timeout_add( 1 , self._refresh )
def build(self):
self.matrixPose = tm.identity_matrix()
self.build0()
truem = utils.apply_transform(T, truem)
return mlab.points3d(truem[:,0], truem[:,1], truem[:, 2], scale_factor=0.01)
@disable_render
def _plot_cams(*args):
for a in args:
_plot_cam(*a)
@mlab.show
def plot_cams(*args):
_plot_cams(*args)
@disable_render
def _plot_mutloc((img0, K0, img0markers, truem0), (img1, K1, img1markers,
truem1), T1):
_plot_cam(img0, K0, img0markers, truem0, tf.identity_matrix())
return _plot_cam(img1, K1, img1markers, truem1, tf.identity_matrix())
@mlab.show
def plot_mutloc(*args):
return _plot_mutloc(*args)
@mlab.show
def plot_triangulation((img0, K0, T0, img_pos_target0),
(img1, K1, T1, img_pos_target1),
target_loc3d=None,
plot_triangulation_lines=True,
target_img_patch=None):
obj = mlab.gcf()
obj.scene.disable_render = True
_plot_coordinate_transforms(T0, T1)
def build(self):
self.matrixPose = tm.identity_matrix()
self.build0()
import numpy as np
import transformations as trans
from probreg import filterreg
from probreg import callbacks
import utils
source, target = utils.prepare_source_and_target_rigid_3d('bunny.pcd')
cbs = [callbacks.Open3dVisualizerCallback(source, target)]
tf_param, _, _ = filterreg.registration_filterreg(source,
target,
sigma2=None,
callbacks=cbs)
rot = trans.identity_matrix()
rot[:3, :3] = tf_param.rot
print("result: ", np.rad2deg(trans.euler_from_matrix(rot)), tf_param.scale,
tf_param.t)