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 set_base(self, base_frame):
# move to UR !!!! ???
self.base_frame = base_frame
# transformation matrix from world coordinate system to robot coordinate system
self.transformation_WCS_RCS = Transformation.from_frame_to_frame(
Frame.worldXY(), self.base_frame)
# transformation matrix from robot coordinate system to world coordinate system
self.transformation_RCS_WCS = Transformation.from_frame_to_frame(
self.base_frame, Frame.worldXY())
def test_move_to_frame():
inst = rrc.MoveToFrame(Frame.worldXY(), 100, rrc.Zone.FINE,
rrc.Motion.JOINT)
assert inst.float_values == [
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0, 0, 100, -1
]
assert inst.string_values == ['FrameJ']
inst = rrc.MoveToFrame(Frame.worldXY(), 100, rrc.Zone.FINE,
rrc.Motion.LINEAR)
assert inst.string_values == ['FrameL']
def three_pts_localization(
rcs_coords, wcs_coords): # type: (List[Point], List[Point]) -> Frame
"""Get the robot base frame in WCS using three points method.
Parameters
----------
rcs_coords
List of the RCS coordinates used for measurements.
wcs_coords
List of the WCS coordinates used for measurements.
Returns
-------
The base frame of the robot in WCS.
"""
recorded_frame_rcs = _coerce_frame(rcs_coords)
recorded_frame_wcs = _coerce_frame(wcs_coords)
T = Transformation.from_frame_to_frame(recorded_frame_rcs,
recorded_frame_wcs)
wcs_robot_base = Frame.worldXY()
wcs_robot_base.transform(T)
return wcs_robot_base
def forward_kinematics(self, joint_state, link_name=None):
"""Calculate the robot's forward kinematic.
Parameters
----------
:class:`compas.robots.Configuration` or joint_state : dict
A dictionary with the joint names as keys and values in radians and
meters (depending on the joint type).
link_name : str, optional
The name of the link we want to calculate the forward kinematics for.
Defaults to the end-effector link name.
Returns
-------
:class:`Frame`
The (ee) link's frame in the world coordinate system.
Examples
--------
>>> config = robot.random_configuration()
>>> frame_WCF = robot.forward_kinematics(config)
"""
if link_name is None:
ee_link = self.get_end_effector_link()
else:
ee_link = self.get_link_by_name(link_name)
joint = ee_link.parent_joint
if joint:
transformations = self.compute_transformations(joint_state)
return joint.current_origin.transformed(
transformations[joint.name])
else:
return Frame.worldXY() # if we ask forward from base link
def __init__(self, radiusA=3, radiusB=4, k=0.1, frame=Frame.worldXY()):
self.ra = radiusA
self.rb = radiusB
self.k = k
self.frame = frame
self.matrix = matrix_from_frame(self.frame)
self.inversedmatrix = matrix_inverse(self.matrix)
def local_to_world_coordinates(frame, xyz):
"""Convert local coordinates to global coordinates.
Parameters
----------
frame : [point, vector, vector] | :class:`compas.geometry.Frame`
The local coordinate system.
xyz : array-like[[float, float, float] | :class:`compas.geometry.Point`]
The global coordinates of the points to convert.
Returns
-------
list[[float, float, float]]
The coordinates of the given points in the local coordinate system.
Examples
--------
>>> from compas.geometry import Point, Frame
>>> f = Frame([0, 1, 0], [3, 4, 1], [1, 5, 9])
>>> xyz = [Point(3.726, 4.088, 1.550)]
>>> Point(*local_to_world_coordinates(f, xyz)[0])
Point(2.000, 3.000, 5.000)
"""
from compas.geometry import Frame # noqa: F811
T = matrix_from_change_of_basis(frame, Frame.worldXY())
return transform_points(xyz, T)
def world_to_local_coords(frame, xyz):
"""Convert global coordinates to local coordinates.
Parameters
----------
frame : :class:`Frame` or [point, xaxis, yaxis]
The local coordinate system.
xyz : array-like
The global coordinates of the points to convert.
Returns
-------
list of list of float
The coordinates of the given points in the local coordinate system.
Examples
--------
>>> import numpy as np
>>> f = Frame([0, 1, 0], [3, 4, 1], [1, 5, 9])
>>> xyz = [Point(2, 3, 5)]
>>> Point(*world_to_local_coords(f, xyz)[0])
Point(3.726, 4.088, 1.550)
"""
from compas.geometry import Frame # noqa: F811
T = matrix_change_basis(Frame.worldXY(), frame)
return transform_points(xyz, T)
def __init__(self, radiusX=3, radiusY=2, radiusZ=1, frame=Frame.worldXY()):
self.radiusX = radiusX
self.radiusY = radiusY
self.radiusZ = radiusZ
self.frame = frame
transform = matrix_from_frame(self.frame)
self.inversetransform = matrix_inverse(transform)
def __init__(self, obj, frame=Frame.worldXY(), angle=0.0):
# needs a distance object to act on
# ev. a plane? origin and normal
# to rotate query point around
self.obj = obj
self.frame = frame
self.angle = angle
def from_t0cf_to_tcf(self, frames_t0cf):
"""Converts frames at the robot's flange (tool0 frame) to frames at the robot's tool tip (tcf frame).
Parameters
----------
frames_t0cf : list[:class:`compas.geometry.Frame`]
Frames (in WCF) at the robot's flange (tool0).
Returns
-------
list[:class:`compas.geometry.Frame`]
Frames (in WCF) at the robot's tool tip (tcf).
Examples
--------
>>> import compas
>>> from compas.datastructures import Mesh
>>> from compas.geometry import Frame
>>> mesh = Mesh.from_stl(compas.get('cone.stl'))
>>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1])
>>> tool = ToolModel(mesh, frame)
>>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))]
>>> tool.from_t0cf_to_tcf(frames_t0cf)
[Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))]
"""
Te = Transformation.from_frame_to_frame(Frame.worldXY(), self.frame)
return [
Frame.from_transformation(Transformation.from_frame(f) * Te)
for f in frames_t0cf
]
def __init__(self, tcp_frame):
self.model = []
self.tool0_frame = Frame.worldXY()
self.tcp_frame = tcp_frame
self.transformation_tool0_tcp = Transformation.from_frame_to_frame(self.tcp_frame, self.tool0_frame)
self.transformation_tcp_tool0 = Transformation.from_frame_to_frame(self.tool0_frame, self.tcp_frame)
def from_ellipse(cls, ellipse):
"""Construct a NURBS curve from an ellipse.
This construction method is similar to the method ``CreateFromEllipse`` of the Rhino API for NURBS curves [1]_.
References
----------
.. [1] https://developer.rhino3d.com/api/RhinoCommon/html/Overload_Rhino_Geometry_NurbsCurve_CreateFromEllipse.htm
"""
frame = Frame.from_plane(ellipse.plane)
frame = Frame.worldXY()
w = 0.5 * sqrt(2)
dx = frame.xaxis * ellipse.major
dy = frame.yaxis * ellipse.minor
points = [
frame.point - dy, frame.point - dy - dx, frame.point - dx,
frame.point + dy - dx, frame.point + dy, frame.point + dy + dx,
frame.point + dx, frame.point - dy + dx, frame.point - dy
]
knots = [0, 1 / 4, 1 / 2, 3 / 4, 1]
mults = [3, 2, 2, 2, 3]
weights = [1, w, 1, w, 1, w, 1, w, 1]
return cls.from_parameters(points=points,
weights=weights,
knots=knots,
multiplicities=mults,
degree=2)
def from_ellipse(cls, ellipse):
"""Construct a NURBS curve from an ellipse.
Parameters
----------
ellipse : :class:`~compas.geometry.Ellipse`
The ellipse geometry.
Returns
-------
:class:`OCCNurbsCurve`
"""
frame = Frame.from_plane(ellipse.plane)
frame = Frame.worldXY()
w = 0.5 * sqrt(2)
dx = frame.xaxis * ellipse.major
dy = frame.yaxis * ellipse.minor
points = [
frame.point - dy, frame.point - dy - dx, frame.point - dx,
frame.point + dy - dx, frame.point + dy, frame.point + dy + dx,
frame.point + dx, frame.point - dy + dx, frame.point - dy
]
knots = [0, 1 / 4, 1 / 2, 3 / 4, 1]
mults = [3, 2, 2, 2, 3]
weights = [1, w, 1, w, 1, w, 1, w, 1]
return cls.from_parameters(points=points,
weights=weights,
knots=knots,
multiplicities=mults,
degree=2)
def attach_tool_model(self, tool_model):
"""Attach a tool to the robot artist for visualization.
Parameters
----------
tool_model : :class:`compas.robots.ToolModel`
The tool that should be attached to the robot's flange.
"""
self.attached_tool_model = tool_model
self.create(tool_model.root, 'attached_tool')
if not tool_model.link_name:
link = self.model.get_end_effector_link()
tool_model.link_name = link.name
else:
link = self.model.get_link_by_name(tool_model.link_name)
ee_frame = link.parent_joint.origin.copy()
initial_transformation = Transformation.from_frame_to_frame(Frame.worldXY(), ee_frame)
sample_geometry = link.collision[0] if link.collision else link.visual[0] if link.visual else None
if hasattr(sample_geometry, 'current_transformation'):
relative_transformation = sample_geometry.current_transformation
else:
relative_transformation = Transformation()
transformation = relative_transformation.concatenated(initial_transformation)
self.update_tool(transformation=transformation)
tool_model.parent_joint_name = link.parent_joint.name
def update(self, joint_state, visual=True, collision=True):
"""Triggers the update of the robot geometry.
Parameters
----------
joint_state : :class:`compas.robots.Configuration` | dict[str, float]
A dictionary with joint names as keys and joint positions as values.
visual : bool, optional
If True, the visual geometry will also be updated.
collision : bool, optional
If True, the collision geometry will also be updated.
Returns
-------
None
"""
_ = self._update(self.model, joint_state, visual, collision)
if self.attached_tool_model:
frame = self.model.forward_kinematics(
joint_state, link_name=self.attached_tool_model.link_name)
self.update_tool(visual=visual,
collision=collision,
transformation=Transformation.from_frame_to_frame(
Frame.worldXY(), frame))
def forward_kinematics(self, joint_state, link_name=None):
"""Calculate the robot's forward kinematic.
Parameters
----------
joint_state : dict
A dictionary with the joint names as keys and values in radians and
meters (depending on the joint type).
link_name : str, optional
The name of the link we want to calculate the forward kinematics for.
Defaults to the end-effector link name.
Returns
-------
:class:`Frame`
The (ee) link's frame in the world coordinate system.
Examples
--------
>>> names = robot.get_configurable_joint_names()
>>> values = [-2.238, -1.153, -2.174, 0.185, 0.667, 0.000]
>>> joint_state = dict(zip(names, values))
>>> frame_WCF = robot.forward_kinematics(joint_state)
"""
if link_name is None:
ee_link = self.get_end_effector_link()
else:
ee_link = self.get_link_by_name(link_name)
joint = ee_link.parent_joint
if joint:
transformations = self.compute_transformations(joint_state)
return joint.origin.transformed(transformations[joint.name])
else:
return Frame.worldXY() # if we ask forward from base link
def local_to_world_coordinates(frame, xyz):
"""Convert local coordinates to global coordinates.
Parameters
----------
frame : :class:`Frame` or [point, xaxis, yaxis]
The local coordinate system.
xyz : list of `Points` or list of list of float
The global coordinates of the points to convert.
Returns
-------
list of list of float
The coordinates of the given points in the local coordinate system.
Examples
--------
>>> import numpy as np
>>> f = Frame([0, 1, 0], [3, 4, 1], [1, 5, 9])
>>> xyz = [Point(3.726, 4.088, 1.550)]
>>> Point(*local_to_world_coordinates(f, xyz)[0])
Point(2.000, 3.000, 5.000)
"""
from compas.geometry import Frame # noqa: F811
T = matrix_from_change_of_basis(frame, Frame.worldXY())
return transform_points(xyz, T)
def __init__(self, radius, type=0, frame=None):
self.radius = radius
self.type = type
self.frame = frame or Frame.worldXY()
self.inversetransform = matrix_inverse(matrix_from_frame(self.frame))
self.sqrt3 = sqrt(3)
self.tan30 = tan(pi/6)
def test_move_to_robtarget():
inst = rrc.MoveToRobtarget(Frame.worldXY(), [50, 20], 100, rrc.Zone.FINE,
rrc.Motion.JOINT)
assert inst.float_values == [
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 50, 20, 0, 0, 0, 0, 100, -1
]
assert inst.string_values == ['J']
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_posearray():
from compas.geometry import Frame
p = [Pose.from_frame(f) for f in [Frame.worldXY()]]
m = PoseArray(header=Header(), poses=p)
assert (
repr(m) ==
"PoseArray(header=Header(seq=0, stamp=Time(secs=0, nsecs=0), frame_id='/world'), poses=[Pose(position=Point(x=0.0, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))])"
) # noqa E501
def test_pickling():
f1 = Frame.worldXY()
s = pickle.dumps(f1, protocol=pickle.HIGHEST_PROTOCOL)
f2 = pickle.loads(s)
assert all(a == b for a, b in zip(f1.point, f2.point))
assert all(a == b for a, b in zip(f1.xaxis, f2.xaxis))
assert all(a == b for a, b in zip(f1.yaxis, f2.yaxis))
assert all(a == b for a, b in zip(f1.zaxis, f2.zaxis))
def RunScript(self, robot, group, configuration, attached_collision_meshes,
show_visual, show_collision, show_frames, show_base_frame,
show_end_effector_frame, show_acm):
visual = None
collision = None
attached_meshes = None
frames = None
base_frame = None
ee_frame = None
if robot:
show_visual = True if show_visual is None else show_visual
configuration = configuration or robot.zero_configuration()
robot.update(configuration,
visual=show_visual,
collision=show_collision)
compas_frames = robot.transformed_frames(configuration, group)
if show_visual:
visual = robot.artist.draw_visual()
if show_collision:
collision = robot.artist.draw_collision()
if show_base_frame:
base_compas_frame = compas_frames[0]
artist = FrameArtist(base_compas_frame)
base_frame = artist.draw()
if show_end_effector_frame:
ee_compas_frame = robot.forward_kinematics(
configuration, group, options=dict(solver='model'))
artist = FrameArtist(ee_compas_frame)
ee_frame = artist.draw()
if show_frames:
frames = []
for compas_frame in compas_frames[1:]:
artist = FrameArtist(compas_frame)
frame = artist.draw()
frames.append(frame)
if show_acm:
attached_meshes = []
for acm in attached_collision_meshes:
frame = robot.forward_kinematics(
configuration,
options=dict(solver='model', link_name=acm.link_name))
T = Transformation.from_frame_to_frame(
Frame.worldXY(), frame)
mesh = acm.collision_mesh.mesh.transformed(T)
attached_meshes.append(MeshArtist(mesh).draw())
return (visual, collision, attached_meshes, frames, base_frame,
ee_frame)
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 get_base_frame(self, group=None):
# TODO: check this
link = self.get_base_link(group)
if link.parent_joint:
base_frame = link.parent_joint.init_origin.copy()
else:
base_frame = Frame.worldXY()
if not self.artist:
base_frame.point *= self._scale_factor
return base_frame
def to_global_geometry(self, world_frame=Frame.worldXY()):
geometry = self.to_local_geometry_xy()
transformed_geometry = []
T = Transformation.from_frame_to_frame(self.frame, world_frame)
for part in geometry:
transformed_part = []
for point in part:
p_point = Point(point['x'], point['y'], point['z'])
transformed_part.append(p_point.transformed(T))
transformed_geometry.append(transformed_part)
return transformed_geometry
def _get_visual_args(self, geometry_args, frame=Frame.worldXY(), color=const.RED, specular=None):
point, quaternion = pose_from_frame(frame)
visual_args = {
'rgbaColor': color,
'visualFramePosition': point,
'visualFrameOrientation': quaternion,
'physicsClientId': self.client_id,
}
visual_args.update(geometry_args)
if specular is not None:
visual_args['specularColor'] = specular
return visual_args
def offset_polygon_xy(points, dist, planarize=False):
if len(points) < 3:
return None
frame = Frame.from_plane(
Plane.from_three_points(points[0], points[1], points[2]))
xform = Transformation.from_frame_to_frame(frame, Frame.worldXY())
if planarize:
points = [point_on_plane(point, frame) for point in points]
points = transform_points(points, xform)
points = offset_polygon(points, dist)
points = transform_points(points, xform.inverse())
return points
def __init__(self, ltype=0, unitcell=1.0, thickness=0.1, polarnumber=6, frame=Frame.worldXY()):
self.pointlist = self.create_points()
self.ltypes = self.create_types()
self._ltype = None
self.ltype = ltype
self.unitcell = unitcell
self.thickness = thickness
self.polarnumber = polarnumber
self._frame = None
self.frame = frame
transform = matrix_from_frame(self.frame)
self.inversetransform = matrix_inverse(transform)
