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)