baxter_smacha

This package provides a sample use case for applying the SMACHA API in conjunction with the Baxter robot simulator.

Overview

SMACHA enables rapid, compact meta-scripting of SMACH state machines using YAML files, Jinja2-based code templates and a code generator.

Scripts

For example, the SMACHA script for a simple pick and place demo using the Baxter simulator looks like this:

--- # Modular SMACHA pick and place test script for the Baxter simulator.
name: sm
template: BaxterBase
node_name: baxter_smach_pick_and_place_test
outcomes: [succeeded, aborted, preempted]
userdata:
  limb: left
  hover_offset: [[0.0, 0.0, 0.15], [0.0, 0.0, 0.0, 0.0]]
states:
  - LOAD_TABLE_MODEL:
      template: LoadGazeboModelState
      model_name: cafe_table
      model_path: rospkg.RosPack().get_path('baxter_sim_examples')+'/models/cafe_table/model.sdf'
      userdata:
        table_model_pose_world: Pose(position=Point(x=1.0, y=0.0, z=0.0))
        table_model_ref_frame: world
      remapping: {pose: table_model_pose_world, reference_frame: table_model_ref_frame}
      transitions: {succeeded: LOAD_BLOCK_MODEL}

  - LOAD_BLOCK_MODEL:
      template: LoadGazeboModelState
      model_name: block
      model_path: rospkg.RosPack().get_path('baxter_sim_examples')+'/models/block/model.urdf'
      userdata: 
        block_model_pick_pose_world: [[0.6725, 0.1265, 0.7825], [0.0, 0.0, 0.0, 0.0]]
        block_model_pick_ref_frame: world
        block_model_pick_pose: [[0.7, 0.15, -0.129],
                                [-0.02496, 0.99965, 0.00738, 0.00486]]
        block_model_place_pose: [[0.75, 0.0, -0.129],
                                 [-0.02496, 0.99965, 0.00738, 0.00486]]
      remapping: {pose: block_model_pick_pose_world, reference_frame: block_model_pick_ref_frame}
      transitions: {succeeded: MOVE_TO_START_POSITION}
  
  - MOVE_TO_START_POSITION:
      template: MoveToJointPositionsState
      userdata: {joint_start_positions:
                  [-0.08000, -0.99998, -1.18997, 1.94002, 0.67000, 1.03001, -0.50000]}
      remapping: {limb: limb, positions: joint_start_positions}
      transitions: {succeeded: PICK_BLOCK}
  
  - PICK_BLOCK:
      script: pick_block
      remapping: {limb: limb, pick_pose: block_model_pick_pose, hover_offset: hover_offset}
      transitions: {succeeded: PLACE_BLOCK}

  - PLACE_BLOCK:
      script: place_block
      remapping: {limb: limb, place_pose: block_model_place_pose, hover_offset: hover_offset}
      transitions: {succeeded: succeeded}

The above example illustrates some of the core SMACHA functionality. A base state machine is specified using the BaxterBase template and contains five states that transition between one another and pass data between one another using familiar SMACH concepts like userdata and remappings. Each of the states specifies a template that determines how its respective SMACH code should be rendered in the final generated output script, as well as any parameters that should be passed to the template as necessary. Two of the states, PICK_BLOCK and PLACE_BLOCK make use of included sub-scripts that are specified as container states in the files pick_block.yml and place_block.yml respectively, which are located in the smacha_scripts folder.

There are currently three working demo scripts available under the smacha_scripts folder:

pick_and_place_demo.yml: this script (shown in the above example) loads a block model and a table model into the simulator and instructs Baxter to pick the block up from a given location and place it at another location on the table surface.
stacking_demo.yml: this script loads two block models and instructs Baxter to stack one on top of the other.
dual_arm_stacking.yml: this script loads two block models and uses concurrent state machines to instruct Baxter to stack the blocks one on top of the other using both of its arms.

The SMACH code that is generated by these scripts using the SMACHA API is available in the smacha_generated folder.

Templates

In this package, code templates have been designed specifically for use with the Baxter simulator. The following templates are currently available:

Dependencies

The following dependencies must be installed before using this package:

Install

Clone into the src directory of your catkin workspace and run either catkin_make or catkin build from the root of the workspace. Be sure to source the workspace afterwards, e.g. if your workspace is catkin_ws, run source ~/catkin_ws/devel/setup.bash before continuing.

Usage

Generating SMACH Code using SMACHA Scripts and Templates

To generate the code for the pick and place demo from the above example, run the following:

roscd baxter_smacha
rosrun smacha smacha smacha_scripts/pick_and_place_demo.yml -t smacha_templates/ -s smacha_scripts -i -v -o smacha_generated/pick_and_place_demo.py

The command-line arguments specify the following:

-t: specifies searchable folders for templates,
-s: specifies searchable folders for sub-scripts,
-i: adds an introspection server to the generated code for use with smach_viewer,
-v: specifies verbose output,
-o: specifies the name of the output file for the generated SMACH code.

The other demo scripts may be used in a similar manner.

Running the Simulator and Testing the Generated SMACH Code

After the SMACH code has been generated, open a terminal, navigate to the Baxter simulator workspace and run the following:

./baxter.sh sim
roslaunch baxter_smacha baxter_smacha_world.launch

Note that in this package we use a modified Baxter robot description in order to add paddle tips to the right limb gripper of the robot for use in the dual-arm stacking demo, hence the custom launch file above.

In a separate terminal, run the following to launch the demo using the generated SMACH code:

roscd baxter_smacha
python smacha_generated/pick_and_place_demo.py

The other demos may be run in a similar manner.

abr-ijs/baxter_smacha