def move_l_blend(plane_to, accel, vel, blend_radius=0): """ Function that returns UR script for linear movement in tool-space. Args: plane_to: Rhino.Geometry Plane. A target plane for calculating pose (in UR base coordinate system) accel: tool accel in m/s^2 vel: tool speed in m/s Returns: script: UR script """ # Check acceleration and velocity are non-negative and below a set limit accel = MAX_ACCEL if (abs(accel) > MAX_ACCEL) else abs(accel) vel = MAX_VELOCITY if (abs(vel) > MAX_VELOCITY) else abs(vel) # Check blend radius is positive blend_radius = max(0, blend_radius) _matrix = rg.Transform.PlaneToPlane(rg.Plane.WorldXY, plane_to) _axis_angle = utils.matrix_to_axis_angle(_matrix) # Create pose data _pose = [ plane_to.OriginX / 1000, plane_to.OriginY / 1000, plane_to.OriginZ / 1000, _axis_angle[0], _axis_angle[1], _axis_angle[2] ] _pose_fmt = "p[" + ("%.4f," * 6)[:-1] + "]" _pose_fmt = _pose_fmt % tuple(_pose) # Format UR script script = "movel(%s, a = %.2f, v = %.2f, r = %.4f)\n" % (_pose_fmt, accel, vel, blend_radius) return script
def set_tcp_by_plane(x_offset, y_offset, z_offset, ref_plane=rg.Plane.WorldXY): """ TODO: Need to test if this gives the correct result Function that returns UR script for setting tool center point Args: x_offset: float. tooltip offset in mm y_offset: float. tooltip offset in mm z_offset: float. tooltip offset in mm ref_plane: Plane that defines orientation of the tip. If none specified, world XY plane used as default. (in UR base coordinate system) Returns: script: UR script """ if (ref_plane != rg.Plane.WorldXY): _matrix = rg.Transform.PlaneToPlane(rg.Plane.WorldXY, ref_plane) _axis_angle = utils.matrix_to_axis_angle(_matrix) else: _axis_angle = rg.Vector3d(0, 0, 0) # Create pose data _pose = [ x_offset / 1000, y_offset / 1000, z_offset / 1000, _axis_angle[0], _axis_angle[1], _axis_angle[2] ] _pose_fmt = "p[" + ("%.4f," * 6)[:-1] + "]" _pose_fmt = _pose_fmt % tuple(_pose) # Format UR script script = "set_tcp(%s)\n" % (_pose_fmt) return script
def set_tcp_by_angles(x_offset, y_offset, z_offset, x_rotate, y_rotate, z_rotate): """ TODO(Jason): Need to test this Function that returns UR script for setting tool center point Args: x_offset: float. tooltip offset in mm y_offset: float. tooltip offset in mm z_offset: float. tooltip offset in mm x_rotation: float. rotation around world x axis in radians y_rotation: float. rotation around world y axis in radians z_rotation: float. rotation around world z axis in radians Returns: script: UR script """ #Create rotation matrix _rX = rg.Transform.Rotation(x_rotate, rg.Vector3d(1,0,0), rg.Point3d(0,0,0)) _rY = rg.Transform.Rotation(y_rotate, rg.Vector3d(0,1,0), rg.Point3d(0,0,0)) _rZ = rg.Transform.Rotation(z_rotate, rg.Vector3d(0,0,1), rg.Point3d(0,0,0)) _r = _rX * _rY * _rZ _axis_angle= utils.matrix_to_axis_angle(_r) # Create pose data _pose = [x_offset/1000, y_offset/1000, z_offset/1000,_axis_angle[0], _axis_angle[1], _axis_angle[2]] _pose_fmt = "p[" + ("%.4f,"*6)[:-1]+"]" _pose_fmt = _pose_fmt%tuple(_pose) # Format UR script script = "set_tcp(%s)\n"%(_pose_fmt) return script