def test_booleans():
# ==============================================================================
# Make a box and a sphere
# ==============================================================================
box = Box.from_width_height_depth(2, 2, 2)
box = Mesh.from_shape(box)
box.quads_to_triangles()
A = box.to_vertices_and_faces()
sphere = Sphere(Point(1, 1, 1), 1)
sphere = Mesh.from_shape(sphere, u=30, v=30)
sphere.quads_to_triangles()
B = sphere.to_vertices_and_faces()
# ==============================================================================
# Remesh the sphere
# ==============================================================================
B = remesh(B, 0.3, 10)
# ==============================================================================
# Compute the boolean mesh
# ==============================================================================
V, F = boolean_union(A, B)
mesh = Mesh.from_vertices_and_faces(V, F)
def test_sphere_scaling():
s = Sphere(Point(1, 1, 1), 10)
s.transform(Scale.from_factors([2, 3, 4]))
assert s.point.x == 2
assert s.point.y == 3
assert s.point.z == 4
assert s.radius == 40
def position_constraint_from_frame(self,
frame_WCF,
tolerance_position,
group=None):
"""Returns a position and orientation constraint on the group's end-effector link.
Parameters
----------
frame_WCF : :class:`compas.geometry.Frame`
The frame from which we create position and orientation constraints.
tolerance_position : float
The allowed tolerance to the frame's position. (Defined in the
robot's units)
group: str
The planning group for which we specify the constraint. Defaults to
the robot's main planning group.
Examples
--------
>>> frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
>>> tolerance_position = 0.001
>>> goal_constraints = robot.position_constraint_from_frame(frame, tolerance_position)
Notes
-----
There are many other possibilities of how to create a position and
orientation constraints. Checkout :class:`compas_fab.robots.PositionConstraint`
and :class:`compas_fab.robots.OrientationConstraint`.
"""
ee_link = self.get_end_effector_link_name(group)
sphere = Sphere(frame_WCF.point, tolerance_position)
return PositionConstraint.from_sphere(ee_link, sphere)
def sphere_to_compas(sphere):
"""Convert a Rhino sphere to a COMPAS sphere.
Parameters
----------
sphere: :class:`Rhino.Geometry.Sphere`
Returns
-------
:class:`compas.geometry.Sphere`
"""
return Sphere(point_to_compas(sphere.Center), sphere.Radius)
return 'VolSphere({})'.format(str(self.sphere))
def get_distance(self, point):
if not isinstance(point, Point):
point = Point(*point)
d = point.distance_to_point(self.sphere.center)
return d - self.sphere.radius
def get_distance_numpy(self, x, y, z):
import numpy as np
d = np.sqrt((x - self.sphere.center.x)**2 +
(y - self.sphere.center.y)**2 +
(z - self.sphere.center.z)**2) - self.sphere.radius
return d
if __name__ == "__main__":
o = VolSphere(Sphere(Point(4, 2, 0), 8.5))
print(str(o))
for y in range(-15, 15):
s = ''
for x in range(-30, 30):
d = o.get_distance(Point(x * 0.5, 0, -y))
if d < 0:
s += 'x'
else:
s += '.'
# print(s)
def from_data(cls, data):
sphere = Sphere.from_data(data['geom'])
vsphere = cls(sphere)
return vsphere
from compas.geometry import Sphere, Translation, centroid_points from compas.utilities import geometric_key from compas_view2.app import App from compas_view2.objects import Collection from compas_gmsh.models import ShapeModel # ============================================================================== # Geometry # ============================================================================== sphere = Sphere([0, 0, 0], 2) # ============================================================================== # Solid Model # ============================================================================== model = ShapeModel(name="tets") model.add_sphere(sphere) model.options.mesh.lmax = 0.2 model.generate_mesh(3) # ============================================================================== # COMPAS mesh # ============================================================================== tets = model.mesh_to_tets() shell = model.mesh_to_compas()
import math
from compas.geometry import Point
from compas.geometry import Plane
from compas.geometry import Frame
from compas.geometry import Sphere
from compas_fab.backends import AnalyticalInverseKinematics
from compas_fab.backends import PyBulletClient
from compas_fab.robots import ReachabilityMap
from compas_fab.robots.reachability_map import DeviationVectorsGenerator
from compas_fab import DATA
# 1. Define generators
sphere = Sphere((0.4, 0, 0), 0.15)
def points_on_sphere_generator(sphere):
for theta_deg in range(0, 360, 20):
for phi_deg in range(0, 90, 10):
theta = math.radians(theta_deg)
phi = math.radians(phi_deg)
x = sphere.point.x + sphere.radius * math.cos(theta) * math.sin(phi)
y = sphere.point.y + sphere.radius * math.sin(theta) * math.sin(phi)
z = sphere.point.z + sphere.radius * math.cos(phi)
point = Point(x, y, z)
axis = sphere.point - point
plane = Plane((x, y, z), axis)
f = Frame.from_plane(plane)
# for UR5 is zaxis the xaxis
import rhinoscriptsyntax as rs
import Rhino.Geometry as rg
from compas_vol.primitives import VolSphere, VolBox
from compas_vol.combinations import Union,Subtraction
from compas.geometry import Sphere, Box, Frame
from compas.rpc import Proxy
s = VolSphere(Sphere((0, 0, 0), 6))
b = VolBox(Box(Frame.worldXY(), 10, 10, 10), 1.5)
u = Union(s, b)
t = Subtraction(b, s)
p = Proxy('compas_vol.utilities')
#p.stop_server()
#p = Proxy('compas_vol.utilities')
bounds = ((-25,25,100), (-25,25,100), (-25,25,100))
vs,fs = p.get_vfs_from_tree(str(t), bounds, 1.0)
mesh = rg.Mesh()
for v1, v2, v3 in vs:
mesh.Vertices.Add(v1, v2, v3)
for f in fs:
if len(set(f))>2:
mesh.Faces.AddFace(f[0], f[1], f[2])
mesh.Compact()
mesh.Normals.ComputeNormals()
a = mesh
b = [rs.CreatePoint(v1,v2,v3) for v1, v2, v3 in vs]
# @me add restart option to init from compas.rpc import Proxy proxy = Proxy() # ============================================================================== # Make a box and a sphere # ============================================================================== box = Box.from_width_height_depth(2, 2, 2) box = Mesh.from_shape(box) box.quads_to_triangles() A = box.to_vertices_and_faces() sphere = Sphere(Point(1, 1, 1), 1) sphere = Mesh.from_shape(sphere, u=30, v=30) sphere.quads_to_triangles() B = sphere.to_vertices_and_faces() # ============================================================================== # Remesh the sphere # ============================================================================== proxy.package = 'compas_cgal.meshing' B = proxy.remesh(B, 0.3, 10) # ============================================================================== # Compute the intersections
import rhinoscriptsyntax as rs
import Rhino.Geometry as rg
import math
frame = Frame([10, 0, 0], [1, 0, 0], [0, 1, 0])
# box = Box(Frame.worldXY(), 150, 90, 200)
box = Box(frame, 150, 90, 200)
volbox = VolBox(box)
plane = Plane([0, 0, 0], [0, 0, 1])
circle = Circle(plane, 50)
cylinder = Cylinder(circle, 90)
volcylinder = VolCylinder(cylinder)
sphere = Sphere((40, 20, 0), 50)
volshpere = VolSphere(sphere)
sphere_big = Sphere((40, 20, 0), 100)
volshpere_big = VolSphere(sphere_big)
union = Union(volbox, volshpere)
substract = Subtraction(volshpere_big, volshpere)
class FuckYouBernhardFunction:
X_SCALE = 5.0
Y_SCALE = 5.0
Z_SCALE = 5.0
import numpy as np
import matplotlib.pyplot as plt
from compas_vol.primitives import VolSphere
from compas_vol.analysis import Gradient
from compas.geometry import Point, Sphere
s = Sphere(Point(1, 2, 3), 4)
vs = VolSphere(s)
g = Gradient(vs)
print(vs.get_distance(Point(4, 5, 6)))
print(g.get_gradient(Point(5, 2, 3)))
x, y, z = np.ogrid[-10:10:20j, -10:10:20j, -10:10:20j]
d = g.get_gradient_numpy(x, y, z)
plt.quiver(x, y, d[:, :, 0, 1], d[:, :, 0, 2])
plt.axis('equal')
plt.show()
db = self.b.get_distance_numpy(x, y, z)
h = np.minimum(np.maximum(0.5 + 0.5 * (db - da) / self.r, 0), 1)
return (db * (1 - h) + h * da) - self.r * h * (1 - h)
# ==============================================================================
# Main
# ==============================================================================
if __name__ == "__main__":
from compas_vol.primitives import VolSphere, VolBox
from compas.geometry import Box, Frame, Point, Sphere
import numpy as np
import matplotlib.pyplot as plt
s = Sphere(Point(4, 5, 0), 7)
b = Box(Frame.worldXY(), 20, 15, 10)
vs = VolSphere(s)
vb = VolBox(b, 2.5)
u = SmoothUnion(vs, vb, 6.5)
# for y in range(-15, 15):
# s = ''
# for x in range(-30, 30):
# d = u.get_distance(Point(x*0.5, y, 0))
# if d < 0:
# s += 'x'
# else:
# s += '.'
# print(s)
x, y, z = np.ogrid[-15:15:100j, -15:15:100j, -15:15:100j]
foot_2 = Box(Frame([-2, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5)
leg_1 = Box(Frame([2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7)
leg_2 = Box(Frame([-2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7)
axis = Cylinder(Circle(Plane([0, 0, 7], [1, 0, 0]), .01), 4)
legs_link = model.add_link('legs',
visual_meshes=[foot_1, foot_2, leg_1, leg_2, axis])
torso = Cylinder(Circle(Plane([0, 0, 0], [0, 0, 1]), .5), 8)
torso_link = model.add_link('torso', visual_meshes=[torso])
legs_joint_origin = Frame([0, 0, 7], [1, 0, 0], [0, 1, 0])
joint_axis = Vector(1, 0, 0)
model.add_joint('torso_base_joint', Joint.CONTINUOUS, legs_link, torso_link,
legs_joint_origin, joint_axis)
head = Sphere([0, 0, 0], 1)
beak = Cylinder(Circle(Plane([0, 1, -.3], [0, 1, 0]), .3), 1.5)
head_link = model.add_link('head', visual_meshes=[head, beak])
neck_joint_origin = Frame([0, 0, 4], [1, 0, 0], [0, 1, 0])
model.add_joint('neck_joint',
Joint.FIXED,
torso_link,
head_link,
origin=neck_joint_origin)
tail = Sphere([0, 0, 0], 1)
tail_link = model.add_link('tail', visual_meshes=[tail])
tail_joint_origin = Frame([0, 0, -4], [1, 0, 0], [0, 1, 0])
model.add_joint('tail_joint',
Joint.FIXED,
torso_link,
from compas.datastructures import mesh_quads_to_triangles from compas_viewers.multimeshviewer import MultiMeshViewer from compas_viewers.multimeshviewer import MeshObject import compas_libigl as igl # ============================================================================== # Input Geometry # ============================================================================== R = 1.4 point = [0.0, 0.0, 0.0] cube = Box.from_width_height_depth(2 * R, 2 * R, 2 * R) sphere = Sphere(point, R * 1.25) a = Mesh.from_shape(cube) b = Mesh.from_shape(sphere, u=30, v=30) mesh_quads_to_triangles(a) mesh_quads_to_triangles(b) A = a.to_vertices_and_faces() B = b.to_vertices_and_faces() # ============================================================================== # Booleans # ============================================================================== D = igl.mesh_intersection(A, B)
from compas.geometry import Box
from compas.geometry import Circle
from compas.geometry import Cylinder
from compas.geometry import Frame
from compas.geometry import Plane
from compas.geometry import Sphere
# Box
b1 = Box(Frame.worldXY(), 5, 1, 3) # xsize, ysize, zsize
b2 = Box.from_width_height_depth(5, 3,
1) # width=xsize, height=zsize, depth=ysize
assert str(b1) == str(b2)
print(b1)
# Sphere
s1 = Sphere([0, 0, 0], 5)
print(s1)
# Cylinder
plane = Plane([0, 0, 0], [0, 0, 1])
circle = Circle(plane, 5)
c1 = Cylinder(circle, height=4)
print(c1)
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 sphere_generator():
sphere = Sphere((0.35, 0, 0), 0.15)
for x in range(5):
for z in range(7):
center = sphere.point + Vector(x, 0, z) * 0.05
yield Sphere(center, sphere.radius)
# ============================================================================== # Geometry # ============================================================================== R = 1.4 P = Point(0, 0, 0) X = Vector(1, 0, 0) Y = Vector(0, 1, 0) Z = Vector(0, 0, 1) YZ = Plane(P, X) ZX = Plane(P, Y) XY = Plane(P, Z) box = Box.from_width_height_depth(2 * R, 2 * R, 2 * R) sphere = Sphere(P, 1.25 * R) cylx = Cylinder((YZ, 0.7 * R), 3 * R) cyly = Cylinder((ZX, 0.7 * R), 3 * R) cylz = Cylinder((XY, 0.7 * R), 3 * R) # ============================================================================== # CSG Model # ============================================================================== model = ShapeModel(name="booleans") model.mesh_lmin = 0.2 model.mesh_lmax = 0.2 model.boolean_difference(
from compas.geometry import centroid_points from compas.datastructures import Mesh from compas.datastructures import mesh_quads_to_triangles from compas.datastructures import mesh_subdivide_quad from compas_viewers.multimeshviewer import MultiMeshViewer from compas_viewers.multimeshviewer.model import MeshObject import compas_libigl as igl origin = Point(0.0, 0.0, 0.0) cube = Box.from_width_height_depth(1.0, 1.0, 1.0) sphere = Sphere(origin, 0.95 * sqrt(0.5 ** 2 + 0.5 ** 2)) xcyl = Cylinder(Circle(Plane(origin, Vector(1.0, 0.0, 0.0)), 0.35), 2.0) ycyl = Cylinder(Circle(Plane(origin, Vector(0.0, 1.0, 0.0)), 0.35), 2.0) zcyl = Cylinder(Circle(Plane(origin, Vector(0.0, 0.0, 1.0)), 0.35), 2.0) a = Mesh.from_vertices_and_faces(cube.vertices, cube.faces) a = mesh_subdivide_quad(a, k=3) b = Mesh.from_vertices_and_faces(* sphere.to_vertices_and_faces(u=30, v=30)) c = Mesh.from_vertices_and_faces(* xcyl.to_vertices_and_faces(u=30)) d = Mesh.from_vertices_and_faces(* ycyl.to_vertices_and_faces(u=30)) e = Mesh.from_vertices_and_faces(* zcyl.to_vertices_and_faces(u=30)) mesh_quads_to_triangles(a) mesh_quads_to_triangles(b) mesh_quads_to_triangles(c)
mesh = Mesh.from_off(igl.get('tubemesh.off'))
mesh.quads_to_triangles()
z = mesh.vertices_attribute('z')
zmin = min(z)
T = Translation.from_vector([0, 0, -zmin])
mesh.transform(T)
# ==============================================================================
# Rays
# ==============================================================================
base = Point(-7, 0, 0)
sphere = Sphere(base, 1.0)
theta = np.linspace(0, np.pi, 20, endpoint=False)
phi = np.linspace(0, 2 * np.pi, 20, endpoint=False)
theta, phi = np.meshgrid(theta, phi)
theta = theta.ravel()
phi = phi.ravel()
r = 1.0
x = r * np.sin(theta) * np.cos(phi) + base.x
y = r * np.sin(theta) * np.sin(phi) + base.y
z = r * np.cos(theta)
xyz = np.vstack((x, y, z)).T
mask = xyz[:, 2] > 0
hemi = xyz[mask]
distances = ([o.get_distance_numpy(x, y, z) for o in self.objs])
return np.maximum.reduce(distances)
# ==============================================================================
# Main
# ==============================================================================
if __name__ == "__main__":
import numpy as np
import matplotlib.pyplot as plt
from compas_vol.primitives import VolSphere, VolBox
from compas.geometry import Box, Frame, Point, Sphere
s = Sphere(Point(5, 6, 0), 9)
b = Box(Frame.worldXY(), 20, 15, 10)
vs = VolSphere(s)
vb = VolBox(b, 2.5)
u = Intersection(vs, vb)
x, y, z = np.ogrid[-15:15:60j, -15:15:60j, -15:15:60j]
d = u.get_distance_numpy(x, y, z)
plt.imshow(
d[:, :, 25].T, cmap='RdBu'
) # transpose because numpy indexing is 1)row 2) column instead of x y
plt.show()
for y in range(-15, 15):
s = ''
for x in range(-30, 30):
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)]
from compas_cgal.booleans import boolean_difference
from compas_cgal.booleans import boolean_intersection
from compas_cgal.meshing import remesh
# ==============================================================================
# Make a box and a sphere
# ==============================================================================
box = Box.from_width_height_depth(2, 2, 2)
box = Mesh.from_shape(box)
box.quads_to_triangles()
A = box.to_vertices_and_faces()
sphere = Sphere(Point(1, 1, 1), 1)
sphere = Mesh.from_shape(sphere, u=30, v=30)
sphere.quads_to_triangles()
B = sphere.to_vertices_and_faces()
bbox = bounding_box(box.vertices_attributes('xyz') + sphere.vertices_attributes('xyz'))
bbox = Box.from_bounding_box(bbox)
width = bbox.xsize
# ==============================================================================
# Remesh the sphere
# ==============================================================================
B = remesh(B, 0.3, 10)
def sphere():
sphere = Sphere([0, 0, 0], 1.0)
mesh = Mesh.from_shape(sphere, u=16, v=16)
return mesh
return da + self.f * db
def get_distance_numpy(self, x, y, z):
"""
vectorized distance function
"""
da = self.a.get_distance_numpy(x, y, z)
db = self.b.get_distance_numpy(x, y, z)
return da + self.f * db
if __name__ == "__main__":
from compas_vol.primitives import VolSphere, VolBox
from compas.geometry import Box, Frame, Point, Sphere
s = Sphere(Point(2, 3, 0), 7)
b = Box(Frame.worldXY(), 20, 15, 10)
vs = VolSphere(s)
vb = VolBox(b, 2.5)
f = Overlay(vs, vb, 0.2)
print(f)
for y in range(-15, 15):
s = ''
for x in range(-30, 30):
d = f.get_distance((x * 0.5, y, 0))
if d < 0:
s += 'x'
else:
s += '.'
print(s)
from compas.geometry import Box
from compas.geometry import Circle
from compas.geometry import Cylinder
from compas.geometry import Frame
from compas.geometry import Plane
from compas.geometry import Sphere
# Box
b1 = Box(Frame.worldXY(), 10, 1, 4) # xsize, ysize, zsize
b2 = Box.from_width_height_depth(10, 4,
1) # width=xsize, height=zsize, depth=ysize
assert str(b1) == str(b2)
print(b1)
# Sphere
s1 = Sphere([10, 0, 0], 4)
print(s1)
# Cylinder
plane = Plane([20, 0, 0], [0, 0, 1])
circle = Circle(plane, 5)
c1 = Cylinder(circle, height=4)
print(c1)
# Draw!
artist = BoxArtist(b1, layer='shapes')
artist.draw()
artist = SphereArtist(s1, layer='shapes')
artist.draw()
