Skip to content

birlrobotics/smach_based_introspection_framework

Repository files navigation

Package: smach_based_introspection_framework

If you use this toolbox, please cite the following related papers:

[1] "A Latent State-based Multimodal Execution Monitor with Anomaly Detection and Classification for Robot Introspection", Hongmin Wu, Yisheng Guan , Juan Rojas, Appl. Sci. 2019, 9(6), 1072; doi: 10.3390/app9061072.http://www.juanrojas.net/files/papers/2019AS-Wu-LatState_MMExecMntr_AnmlyDetClassif.pdf;

[2]"Robot Introspection for Process Monitoring in Robotic Contact Tasks through Bayesian Nonparametric Autoregressive Hidden Markov Models",Hongmin Wu, Yisheng Guan and Juan Rojas, International Journal of Advanced Robotics Systems, 2018 (IF 0.952).http://www.juanrojas.net/files/papers/2019IJARS-Wu_Rojas-AnalysisHDPVARHMM.pdf

Why This Package Exists

We want a finite-state-machine-based control framework for Rethink Robotics Baxter which satisfies the following needs:

  1. During motion, by observing sensory data, the robot should be able to detect and classify anomalies, then execute recovery policy accordingly.
  2. If the anomaly detected is never seen before, the robot can ask the operator to label the anomaly and demonstrate recovery policy for it.
  3. As the operator gives more help, the robot becomes more autonomous.

This framework attempts to meet these needs.

What Can This Package Do

Its core functionalities include:

  • Record sensory data on a per trial basis.
  • Extract samples of successful motions and samples of anomalies from trial recordings.
  • Train models for anomaly detection and classification.
  • Provide a console interface to the operator for labelling anomaly and demonstrating recovery policy.
  • Upon known anomalies, execute recovery policies accordingly.

Demo

A simple demo of picking objects can be found in ./test/demo. You can follow the steps below to experience how this framework works.

  1. Boot The Robot

    Run the follwoing commands to setup baxter (No need to launch simulation if you're using real robot):

    roslaunch baxter_gazebo baxter_world.launch
    rosrun baxter_interface joint_trajectory_action_server.py
    roslaunch baxter_moveit_config baxter_grippers.launch
  2. Collect Enough Successful Trials

    To collect one successful trial, run the following commands:

    cd ./test/demo
    python pick_n_place_runner.py

    After each trial, you will notice one folder is created in ./introspection_data_folder/experiment_record_folder. If the trial is not successful, delete the latest folder. Make sure you collect enough successful trials (>=5) in the folder.

    Since during this step there is no model for anomaly detection, you will notice that baxter head screen shows yellow lights during the motion of each state, which indicates that no introspection model is found.

  3. Train Models For Anomaly Detection

    To train models, we need to first extract samples from trial recordings which are effectively rosbag files.

    To process these rosbag files into samples, run:

    cd ./scripts
    python process_experiment_record_to_dataset.py

    When it's finished, you will notice ./introspection_data_folder/dataset_folder/latest is created which contains samples that can be used to train models.

    Now, to train models using these samples, run:

    cd ./scripts
    python process_dataset_to_models.py

    When it's finished, you will notice ./introspection_data_folder/model_folder/latest is created which contains models trained. A training log is generated in run.log there so you can have a look at the training details.

  4. Collect Anomalous Trials

    Since anomaly detection models are in place now, the robot now can detect anomalies and ask help from the operator.

    Run more trials and make them anomalous if possible.

    To run one trial:

    cd ./test/demo
    python pick_n_place_runner.py

    When an anomaly is detected, look at the terminal and you will find an interface to label the anomaly and teach the recovery policy for it.

  5. Train Anomaly Classifier

    Since anomalous trials are collected and labeled, we can train classifiers for them:

    cd ./scripts
    python process_experiment_record_to_dataset.py
    cd ./scripts
    python process_dataset_to_models.py
  6. Done

    Now the robot is able to detect anomalies, classify them and execute recovery policies accordingly.

Minimal tutorial

A minimal tutorial on this framework can be:

  1. Define smach state as in ./test/demo/task_states.py:
    class GoToPickPosition(smach.State):
        def __init__(self):
            smach.State.__init__(self,
                                 outcomes=['Successful'])
            self.state_no = 3 # Skill tag
            self.depend_on_prev_state = True # Set this flag accordingly
    
        def before_motion(self): # Things to do before motion
            limb = 'right'
            traj = BreakOnAnomalyTrajectoryClient(limb)
            traj.gripper_open()
    
        def after_motion(self): # Things to do after motion
            limb = 'right'
            traj = BreakOnAnomalyTrajectoryClient(limb)
            traj.gripper_close()
    
        def get_pose_goal(self): # Goal getter, will be reused in executing recovery policy
            return hardcoded_data.pick_object_pose
    
        def determine_successor(self): # Determine next state
            return 'Successful'
  2. Pass in smach state machine as in ./test/demo/pick_n_place_runner.py:
    from smach_based_introspection_framework.online_part import (
        smach_runner
    )
    if __name__ == '__main__':
        from task_states import assembly_user_defined_sm
        sm = assembly_user_defined_sm() # Assembly state machine in a user-defined way
        smach_runner.run(sm) # Pass in the state machine and it will be run by our framework

Installation

Install Python Dependencies

sudo pip install -r requirments.txt
sudo apt-get install redis-server

Catkin Dependencies

Since this is a catkin package, you can find out full dependencies by running catkin_make. Important dependencies are listed below, they are all catkin packages so you can just git clone them into your catkin workspace:

Utilities

Automatically record PC screen when an experiment starts

  1. Assume the PC whose screen you want to record is named RecordPC. Install avconv on RecordPC :

    sudo apt-get install libav-tools
    
  2. Join RecordPC to the ROS network where you're conducting the experiment, in the case of Baxter, run:

    [On "RecordPC", cd to baxter_ws]
    ./baxter.sh
    

    Then, start screen recording service on RecordPC:

    rosrun smach_based_introspection_framework start_cam_recording_service.py 
    
  3. Now, you're all set. Run the experiment and when it's finished, you will have a mp4 file on RecordPC. It's saved at introspection_data_folder/experiment_video_folder/*