"""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)
"""
from compas.geometry import Frame
from compas.geometry import Box
from compas_rhino.artists import FrameArtist
from compas_rhino.artists import BoxArtist
# Given: box in the world coordinate system
frame = Frame([1, 0, 0], [-0.45, 0.1, 0.3], [1, 0, 0])
width, length, height = 1, 1, 1
box = Box(frame, width, length, height)
# Given: frame F representing a coordinate system
F = Frame([2, 2, 2], [0.978, 0.010, -0.210], [0.090, 0.882, 0.463])
# Task: represent box frame in frame F and construct new box
box_frame_transformed = F.to_world_coordinates(box.frame)
box_transformed = Box(box_frame_transformed, width, length, height)
print("Box frame transformed:", box_transformed.frame)
# create artists
artist1 = FrameArtist(Frame.worldXY())
artist2 = BoxArtist(box)
artist3 = FrameArtist(F)
artist4 = BoxArtist(box_transformed)
# draw
artist1.draw()
artist2.draw()
artist3.draw()
artist4.draw()
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)
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)]
"""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))
