def icp_numpy(source, target, tol=1e-3): """Align two point clouds using the Iterative Closest Point (ICP) method. Parameters ---------- source : list of point The source data. target : list of point The target data. tol : float, optional Tolerance for finding matches. Default is ``1e-3``. Returns ------- The transformed points Notes ----- First we align the source with the target cloud using the frames resulting from a PCA of each of the clouds, simply by calculating a frame-to-frame transformation. This initial alignment is used to establish an initial correspondence between the points of the two clouds. Then we iteratively improve the alignment by computing successive "best-fit" transformations using SVD of the cross-covariance matrix of the two data sets. During this iterative process, we continuously update the correspondence between the point clouds by finding the closest point in the target to each of the source points. The algorithm terminates when the alignment error is below a specified tolerance. Examples -------- >>> """ A = asarray(source) B = asarray(target) origin, axes, _ = pca_numpy(A) A_frame = Frame(origin, axes[0], axes[1]) origin, axes, _ = pca_numpy(B) B_frame = Frame(origin, axes[0], axes[1]) X = Transformation.from_frame_to_frame(A_frame, B_frame) A = transform_points_numpy(A, X) for i in range(20): D = cdist(A, B, 'euclidean') closest = argmin(D, axis=1) if norm(normrow(A - B[closest])) < tol: break X = bestfit_transform(A, B[closest]) A = transform_points_numpy(A, X) return A, X
def test_kinematics_cartesian(): if compas.IPY: return frames_WCF = [ Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)), Frame((0.404, 0.057, 0.324), (0.919, 0.000, 0.394), (0.090, 0.974, -0.210)), Frame((0.390, 0.064, 0.315), (0.891, 0.000, 0.454), (0.116, 0.967, -0.228)), Frame((0.388, 0.079, 0.309), (0.881, 0.000, 0.473), (0.149, 0.949, -0.278)), Frame((0.376, 0.087, 0.299), (0.850, 0.000, 0.528), (0.184, 0.937, -0.296)) ] with AnalyticalPyBulletClient(connection_type='direct') as client: robot = client.load_robot(urdf_filename) client.load_semantics(robot, srdf_filename) options = {"solver": "ur5", "check_collision": True} start_configuration = list( robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1] trajectory = robot.plan_cartesian_motion( frames_WCF, start_configuration=start_configuration, options=options) assert (trajectory.fraction == 1.)
def frame_list(): return [ Frame([0, 0, 0], [1, 0, 0], [0, 1, 0]), Frame([1, 0, 0], [1, 0, 0], [0, 1, 0]), Frame([2, 0, 0], [1, 0, 0], [0, 1, 0]), Frame([3, 0, 0], [1, 0, 0], [0, 1, 0]), Frame([4, 0, 0], [1, 0, 0], [0, 1, 0]), ]
def get_frame(self): """ Returns a Frame with x-axis pointing up, y-axis pointing towards the mesh normal. """ if abs(dot_vectors(self.up_vector, self.mesh_normal) ) < 1.0: # if the normalized vectors are not co-linear c = cross_vectors(self.up_vector, self.mesh_normal) return Frame(self.pt, c, self.mesh_normal) else: # in horizontal surfaces the vectors happen to be co-linear return Frame(self.pt, Vector(1, 0, 0), Vector(0, 1, 0))
def coerce_frame(plane): import Rhino from compas.geometry import Frame if isinstance(plane, Rhino.Geometry.Plane): return Frame(plane.Origin, plane.XAxis, plane.YAxis) elif isinstance(plane, Frame): return plane else: return Frame(*plane)
def board_geometry_setup(): for my_element in self.elements(): my_board = my_element[1] if my_board.layer % 2 == 0: my_frame = self.origin_fr layer_standard_length = self.primary_length else: my_frame = self.sec_fr layer_standard_length = self.secondary_length my_dir1 = normalize_vector(my_frame[1]) my_dir2 = normalize_vector(my_frame[2]) dist = my_board.grid_position if my_board.location == "high": if not my_board.supporter: length_attribute_1 = layer_standard_length - my_board.length / 2 else: length_attribute_1 = layer_standard_length - my_board.width / 2 elif my_board.location == "low": if not my_board.supporter: length_attribute_1 = my_board.length / 2 else: length_attribute_1 = my_board.width / 2 else: length_attribute_1 = layer_standard_length / 2 # position parallel to the boards (if not sup) my_vec1 = scale_vector(my_dir1, length_attribute_1) # position perpendicular to board direction (if not sup) my_vec2 = scale_vector(my_dir2, dist) # height vector my_vec3 = Vector(0, 0, my_board.z_drop - my_board.height / 2) my_centre = self.origin_pt + my_vec1 + my_vec2 + my_vec3 my_board.centre_point = my_centre my_board.drop_point = my_centre + Vector(0, 0, my_board.height / 2) if not my_board.perp: my_board.length_vector = normalize_vector(my_vec1) my_board.width_vector = normalize_vector(my_vec2) else: my_board.length_vector = normalize_vector(my_vec2) my_board.width_vector = normalize_vector(my_vec1) old_centre = my_board.center T = Translation(my_centre - old_centre) self.network.node[my_board.global_count]['x'] = my_centre[0] self.network.node[my_board.global_count]['y'] = my_centre[1] self.network.node[my_board.global_count]['z'] = my_centre[2] my_board.transform(T) my_board.board_frame = Frame(my_board.centre_point, my_board.length_vector, my_board.width_vector) my_board.tool_frame = Frame(my_board.drop_point, my_board.length_vector, my_board.width_vector) my_board.box = Box(my_board.board_frame, my_board.length, my_board.width, my_board.height)
def test_from_tcf_to_t0cf(mesh, frame): tool = ToolModel(mesh, frame) frames_tcf = [ Frame((-0.309, -0.046, -0.266), (0.276, 0.926, -0.256), (0.879, -0.136, 0.456)) ] result = tool.from_tcf_to_t0cf(frames_tcf) expected = [ Frame(Point(-0.363, 0.003, -0.147), Vector(0.388, -0.351, -0.852), Vector(0.276, 0.926, -0.256)) ] assert allclose(result[0], expected[0], tol=1e-03)
def get_boundary_plane(boundary): a = cablenet.vertex_coordinates(boundary[0]) b = cablenet.vertex_coordinates(boundary[-1]) x_axis = subtract_vectors(b, a) y_axis = [0, 0, 1.0] z_axis = cross_vectors(x_axis, y_axis) x_axis = cross_vectors(y_axis, z_axis) frame_0 = Frame(a, x_axis, y_axis) point = add_vectors(frame_0.point, scale_vector(frame_0.zaxis, OFFSET)) frame_1 = Frame(point, x_axis, y_axis) return frame_0, frame_1
def RunCommand(is_interactive): #load Derivation and model derivation = Derivation.from_json( rhino_UI_utilities.get_json_file_location()) model = derivation.get_next_step() #user input rc, corners = Rhino.Input.RhinoGet.GetRectangle() if rc != Rhino.Commands.Result.Success: return rc plane = Rhino.Geometry.Plane(corners[0], corners[1], corners[2]) beam_frame = Frame(plane[0], plane[1], plane[2]) length = rs.GetReal("length", 4000, 300, None) #Generate unique name for the Beam name = create_id() #Create Beam model.rule_create_beam(beam_frame, length, 100, 100, name) #Save Derivation (Model is also saved) derivation.to_json(rhino_UI_utilities.get_json_file_location(), pretty=True) #Visualization artist = MeshArtist(None, layer='BEAM::Beams_out') artist.clear_layer() for beam in model.beams: artist = MeshArtist( beam.mesh, layer='BEAM::Beams_out' ) #.mesh is not ideal fix in beam and assemble class artist.draw_faces(join_faces=True) return 0
def __init__(self): super(UR, self).__init__() self.model = [] # a list of meshes self.model_breps = [] self.basis_frame = None # move to UR !!!! self.transformation_RCS_WCS = None self.transformation_WCS_RCS = None self.set_base(Frame.worldXY()) self.tool = Tool(Frame.worldXY()) self.configuration = None d1, a2, a3, d4, d5, d6 = self.params # j0 - j5 are the axes around which the joints m0 - m5 rotate, e.g. m0 # rotates around j0, m1 around j1, etc. self.j0 = [(0, 0, 0), (0, 0, d1)] self.j1 = [(0, 0, d1), (0, -self.shoulder_offset, d1)] self.j2 = [(a2, -self.shoulder_offset - self.elbow_offset, d1), (a2, -self.shoulder_offset, d1)] self.j3 = [(a2 + a3, 0, d1), (a2 + a3, -d4, d1)] self.j4 = [(a2 + a3, -d4, d1), (a2 + a3, -d4, d1 - d5)] self.j5 = [(a2 + a3, -d4, d1 - d5), (a2 + a3, -d4 - d6, d1 - d5)] # check difference ur5 and ur10!!! self.tool0_frame = Frame(self.j5[1], [1, 0, 0], [0, 0, 1])
def frame_generator(pt): zaxis = Vector(0, 0, 1) for axis in DeviationVectorsGenerator(zaxis, math.radians(40), 1): for xaxis in OrthonormalVectorsFromAxisGenerator( axis, math.radians(60)): yaxis = axis.cross(xaxis) yield Frame(pt, xaxis, yaxis)
def forward_kinematics(configuration, ur_params): """Forward kinematics function. This is the wrapper for the forward kinematics function from ROS. Our robots somehow differ to the standard configuration. Therefore we need to swap angles and rotate the first joint by -pi. (The initial position can be visualized by loading the meshes.) Args: configuration, the 6 joint angles in radians ur_params: UR defined parameters for the model Returns: the frame """ configuration[0] += math.pi T = forward_ros(configuration, ur_params) xaxis = [T[1], T[5], T[9]] yaxis = [T[2], T[6], T[10]] point = [T[3], T[7], T[11]] return Frame(point, xaxis, yaxis)
def draw_halfedges( self, halfedges: Optional[List[Tuple[int, int]]] = None, color: Union[str, Color, List[Color], Dict[int, Color]] = (0.7, 0.7, 0.7), distance: float = 0.05, width: float = 0.01, shrink: float = 0.8, ) -> None: """Draw a selection of halfedges. Parameters ---------- edges : list, optional A list of halfedges to draw. The default is ``None``, in which case all halfedges are drawn. color : rgb-tuple or dict of rgb-tuples, optional The color specification for the halfedges. Returns ------- None """ self.clear_halfedges() self._halfedgecollection = [] if color: self.halfedge_color = color if halfedges: self.halfedges = halfedges for u, v in self.halfedges: face = self.mesh.halfedge_face(u, v) if face is None: normal = self.mesh.face_normal(self.mesh.halfedge_face(v, u)) else: normal = self.mesh.face_normal(face) a, b = self.mesh.edge_coordinates(u, v) line = Line(*offset_line((a, b), distance, normal)) frame = Frame(line.midpoint, [1, 0, 0], [0, 1, 0]) scale = Scale.from_factors([shrink, shrink, shrink], frame=frame) line.transform(scale) artist = self.plotter.axes.arrow(line.start[0], line.start[1], line.vector[0], line.vector[1], width=width, head_width=10 * width, head_length=10 * width, length_includes_head=True, shape='right', color=self.halfedge_color.get( (u, v), self.default_halfedgecolor), zorder=10000) self._halfedgecollection.append(artist)
def pick_frame_from_grid(index, bullet_height): """Get next picking frame. Parameters ---------- index : int Counter to iterate through picking positions. bullet_height : float Height of bullet to pick up. Returns ------- list of `class`:compas.geometry.Frame """ # If index is larger than amount on picking plate, start from zero again index = index % (fab_conf["pick"]["xnum"].get() * fab_conf["pick"]["ynum"].get()) xpos = index % fab_conf["pick"]["xnum"].get() ypos = index // fab_conf["pick"]["xnum"].get() x = (fab_conf["pick"]["origin_grid"]["x"].get() + xpos * fab_conf["pick"]["grid_spacing"].get()) y = (fab_conf["pick"]["origin_grid"]["y"].get() + ypos * fab_conf["pick"]["grid_spacing"].get()) z = bullet_height * fab_conf["pick"]["compression_height_factor"].get() frame = Frame( Point(x, y, z), Vector(*fab_conf["pick"]["xaxis"].get()), Vector(*fab_conf["pick"]["yaxis"].get()), ) log.debug("Picking frame {:03d}: {}".format(index, frame)) return frame
def __init__(self, board_layer, identification, board_no_in_layer, board_dimensions, z_value_toppoint, length_dir, width_dir): self.index = identification self.layer = board_layer self.no_in_layer = board_no_in_layer self.dimensions = board_dimensions self.width = self.dimensions[0] self.height = self.dimensions[1] self.length = self.dimensions[2] self.drop_point = Point(0, 0, 0) self.centre_point = Point(0, 0, 0) self.z_drop = z_value_toppoint self.length_direction = length_dir self.width_direction = width_dir self.glue_givers = [] self.glue_receivers = [] self.receiving_neighbours = [] self.giving_neighbours = [] if self.length_direction == 0: self.length_vector = Vector(1, 0, 0) self.width_vector = Vector(0, 1, 0) else: self.length_vector = Vector(0, 1, 0) self.width_vector = Vector(1, 0, 0) self.board_frame = Frame(self.centre_point, self.length_vector, self.width_vector) self.box = Box(self.board_frame, self.length, self.width, self.height)
def get_pose(self, input): if isinstance(input, rg.plane): frame = Frame(input.Origin, input.XAxis, input.YAxis) pose = frame.point.data + frame.quaternion.wxyz else: pose = input.point.data + input.quaternion.wxyz return pose
def __init__(self, identifier, host='127.0.0.1', port_snd=30003, port_rcv=30004, ghenv = None): ClientContainer.__init__(self, identifier, host, port_snd, port_rcv, ghenv = ghenv) " create " self.tool_frame = Frame([0, 0, 0], [1, 0, 0], [0, 1, 0]) # init values for command messages self.int_speed = 0 # speed: 0 = slow, 1 = mid, 2 = fast self.float_duration = 0 #duration is not used, use velocity instead self.int_zonedata = 10 # zonedata: in mm self.int_tool = 0 # toolnumber: 0 = tool0, 1 = vacgrip_vert, 2 = hokuyo self.int_wobj = 0 # wobjnumber: 0 = wobj0, 1 = wobj_common, 2 = wobj_base # self.int_rob_num = 0 self.float_arbitrary = 0 #robot number to access # home positions cartesian - to change!!!!! - stefana self.tool_plane_home_mid = rg.Plane(rg.Point3d(1000,0,650), rg.Vector3d(1,0,0), rg.Vector3d(0,-1,0)) self.tool_plane_home_left = rg.Plane(rg.Point3d(-665.507, 629.086, 720.0), rg.Vector3d(-1,0,0), rg.Vector3d(0,1,0)) # home positions joint targets - to change!!!! - stefana self.jointtarget_home_zero = [0,0,0,0,0,0] self.jointtarget_home_pickbrick = [130.9, -56.8, 58.3, 7.4, 30.1, 51.9] ##################################### changed stefana ######################################### self.current_joint_values = [0,0,0,0,0,0,0,0,0] self.current_tool0_pose = [0,0,0,0,0,0,0,0,0,0] #self.ghComp = None self.debug_print = []
def read_mesh_from_filename(self, filename, meshcls): if not os.path.isfile(filename): raise FileNotFoundError("No such file: '%s'" % filename) extension = filename[(filename.rfind(".") + 1):] if extension == "dae": # no dae support yet #mesh = Mesh.from_dae(filename) obj_filename = filename.replace(".dae", ".obj") if os.path.isfile(obj_filename): mesh = Mesh.from_obj(obj_filename) # former DAE files have yaxis and zaxis swapped # TODO: already fix in conversion to obj frame = Frame([0,0,0], [1,0,0], [0,0,1]) T = Transformation.from_frame(frame) mesh_transform(mesh, T) else: raise FileNotFoundError("Please convert '%s' into an OBJ file, \ since DAE is currently not supported \ yet." % filename) elif extension == "obj": mesh = Mesh.from_obj(filename) elif extension == "stl": mesh = Mesh.from_stl(filename) else: raise ValueError("%s file types not yet supported" % extension.upper()) return meshcls(mesh)
def __init__(self, no_layers, min_gap, len_prim, len_sec, omni, prim_wid_out, prim_hi_out, prim_wid_in, prim_hi_in, sec_wi, sec_hi, prim_intv, prim_fal, prim_dedens, sec_intv, sec_intv_dev, orgn_fram, skip): self.layer_no = no_layers self.gap_min = min_gap self.primary_length = len_prim self.secondary_length = len_sec self.omnidirectional = omni self.primary_board_width_outside = prim_wid_out self.primary_board_height_outside = prim_hi_out self.primary_board_width_inside = prim_wid_in self.primary_board_height_inside = prim_hi_in self.primary_board_outside_dimensions = [self.primary_board_width_outside, self.primary_board_height_outside, self.primary_length] self.primary_board_inside_dimensions = [self.primary_board_width_inside, self.primary_board_height_inside, self.primary_length] self.secondary_board_width = sec_wi self.secondary_board_height = sec_hi self.secondary_board_dimensions = [self.secondary_board_width, self.secondary_board_height, self.secondary_length] self.primary_interval = prim_intv self.primary_falloff = prim_fal self.primary_dedensification = prim_dedens self.secondary_interval = sec_intv self.secondary_interval_development = sec_intv_dev self.skipping = skip self.primary_direction = 0 self.secondary_direction = 1 self.origin_fr = orgn_fram self.origin_pt = orgn_fram[0] self.prim_dir = orgn_fram[1] self.sec_dir = orgn_fram[2] self.sec_fr = Frame(self.origin_pt, self.sec_dir, self.prim_dir) self.timberboards = [] # ADVANCED PARAMETERS self.advanced_setup = False
def offset_bbox_xy(pts, dist): bbox = pca_numpy(pts) frame1 = Frame(bbox[0], bbox[1][0], bbox[1][1]) xform = Transformation.from_frame_to_frame(frame1, Frame.worldXY()) pts = transform_points(pts, xform) bbox = bounding_box_xy(pts) bbox = offset_polygon(bbox, dist) return bbox, xform
def test_basis_vectors_from_matrix(): f = Frame([0, 0, 0], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) R = matrix_from_frame(f) xaxis, yaxis = basis_vectors_from_matrix(R) assert allclose( xaxis, [0.6807833515407016, 0.6807833515407016, 0.2703110366411609]) assert allclose( yaxis, [-0.6687681911461376, 0.7282315441900513, -0.14975955581430114])
def test_matrix_from_frame(): f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) T = matrix_from_frame(f) t = [[0.6807833515407016, -0.6687681911461376, -0.29880283595731283, 1.0], [0.6807833515407016, 0.7282315441900513, -0.0788216106888398, 1.0], [0.2703110366411609, -0.14975955581430114, 0.9510541619236438, 1.0], [0.0, 0.0, 0.0, 1.0]] assert allclose(T, t)
def test_scale(): S = Scale.from_factors([1, 2, 3]) assert S.matrix == [[1.0, 0.0, 0.0, 0.0], [0.0, 2.0, 0.0, 0.0], [0.0, 0.0, 3.0, 0.0], [0.0, 0.0, 0.0, 1.0]] S = Scale.from_factors([2.] * 3, Frame((2, 5, 0), (1, 0, 0), (0, 1, 0))) assert S.matrix == [[2.0, 0.0, 0.0, -2.0], [0.0, 2.0, 0.0, -5.0], [0.0, 0.0, 2.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
def to_compas(self): """Convert to a COMPAS geometry object. Returns ------- :class:`compas.geometry.Frame` A COMPAS frame object. """ return Frame(self.point, self.xaxis, self.yaxis)
def test_reflection_from_frame(): point = [1, 1, 1] x = [1, 0, 0] y = [0, 1, 0] f = Frame(point, x, y) R1 = Reflection.from_frame(f) R2 = Transformation.from_matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 2], [0, 0, 0, 1]]) assert R1 == R2
def oabb_numpy(points): origin, (xaxis, yaxis, zaxis), values = pca_numpy(points) frame = Frame(origin, xaxis, yaxis) world = Frame.worldXY() X = Transformation.from_frame_to_frame(frame, world) points = transform_points_numpy(points, X) bbox = bounding_box(points) bbox = transform_points_numpy(bbox, X.inverse()) return bbox
def rule_Connect_90lap(self,selected_beams,projected_point_list,face_ids,beam_length,ext_start,name): #create joints on selected beams joint_distance_to_selectedBeams = [] for i in range (len(selected_beams)): print("Adding Joint to Beam: ", i) #Prepare information selected_beam = selected_beams[i] projected_point = projected_point_list[i] face_id = face_ids[i] print(face_id) joint_distance = selected_beam.Get_distancefromBeamYZFrame(projected_point) joint_distance_to_selectedBeams.append(joint_distance) #Create Joint new_joint = Joint_90lap(joint_distance,face_id,100,50,100) #Add new Joint to Beam and update beam mesh selected_beam.joints.append(new_joint) new_joint.update_joint_mesh(selected_beam) selected_beam.update_mesh() #Add match beam #####get beam frame #construct a frame similar to face frame with max point as origin face_frame = selected_beams[0].face_frame(face_ids[0]) max_point_frame = Frame(face_frame.point,face_frame.xaxis,face_frame.yaxis) connection_beam_origin = max_point_frame.represent_point_in_global_coordinates([(joint_distance_to_selectedBeams[0]-50),0, ext_start]) connection_beam_frame = Frame(connection_beam_origin, face_frame.normal * -1.0, face_frame.yaxis) match_beam = self.rule_create_beam(connection_beam_frame,beam_length,100,100,name) #calculate distance from projected points to match_beam[0]plane for i in range(len(projected_point_list)): print("Adding Joint " , i) pt = projected_point_list[i] match_beam_joint_distance = match_beam.Get_distancefromBeamYZFrame(pt) joint = Joint_90lap(match_beam_joint_distance,3,100,50,100) #match_beam joint face is always 3 match_beam.joints.append(joint) joint.update_joint_mesh(match_beam) #performs multiple booleans in 1 call match_beam.update_mesh()
def data(self, data): f_origin = data.get('f_origin') or [] f_xaxis = data.get('f_xaxis') or [] f_yaxis = data.get('f_yaxis') or [] c_type = data.get('type') or None c_part = data.get('part') or None c_id = data.get('id') or None self.frame = Frame(f_origin, f_xaxis, f_yaxis) flip_pln_Y = self.frame.yaxis.copy() flip_pln_Y.scale(-1) self.flip_pln = Frame(self.frame.point, self.frame.xaxis, flip_pln_Y) self.type = c_type self.part = c_part self.id = int(c_id) self.rules_table = [] self.active_rules = []
def forward_kinematics_offset_wrist(joint_values, params): """Forward kinematics function for offset wrist 6-axis robots. Parameters ---------- joint_values : list of float List of 6 joint values in radians. params : list of float The offset wrist parameters that specify the robot. Returns ------- :class:`compas.geometry.Frame` Notes ----- Code adapted from https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_kinematics/src/ur_kin.cpp """ d1, a2, a3, d4, d5, d6 = params q = joint_values s1, c1 = sin(q[0]), cos(q[0]) q23, q234, s2, c2 = q[1], q[1], sin(q[1]), cos(q[1]) q23 += q[2] q234 += q[2] s4, c4 = sin(q[3]), cos(q[3]) q234 += q[3] s5, c5 = sin(q[4]), cos(q[4]) s6, c6 = sin(q[5]), cos(q[5]) s23, c23 = sin(q23), cos(q23) s234, c234 = sin(q234), cos(q234) T = [0. for _ in range(4 * 4)] T[0] = c234 * c1 * s5 - c5 * s1 T[1] = c6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * s6 T[2] = -s6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * c6 T[3] = d6*c234*c1*s5 - a3*c23*c1 - a2 * \ c1*c2 - d6*c5*s1 - d5*s234*c1 - d4*s1 T[4] = c1 * c5 + c234 * s1 * s5 T[5] = -c6 * (c1 * s5 - c234 * c5 * s1) - s234 * s1 * s6 T[6] = s6 * (c1 * s5 - c234 * c5 * s1) - s234 * c6 * s1 T[7] = d6*(c1*c5 + c234*s1*s5) + d4*c1 - \ a3*c23*s1 - a2*c2*s1 - d5*s234*s1 T[8] = -s234 * s5 T[9] = -c234 * s6 - s234 * c5 * c6 T[10] = s234 * c5 * s6 - c234 * c6 T[11] = d1 + a3*s23 + a2*s2 - d5 * \ (c23*c4 - s23*s4) - d6*s5*(c23*s4 + s23*c4) T[15] = 1.0 frame = Frame((T[3], T[7], T[11]), (T[0], T[4], T[8]), (T[1], T[5], T[9])) return frame
def icp_numpy(d1, d2, tol=1e-3): """Align two point clouds using the Iterative Closest Point (ICP) method. Parameters ---------- d1 : list of point Point cloud 1. d2 : list of point Point cloud 2. tol : float, optional Tolerance for finding matches. Default is ``1e-3``. Returns ------- Notes ----- Examples -------- References ---------- """ d1 = asarray(d1) d2 = asarray(d2) point, axes, spread = pca_numpy(d1) frame1 = Frame(point, axes[0], axes[1]) point, axes, spread = pca_numpy(d2) frame2 = Frame(point, axes[0], axes[1]) T = Transformation.from_frame_to_frame(frame1, frame2) transform_points_numpy(d1, T) y = cdist(d1, d2, 'eucledian') closest = argmin(y, axes=1)