def ensure_frame(frame_like): # type: (Any) -> cg.Frame
"""Convert geometry objects to :class:`compas.geometry.Frame`.
Parameters
----------
frame_like
Frame like object, currently :class:`compas.geometry.Frame`,
:class:`compas.geometry.Plane`, :class:`compas.geometry.Point`,
:class:`Rhino.Geometry.Plane`, or :class:`Rhino.Geometry.Point3d`.
Returns
-------
:class:`compas.geometry.Frame`
Notes
-----
If a point is given the point is used as the frames origin and the X and
Y will be the X and Y unit vectors.
:class:`compas.geometry.Plane` is defined only by origin and normal, the
X and Y axis will be chosen arbitrarely,
see :meth:`compas.geometry.Frame.from_plane`.
"""
if isinstance(frame_like, cg.Frame):
return frame_like
if isinstance(frame_like, cg.Plane):
return cg.Frame.from_plane(frame_like)
if isinstance(frame_like, cg.Point):
return cg.Frame(frame_like, [1, 0, 0], [0, 1, 0])
try: # try to compare to Rhino objects
import Rhino.Geometry as rg
from rapid_clay_formations_fab.rhino import rgplane_to_cgframe
from rapid_clay_formations_fab.rhino import rgpoint_to_cgpoint
if isinstance(frame_like, rg.Plane):
return rgplane_to_cgframe(frame_like)
if isinstance(frame_like, rg.Point3d):
pt = rgpoint_to_cgpoint(frame_like)
return cg.Frame(pt, [1, 0, 0], [0, 1, 0])
except ImportError:
pass
raise TypeError(
"Can't convert {} to compas.geometry.Frame".format(type(frame_like))
)
def _test_functions():
# test cgpoint_to_rgpoint
point = cgpoint_to_rgpoint(cg.Point(1, 2, 3))
if point.Z != 3.0:
raise Exception("cgpoint_to_rgpoint failed.")
# test cgvector_to_rgvector
vector = cgvector_to_rgvector(cg.Vector(5, 1, 9))
if not vector.Unitize():
raise Exception("cgvector_to_rgvector failed")
# test cgline_to_rgline
line = cgline_to_rgline(cg.Line([1, 2, 3], [3, 2, 1]))
if line.Direction != rg.Vector3d(2, 0, -2):
raise Exception("cgline_to_rgline failed")
# test frame_to_plane
plane = cgframe_to_rgplane(cg.Frame([1, 3, -1], [1, 1, 2], [0, 1, 1]))
if not isinstance(plane.Normal, rg.Vector3d):
raise Exception("cgframe_to_rgplane failed")
# matrix_to_rgtransform
R = cg.Rotation.from_basis_vectors([1, 2, 0], [2, 1, 3])
if not isinstance(matrix_to_rgtransform(R), rg.Transform):
raise Exception("matrix_to_rgtransform failed")
def rgplane_to_cgframe(
plane): # type: (Rhino.Geometry.Plane) -> compas.geometry.Frame
"""Convert :class:`Rhino.Geometry.Plane` to :class:`compas.geometry.Frame`.
Parameters
----------
plane : :class:`Rhino.Geometry.Plane`
Plane object to convert
Returns
-------
:class:`compas.geometry.Frame`
Resulting frame object
"""
pt = rgpoint_to_cgpoint(plane.Origin)
xaxis = rgvector_to_cgvector(plane.XAxis)
yaxis = rgvector_to_cgvector(plane.YAxis)
return cg.Frame(pt, xaxis, yaxis)
def three_pts_localization(rcs_coords, wcs_coords):
"""Get the robot base frame in WCS using three points method.
Parameters
----------
rcs_coords : :obj:`list` of :class:`compas.geometry.Point`
List of the RCS coordinates used for measurements.
wcs_coords : :obj:`list` of :class:`compas.geometry.Point`
List of the WCS coordinates used for measurements.
Returns
-------
:class:`compas.geometry.Frame`
The base frame of the robot in WCS.
"""
# Calculate the directions of the X, Y and X axis of the robot in WCS
rcs_o, _, _ = rcs_coords
wcs_o, wcs_x, wcs_y = wcs_coords
wcs_robot_x_dir = wcs_x - wcs_o
wcs_robot_y_dir = wcs_y - wcs_o
wcs_robot_z_dir = wcs_robot_x_dir.cross(wcs_robot_y_dir)
wcs_robot_x_dir.unitize()
wcs_robot_y_dir.unitize()
wcs_robot_z_dir.unitize()
x_dist = wcs_robot_x_dir * rcs_o.x
y_dist = wcs_robot_y_dir * rcs_o.y
z_dist = wcs_robot_z_dir * rcs_o.z
wcs_robot_base_origin = wcs_o - x_dist - y_dist - z_dist
wcs_robot_base = cg.Frame(wcs_robot_base_origin, wcs_robot_x_dir,
wcs_robot_y_dir)
return wcs_robot_base
:class:`Rhino.Geometry.Transform`
"""
rgM = rg.Transform()
for i, row in enumerate(M):
for j, val in enumerate(row):
rgM[i, j] = val
return rgM
if __name__ == "__main__":
# test cgpoint_to_rgpoint
point = cgpoint_to_rgpoint(cg.Point(1, 2, 3))
assert point.Z == 3.0
# test cgvector_to_rgvector
vector = cgvector_to_rgvector(cg.Vector(5, 1, 9))
assert vector.Unitize()
# test cgline_to_rgline
line = cgline_to_rgline(cg.Line([1, 2, 3], [3, 2, 1]))
assert line.Direction == rg.Vector3d(2, 0, -2)
# test frame_to_plane
plane = cgframe_to_rgplane(cg.Frame([1, 3, -1], [1, 1, 2], [0, 1, 1]))
assert isinstance(plane.Normal, rg.Vector3d)
# matrix_to_rgtransform
R = cg.Rotation.from_basis_vectors([1, 2, 0], [2, 1, 3])
assert isinstance(matrix_to_rgtransform(R), rg.Transform)
