Pick & Place App Tutorial ========================= This chapter is a hands-on introduction to writing Apps. At the end, the App you create will be able to pick an object and place it in a user-taught position. While placing the object, the robot will monitor contact forces, and open the gripper once it touches the surface. This will compensate for inaccuracies in place position and allow for easy stacking. The App will be created step by step, and we will be adding the functionality in the following order: * Print messages to the log * Controlling the gripper * Transforming a state machine into an App * Moving the robot arm * Monitoring if the grasped object was lost * Checking for expected collisions Hello World ----------- To familiarize ourselves with the `ride-cli` tool, we will start by installing a simple state machine that prints a message. All state machines are installed as part of bundles. The bundle in this chapter will be called ``tutorial``: .. code-block:: shell ride bundle new bundles/tutorial This will create the following folder structure and two text files: .. code-block:: none . └── bundles └── tutorial ├── manifest.json ├── resources └── sources └── Tutorial.lf The ``manifest.json`` file indicates the name and the version of a bundle. When installing a bundle on a robot which already has a bundle with the same name, it will only be updated if the version is the same or newer. The ``Tutorial.lf`` file will contain the code we create in this tutorial. Let us start by creating a single state which will print a message to the log. .. code-block:: lf Tutorial { port Success true entry @{ printf("Hello World!") }@ } This creates a root state ``Tutorial``. The root must always have the same name as the file it is contained in. Our state has one exit port called ``Success``. After the port's name we put a condition, which defines when the execution should exit through that port. In this case, the condition is simply ``true``, so it will immediately exit. Finally, we attach a Lua script block (between ``@{...}@``) onto the ``entry`` action. Whenever the ``Tutorial`` state is entered, a "Hello World!" message will be printed onto the log. Compiling our Bundle ^^^^^^^^^^^^^^^^^^^^ Next we have to compile our bundle. We will specify a custom build folder where `ride` should put the built bundle. .. code-block:: shell ride bundle compile bundles/tutorial This will produce our Tutorial bundle which we can install on the robot. Installing the Bundle ^^^^^^^^^^^^^^^^^^^^^ To install our tutorial bundle, you must first log-in using `ride` .. code-block:: shell ride login SERVER ``SERVER`` is the same address you would use to connect to Desk. The command uses ``robot.franka.de`` by default. .. code-block:: shell ride bundle install bundles/tutorial.bundle You should be able now to see the ``tutorial`` bundle listed when using: .. code-block:: shell ride bundle list Running our State Machine ^^^^^^^^^^^^^^^^^^^^^^^^^ New state machines cannot be started when others are currently running. Right now, the robot is most likely running the ``idle_statemachine`` state machine. It is started by the App server at startup and whenever a Task from Desk stops. Stop it using ``ride cli stop``. Run ``ride cli log`` in a separate terminal to see the log. Then, start our newly installed state machine with: .. code-block:: shell ride execution start -t Tutorial Where ``-t`` stands for tracing. You should obtain an output similar to this one: .. code-block:: shell ride execution start -t Tutorial Selected revision: 77 Filtered tracing of path Tutorial Execution (155) [FINISHED] You should see "Hello World!" printed in the log. Opening and Closing the Gripper ------------------------------- Now we will add states responsible for opening and closing the gripper. Instead of writing the functionality for controlling the gripper from scratch, we will reuse states contained in the ``gripper`` bundle. To add two new states to ``Tutorial.lf``, modify it as follows: .. code-block:: lf Tutorial { port Success child("gripper_close").port("success") port Error child("gripper_open").port("error") or child("gripper_close").port("error") entry @{ printf("Hello World!") }@ --> gripper_open <- gripper_move { port success -> gripper_close } where { width: 0.1; speed: 0.2; } gripper_close <- grasp { } where { width: 0.05; speed: 0.1; force: 20.0; } } We nest ``gripper_open`` and ``gripper_close`` as child states of ``Tutorial``. By using the ``<-`` arrow we declare those two as `link states`, inheriting all the elements of the ``gripper_move`` and ``grasp`` states. The library will take care of calling the necessary operations to operate the gripper. The ``-->`` arrow declares ``gripper_open`` as the first child, which will be activated whenever the root state is activated. The ``where { ... }`` clause is used to parameterize the states, here simply using hardcoded values. And finally, the port statements were modified as well: .. code-block:: lf port Success child("gripper_close").port("success") This line specifies that the ``Tutorial`` state should exit through the ``Success`` port whenever the nested state's port ``success`` is reached. Note that we use the ``success`` port which ``gripper_close`` inherits from ``grasp``. Before we can successfully build our tutorial bundle again, we have to specify any extra dependency our bundle uses as a build dependency (see `Handling Dependencies `_) However since ``gripper`` is a *builtin* dependency, we don't need to add the bundle to our manifest. .. code-block:: shell ride bundle compile bundles/tutorial --ignore-dependencies -o build/tutorial.bundle ride bundle install build/tutorial.bundle .. warning:: **TODO**: Check builtin dependencies (such as ``gripper``) against the robot, when connected. This way you must not opt out of the dependency check entirely with the ``--ignore-dependencies`` flag when using builtin bundles. Then start the modified state machine using the commands from above. The gripper should now open and close. The trace indicates how execution transfers from ``gripper_open`` to ``gripper_close``. Notice that the parent state ``Tutorial`` remains active: .. code-block:: shell Execution (5) of Tutorial [FINISHED] Execution (6) of Tutorial [RUNNING] Tutorial.gripper_open [ACTIVE] Execution (6) of Tutorial [RUNNING] Tutorial.gripper_close [ACTIVE] Execution (6) of Tutorial [FINISHED] These states interact with the ``gripper`` service. You can see which operations and events are made available using the ``ride service list`` command. You can call ``ride service echo gripper gripper_state`` in a separate terminal to observe how the ``gripper_state`` is modified during the execution of ``Tutorial`` state machine. .. _app: Wrapping State Machines in Apps --------------------------------- To be used in Franka Desk, the state machine needs to be defined as an app. Apps are state machines which match a specific interface. Apps must have at least one port named ``Success`` and one port named ``Error``. In addition, the ``resultType`` must contain at least one ``string`` field named ``error_cause``, which provides an indication of possible problems for the user in case of an error. Additional fields are possible though. The following code snippet shows the required content of an app. .. code-block:: html name { port Success true port Error false clientData { type : "app"; name : "Name"; } resultType { string error_cause; } } In addition to the introduced parts of a state machine, an app contains client data, which describes the visualization of the app in Franka Desk. * ``type`` [mandatory] (String): ``"app"`` * ``name`` [mandatory] (String): Displayed name of the App * ``color`` (String): The display color of the app * ``image`` (String): A string containing an SVG image to be used as the app's icon * ``requiredParameters`` (Array[Object]): An array of objects which contain source parameters defined by - ``source``: The name of the parameter in the source - ``localParameter``: The name of the parameter as it is used in the state machine * ``contextMenu`` (Script): The definition of steps to configure the app, refer to :ref:`context-menu` for a detailed explanation * ``components`` (Script): Custom components used in the app, refer to :ref:`components` for more information To use the ``Tutorial`` state machine as an app, we add the ``clientData``, ``resultType`` and some actions to pass the ``error_cause`` of the child states to the root state. .. code-block:: html clientData { type : "app"; name : "Pick Contact"; color : "#B6E591"; requiredParameters: [{source: speed; localParameter: speed;}]; image : @{ }@; } resultType { string error_cause; } action child("gripper_open").port("error") @{ setResult({error_cause = child("gripper_open").result.error_cause}) }@ action child("gripper_close").port("error") @{ setResult({error_cause = child("gripper_close").result.error_cause}) }@ The following image depicts how an app with a certain ``color`` and ``image`` appears in the library in Franka Desk. The name is shown below and should be kept short. .. image:: _static/pick.png To access the ``Tutorial_logo.svg`` file, create a folder named ``resources`` inside the tutorial folder and place the :download:`image <_static/Tutorial_logo.svg>` there (see :ref:`svg` for more detailed information). .. code-block:: none . └── bundles └── tutorial ├── manifest.json ├── resources │ └── Tutorial_logo.svg └── sources └── Tutorial.lf Compile and install as before. You should now see a block similar to the one depicted above appear in Desk. Drag it onto a new Task and press start. The gripper should open and close as before. When you trace the execution using ``ride execution trace``, notice how your ``Tutorial`` state now contains a nested state called ``app``. Since tasks are also state machines, you can find the name of your task in the list obtained from ``ride node list``, and similarly start it using ``ride execution start``. User Parameterization --------------------- We want to make the gripper width parameterizable by the user of the app. We therefore introduce parameters, i.e. ``gripper_open_width`` and ``gripper_closed_width``. We will later create a context menu for the app, which contains components to manipulate those parameters in Desk (see :ref:`context-menu`). We replace the hard coded values for ``width`` with the new parameters and set default values in the ``where { ... }`` clause. .. code-block:: lf parameterType { float gripper_open_width; float gripper_closed_width; } --> gripper_open <- gripper_move { port success -> gripper_close } where { width: parameter.gripper_open_width; speed: 0.1; } gripper_close <- grasp { } where { width: parameter.gripper_closed_width; speed: 0.1; force: 20.0; } } where { gripper_open_width: nil; gripper_closed_width: nil; } If the parameter is initialized with ``nil``, the user is forced to parameterize it, because an app can only be executed if all parameters are defined. In order to parameterize this from Desk, the context menu entry needs now be added to the ``clientData``: .. code-block:: lf clientData { contextMenu : @{ }@; ... } You can also download the ``.lf`` file with a basic context menu :download:`here <_static/Tutorial_gripper.lf>`. After installing the App, you should be able to set the gripper width from Desk. Moving the Robot ---------------- Now that we can control the gripper, we need to move to the object we want to grasp. The overall strategy is as follows: 1. Open the gripper. 2. Move to the object. 3. Close the gripper. 4. Retract. Apart from these three new states to handle the motion, we need one more to calculate the trajectory parameters. We will introduce the necessary changes step by step, but you can also see the full ``.lf`` file :download:`here <_static/Tutorial_move.lf>` first, and follow along. First, the parameters of the two robot poses we want the user to set are added. The ``pick_approach`` pose will also be used for retract, i.e. the robot will move from the approach pose to the pick pose and back to the approach pose during the execution. The speed parameter is inherited from the source, as defined in the ``requiredParameters`` in the ``clientData``. The ``velocity`` parameter will be used to control the overall speed of the movement. .. code-block:: lf parameterType { { [16]float pose; []float joint_angles; } pick_approach; { [16]float pose; []float joint_angles; } pick_pose; float speed; float velocity; float gripper_open_width; float gripper_closed_width; } Note that you can use structs and array types for your parameters as is the case for the poses. We will use ``move_via_with_move_configuration`` state to move the robot, but first we need to merge the taught poses into a trajectory. This will be done with the help of the ``merge_trajectory_and_kinematic_parameters``. Add a new start state: .. code-block:: lf --> compute_parameters <- merge_trajectory_and_kinematic_parameters { port done -> gripper_open } where { add_to_approach: true; add_to_retract: false; approach: [parameter.pick_approach]; approach_end: parameter.pick_pose; retract_start: parameter.pick_pose; retract: [parameter.pick_approach]; cartesian_velocity_factor_approach: parameter.velocity*parameter.speed; cartesian_acceleration_factor_approach: 0.8*parameter.velocity*parameter.speed; cartesian_deceleration_factor_approach: 0.8*parameter.velocity*parameter.speed; cartesian_velocity_factor_ptp: parameter.velocity*parameter.speed; cartesian_acceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed; cartesian_deceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed; cartesian_velocity_factor_retract: parameter.velocity*parameter.speed; cartesian_acceleration_factor_retract: 0.8*parameter.velocity*parameter.speed; cartesian_deceleration_factor_retract: 0.8*parameter.velocity*parameter.speed; } Note that the parameters ``approach`` and ``retract`` expect trajectories. As we only teach one approach pose, we wrap the parameter with square brackets. The overall speed of the movement is influenced by two parameters: The ``speed`` parameter which is task-specific and the ``velocity`` parameter which is app-specific. Next, we can introduce the movement states: .. code-block:: lf gripper_open <- gripper_move { port success -> approach } where { width: parameter.gripper_open_width; speed: 0.1; } approach <- move_via_with_move_configuration { port success -> gripper_close } where { poses: child("compute_parameters").result.approach; } gripper_close <- grasp { port success -> retract } where { width: parameter.gripper_closed_width; speed: 0.1; force: 20.0; } retract <- move_via_with_move_configuration { } where { poses: child("compute_parameters").result.retract; } Notice that ``gripper_open`` is no longer the start state, and how we use ``child("compute_parameters").result`` to access the results of ``compute_parameters`` in its siblings. Having defined our states, we can now go back to update our ports: .. code-block:: lf port Success child("retract").port("success") port Error child("approach").port("error") or child("gripper_open").port("error") or child("gripper_close").port("error") or child("retract").port("error") And update the error messages and parameter initialization: .. code-block:: lf action child("gripper_open").port("error") @{ setResult({error_cause = child("gripper_open").result.error_cause}) }@ action child("approach").port("error") @{ setResult({error_cause = child("approach").result.error_cause}) }@ action child("retract").port("error") @{ setResult({error_cause = child("retract").result.error_cause}) }@ action child("gripper_close").port("error") @{ setResult({error_cause = child("gripper_close").result.error_cause}) }@ } where { pick_approach: nil; pick_pose: nil; speed: nil; velocity: nil; gripper_open_width: nil; gripper_closed_width: nil; } You can download the ``.lf`` file with a basic context menu :download:`here <_static/Tutorial_move.lf>`. To have a proper visualization, place :download:`gripper_opened.svg <_static/gripper_opened.svg>`, :download:`gripper_holding.svg <_static/gripper_holding.svg>` and :download:`object.svg <_static/object.svg>` inside the ``resources`` folder. Install your state machine on the robot. If you have Tasks utilizing a previous versions of your ``Tutorial`` App already, you first might have to delete the instances of ``Tutorial`` to prevent conflicts. If the App was installed correctly, you should now be able to teach the poses. Run the App and watch the robot grasp an object! Barriers -------- Next, we want to transport our object. While we move the robot, we would like to monitor the gripper service and exit with an error if the grasped object is lost during transit. In such cases, we can use barriers for parallel execution. For better modularization, we will implement that functionality in a separate file. Create a ``move_monitored.lf`` file next to ``Tutorial.lf``, and enter the following: .. code-block:: lf move_monitored { port success child("move").port("success") port error child("move").port("error") or child("observe_part_lost").port("part_lost") parameterType { []{ {[16]float pose;} pose; bool point2point; float cartesian_velocity_factor; float cartesian_acceleration_factor; float cartesian_deceleration_factor; float q3; } poses; } resultType { string error_cause; } --> barrier move_and_check { -> move -> observe_part_lost } move <- move_via_with_move_configuration { } where { poses: parameter.poses; } observe_part_lost <- gripper_observer { } action child("observe_part_lost").port("part_lost") @{ setResult({ error_cause = "Part was lost during motion." }) }@ action child("move").port("error") @{ setResult({ error_cause = child("move").result.error_cause }) }@ } The ``barrier`` keyword indicates that ``move_and_check`` node is a barrier. It activates two states in parallel, ``move`` and ``observe_part_lost``: .. code-block:: lf --> barrier move_and_check { -> move -> observe_part_lost } The ``move_observe_gripper`` ports are connected to ``move``'s ports as before, but the error port can also be reached when ``observe_part_lost`` exits through ``part_lost``. If that happens, the execution of ``move`` will be preempted. To compute the pose for our new state, let's add a ``compute_parameters_place`` state to ``Tutorial.lf``: .. code-block:: lf compute_parameters_place <- merge_trajectory_and_kinematic_parameters { port done -> gripper_open } where { add_to_approach: true; add_to_retract: false; approach: [parameter.place_approach]; approach_end: parameter.place_pose; retract_start: parameter.place_pose; retract: [parameter.place_approach]; cartesian_velocity_factor_approach: parameter.velocity*parameter.speed; cartesian_acceleration_factor_approach: 0.8*parameter.velocity*parameter.speed; cartesian_deceleration_factor_approach: 0.8*parameter.velocity*parameter.speed; cartesian_velocity_factor_ptp: parameter.velocity*parameter.speed; cartesian_acceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed; cartesian_deceleration_factor_ptp: 0.8*parameter.velocity*parameter.speed; cartesian_velocity_factor_retract: parameter.velocity*parameter.speed; cartesian_acceleration_factor_retract: 0.8*parameter.velocity*parameter.speed; cartesian_deceleration_factor_retract: 0.8*parameter.velocity*parameter.speed; } Now we can use our ``move_monitored`` state! After using it to touch the surface, we want to open the gripper. Let's add two new states: ``transport`` and ``gripper_open_place``: .. code-block:: lf retract <- move_via_with_move_configuration { port success -> transport } where { poses: child("compute_parameters").result.retract; } transport <- move_monitored { port success -> gripper_open_place } where { poses: child("compute_parameters_place").result.approach; } gripper_open_place <- gripper_move { } where { width: parameter.gripper_place_open_width; speed: 0.1; } Another step was added to the context menu, allowing the user to teach the placing motion. Similarly as before, the ports, parameters, context menu and resulting string assignments have to be updated. See :download:`here <_static/Tutorial_monitored.lf>` for the full changes. Execute your App and try to remove the object from the gripper during motion. The App should stop with an error message: "Part was lost during motion". Moving until Contact -------------------- At this stage, we have a functioning pick and place App. Let's extend it by moving to contact for the place motion. When the robot collides, instead of stopping with an error, we will check if the collision took place within a defined area. If it did, we will recover from the error, and continue the execution instead. Add a ``check_error.lf`` state: .. code-block:: lf check_error { port success result ~= nil and result.expected -> reset port error result ~= nil and result.expected == false parameterType { [16]float expected_pose; string error_cause; { [7]float tau_ext; [6]float K_F_ext_hat_K; { [16]float pose; } O_T_EE; [2]float elbow; int cartesian_goal_id; [7]float q; [6]float cartesian_collision; [7]float joint_collision; []string robot_errors; } move_error; { float x; float y; float z; } contact; } resultType { bool expected; string error_cause; } entry @{ setResult(nil) }@ action result == nil @{ if parameter.move_error == nil or not utils.contains(parameter.move_error.robot_errors, {"cartesian_reflex_flag", "joint_reflex_flag"}) then setResult({expected = false, error_cause = "Unexpected error. " .. parameter.error_cause }) else local current_ee = parameter.move_error.O_T_EE.pose if (abs(current_ee[13] - parameter.expected_pose[13]) > parameter.contact.x or abs(current_ee[14] - parameter.expected_pose[14]) > parameter.contact.y or abs(current_ee[15] - parameter.expected_pose[15]) > parameter.contact.z) then setResult({expected = false, error_cause = "Error outside of defined radius. " .. parameter.error_cause}) else setResult({expected = true}) end end }@ } The new state is used in ``move_monitored.lf``: .. code-block:: lf move <- move_via_with_move_configuration { port error -> check } where { poses: parameter.poses; } check <- check_error { port success -> reset } where { expected_pose: parameter.poses[#parameter.poses].pose.pose; contact: parameter.contact; error_cause: child("move").result.error_cause; move_error: child("move").result.move_error; } reset <- wait_until_reset_collision { } The ``error`` port leads to ``check`` state, where its ``move_error`` result is inspected. The ``#`` operator in ``#parameter.poses`` returns the length of its operand. Here it's used to obtain the last pose passed to the ``move`` state. The collision area check will be computed with respect to that position. Port definitions have to be updated, see :download:`here <_static/move_monitored.lf>` for full changes. The new parameter, ``contact``, has to be added in ``Tutorial.lf``: .. code-block:: lf { float x; float y; float z; } contact; The values should be passed into ``move_monitored`` state: .. code-block:: lf transport <- move_monitored { port success -> gripper_open_place } where { poses: child("compute_parameters_place").result.approach; contact: parameter.contact; } And, finally, we can add some default values for the contact area: .. code-block:: lf } where { pick_approach: nil; pick_pose: nil; speed: nil; velocity: nil; gripper_open_width: nil; gripper_closed_width: nil; place_approach: nil; place_pose: nil; contact: { x: 0.01; y: 0.01; z: 0.01; }; } See the full version of the file :download:`here <_static/Tutorial_no_context_menu.lf>`. If you'd like to learn more by improving the App further, here are possible next steps: * The place motion could be divided into two parts: approach, and sensitive place. The first part could be made faster, while the second could increase the compliance values. * Parameters passed to the states can be integrated with the Task-wide settings, accessible when the Task name is pressed in Desk.