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)