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 init_points(self): if self._tf_buffer.can_transform('odom', 'front_point', Time()): transform = self._tf_buffer.lookup_transform( 'odom', 'front_point', Time()) self.front_point.position.x = transform.transform.translation.x self.front_point.position.y = transform.transform.translation.y self.front_point.position.z = transform.transform.translation.z else: self.publish_transform( 'odom', 'front_point', tf.translation_matrix((0, 0, 0))) if self._tf_buffer.can_transform('odom', 'rear_point', Time()): transform = self._tf_buffer.lookup_transform( 'odom', 'rear_point', Time()) self.rear_point.position.x = transform.transform.translation.x self.rear_point.position.y = transform.transform.translation.y self.rear_point.position.z = transform.transform.translation.z else: self.publish_transform( 'odom', 'rear_point', tf.translation_matrix((0, 0, 0))) while self.pub.get_subscription_count() < 1: pass sleep(2) self.publish_transform('odom', 'base_link', self.base_link_start_height)
def callback_camera_pose(msg): model_name = msg.name model_pose = msg.pose for i in range(0, 3): model_name[i] if model_name[i] == 'realsense_camera': pose_cam_x = model_pose[i].position.x pose_cam_y = model_pose[i].position.y pose_cam_z = model_pose[i].position.z orient_cam_x = model_pose[i].orientation.x orient_cam_y = model_pose[i].orientation.y orient_cam_z = model_pose[i].orientation.z orient_cam_w = model_pose[i].orientation.w elif model_name[i] == 'ce_jacket': pose_jac_x = model_pose[i].position.x pose_jac_y = model_pose[i].position.y pose_jac_z = model_pose[i].position.z orient_jac_x = model_pose[i].orientation.x orient_jac_y = model_pose[i].orientation.y orient_jac_z = model_pose[i].orientation.z orient_jac_w = model_pose[i].orientation.w Transl_cam = tf.translation_matrix( np.array((pose_cam_x, pose_cam_y, pose_cam_z))) Transl_jac = tf.translation_matrix( np.array((pose_jac_x, pose_jac_y, pose_jac_z))) Rot_cam = np.array( [orient_cam_x, orient_cam_y, orient_cam_z, orient_cam_w])
def __init__(self): super().__init__('executor') """Update frequency Single targets: {baudrate} / ({uart frame length} * {cmd length} * {number of joint}) = 115200 / (9 * 4 * 18) ~= 177Hz <- autodetect-baudrate on maestro = 200000 / (9 * 4 * 1x8) ~= 300Hz <- fixed-baudrate on maestro Multiple targets: {baudrate} / ({uart frame length} * ({header length} + {mini-cmd length} * {number of joints})) = 115200 / (9 * (3 + 2 * 18)) ~= 320Hz = 200000 / (9 * (3 + 2 * 18)) ~= 560Hz""" self.subscription = self.create_subscription( Path, 'sparse_path', self.new_trajectory, 10 ) self.spi_bridge = self.create_publisher( UInt8MultiArray, 'stm32_cmd', 10 ) self.steps_trajectory = [] self.init_test_path() self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, self) self.broadcaster = self.create_publisher(TFMessage, '/tf', 10) self.publish_transform = get_transform_publisher(self.broadcaster, self.get_clock()) self.pub = self.create_publisher(JointState, 'joint_states', 10) self.base_link_start_height = tf.translation_matrix((0.0, 0.0, 0.1)) self.path_proxy = PathProxy(self.steps_trajectory, self.get_logger()) self.trajectory_generator = TrajectoryGenerator(self.path_proxy) self.trajectory_encoder = TrajectoryEncoder() self.front_point = Pose() self.rear_point = Pose() self.init_points() self.inv_kin_calc = TripodInverseKinematics(self._tf_buffer, self.get_clock()) self.prev_time = self.get_clock().now() first_step = TrajectoryPoint() first_step.timestamp = self.prev_time.nanoseconds / 10**9 first_step.left_moving = True first_step.left_pose = tf.translation_matrix((0, 0, 0)) first_step.right_pose = tf.translation_matrix((0, 0, 0)) self.prev_trajectory = self.trajectory_generator.generate_trajectory( first_step) self.first_time = False self.timer_callback() self.first_time = False self.start_timer = self.create_timer(START_DELAY, self.timer_callback) self.timer = None self.tf_viz_tim = self.create_timer(0.1, self.tf_viz_callback)
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 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 __init__(self, transformation_plane): origo = np.array([0, 0, 0]) xp = transformation_plane.x_axis() yp = transformation_plane.y_axis() zp = Point3.Cross(xp, yp) zp = Point3.Normalize(zp) # y' projected y on yz (normal=x) X1 = np.array([1, 0, 0]) # XAxisWorld X2 = np.array([0, 1, 0]) # YAxisWorld X3 = np.array([0, 0, 1]) # ZAxisWorld # These vectors are the local X,Y,Z of the rotated object X1Prime = xp.ToArr() X2Prime = yp.ToArr() X3Prime = zp.ToArr() # This matrix will transform points # from the world to the rotated axis aWorldToLocalTransform = np.matrix([ [np.dot(X1Prime, X1), np.dot(X1Prime, X2), np.dot(X1Prime, X3), 0], [np.dot(X2Prime, X1), np.dot(X2Prime, X2), np.dot(X2Prime, X3), 0], [np.dot(X3Prime, X1), np.dot(X3Prime, X2), np.dot(X3Prime, X3), 0], [0, 0, 0, 1] ]) matrix2 = translation_matrix(-1 * transformation_plane.origin()) self.WorldToLocalTransform = aWorldToLocalTransform * matrix2 # This matrix will transform points # from the rotated axis back to the world aLocalToWorldTransform = np.matrix([ [np.dot(X1, X1Prime), np.dot(X1, X2Prime), np.dot(X1, X3Prime), 0], [np.dot(X2, X1Prime), np.dot(X2, X2Prime), np.dot(X2, X3Prime), 0], [np.dot(X3, X1Prime), np.dot(X3, X2Prime), np.dot(X3, X3Prime), 0], [0, 0, 0, 1] ]) matrix4 = translation_matrix(transformation_plane.origin()) self.LocalToWorldTransform = matrix4 * aLocalToWorldTransform
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 __init__( self , fovy , ratio , near , far , robot_files ) : self.fovy = fovy self.near = near self.far = far self.ratio = ratio self.camera = None self.plane = Plane( (2,2) ) self.wall = Plane( (20,10) ) self.mw = tr.rotation_matrix( -m.pi / 2.0 , (1,0,0) ) self.mw = np.dot( self.mw , tr.translation_matrix( (0,3,0) ) ) self.robot = Robot( robot_files ) self.x = 0.0 self.last_time = timer() self.plane_alpha = 65.0 / 180.0 * m.pi self.lpos = [ 1 ,-1 , 0 ] self._make_plane_matrix() self.draw_robot = True self.draw_sparks = True self.draw_front = False self.draw_back = False
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None): rospy.logdebug("joint: %s"%jointname) U = get_pr2_urdf() joint = U.joints[jointname] joint.origin = joint.origin or urdf.Pose() if not joint.origin.position: joint.origin.position = [0,0,0] if not joint.origin.rotation: joint.origin.rotation = [0,0,0] parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation)) childFromRotated = np.eye(4) if valuedict and jointname in valuedict: if joint.joint_type == 'revolute' or joint.joint_type == 'continuous': childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis) elif joint.joint_type == 'prismatic': childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname]) parentFromRotated = np.dot(parentFromChild, childFromRotated) originFromParent = conversions.pose_to_hmat(ps.pose) originFromRotated = np.dot(originFromParent, parentFromRotated) newps = gm.PoseStamped() newps.header = ps.header newps.pose = conversions.hmat_to_pose(originFromRotated) return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
def get_bbox2d_from_bbox3d(cam_proj_matrix, rx, ry, rz, sl, sh, sw, tx, ty, tz): bbox_to_velo_matrix = np.matmul( transformations.translation_matrix([tx, ty, tz]), transformations.euler_matrix(rx, ry, rz, 'sxyz')) bbox_proj_matrix = np.matmul(cam_proj_matrix, bbox_to_velo_matrix) vertices = np.empty([2, 2, 2, 2]) for k in [0, 1]: for l in [0, 1]: for m in [0, 1]: v = np.array([(k - 0.5) * sl, -(l) * sh, (m - 0.5) * sw, 1.]) v = np.matmul(bbox_proj_matrix, v) vertices[k, l, m, :] = spherical_project(v[0], v[1], v[2]) vertices = vertices.astype(np.int32) x1 = np.amin(vertices[:, :, :, 0]) x2 = np.amax(vertices[:, :, :, 0]) y1 = np.amin(vertices[:, :, :, 1]) y2 = np.amax(vertices[:, :, :, 1]) return (x1, y1, x2, y2)
def rule_branch(self): ''' @brief Create a branch ''' # Translate frame = self.current_node.frame frame = np.dot(frame, tr.translation_matrix([0, 0, self.current_node.size])) # Add noise if self.noise != 0: self.current_phi_angle += random.uniform(-self.noise, self.noise) self.current_rho_angle += random.uniform(-self.noise, self.noise) # Phi Rotation phi_matrix = tr.rotation_matrix(self.current_phi_angle, [0, 0, 1]) frame[:3,:3] = np.dot(frame[:3,:3], phi_matrix[:3,:3]) # Rho rotation rho_matrix = tr.rotation_matrix(self.current_rho_angle, [1, 0, 0]) frame[:3,:3] = np.dot(frame[:3,:3], rho_matrix[:3,:3]) # Create a new node node = Node(frame, self.current_node.size * self.scale_factor, self.current_node.diameter * self.current_subdivision_ratio * self.scale_factor) self.current_node.add_node(node) self.current_node = node self.current_rho_angle = 0 self.current_phi_angle = 0 self.current_subdivision_ratio = 1
def recebe(msg): global x global y global z global id for marker in msg.markers: x = round(marker.pose.pose.position.x, 2) y = round(marker.pose.pose.position.y, 2) z = round(marker.pose.pose.position.z, 2) #print(x) id = marker.id #print(marker.pose.pose) if id == 100: print( buffer.can_transform("base_link", "ar_marker_100", rospy.Time(0))) header = Header(frame_id="ar_marker_100") trans = buffer.lookup_transform("base_link", "ar_marker_100", rospy.Time(0)) t = transformations.translation_matrix([ trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z ]) r = transformations.quaternion_matrix([ trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w ]) m = numpy.dot(r, t) v2 = numpy.dot(m, [0, 0, 1, 0]) v2_n = v2[0:-1] n2 = v2_n / linalg.norm(v2_n) cosa = numpy.dot(n2, [1, 0, 0]) print("ang", math.degrees(math.acos(cosa)))
def calibrate_extrinsics_depth(frame,K,rect,pi0,pi1,shift,z_direction=-1): """calibrating depth camera w.r.t. ground plane frame - depth frame K - camera matrix rect - [u,v,w,h] image rectangle that contains plane points pi0 - (u,v) image point corresponding to the 0,0,0 pi1 - (u,v) image point corresponding to the direction 0,1,0 z_directon - -1 or 1, for the direction of the z returns Pwc, Pcw - transformation matrices world->camera, camera->world """ u0,v0,h,w = rect uv = np.array(zip(*(x.flat for x in np.mgrid[u0:u0+h,v0:v0+w]))) z = frame[uv[:,0], uv[:,1]].reshape((-1,1)) indices = np.where(z != 0)[0] uvz = np.hstack([uv[indices], z[indices]]) pts_c = image_to_camera(uvz[:,1], uvz[:,0], uvz[:,2], K) plane, ctr = estimate_plane(pts_c, 1) plane *= z_direction # the direction for X (rows) u,v = pi0 p0 = image_to_camera(v, u, frame[u,v], K) u,v = pi1 p1 = image_to_camera(v, u, frame[u,v], K) Pcw = estimate_camera_to_plane(plane, pts_c, p0, p1) # tune this to move coordinate system T = translation_matrix(np.array(shift)) Pwc = la.pinv(Pcw).dot(T) Pcw = la.pinv(Pwc) return Pwc, Pcw
def sample_point_cloud(self, num): """Generate a sampling of the points from all the vertices using a reverse area integral """ sample = np.empty((num,3), dtype='f') norm = np.empty((num,3), dtype='f') if not 'area' in self.__dict__: self.calculate_area_lookup() rnds = np.random.rand(num) * self.area.total inds = [bisect.bisect_left(self.area.cumsum, rnd) for rnd in rnds] coords = np.random.rand(num,2).astype('f') for i, ind, (r1,r2) in zip(range(num), inds, coords): v1,v2,v3 = self.vertices[self.faces[ind,:3],:3] if r1+r2 >= 1: r1,r2 = 1-r1,1-r2 point = v1 + (v2-v1)*r1 + (v3-v1)*r2 sample[i,:] = point normal = np.cross(v2-v1,v3-v1) normal /= np.sqrt(np.dot(normal,normal)) norm[i,:] = normal # Center all the points to make it easier to apply handbuilt rotations import transformations T1 = transformations.translation_matrix(sample.mean(0)) RT = np.dot(self.RT, T1) sample -= sample.mean(0) return pointmodel.PointModel(sample, norm, RT)
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None): rospy.logdebug("joint: %s" % jointname) U = get_pr2_urdf() joint = U.joints[jointname] joint.origin = joint.origin or urdf.Pose() if not joint.origin.position: joint.origin.position = [0, 0, 0] if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0] parentFromChild = conversions.trans_rot_to_hmat( joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation)) childFromRotated = np.eye(4) if valuedict and jointname in valuedict: if joint.joint_type == 'revolute' or joint.joint_type == 'continuous': childFromRotated = transformations.rotation_matrix( valuedict[jointname], joint.axis) elif joint.joint_type == 'prismatic': childFromRotated = transformations.translation_matrix( np.array(joint.axis) * valuedict[jointname]) parentFromRotated = np.dot(parentFromChild, childFromRotated) originFromParent = conversions.pose_to_hmat(ps.pose) originFromRotated = np.dot(originFromParent, parentFromRotated) newps = gm.PoseStamped() newps.header = ps.header newps.pose = conversions.hmat_to_pose(originFromRotated) return make_kin_tree_from_link(newps, joint.child, ns=ns, valuedict=valuedict)
def 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 __init__(self, filename='ipm/camera_info.yaml'): self.info = self.load_camera_info(osp.join(PKG_PATH, filename)) self.frame_width = self.info['width'] self.frame_height = self.info['height'] # Intrinsics self.K = np.zeros((3, 4)) self.K[:, :3] = np.asarray(self.info['intrinsics']['K']).reshape(3, 3) self.D = np.asarray(self.info['intrinsics']['D']) # Extrinsics self.extrinsics_euler = np.radians(self.info['extrinsics']['rpy']) self.extrinsics_rotation = euler_matrix(self.extrinsics_euler[0], self.extrinsics_euler[1], self.extrinsics_euler[2]) self.extrinsics_translation = translation_matrix( self.info['extrinsics']['position']) # Overwrite calibration data if available if 'calibration' not in self.info: print( 'ERROR: Calibration not performed, run InversePerspectiveMapping.calibrate() first!' ) return else: print('Loading calibration data from file...') self.ipm_matrix = np.asarray( self.info['calibration']['ipm_matrix']).reshape(3, 3) self.ipm_matrix_inv = np.linalg.inv(self.ipm_matrix) self.ipm_image_dims = tuple( self.info['calibration']['ipm_image_dims'][::-1]) self.ipm_px_to_m = self.info['calibration']['ipm_px_to_m'] self.ipm_m_to_px = self.info['calibration']['ipm_m_to_px'] self.calibration_ego_y = self.info['calibration'][ 'calibration_ego_y']
def recebe(msg): global x # O global impede a recriacao de uma variavel local, para podermos usar o x global ja' declarado global y global z global id for marker in msg.markers: x = round(marker.pose.pose.position.x, 2) # para arredondar casas decimais e impressao ficar ok y = round(marker.pose.pose.position.y, 2) z = round(marker.pose.pose.position.z, 2) #print(x) id = marker.id #print(marker.pose.pose) if id == 100: print(buffer.can_transform("base_link", "ar_marker_100", rospy.Time(0))) header = Header(frame_id= "ar_marker_100") # Procura a transformacao em sistema de coordenadas entre a base do robo e o marcador numero 100 # Note que para seu projeto 1 voce nao vai precisar de nada que tem abaixo, a # Nao ser que queira levar angulos em conta trans = buffer.lookup_transform("base_link", "ar_marker_100", rospy.Time(0)) # Separa as translacoes das rotacoes t = transformations.translation_matrix([trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z]) # Encontra as rotacoes r = transformations.quaternion_matrix([trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w]) m = numpy.dot(r,t) v2 = numpy.dot(m,[0,0,1,0]) v2_n = v2[0:-1] n2 = v2_n/linalg.norm(v2_n) cosa = numpy.dot(n2,[1,0,0]) angulo_marcador_robo = math.degrees(math.acos(cosa)) print("Angulo entre marcador e robo", angulo_marcador_robo)
def getMarkerTFFromMap(m): # This doesn't work mid = "Marker%s" % m marker = marker_dict[mid] w_mat = None if marker is not None: w = 0 # east p = (-marker["x"], -marker["y"], -0.25) #east if marker["wall"] == "north": p = (marker["y"], -marker["x"], -0.25) w = math.pi / 2 elif marker["wall"] == "south": p = (-marker["y"], marker["x"], -0.25) w = -math.pi / 2 elif marker["wall"] == "west": p = (marker["x"], marker["y"], -0.25) w = math.pi q = tr.quaternion_from_euler(0, 0, w) print("Map: (%s) Marker%s: %s %s" % (marker["wall"], m, p, q)) t = tr.translation_matrix(p) r = tr.quaternion_matrix(q) w_mat = numpy.dot(t, r) return (w_mat, None)
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 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 __init__(self, fovy, ratio, near, far, robot_files): self.fovy = fovy self.near = near self.far = far self.ratio = ratio self.camera = None self.plane = Plane((2, 2)) self.wall = Plane((20, 10)) self.mw = tr.rotation_matrix(-m.pi / 2.0, (1, 0, 0)) self.mw = np.dot(self.mw, tr.translation_matrix((0, 3, 0))) self.robot = Robot(robot_files) self.x = 0.0 self.last_time = timer() self.plane_alpha = 65.0 / 180.0 * m.pi self.lpos = [1, -1, 0] self._make_plane_matrix() self.draw_robot = True self.draw_sparks = True self.draw_front = False self.draw_back = False
def 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 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 _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 compute_transform(rx, rz, tx, ty, tz): origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1] Rx = tf.rotation_matrix(rx, xaxis) Rz = tf.rotation_matrix(rz, zaxis) T = tf.translation_matrix([tx, ty, tz]) return T.dot(Rz).dot(Rx)
def _make_plane_matrix(self): r = tr.rotation_matrix(self.plane_alpha, (0, 0, 1)) s = tr.scale_matrix(1) t = tr.translation_matrix((-1.25, .7, .05)) self.m = np.dot(np.dot(t, s), r) self.im = la.inv(self.m) self.im[3] = [0, 0, 0, 1]
def _make_plane_matrix( self ) : r = tr.rotation_matrix( self.plane_alpha , (0,0,1) ) s = tr.scale_matrix( 1 ) t = tr.translation_matrix( (-1.25,.7,.05) ) self.m = np.dot( np.dot( t , s ) , r ) self.im = la.inv( self.m ) self.im[3] = [ 0 , 0 , 0 , 1 ]
def 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 global_pose(matrix, x_ob, y_ob, angle): #obtiene el angulo del tag con respecto al mapa q1 = math.atan2(y_ob, x_ob) # invierte el angulo del tag segun el plano del mapa angle = -angle # Calcula la distancia del robot al tag z = dist(matrix) # Calcula la distancia del tag al mapa d = math.sqrt(x_ob**2 + y_ob**2) # Calcula el angulo del robot c/r a q1 q2 = angle2(q1, angle, tf.euler_from_matrix(matrix)) R1 = tf.rotation_matrix(q1, [0, 0, 1]) T1 = tf.translation_matrix([d, 0, 0]) R2 = tf.rotation_matrix(q2, [0, 0, 1]) T2 = tf.translation_matrix([z, 0, 0]) result = R1.dot(T1.dot(R2.dot(T2.dot([0, 0, 0, 1])))) return result
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 RotateVectorArr( self, q_arr, p_arr): #geometry_msgs/Quaternion and geometry_msgs/Point angle_transform = transformations.quaternion_matrix(q_ar) p_transform = transformations.translation_matrix(p_arr) new_p_transform = np.dot(angle_transform, p_transform) new_p_arr = new_p_transform[:3, 3] return new_p_arr
def filter_points_inside_box(points, dim, loc, ry): box_T = transformations.translation_matrix(loc) box_R = transformations.rotation_matrix(ry, [0,1,0]) box_S = np.diag([dim[2], dim[0], dim[1], 1.0]) # transformations.scale_matrix(dim) inv_mat = np.linalg.inv(np.dot(box_T, np.dot(box_R, box_S))) # post multiply points_box_trans = np.dot(inv_mat, points.T).T inside_idxs = (points_box_trans[:, 0] >= -0.5) & (points_box_trans[:, 0] <= 0.5) & \ (points_box_trans[:, 1] >= -0.5) & (points_box_trans[:, 1] <= 0.5) & \ (points_box_trans[:, 2] >= -0.5) & (points_box_trans[:, 2] <= 0.5) return points_box_trans[inside_idxs], inside_idxs
def make_transform_matrix(quaternion, translation): M = np.identity(4) T = transformations.translation_matrix(translation) M = np.dot(M, T) R = transformations.quaternion_matrix(quaternion) M = np.dot(M, R) M /= M[3, 3] return M
def _draw(self): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glClearColor(0.0, 0.0, 0.0, 1.0) view_mat = translation_matrix((0, 0, -self.dist)) view_mat = view_mat.dot(self.ball.matrix()) view_mat = view_mat.dot(scale_matrix(self.zoom)) self.VMatrix = view_mat self.draw_hook() OpenGL.GLUT.glutSwapBuffers()
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 setPersp(scale_factor = 1, distort_location = '/afs/cs.stanford.edu/u/twangcat/scratch/sail-car-log/process/'): distort = { } distort['M_1'] = [np.eye(3)] # perspective distrotion in cam1 image space distort['M_2'] = [np.eye(3)] # perspective distrotion in cam2 image space distort['T'] = [np.eye(4)] # corresponding real-world transformation distort['T'].insert(0,translation_matrix(np.array([1.0,0,0]))) distort['T'].insert(0,translation_matrix(np.array([-1.0,0,0]))) distort['T'].insert(0,euler_matrix(0, 0.06, 0)) distort['T'].insert(0,euler_matrix(0, -0.06, 0)) distort['T'].insert(0,euler_matrix(0, 0.12, 0)) distort['T'].insert(0,euler_matrix(0, -0.12, 0)) distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam1.pickle'))) # shift 1.0 distort['M_1'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam1.pickle'))) # shift -1.0 distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam1.pickle','r'))) # rotate 0.06 distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam1.pickle','r'))) # rotate -0.06 distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam1.pickle','r'))) # rotate 0.12 distort['M_1'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam1.pickle','r'))) # rotate -0.12 distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift1.0_cam2.pickle'))) # shift 1.0 distort['M_2'].insert(0,pickle.load(open(distort_location+'M_shift-1.0_cam2.pickle'))) # shift -1.0 distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.06_cam2.pickle','r'))) # rotate 0.06 distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.06_cam2.pickle','r'))) # rotate -0.06 distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot0.12_cam2.pickle','r'))) # rotate 0.12 distort['M_2'].insert(0,pickle.load(open(distort_location+'M_rot-0.12_cam2.pickle','r'))) # rotate -0.12 assert len(distort['M_1']) == len(distort['M_2']) and len(distort['M_1'])==len(distort['T']) if scale_factor != 1: for i in xrange(len(distort['M_1'])-1): # last one is always identity, no need change. distort['M_1'][i][:,0:2]/=scale_factor distort['M_1'][i][2,:]/=scale_factor distort['M_2'][i][:,0:2]/=scale_factor distort['M_2'][i][2,:]/=scale_factor return distort
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 getWorldMarkerMatrixFromTF(m): global listener marker_link = "Marker%s__link" % m try: tw = listener.getLatestCommonTime(marker_link, 'world') (t, r) = listener.lookupTransform(marker_link, 'world', tw) print("TF: Marker%s: %s %s" % (m, t, r)) w_mat = numpy.dot(tr.translation_matrix(t), tr.quaternion_matrix(r)) except: return (None, None) return (w_mat, tw)
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): # sagital self.RAnkleRotY = None self.LAnkleRotY = None self.RKneeRotY = None self.LKneeRotY = None self.RHipRotY = None self.LHipRotY = None self.AbsRotY = None self.BustRotY = None self.FootT = tf.translation_matrix((0, 0, L_FOOT_Z)) self.ThighT = tf.translation_matrix((0, 0, L_THIGH)) self.LegT = tf.translation_matrix((0, 0, L_LEG)) self.HipT = tf.translation_matrix((0, 0, L_HIP_Z)) self.AbsT = tf.translation_matrix((0, 0, L_ABS_Z)) self.BustT = tf.translation_matrix((0, 0, L_BUST_Z)) # Totally guessed com of segments self.SegCom = array([[0.025, 0, L_FOOT_Z, 1], [0, 0, L_THIGH, 1], [0, 0, L_LEG, 1], [0, 0, 3.0 * L_HIP_Z / 4.0, 1], [- 0.02, 0, 3.0 * L_ABS_Z / 4.0, 1], [- 0.02, 0, 3.0 * L_BUST_Z / 4.0, 1]]) # Totally guessed masses of segments (roughtly the mass of the motors) self.Masses = array([2 * (2 * MX28_MASS), 2 * (MX28_MASS), 2 * (MX28_MASS), 2 * (2 * MX28_MASS), (2 * MX64_MASS), 13 * MX28_MASS]) self.TotalMass = self.Masses.sum() self.Masses /= self.TotalMass self.roty = None self.xaxis = (1, 0, 0) self.yaxis = (0, 1, 0) self.zaxis = (0, 0, 1)
def controls_3d(self, dx, dy, zooming_one_shot=False): """ Orbiting the camera is implemented the following way: - the rotation is split into a rotation around the *world* Z axis (controlled by the horizontal mouse motion along X) and a rotation around the *X* axis of the camera (pitch) *shifted to the focal origin* (the world origin for now). This is controlled by the vertical motion of the mouse (Y axis). - as a result, the resulting transformation of the camera in the world frame C' is: C' = (T · Rx · T⁻¹ · (Rz · C)⁻¹)⁻¹ where: - C is the original camera transformation in the world frame, - Rz is the rotation along the Z axis (in the world frame) - T is the translation camera -> world (ie, the inverse of the translation part of C - Rx is the rotation around X in the (translated) camera frame """ CAMERA_TRANSLATION_FACTOR = 0.01 CAMERA_ROTATION_FACTOR = 0.01 if not (self.is_rotating or self.is_panning or self.is_zooming): return current_pos = self.current_cam.transformation[:3, 3].copy() distance = numpy.linalg.norm(self.focal_point - current_pos) if self.is_rotating: rotation_camera_x = dy * CAMERA_ROTATION_FACTOR rotation_world_z = dx * CAMERA_ROTATION_FACTOR world_z_rotation = transformations.euler_matrix(0, 0, rotation_world_z) cam_x_rotation = transformations.euler_matrix(rotation_camera_x, 0, 0) after_world_z_rotation = numpy.dot(world_z_rotation, self.current_cam.transformation) inverse_transformation = transformations.inverse_matrix(after_world_z_rotation) translation = transformations.translation_matrix( transformations.decompose_matrix(inverse_transformation)[3]) inverse_translation = transformations.inverse_matrix(translation) new_inverse = numpy.dot(inverse_translation, inverse_transformation) new_inverse = numpy.dot(cam_x_rotation, new_inverse) new_inverse = numpy.dot(translation, new_inverse) self.current_cam.transformation = transformations.inverse_matrix(new_inverse).astype(numpy.float32) if self.is_panning: tx = -dx * CAMERA_TRANSLATION_FACTOR * distance ty = dy * CAMERA_TRANSLATION_FACTOR * distance cam_transform = transformations.translation_matrix((tx, ty, 0)).astype(numpy.float32) self.current_cam.transformation = numpy.dot(self.current_cam.transformation, cam_transform) if self.is_zooming: tz = dy * CAMERA_TRANSLATION_FACTOR * distance cam_transform = transformations.translation_matrix((0, 0, tz)).astype(numpy.float32) self.current_cam.transformation = numpy.dot(self.current_cam.transformation, cam_transform) if zooming_one_shot: self.is_zooming = False self.update_view_camera()
NOM = NOM.T #NOM_A = NOM_A.T MEAS = MEAS.T #MEAS_A = MEAS_A.T print "\nNOMINAL\n\n",NOM,"\n\nMEASURED\n\n",MEAS,"\n" #print "\nNOMINAL_A\n\n",NOM_A,"\n\nMEASURED_A\n\n",MEAS_A,"\n" #Rotation matrix may be pre- or post- multiplied (changing between a right-handed system and a left-handed system). R = transformations.superimposition_matrix(MEAS,NOM,usesvd=True) #R = transformations.inverse_matrix(R) print "Rotation matrix calulated from points difference\n\n",R,"\n" rot=transformations.inverse_matrix(R) rob = transformations.euler_matrix(math.radians(OLDBASE[3]),math.radians(OLDBASE[4]),math.radians(OLDBASE[5]), axes='sxyz') tob = transformations.translation_matrix(OLDBASE[0:3]) robn = np.dot(tob,rob) #rotation matrix from old base print "Rotation matrix from old base\n\n",robn,"\n" robnew = np.dot(rot,robn) print "Rotation matrix RESULT\n\n",robnew,"\n" scale, shear, angles, translate, perspective = transformations.decompose_matrix(robnew) roll = math.degrees(angles[0]) pitch = math.degrees(angles[1]) yaw = math.degrees(angles[2]) print "OLD BASE(%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)\n" %(OLDBASE[0],OLDBASE[1],OLDBASE[2],OLDBASE[3],OLDBASE[4],OLDBASE[5]) print "NEW BASE(%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)\n" %(translate[0],translate[1],translate[2],roll,pitch,yaw) print "----- COPY & PASTE -----\n\n[Base]" print "BaseX = %.3f" % translate[0] print "BaseY = %.3f" % translate[1]
def _xyz_to_mat44(self, pos): return transformations.translation_matrix((pos['x'], pos['y'], pos['z']))
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'))
def transform(R, t): Tr = tf.identity_matrix() Tr[:3, :3] = R return tf.concatenate_matrices(Tr, tf.translation_matrix(t))
def getMatrix(self): return rotation_matrix(90 + self.rot[1], [1.0, 0.0, 0.0]).dot(rotation_matrix(-self.rot[0], [0.0, 0.0, 1.0]).dot(translation_matrix(-self.pos)))
MEAS_A = MEAS_A.T print "\nNOMINAL\n\n",NOM,"\n\nMEASURED\n\n",MEAS,"\n" print "\nNOMINAL_A\n\n",NOM_A,"\n\nMEASURED_A\n\n",MEAS_A,"\n" #Rotation matrix may be pre- or post- multiplied (changing between a right-handed system and a left-handed system). R = transformations.superimposition_matrix(MEAS,NOM,usesvd=True) scale, shear, angles, translate, perspective = transformations.decompose_matrix(R) #R = transformations.inverse_matrix(R) print "Rotation matrix\n\n",R,"\n" #rot=R.T p1 = transformations.euler_matrix(MEAS_A[0,0]/180*np.pi,MEAS_A[1,0]/180*np.pi,MEAS_A[2,0]/180*np.pi, axes='sxyz') p2 = transformations.euler_matrix(MEAS_A[0,1]/180*np.pi,MEAS_A[1,1]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p3 = transformations.euler_matrix(MEAS_A[0,2]/180*np.pi,MEAS_A[1,2]/180*np.pi,MEAS_A[2,2]/180*np.pi, axes='sxyz') p4 = transformations.euler_matrix(MEAS_A[0,3]/180*np.pi,MEAS_A[1,3]/180*np.pi,MEAS_A[2,3]/180*np.pi, axes='sxyz') t1 = transformations.translation_matrix(MEAS[0:,0]) t2 = transformations.translation_matrix(MEAS[0:,1]) t3 = transformations.translation_matrix(MEAS[0:,2]) t4 = transformations.translation_matrix(MEAS[0:,3]) #print "p1\n",p1,"\n","p2\n",p2,"\n","p3\n",p3,"\n","p4\n",p4,"\n" #print "t1\n",t1,"\n","t2\n",t2,"\n","t3\n",t3,"\n","t4\n",t4,"\n" p1n = np.dot(t1,p1) p2n = np.dot(t2,p2) p3n = np.dot(t3,p3) p4n = np.dot(t4,p4) print "p1n\n",p1n,"\n","p2n\n",p2n,"\n","p3n\n",p3n,"\n","p4n\n",p4n,"\n" p1n = np.dot(R,p1n) p2n = np.dot(R,p2n) p3n = np.dot(R,p3n) p4n = np.dot(R,p4n) print "p1n\n",p1n,"\n","p2n\n",p2n,"\n","p3n\n",p3n,"\n","p4n\n",p4n,"\n"
def mkpln( s , p , r , a , c ) : p = Plane((s,s),np.dot(tr.translation_matrix(p),tr.rotation_matrix(r*m.pi/180.0,a))) p.c = c return p