HAPTIX Simulation Scoring Plugin Example
이 튜토리얼은 Valero-Cuevas 등이 2003년 작성한 "The strength-dexterity test as a measure of dynamic pinch performance"라는 제목의 논문에서 영감을 얻은 시뮬레이션 기반 손재주 테스트를 위해 만들어진 새로운 사용자 handsim world를 실습한다.
이 튜토리얼에서는 HAPTIX handsim 설치 단계를 이미 완료했다고 가정하며, 시뮬레이션 world API 튜토리얼을 완료하는 것이 좋다.
가제보 hansim 채점 플러그인을 시작하고, 터미널에서 가제보를 시작한다:
gazebo --verbose worlds/luke_hand.world
기본적으로 Luke Hand 모델을 사용하여 데스크톱 환경을 제공한다.
이 world에는 SimEventsPlugin을 사용하여 앞서 언급 한 작업 완료 상태를 추적하는 스프링 압축 테스트가 있다.
아래는 키보드와 spacenav options를 사용하여 원격 조종되는 사용자 world의 예이다:
이 비디오에는, 손 시각화 GUI의 오른쪽 하단에 3개의 작업 완료 표시 점이 있다. 왼쪽 가장 원형의 점은 구부러짐 없는 정확한 압축을 나타내며, 녹색은 구부러짐 없는 압축하면 녹색을 나타내며, 압축되지 않았거나 구부러지면 빨간색을 나타낸다. 중간에 원형 점은 압축 유지 시간을 나타내며, 스프링이 올바르게 압축되어 3초 동안 유지되면 흰색에서 녹색으로 희미해진다. 오른쪽 가장 원형인 점은 작업 성공을 나타내며 흰색에서 녹색으로 바뀐다.
처음에는 세 개의 동그라미가 빨간색-흰색-흰색으로, 스프링이 방해받지 않았음을 나타낸다. 스프링이 충분히 압축되고 (1~10cm의 압축 길이) 스프링은 상대적으로 직선이면 (스프링 중앙에서 비틀림 스프링 조인트가 0.1 radian 미만을 나타냄), 첫 번째 원이 녹색으로 바뀐다. 이 성공적인 압축 해제를 위하여 타이머가 시작되고, 두 번째 원이 흰색에서 녹색으로 사라집니다. 타이머가 3초에 도달하면 두 번째 및 세 번째 원이 녹색으로 바뀌며, 프로그램은이 시험을 성공적인 시험 시행으로 간주한다.
luke_hand.world에서 새로운 libSimEventsPlugin.so 플러그인 블럭을 추가한다:
<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>(참고를 위해 SDF 형식에 대한 설명서가 있으며 여기에는 SDF를 사용하여 시뮬레이션 world와 모델을 작성하는 것에 대한 기본 튜토리얼이 있다.)
그리고 이어지는 코드 블록은 SimEventsPlugin 데이터의 결과를 해석하는 것이며 HaptixGUIPlugin.cc에서 찾을 수 있습니다:
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세 개의 원의 색을 접촉 센서 시각화에 사용되는 손 그림의 오른쪽 아래쪽으로 변경하여 작업 완료 상태를 추적하도록 GUI 비주얼을 업데이트한다.HaptixGUIPlugin::ScoringUpdate 함수는 자체 스레드에서 생성된다. 그리고 여기서 참고한 HaptixGUIPlugin::OnSimEvents 함수는 SimEventsPlugin에 의해 게시된 상태 업데이트'를 구독합니다.
참고로, Gazebo SimEvents API 문서는 여기에서 찾을 수 있습니다. 이 플러그인은 시뮬레이션된 스프링 조인트의 변화를 모니터링하기 위해 가제보 topic /gazebo/sim_events를 구독한다.