HAPTIX Simulation Scoring Plugin Example


Overview

This tutorial demonstrates a new custom handsim world created for simulation based manipulation dexterity testing inspired by a paper titled "The strength-dexterity test as a measure of dynamic pinch performance" written by Valero-Cuevas et. al. in 2003.

For this tutorial, we assume that you have already completed the HAPTIX handsim installation step and we strongly recommend that you have completed the simulation world API tutorials.

Running the Simulation Example

To start Gazebo handsim scoring plugin example, run gazebo in terminal:

gazebo --verbose worlds/luke_hand.world

By default it brings up the desktop world with the Luke Hand model.

Integrated in this world is a spring compression test that uses the SimEventsPlugin to keep track of the status of aforementioned task completion.

Example Video

Below is an example of the custom world as tele-operated by keyboard and spacenav options:

In the video, there are three little task completion indicator dots at the lower right hand side of the hand visualization GUI. The left most circular dot indicates correct compression without buckling, green if compressed without buckling, red if uncompressed or buckled. The middle circular dot indicates compression hold time, it fades from white to green as the spring is compressed correctly and held for 3 seconds. The right most circular dot indicates task success as it turns from white to green.

Initially, the three circles are red-white-white, indicating the spring is undisturbed. When the spring is compressed sufficiently (compression length between 1 to 10cm), and the springs are relatively straight (torsional spring joint at the middle of the spring exhibits < 0.1 radians in flex), the first circle turns green. A timer is started for this successful unbuckled compression, and the second circle fades from white to green. When the timer reaches 3 seconds, the second and third circular indicators turn green, and the program considers this a successful test trial.

Relevant Documentations

In the luke_hand.world, a new libSimEventsPlugin.so plugin block has been added:

    <plugin name="SimEvents" filename="libSimEventsPlugin.so">
      <!-- spring 3 -->
      <event>
        <name>compressed_bottom</name>
        <type>joint</type>
        <model>spring_buckle_test_3</model>
        <joint>joint_bottom_1</joint>
        <range>
          <type>position</type>
          <min>-0.10</min>
          <max>-0.01</max>
        </range>
      </event>
      <event>
        <name>buckled_x</name>
        <type>joint</type>
        <model>spring_buckle_test_3</model>
        <joint>joint_1_2</joint>
        <range>
          <type>normalized_angle</type>
          <min>-0.1</min>
          <max> 0.1</max>
        </range>
      </event>
      <event>
        <name>buckled_y</name>
        <type>joint</type>
        <model>spring_buckle_test_3</model>
        <joint>joint_2_3</joint>
        <range>
          <type>normalized_angle</type>
          <min>-0.1</min>
          <max> 0.1</max>
        </range>
      </event>
      <!-- spring 2 -->
      <event>
        <name>compressed_bottom</name>
        <type>joint</type>
        <model>spring_buckle_test_2</model>
        <joint>joint_bottom_1</joint>
        <range>
          <type>position</type>
          <min>-0.10</min>
          <max>-0.01</max>
        </range>
      </event>
      <event>
        <name>buckled_x</name>
        <type>joint</type>
        <model>spring_buckle_test_2</model>
        <joint>joint_1_2</joint>
        <range>
          <type>normalized_angle</type>
          <min>-0.1</min>
          <max> 0.1</max>
        </range>
      </event>
      <event>
        <name>buckled_y</name>
        <type>joint</type>
        <model>spring_buckle_test_2</model>
        <joint>joint_2_3</joint>
        <range>
          <type>normalized_angle</type>
          <min>-0.1</min>
          <max> 0.1</max>
        </range>
      </event>
      <!-- spring 1 -->
      <event>
        <name>compressed_bottom</name>
        <type>joint</type>
        <model>spring_buckle_test_1</model>
        <joint>joint_bottom_1</joint>
        <range>
          <type>position</type>
          <min>-0.10</min>
          <max>-0.01</max>
        </range>
      </event>
      <event>
        <name>buckled_x</name>
        <type>joint</type>
        <model>spring_buckle_test_1</model>
        <joint>joint_1_2</joint>
        <range>
          <type>normalized_angle</type>
          <min>-0.1</min>
          <max> 0.1</max>
        </range>
      </event>
      <event>
        <name>buckled_y</name>
        <type>joint</type>
        <model>spring_buckle_test_1</model>
        <joint>joint_2_3</joint>
        <range>
          <type>normalized_angle</type>
          <min>-0.1</min>
          <max> 0.1</max>
        </range>
      </event>
    </plugin>

(For reference, here are the documentation for SDF format and here are some basic tutorials on using SDF to build simulation worlds and models).

And the accompanying source code blocks that interprets the results of the SimEventsPlugin data can be found in HaptixGUIPlugin.cc.

