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)
