"""Example: 'frame in frame' """ from compas.geometry import Point from compas.geometry import Vector from compas.geometry import Frame point = Point(146.00, 150.00, 161.50) xaxis = Vector(0.9767, 0.0010, -0.214) yaxis = Vector(0.1002, 0.8818, 0.4609) F0 = Frame(point, xaxis, yaxis) # coordinate system F0 point = Point(35., 35., 35.) xaxis = Vector(0.604, 0.430, 0.671) yaxis = Vector(-0.631, 0.772, 0.074) f_lcf = Frame(point, xaxis, yaxis) # frame f_lcf in F0 (local coordinates) # frame in global (world) coordinate system f_wcf = F0.to_world_coordinates(f_lcf) print(f_wcf) f_lcf2 = F0.to_local_coordinates(f_wcf) # world coords back to local coords print(f_lcf2) # should equal f_lcf print(f_lcf == f_lcf2)
def inverse_kinematics_spherical_wrist(target_frame, points): """Inverse kinematics function for spherical wrist robots. Parameters ---------- frame : :class:`compas.geometry.Frame` The frame we search the inverse kinematics for. points : list of point A list of 4 points specifying the robot's joint positions. Returns ------- list of list of float The 8 analytical IK solutions. Notes ----- This is a modification of the *Lobster* tool for solving the inverse kinematics problem for a spherical wrist robots. https://www.grasshopper3d.com/forum/topics/lobster-reloaded?groupUrl=lobster& This is the instruction on how to determine the 4 points. http://archive.fabacademy.org/archives/2017/fablabcept/students/184/assets/lobster_ik.pdf Check that p2, p3, and p4 are all in the XZ plane (y coordinates are zero)! """ p1, p2, p3, p4 = points end_frame = Frame(target_frame.point, target_frame.yaxis, target_frame.xaxis) wrist_offset = p4.x - p3.x lower_arm_length = p2.z - p1.z upper_arm_length = (p3 - p2).length pln_offset = math.fabs(p2.y - p1.y) axis4_offset_angle = math.atan2(p3.z - p2.z, p3.x - p2.x) wrist = end_frame.to_world_coordinates(Point(0, 0, wrist_offset)) p1_proj = p1.copy() p1_proj.y = p2.y axis1_angles = [] axis2_angles = [] axis3_angles = [] axis4_angles = [] axis5_angles = [] axis6_angles = [] if pln_offset == 0: axis1_angle = -1 * math.atan2(wrist.y, wrist.x) if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 axis1_angle += math.pi if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 else: circle = (0, 0, 0), pln_offset point = (wrist.x, wrist.y, 0) t1, t2 = tangent_points_to_circle_xy(circle, point) a1 = Vector(0, 1, 0).angle_signed(t1, (0, 0, -1)) a2 = Vector(0, 1, 0).angle_signed(t2, (0, 0, -1)) axis1_angles += [a1] * 4 axis1_angles += [a2] * 4 for i in range(2): axis1_angle = axis1_angles[i * 4] Rot1 = Rotation.from_axis_and_angle([0, 0, 1], -1 * axis1_angle, point=[0, 0, 0]) p1A = p1_proj.transformed(Rot1) elbow_dir = Vector(1, 0, 0).transformed(Rot1) sphere1 = Sphere(p1A, lower_arm_length) sphere2 = Sphere(wrist, upper_arm_length) elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1]) elbow_plane = (p1A, elbow_frame.normal) _, (center, radius, normal) = intersection_sphere_sphere(sphere1, sphere2) circle = ((center, normal), radius) intersect_pt1, intersect_pt2 = intersection_plane_circle( elbow_plane, circle) for j in range(2): if j == 0: elbow_pt = intersect_pt1 else: elbow_pt = intersect_pt2 elbow_pt = Point(*elbow_pt) elbowx, elbowy, _ = elbow_frame.to_local_coordinates(elbow_pt) wristx, wristy, _ = elbow_frame.to_local_coordinates(wrist) axis2_angle = math.atan2(elbowy, elbowx) axis3_angle = math.pi - axis2_angle + math.atan2( wristy - elbowy, wristx - elbowx) - axis4_offset_angle for k in range(2): axis2_angles.append(-axis2_angle) axis3_angle_wrapped = -axis3_angle + math.pi while (axis3_angle_wrapped >= math.pi): axis3_angle_wrapped -= 2 * math.pi while (axis3_angle_wrapped < -math.pi): axis3_angle_wrapped += 2 * math.pi axis3_angles.append(axis3_angle_wrapped) for k in range(2): axis4 = wrist - elbow_pt axis4.transform( Rotation.from_axis_and_angle(elbow_frame.zaxis, -axis4_offset_angle)) temp_frame = elbow_frame.copy() temp_frame.transform( Rotation.from_axis_and_angle(temp_frame.zaxis, axis2_angle + axis3_angle)) # // B = TempPlane axis4_frame = Frame(wrist, temp_frame.zaxis, temp_frame.yaxis * -1.0) axis6x, axis6y, axis6z = axis4_frame.to_local_coordinates( end_frame.point) axis4_angle = math.atan2(axis6y, axis6x) if k == 1: axis4_angle += math.pi if axis4_angle > math.pi: axis4_angle -= 2 * math.pi axis4_angle_wrapped = axis4_angle + math.pi / 2 while (axis4_angle_wrapped >= math.pi): axis4_angle_wrapped -= 2 * math.pi while (axis4_angle_wrapped < -math.pi): axis4_angle_wrapped += 2 * math.pi axis4_angles.append(axis4_angle_wrapped) axis5_frame = axis4_frame.copy() axis5_frame.transform( Rotation.from_axis_and_angle(axis4_frame.zaxis, axis4_angle)) axis5_frame = Frame(wrist, axis5_frame.zaxis * -1, axis5_frame.xaxis) axis6x, axis6y, _ = axis5_frame.to_local_coordinates( end_frame.point) axis5_angle = math.atan2(axis6y, axis6x) axis5_angles.append(axis5_angle) axis6_frame = axis5_frame.copy() axis6_frame.transform( Rotation.from_axis_and_angle(axis5_frame.zaxis, axis5_angle)) axis6_frame = Frame(wrist, axis6_frame.yaxis * -1, axis6_frame.zaxis) endx, endy, _ = axis6_frame.to_local_coordinates( end_frame.to_world_coordinates(Point(1, 0, 0))) axis6_angle = math.atan2(endy, endx) axis6_angles.append(axis6_angle) return [[a1, a2, a3, a4, a5, a6] for a1, a2, a3, a4, a5, a6 in zip( axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles)]
"""Change-basis transformation vs transformation between two frames. """ from compas.geometry import Point from compas.geometry import Frame from compas.geometry import Transformation F1 = Frame([2, 2, 2], [0.12, 0.58, 0.81], [-0.80, 0.53, -0.26]) F2 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) # transformation between 2 frames F1, F2 Tf = Transformation.from_frame_to_frame(F1, F2) # change-basis transformation between two frames F1 and F2. Tc = Transformation.from_change_of_basis(F1, F2) # they are different print(Tf) print(Tc) # This is how to use Tf: transform geometry into another coordinate frame pt = Point(2, 2, 2) pt.transform(Tf) print(pt, "==", F2.point) # This is how to use Tc: represent geometry in another coordinate frame pt = Point(0, 0, 0) # local point in F1 pt.transform(Tc) print(pt, "==", F2.to_local_coordinates(F1.point))
def inverse_kinematics_spherical_wrist(p1, p2, p3, p4, target_frame): """Inverse kinematics function for spherical wrist robots. This is a modification of the *Lobster* tool for solving the inverse kinematics problem for a spherical wrist robots. https://www.grasshopper3d.com/forum/topics/lobster-reloaded?groupUrl=lobster& This is the instruction on how to determine the 4 points. https://api.ning.com/files/KRgE1yt2kgG2IF9H8sre4CWfIDL9ytv5WvVn54zdOx6HE84gDordaHzo0jqwP-Qhry7MyRQ4IQxY1p3cIkqEDj1FAVVR2Xg0/Lobster_IK.pdf check p2, p3, p4 must be in one XY plane! """ end_frame = Frame(target_frame.point, target_frame.yaxis, target_frame.xaxis) wrist_offset = p4.x - p3.x lower_arm_length = p2.z - p1.z upper_arm_length = (p3 - p2).length pln_offset = math.fabs(p2.y - p1.y) axis4_offset_angle = math.atan2(p3.z - p2.z, p3.x - p2.x) wrist = end_frame.to_world_coordinates(Point(0, 0, wrist_offset)) p1_proj = p1.copy() p1_proj.y = p2.y axis1_angles = [] axis2_angles = [] axis3_angles = [] axis4_angles = [] axis5_angles = [] axis6_angles = [] if pln_offset == 0: axis1_angle = -1 * math.atan2(wrist.y, wrist.x) if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 axis1_angle += math.pi if axis1_angle > math.pi: axis1_angle -= 2 * math.pi axis1_angles += [axis1_angle] * 4 else: circle = (0, 0, 0), pln_offset point = (wrist.x, wrist.y, 0) t1, t2 = tangent_points_to_circle_xy(circle, point) a1 = Vector(0, 1, 0).angle_signed(t1, (0, 0, -1)) a2 = Vector(0, 1, 0).angle_signed(t2, (0, 0, -1)) axis1_angles += [a1] * 4 axis1_angles += [a2] * 4 for i in range(2): axis1_angle = axis1_angles[i * 4] Rot1 = Rotation.from_axis_and_angle([0, 0, 1], -1 * axis1_angle, point=[0, 0, 0]) p1A = p1_proj.transformed(Rot1) elbow_dir = Vector(1, 0, 0).transformed(Rot1) sphere1 = Sphere(p1A, lower_arm_length) sphere2 = Sphere(wrist, upper_arm_length) elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1]) elbow_plane = (p1A, elbow_frame.normal) result = intersection_sphere_sphere(sphere1, sphere2) if result is not None: case, (center, radius, normal) = result else: return None circle = ((center, normal), radius) intersect_pt1, intersect_pt2 = intersection_plane_circle( elbow_plane, circle) for j in range(2): if j == 0: elbow_pt = intersect_pt1 else: elbow_pt = intersect_pt2 elbow_pt = Point(*elbow_pt) elbowx, elbowy, elbowz = elbow_frame.to_local_coordinates(elbow_pt) wristx, wristy, wristz = elbow_frame.to_local_coordinates(wrist) axis2_angle = math.atan2(elbowy, elbowx) axis3_angle = math.pi - axis2_angle + math.atan2( wristy - elbowy, wristx - elbowx) - axis4_offset_angle for k in range(2): axis2_angles.append(-axis2_angle) axis3_angle_wrapped = -axis3_angle + math.pi while (axis3_angle_wrapped >= math.pi): axis3_angle_wrapped -= 2 * math.pi while (axis3_angle_wrapped < -math.pi): axis3_angle_wrapped += 2 * math.pi axis3_angles.append(axis3_angle_wrapped) for k in range(2): axis4 = wrist - elbow_pt axis4.transform( Rotation.from_axis_and_angle(elbow_frame.zaxis, -axis4_offset_angle)) temp_frame = elbow_frame.copy() # copy yes or no? temp_frame.transform( Rotation.from_axis_and_angle(temp_frame.zaxis, axis2_angle + axis3_angle)) # // B = TempPlane axis4_frame = Frame(wrist, temp_frame.zaxis, temp_frame.yaxis * -1.0) axis6x, axis6y, axis6z = axis4_frame.to_local_coordinates( end_frame.point) axis4_angle = math.atan2(axis6y, axis6x) if k == 1: axis4_angle += math.pi if axis4_angle > math.pi: axis4_angle -= 2 * math.pi axis4_angle_wrapped = axis4_angle + math.pi / 2 while (axis4_angle_wrapped >= math.pi): axis4_angle_wrapped -= 2 * math.pi while (axis4_angle_wrapped < -math.pi): axis4_angle_wrapped += 2 * math.pi axis4_angles.append(axis4_angle_wrapped) axis5_frame = axis4_frame.copy() axis5_frame.transform( Rotation.from_axis_and_angle(axis4_frame.zaxis, axis4_angle)) axis5_frame = Frame(wrist, axis5_frame.zaxis * -1, axis5_frame.xaxis) axis6x, axis6y, axis6z = axis5_frame.to_local_coordinates( end_frame.point) axis5_angle = math.atan2(axis6y, axis6x) axis5_angles.append(axis5_angle) axis6_frame = axis5_frame.copy() axis6_frame.transform( Rotation.from_axis_and_angle(axis5_frame.zaxis, axis5_angle)) axis6_frame = Frame(wrist, axis6_frame.yaxis * -1, axis6_frame.zaxis) endx, endy, endz = axis6_frame.to_local_coordinates( end_frame.to_world_coordinates(Point(1, 0, 0))) axis6_angle = math.atan2(endy, endx) axis6_angles.append(axis6_angle)
"""Change-basis transformation. """ from compas.geometry import Point from compas.geometry import Frame from compas.geometry import Transformation F1 = Frame.worldXY() F2 = Frame([1.5, 1, 0], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) P = Point(2, 2, 2) # local point in F1 # change-basis transformation between two frames F1 and F2. T = Transformation.from_change_of_basis(F1, F2) # Represent geometry (=point P) in another coordinate frame print(P.transformed(T)) # You can also use the following print(F2.to_local_coordinates(P))
"""Example: 'point in frame' The simple example above shows how to use a frame as a coordinate system: Starting from a point `P` in the local (user-defined, relative) coordinate system of frame `F`, i.e. its position is relative to the origin and orientation of `F`, we want to get the position `P_` of `P` in the global (world, absolute) coordinate system. """ from compas.geometry import Point from compas.geometry import Vector from compas.geometry import Frame from compas.geometry import allclose point = Point(146.00, 150.00, 161.50) xaxis = Vector(0.9767, 0.0010, -0.214) yaxis = Vector(0.1002, 0.8818, 0.4609) F = Frame(point, xaxis, yaxis) # coordinate system F P = Point(35., 35., 35.) # point in F (local coordinates) P_ = F.to_world_coordinates(P) # point in global (world) coordinates print("The point's world coordinates: {}".format(P_)) P2 = F.to_local_coordinates(P_) print("The point's local coordinates: {}".format(P2)) # should equal P print(allclose(P2, P))