This block:

void HaptixGUIPlugin::ScoringUpdate()
{
  while(!quit)
  {
    if (this->hxInitialized)
    {
      // hardcoded, tasks 0, 1, 2 are the spring tests
      // hide if task id is greater than 2
      if (this->currentTaskId > 2)
      {
        this->springScoreItem[0]->setBrush(QBrush(QColor(255, 0, 0, 0)));
        this->springScoreItem[1]->setBrush(QBrush(QColor(255, 0, 0, 0)));
        this->springScoreItem[2]->setBrush(QBrush(QColor(255, 0, 0, 0)));
        this->springScoreItem[0]->setPen(QPen(QColor(153, 255, 0, 0)));
        this->springScoreItem[1]->setPen(QPen(QColor(153, 255, 0, 0)));
        this->springScoreItem[2]->setPen(QPen(QColor(153, 255, 0, 0)));
      }
      else
      {
        if (this->springCompressed && !this->springBuckled)
        {
          gazebo::common::Time compressDuration =
            gazebo::common::Time::GetWallTime() -
            this->springCompressedStartTime;
          if (compressDuration > this->springCompressedPassDuration)
          {
            // success! spring compressed correctly for 3 seconds.
            gzdbg << "task completed!\n";
            this->springScoreItem[0]->setBrush(QBrush(QColor(0, 255, 0, 255)));
            this->springScoreItem[1]->setBrush(QBrush(QColor(0, 255, 0, 255)));
            this->springScoreItem[2]->setBrush(QBrush(QColor(0, 255, 0, 255)));
          }
          else
          {
            double timeLeft = (this->springCompressedPassDuration -
                               compressDuration).Double();
            // spring compressed correctly, just a few more seconds...
            gzdbg << "compressed, great work! Please hold it for"
                  << " [" << timeLeft
                  << "] more seconds!\n";
            this->springScoreItem[0]->setBrush(
                QBrush(QColor(0, 255, 0, 255)));
            this->springScoreItem[1]->setBrush(
                QBrush(QColor(static_cast<int>(
                255*(timeLeft/this->springCompressedPassDuration.Double())),
                255, 0, 255)));
            this->springScoreItem[2]->setBrush(
                QBrush(QColor(static_cast<int>(
                255*(timeLeft/this->springCompressedPassDuration.Double())),
                255, 0, 0)));
          }
        }
        else
        {
          if (!this->springCompressed)
          {
            gzdbg << "spring not compressed, try squeezing it [some more]!\n";
            this->springScoreItem[0]->setBrush(QBrush(QColor(255, 0, 0, 255)));
            this->springScoreItem[1]->setBrush(QBrush(QColor(255, 0, 0, 0)));
            this->springScoreItem[2]->setBrush(QBrush(QColor(255, 0, 0, 0)));
          }
          else if (this->springBuckled)
          {
            gzdbg << "spring buckled, try to keep it straight!\n";
            this->springScoreItem[0]->setBrush(QBrush(QColor(0, 255, 0, 255)));
            this->springScoreItem[1]->setBrush(QBrush(QColor(0, 0, 255, 255)));
            this->springScoreItem[2]->setBrush(QBrush(QColor(0, 0, 255, 0)));
          }
          else
          {
            // user has not started compressing the spring
            // or the spring has bucked beyond tolerance.
            // gzerr << "Red!\n";
            // gzerr << "compressed [" << this->springCompressed
            //       << "] buckled [" << this->springBuckled << "]\n";
            this->springScoreItem[0]->setBrush(QBrush(QColor(255, 0, 0, 255)));
            this->springScoreItem[1]->setBrush(QBrush(QColor(255, 0, 0, 0)));
            this->springScoreItem[2]->setBrush(QBrush(QColor(255, 0, 0, 0)));
            this->springScoreItem[0]->setPen(QPen(QColor(153, 255, 0, 255)));
            this->springScoreItem[1]->setPen(QPen(QColor(153, 255, 0, 255)));
            this->springScoreItem[2]->setPen(QPen(QColor(153, 255, 0, 255)));
          }
        }
      }
    }
    usleep(100000);  // 10Hz max on scoring check
  }
}

/////////////////////////////////////////////////
void HaptixGUIPlugin::PollTracking

updates the GUI visual to keep track of the task completion state by changing colors of three circles to the lower right hand side of the hand diagram used for contact sensor visualization. Note the HaptixGUIPlugin::ScoringUpdate function is spawned in its own thread here. And the HaptixGUIPlugin::OnSimEvents function referenced here subscribes to state updates published by the SimEventsPlugin.

For reference, the Gazebo SimEvents API documentation can be found here. This plugin subscribes to gazebo topic /gazebo/sim_events to monitor for changes in the simulated spring joints.