diff --git a/examples/grid2grid/README.md b/examples/grid2grid/README.md new file mode 100644 index 0000000..e2daf92 --- /dev/null +++ b/examples/grid2grid/README.md @@ -0,0 +1,117 @@ +# Grid-to-grid pick and place application +This example shows how to set-up a grid to grid tool-path for pick and place use-cases. +For any questions or comments, +please email Dr. Andrea Antonello at __[andrea@automata.tech]()__. + + +## Requirements + +This example assumes you have two planar grids. The number of columns, rows, their respective pitch, +the relative orientation, the objects' pickup angles and the grid's _z axis_ elevation are fully customizable. + +In this version, we assumes that the grid planes are parallel to Eva's _xy_ plane (that is, they are in an horizontal +absolute plane). The rows and the columns, in addition, have to be perpendicular to each other. Finally, the number of +points of the first grid (```number of columns``` * ```number of rows```) needs to be equal or smaller than the points +of the second grid; if not, the first grid can not be correctly completed. + + +## Supported Version + +Requirements to run this script as is: + +- Eva software version: 3.x.x +- Eva Python SDK: 2.x.x + +## Project description + +__[main.py](main.py)__ + +This file is the main script and contains the logic behind the creation of the two grids. +This file also creates the complete grid-to-grid tool-path, +which can be run a fixed number of times or looped indefinitely. + +You can use this file to modify the tool-path settings (speed, additional waypoints, IOs, etc). +In particular, this file **must** be modified if additional operations have to be performed between +the pickup and the drop-off points (i.e. inspection, measuring, filling, etc.). + +__[evaUtilities.py](evaUtilities.py)__ + +This file contains the auxiliary functions needed by the main __[main.py](main.py)__ script +(frame of reference handling, angle and quaternion conversion, forward and inverse kinematics, plotting, grid points creation). +You **should not** change this file. + + +__[config/config_manager.py](config/config_manager.py)__ + +This file loads the YAML parameter file. You **should not** change this file. + +__[config/use_case_config.yaml](config/use_case_config.yaml)__ + +This YAML file contains the parameters needed for the simulation, which are initialized to zero by default. + +In detailed, you will have to input: +- EVA setup (host, token, home position) +- grids characteristic (explained in detail in the next section) + +**NOTE**: without changing these parameters, EVA will automatically set its home position +in the upright configuration, ```joints_home = [0, 0, 0, 0, 0, 0].``` + + +## Parameters description + +The use-case is composed by 2 grids. The first one is referred in the YAML file as 'A', the second one as 'B'. +The code solves the inverse kinematics of all the points of the grids, including the hover approach, which are +positioned on top of the grid points, with a selectable offset on the _z axis_. +This examples assumes you have familiarity with reference frames. + +Eva's base reference frame is depicted in _Fig. 1_: all the definitions for the grids +will be referred to Eva's base frame. +![Fig. 1 - Automata Eva's base reference frame](readme_images/eva_frame.png) + +In order to illustrate how the grids are defined, we will use the nomenclature of the YAML file +dictionary (__[config/use_case_config.yaml](config/use_case_config.yaml)__) and the help of some illustrations. +The YAML contains two entries for each tag, one for ```grid A```, and one for ```grid B```. + +In detail, going through the parameters of the **[grids]** tag: + +- **names_verbose** [type: string]: this contains the verbose name of the grids, that can be +chaged at the user's discretion. +- **row** [type: int, >0]: this is the number of rows of the grid +- **col** [type: int, >0]: this is the number of columns of the grid +- **row_pitch** [type: float, >0, unit: mm]: this is the distance (absolute value), measured in millimeters between two adjacent rows of the grid +- **col_pitch** [type: float, >0, unit: mm]: this is the distance (absolute value), measured in millimeters between two adjacent columns of the grid +- **x0** [type: float, unit: mm]: this is the _x-axis_ coordinate of the first corner point of the grid, +measured with respect to the frame depicted in _Fig. 1_, expressed in millimeters. This will be the first object to be picked up. +- **y0** [type: float, unit: mm]: this is the _y-axis_ coordinate of the first corner point of the grid, +measured with respect to the frame depicted in _Fig. 1_, expressed in millimeters. This will be the first object to be picked up. +- **angle** [type: float, unit: deg]: this is the rotation of the grid with respect to the frame depicted in _Fig. 1_, measured in degrees. +_Fig. 2_ depicts the two frames of reference and the definition of this angle. +- **angle_pickup** [type: float, unit: deg]: this is the angle with which the end effector will pick up the object, with respect +to the above defined grid reference frame. By default, this is set to 0. +- **surface** [type: float, unit: mm]: this is the height of the grid surface with respect to Eva's base-plate. +If positive, the grid plane is higher than Eva's mounting surface. If negative, the grid's plane is lower than Eva's mounting surface . +- **object** [type: float, >0, unit: mm]: this is _z-axis_ length of the object, to allow the end-effector to correctly approach the object +- **guess** [type: list, unit: rad]: this contains a 6 elements joints list used to solve the inverse kinematics. +This is critical to the success of the algorithm computations: please back-drive the robot in the ball-park of +the grid centre, and use the values provided by Choreograph as the guess (make sure to appropriately convert them into radians). +This operation **must** be performed for each of the two grids. + +**NOTE**: by default, the inverse kinematics solver for this use-case assumes that the end-effector tool-plate +is parallel to the horizontal _xy_ plane. That is, the orientation quaternion is set to ```q = [0, 0, 1, 0]```. +In the unlikely case that another orientation is desired, the user can modify the IK solver by using the general +built-in solver ```eva.calc_inverse_kinematics()```. +![Fig. 2 - Grid parameters definition](readme_images/grid_def.png) + + +## Visualization tool: +The script contains a graphical simulator to visualise the computed grids. This mode can be turned on and off +by setting the ```show_plot``` variable to ```True``` or ```False```, respectively. + +**NOTE: It is highly recommended to turn this function on when the use-case is in the prototyping phase, + in order to avoid damages due to incorrect user entries.** + + This tool provides both a 2D and a 3D depiction of + the defined grids, and allows zooming in and rotating the 3D views for verification (see _Fig. 3_). The start and the cross at the grid corners + represent the initial and final grid points, respectively; arrows will + indicate the order of object pick-up and drop-off. +![Fig. 3 - Visual 2D/3D tool](readme_images/visual.png) diff --git a/examples/grid2grid/config/config_manager.py b/examples/grid2grid/config/config_manager.py new file mode 100644 index 0000000..657c720 --- /dev/null +++ b/examples/grid2grid/config/config_manager.py @@ -0,0 +1,16 @@ +import os +import yaml +from pathlib import Path + + +def load_use_case_config(): + return load_config("config/use_case_config.yaml") + + +def load_config(config_file): + with open(os.path.join(os.getcwd(), config_file), 'r') as f: + config = yaml.safe_load(f) + + assert config + + return config diff --git a/examples/grid2grid/config/use_case_config.yaml b/examples/grid2grid/config/use_case_config.yaml new file mode 100755 index 0000000..bfbc87c --- /dev/null +++ b/examples/grid2grid/config/use_case_config.yaml @@ -0,0 +1,76 @@ +# SETUP YAML file for Grid-to-grid demo + +EVA: + work_radius: 0.6 # Eva's workspace radius [m] - DO NOT CHANGE THIS PARAMETER + base_plate: 0.132 # Eva's base-plate dimension [m] - DO NOT CHANGE THIS PARAMETER + comm: + host: "0.0.0.0" # USER DEFINED + token: "abcd1234" # USER DEFINED + end_effector: + payload: 0 # payload [kg] - USER DEFINED + length: 80 # length of tool [mm] - USER DEFINED + offset: # ee offset in ee-frame [m] - USER DEFINED + - 0 + - 0 + - 0 + hover_height: 0.1 # elevation of idle z axis wrt to the object [m] - USER DEFINED + home: # joints for home position [rad] - USER DEFINED + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + +grids: + names: # internal names - DO NOT CHANGE + - 'A' + - 'B' + names_verbose: # object names, customizable - USER DEFINED + A: 'Origin' + B: 'Target' + row: # number of grid rows, [#] - USER DEFINED + A: 0 + B: 0 + col: # number of grid columns, [#] - USER DEFINED + A: 0 + B: 0 + row_pitch: # distance between adjacent rows [mm] - USER DEFINED + A: 0 + B: 0 + col_pitch: # distance between adjacent columns [mm] - USER DEFINED + A: 0 + B: 0 + x0: # grid corner, referred to Eva frame's origin - x coordinate [mm] - USER DEFINED + A: 0 + B: 0 + y0: # grid corner, referred to Eva frame's origin - y coordinate [mm] - USER DEFINED + A: 0 + B: 0 + angle: # grid orientation, referred to Eva frame's origin [deg] - USER DEFINED + A: 0 + B: 0 + angle_pickup: # additional pick-up angle, [deg] - USER DEFINED + A: 0 + B: 0 + surface: # surf height of grid, referred to Eva frame's xy plane, [mm] - USER DEFINED + A: 0 + B: 0 + object: # object thickness, referred to Eva frame's z-axis, [mm] - USER DEFINED + A: 0 + B: 0 + guess: + A: # joints guess for origin grid [rad] - USER DEFINED + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 + B: # joints guess for target grid [rad] - USER DEFINED + - 0 + - 0 + - 0 + - 0 + - 0 + - 0 \ No newline at end of file diff --git a/examples/grid2grid/evaUtilities.py b/examples/grid2grid/evaUtilities.py new file mode 100644 index 0000000..5d79c3c --- /dev/null +++ b/examples/grid2grid/evaUtilities.py @@ -0,0 +1,201 @@ +import numpy as np +from mpl_toolkits.mplot3d import proj3d +from matplotlib.patches import Rectangle, Circle, FancyBboxPatch, FancyArrowPatch +import mpl_toolkits.mplot3d.art3d as art3d +import mpl_toolkits.mplot3d as mp3d +import matplotlib.pyplot as plt +import matplotlib.patches as mpatches +from progress.bar import * + + +class Arrow3D(FancyArrowPatch): + def __init__(self, xs, ys, zs, *args, **kwargs): + FancyArrowPatch.__init__(self, (0, 0), (0, 0), *args, **kwargs) + self._verts3d = xs, ys, zs + + def draw(self, renderer): + xs3d, ys3d, zs3d = self._verts3d + xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M) + self.set_positions((xs[0], ys[0]), (xs[1], ys[1])) + FancyArrowPatch.draw(self, renderer) + + +def quaternion_multiply(quaternion1, quaternion0): + w0, x0, y0, z0 = quaternion0 + w1, x1, y1, z1 = quaternion1 + return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, + x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, + -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, + x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64) + + +def solve_ik(eva, guess, theta, xyz_absolute): + with eva.lock(): + # Inputs: Theta [deg], xyz_absolute [m] + pos = [xyz_absolute[0], xyz_absolute[1], xyz_absolute[2]] # [m] + pos_json = {'x': (pos[0]), 'y': (pos[1]), 'z': (pos[2])} + # This provides a rotation of theta [deg] wrt the end effector orientation pointing downwards + orient_rel = [np.cos(np.deg2rad(theta)/2), 0, 0, np.sin(np.deg2rad(theta)/2)] + orient_abs = quaternion_multiply([0, 0, 1, 0], orient_rel) + orient_json = {'w': (orient_abs[0]), 'x': (orient_abs[1]), 'y': (orient_abs[2]), 'z': (orient_abs[3])} + # Compute IK + result_ik = eva.calc_inverse_kinematics(guess, pos_json, orient_json) + success_ik = result_ik['ik']['result'] + joints_ik = result_ik['ik']['joints'] + return success_ik, joints_ik + + +class EvaGrids: + def __init__(self, eva, config, show_plot): + self.config = config + self.eva = eva + self.show_plot = show_plot + self.counter_max = {self.config['grids']['names'][0]: self.config['grids']['row'][self.config['grids']['names'][0]]*self.config['grids']['col'][self.config['grids']['names'][0]]-1, + self.config['grids']['names'][1]: self.config['grids']['row'][self.config['grids']['names'][1]]*self.config['grids']['col'][self.config['grids']['names'][1]]-1} + # Check that slots in second grid are >= than first grid + slots = [abs(self.config['grids']['row'][self.config['grids']['names'][0]]) * abs(self.config['grids']['col'][self.config['grids']['names'][0]]), + abs(self.config['grids']['row'][self.config['grids']['names'][1]]) * abs(self.config['grids']['col'][self.config['grids']['names'][1]])] + if slots[0] > slots[1]: + raise Exception('Drop-off grid is smaller than pick-up grid') + + def _create_grid(self, grid_iter): + """Creates 2D grid from YAML configuration file + grid_x, grid_y: list of lists containing all grid points, in cartesian space. + Units: [m]""" + row = abs(self.config['grids']['row'][grid_iter]) + col = abs(self.config['grids']['col'][grid_iter]) + row_pitch = abs(self.config['grids']['row_pitch'][grid_iter]) + col_pitch = abs(self.config['grids']['col_pitch'][grid_iter]) + x0 = self.config['grids']['x0'][grid_iter] + y0 = self.config['grids']['y0'][grid_iter] + surface = self.config['grids']['surface'][grid_iter] + theta = np.deg2rad(self.config['grids']['angle'][grid_iter]) + grid_x = [[0 for _i in range(col)] for _j in range(row)] + grid_y = [[0 for _i in range(col)] for _j in range(row)] + grid_z = [[0 for _i in range(col)] for _j in range(row)] + grid_points = [] + for i in range(row): + for j in range(col): + grid_x[i][j] = (np.cos(theta+np.pi/2)*col_pitch*j+np.sin(theta+np.pi/2)*row_pitch*i)+x0 + grid_y[i][j] = -(-np.sin(theta+np.pi/2)*col_pitch*j+np.cos(theta+np.pi/2)*row_pitch*i)+y0 + grid_z[i][j] = surface + for i in range(row): + for j in range(col): + grid_points.append([grid_x[i][j], grid_y[i][j], grid_z[i][j]]) + + return grid_x, grid_y, grid_z, grid_points + + def get_grid_points(self, grids): + """Creates 2D grid using the _create_grid() method and extract the grid point + corresponding to the object_name and counter selected. It then solves the IK + for the pickup point [x, y, z] and the hover point [x, y, z + z_hover] and + provides the corresponding joint angles""" + if self.show_plot: + fig = plt.figure(figsize=(10, 5)) + plt.style.use('seaborn-pastel') + ax = [fig.add_subplot(121), fig.add_subplot(122, projection='3d')] + joints = {grids[0]: {'pick': [], 'hover': []}, grids[1]: {'pick': [], 'hover': []}} + + for grid_iter in grids: + obj_h = abs(self.config['grids']['object'][grid_iter]) + ee_h = abs(self.config['EVA']['end_effector']['length']) + hover_h = abs(self.config['EVA']['hover_height']) + guess = self.config['grids']['guess'][grid_iter] + grid_x, grid_y, grid_z, grid_points = self._create_grid(grid_iter) + + with ChargingBar('Computing ' + self.config['grids']['names_verbose'][grid_iter] + ' grid', + max=len(grid_points)) as bar: + for _counter in range(len(grid_points)): + x = grid_points[_counter][0]/1000 # [m] + y = grid_points[_counter][1]/1000 # [m] + z = (grid_points[_counter][2]+ee_h+obj_h)/1000 # [m] + pos_obj = [x, y, z] # [m] + pos_obj_hover = [x, y, z + hover_h] # [m] + extra_angle = self.config['grids']['angle_pickup'][grid_iter] # Additional pickup angle + success_pick, joints_pick = solve_ik(self.eva, guess, extra_angle, pos_obj) + success_hover, joints_hover = solve_ik(self.eva, guess, extra_angle, pos_obj_hover) + if ('success' not in success_pick) or ('success' not in success_hover): + raise Exception('IK error') # Failed IK. Position not reachable + joints[grid_iter]['pick'].append(joints_pick) + joints[grid_iter]['hover'].append(joints_hover) + bar.next() + if self.show_plot: + self.plot_grids(grid_x, grid_y, grid_z, ax, grid_iter) + if self.show_plot: + plt.show() + move_eva = input("Please verify the correctness of grid placement before continuing " + "(to do this, set plot_on_off variable to True and verify grids placement from the plots).\n" + "Proceed (this will move the robot)? yes/no\n") + if 'yes' not in move_eva: + raise Exception('\n\n\nScript aborted by user: \n - set the plot_on_off variable to True\n ' + '- run the script again\n - verify grids placement from the plots ') + return joints + + def plot_grids(self, grid_x, grid_y, grid_z, ax_all, grid_iter): + all_points = [] + ax1 = ax_all[0] + ax2 = ax_all[1] + + # Plot settings + grid_color = {'A': 'blue', 'B': 'red'} + box_side = 600 + arr_length = 300 + work = Circle((0, 0), self.config['EVA']['work_radius'] * 1000, color='k', alpha=0.1) + base = self.config['EVA']['base_plate'] * 1000 / 2 + + # 2D Plot + base_plate_2d = FancyBboxPatch((-base, -base), base * 2, base * 2, edgecolor='black', facecolor='gray', + alpha=0.2) + io_port_2d = Rectangle((-0.08 * 1000, -0.08 * 1000 / 2), 0.015 * 1000, 0.08 * 1000, edgecolor='green', + facecolor='green') + grid_vertices_2d = [(grid_x[0][0], grid_y[0][0]), (grid_x[-1][0], grid_y[-1][0]), + (grid_x[-1][-1], grid_y[-1][-1]), (grid_x[0][-1], grid_y[0][-1])] + grid_footprint_2d = mp3d.art3d.PolyCollection([grid_vertices_2d], color=grid_color[grid_iter], alpha=0.3) + ax1.add_artist(plt.Circle((0, 0), self.config['EVA']['work_radius'] * 1000, facecolor='gray', alpha=0.1)) + ax1.add_patch(base_plate_2d), ax1.add_patch(io_port_2d) + ax1.set_xlim(-box_side, box_side), ax1.set_ylim(-box_side, box_side) + ax1.set_aspect('equal'), ax1.set_xlabel('x [mm]'), ax1.set_ylabel('y [mm]') + ax1.arrow(0, 0, 200, 0, head_width=10, head_length=10, fc='red', ec='red', linewidth=2, alpha=0.6) + ax1.arrow(0, 0, 0, 200, head_width=10, head_length=10, fc='green', ec='green', linewidth=2, alpha=0.6) + ax1.plot(0, 0, 'o', markersize=5, color='blue', alpha=0.6) + ax1.plot(grid_x, grid_y, 'o', color='grey', markersize=2) + ax1.plot(grid_x[0][0], grid_y[0][0], '*', markersize=10, color='green') # First point + ax1.plot(grid_x[-1][-1], grid_y[-1][-1], 'X', markersize=8, color='red') # Last point + ax1.add_collection(grid_footprint_2d) + blue_patch = mpatches.Patch(color='blue', label='Origin grid', alpha=0.4) + red_patch = mpatches.Patch(color='red', label='Target grid', alpha=0.4) + plt.legend(handles=[blue_patch, red_patch]) + + for i in range(self.config['grids']['row'][grid_iter]): + for j in range(self.config['grids']['col'][grid_iter]): + all_points.append([grid_x[i][j], grid_y[i][j]]) + + for i in range(len(all_points) - 1): + ax1.arrow(all_points[i][0], all_points[i][1], (all_points[i + 1][0] - all_points[i][0]), + (all_points[i + 1][1] - all_points[i][1]), head_width=15, head_length=15, + fc='black', ec='black', linewidth=1, alpha=0.5) + + # 3D Plot + base_plate_3d = [(-base, -base, 0), (base, -base, 0), (base, base, 0), (-base, base, 0)] + base_plate_shape_3d = mp3d.art3d.Poly3DCollection([base_plate_3d], color='black', alpha=0.5, linewidth=1) + grid_vertices_3d = [(grid_x[0][0], grid_y[0][0], grid_z[0][0]), (grid_x[-1][0], grid_y[-1][0], grid_z[0][0]), + (grid_x[-1][-1], grid_y[-1][-1], grid_z[0][0]), + (grid_x[0][-1], grid_y[0][-1], grid_z[0][0])] + grid_footprint_3d = mp3d.art3d.Poly3DCollection([grid_vertices_3d], color=grid_color[grid_iter], alpha=0.3) + + a = Arrow3D([0, arr_length], [0, 0], [0, 0], mutation_scale=5, lw=1, arrowstyle="-|>", color="r") + b = Arrow3D([0, 0], [0, arr_length], [0, 0], mutation_scale=5, lw=1, arrowstyle="-|>", color="g") + c = Arrow3D([0, 0], [0, 0], [0, 0.5 * arr_length], mutation_scale=5, lw=1, arrowstyle="-|>", color="b") + + ax2.set_facecolor('white') + ax2.add_collection3d(base_plate_shape_3d) + ax2.add_patch(work) + art3d.pathpatch_2d_to_3d(work, z=0, zdir="z") + ax2.add_artist(a), ax2.add_artist(b), ax2.add_artist(c) + ax2.xaxis.pane.set_edgecolor('black'), ax2.yaxis.pane.set_edgecolor('black'), ax2.zaxis.pane.set_edgecolor( + 'black') + ax2.set_xlabel('x [mm]'), ax2.set_ylabel('y [mm]'), ax2.set_zlabel('z [mm]') + ax2.set_xlim(-box_side, box_side), ax2.set_ylim(-box_side, box_side), ax2.set_zlim(0, 0.5 * box_side) + ax2.add_collection3d(grid_footprint_3d) + ax2.scatter(grid_x, grid_y, grid_z, 'o', s=2, color='black') + ax2.scatter(grid_x[0][0], grid_y[0][0], grid_z[0][0], '*', s=10, color='green') # First point diff --git a/examples/grid2grid/main.py b/examples/grid2grid/main.py new file mode 100644 index 0000000..d2c023d --- /dev/null +++ b/examples/grid2grid/main.py @@ -0,0 +1,71 @@ +from evasdk import Eva +from evaUtilities import EvaGrids +from config.config_manager import load_use_case_config + + +if __name__ == "__main__": + # Load use-case parameters + config = load_use_case_config() + + # Connection to robot + host = config['EVA']['comm']['host'] + token = config['EVA']['comm']['token'] + eva = Eva(host, token) + + # Compute grid points and robot joints + eva_box = EvaGrids(eva, config, show_plot=True) + joints = eva_box.get_grid_points(config['grids']['names']) + + # Go home before starting + with eva.lock(): + eva.control_go_to(config['EVA']['home']) + + while True: + for counter in range(len(joints[(config['grids']['names'][0])]['pick'])): + joints_home = config['EVA']['home'] + joints_pick = joints[config['grids']['names'][0]]['pick'][counter] + joints_drop = joints[config['grids']['names'][1]]['pick'][counter] + joints_pick_hover = joints[config['grids']['names'][0]]['hover'][counter] + joints_drop_hover = joints[config['grids']['names'][1]]['hover'][counter] + + # USER DEFINED WAY-POINTS + joints_operation_A = [] + joints_operation_B = [] + joints_operation_C = [] + + tool_path_grid_to_grid = { + "metadata": { + "version": 2, + "default_max_speed": 0.1, + "next_label_id": 7, + "payload": config['EVA']['end_effector']['payload'], + "analog_modes": {"i0": "voltage", "i1": "voltage", "o0": "voltage", "o1": "voltage"} + }, + "waypoints": [ + {"label_id": 1, "joints": joints_home}, + {"label_id": 2, "joints": joints_pick_hover}, + {"label_id": 3, "joints": joints_pick}, + {"label_id": 4, "joints": joints_operation_A}, + {"label_id": 5, "joints": joints_drop_hover}, + {"label_id": 6, "joints": joints_drop}, + ], + "timeline": [ + {"type": "home", "waypoint_id": 0}, + {"type": "trajectory", "trajectory": "joint_space", "waypoint_id": 1}, + {"type": "output-set", "io": {"location": "base", "type": "digital", "index": 0}, "value": True}, + {"type": "trajectory", "trajectory": "linear", "waypoint_id": 2}, + {"type": "wait", "condition": {"type": "time", "duration": 0.2}}, + {"type": "trajectory", "trajectory": "joint_space", "waypoint_id": 1}, + {"type": "trajectory", "trajectory": "joint_space", "waypoint_id": 3}, + {"type": "trajectory", "trajectory": "joint_space", "waypoint_id": 4}, + {"type": "trajectory", "trajectory": "linear", "waypoint_id": 5}, + {"type": "output-set", "io": {"location": "base", "type": "digital", "index": 0}, "value": False}, + {"type": "trajectory", "trajectory": "linear", "waypoint_id": 4}, + {"type": "trajectory", "trajectory": "joint_space", "waypoint_id": 0}, + ] + } + + with eva.lock(): + eva.control_wait_for_ready() + eva.toolpaths_use(tool_path_grid_to_grid) + eva.control_run(loop=1, mode="automatic") diff --git a/examples/grid2grid/readme_images/eva_frame.png b/examples/grid2grid/readme_images/eva_frame.png new file mode 100644 index 0000000..814845e Binary files /dev/null and b/examples/grid2grid/readme_images/eva_frame.png differ diff --git a/examples/grid2grid/readme_images/grid_def.png b/examples/grid2grid/readme_images/grid_def.png new file mode 100644 index 0000000..75a1957 Binary files /dev/null and b/examples/grid2grid/readme_images/grid_def.png differ diff --git a/examples/grid2grid/readme_images/visual.png b/examples/grid2grid/readme_images/visual.png new file mode 100644 index 0000000..ee99767 Binary files /dev/null and b/examples/grid2grid/readme_images/visual.png differ diff --git a/examples/machine_vision_object_pickup/config/use_case_config.yaml b/examples/machine_vision_object_pickup/config/use_case_config.yaml index 5db22ea..58b05fe 100755 --- a/examples/machine_vision_object_pickup/config/use_case_config.yaml +++ b/examples/machine_vision_object_pickup/config/use_case_config.yaml @@ -1,60 +1,56 @@ -# SETUP YAML file for Machine Vision Example - December 2019 +# SETUP YAML file for Machine Vision Example EVA: comms: - host: '0.0.0.0' - token: 'abcd1234' + host: '0.0.0.0' # USER DEFINED + token: 'abcd1234' # USER DEFINED end_effector: - payload: 0 # payload [kg] - length: 0 # length of tool [m] - offset: # ee offset in ee-frame [m] + payload: 0 # payload [kg] - USER DEFINED + length: 0 # length of tool [m] - USER DEFINED + offset: # ee offset in ee-frame [m] - USER DEFINED - 0 - 0 - 0 - hover_height: 0 # elevation of idle z axis wrt to the object [m] - surface_height: 0 # elevation of the pickup surface wrt to the robot [m, signed] + hover_height: 0 # elevation of idle z axis wrt to the object [m] - USER DEFINED + surface_height: 0 # elevation of the pickup surface wrt to the robot [m, signed] - USER DEFINED TCP: - server: '0.0.0.0' - port: 0 + server: '0.0.0.0' # USER DEFINED + port: 0 # USER DEFINED objects: - names: # object names + names: # object names - DO NOT MODIFY THIS - 'C' - 'M' - 'R' - heights: # object thickness [m] - C: 0 - M: 0 - R: 0 - weights: # object weight [k] + heights: # object thickness [m] - USER DEFINED C: 0 M: 0 R: 0 waypoints: - joints_cal_zero: # joints @ (0,0) of calibration board [rad] + joints_cal_zero: # joints @ (0,0) of calibration board [rad] - USER DEFINED - 0 - 0 - 0 - 0 - 0 - 0 - joints_guess: # joints guess for pickup/hover position [rad] + joints_guess: # joints guess for pickup/hover position [rad] - USER DEFINED - 0 - 0 - 0 - 0 - 0 - 0 - joints_home: # joints for home position [rad] + joints_home: # joints for home position [rad] - USER DEFINED - 0 - 0 - 0 - 0 - 0 - 0 - joints_drop: # joints for drop position [rad] + joints_drop: # joints for drop position [rad] - USER DEFINED - 0 - 0 - 0