RTコンポーネントにおけるトルク指令の入力¶

ここでは、Choreonoidを用いて、ロボットの動作パターンを作成する方法、そのパターンをロボットで実行できるように前節で作成したRTコンポーネントを拡張する方法について解説します。

RTコンポーネントの拡張¶

関節のPD制御を行うために、前の節で作成したRTコンポーネントRobotControllerRTCを拡張します。PD制御を行うためには、関節へのトルク指令を出力する必要があるため、トルク指令用のデータポートをRTCBuilderを用いて追加します。[RobotControllerRTC]の[データポート]タブから、追加([Add])するデータポートのプロファイルは次のようになります。

OutPort プロファイル:

ポート名: u

データ型: RTC::TimeDoubleSeq

変数名: torque

表示位置: RIGHT

データポートを追加し、[基本]タブから再度コード生成([コード生成]と[Generate])を行ったら、ビルド(buildディレクトリにてmake)ができることを確認しておきます。

コントローラのソースコード¶

コントローラのヘッダファイル($HOME/workspace/RobotControllerRTC/include/RobotControllerRTC.h)は以下のようになります。（編集済みのサンプルファイルはsamples/tutorials/rtc/RobotControllerRTC3/include/RobotControllerRTC.hに収録されています）

/*!
 * @file  RobotControllerRTC.h
 * @brief Robot Controller component
 * @date  $Date$
 *
 * $Id$
 */

#ifndef ROBOTCONTROLLERRTC_H
#define ROBOTCONTROLLERRTC_H

#include <rtm/idl/BasicDataTypeSkel.h>
#include <rtm/idl/ExtendedDataTypesSkel.h>
#include <rtm/idl/InterfaceDataTypesSkel.h>
#include <cnoid/MultiValueSeq>
#include <vector>

using namespace RTC;

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>

class RobotControllerRTC
  : public RTC::DataFlowComponentBase
{
  public:
    RobotControllerRTC(RTC::Manager* manager);
    ~RobotControllerRTC();
    virtual RTC::ReturnCode_t onInitialize();
    virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
    virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
    virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

 protected:
   RTC::TimedDoubleSeq m_angle;
   InPort<RTC::TimedDoubleSeq> m_angleIn;

   RTC::TimedCharSeq m_torque;
   OutPort<RTC::TimedCharSeq> m_torqueOut;
 private:
   cnoid::MultiValueSeqPtr qseq;
   std::vector<double> q0;
   cnoid::MultiValueSeq::Frame oldFrame;
   int currentFrame;
   double timeStep_;

};

extern "C"
{
  DLL_EXPORT void RobotControllerRTCInit(RTC::Manager* manager);
};

#endif // ROBOTCONTROLLERRTC_H

今回はトルクの出力をしなければならないので、出力ポートのための設定が増加しています。 RTC::OutPort<RTC::TimedDoubleSeq> はRTCの出力ポートを表す型であり、出力ポートを操作するにはこれを利用します。

コントローラのソースコード($HOME/workspace/RobotControllerRTC/src/RobotControllerRTC.cpp)は以下のようになります。(編集済みのファイルは samples/tutorials/rtc/RobotControllerRTC3/src/RobotControllerRTC.cppに収録されています)

/*!
 * @file  RobotControllerRTC.cpp
 * @brief Robot Controller component
 * @date $Date$
 *
 * $Id$
 */

#include "RobotControllerRTC.h"
#include <iostream>
#include <cnoid/BodyMotion>
#include <cnoid/ExecutablePath>
#include <cnoid/FileUtil>

using namespace std;
using namespace cnoid;

namespace {

static const double pgain[] = {
    50000.0, 30000.0, 30000.0, 30000.0, 30000.0,
    80000.0, 80000.0, 10000.0, 3000.0, 30000.0,
    30000.0, 80000.0, 3000.0, 30000.0, 10000.0,
    3000.0, 3000.0, 30000.0, 30000.0, 10000.0,
    3000.0, 30000.0, 3000.0, 3000.0, 3000.0,
    3000.0, 3000.0, 3000.0, 3000.0, 3000.0,
    3000.0, 3000.0, 10000.0, 3000.0, 3000.0,
    30000.0, 3000.0, 3000.0, 3000.0, 3000.0,
    3000.0, 3000.0, 3000.0, 3000.0,
    };

static const double dgain[] = {
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0, 100.0,
    100.0, 100.0, 100.0, 100.0,
    };
};

// Module specification
static const char* robotcontrollerrtc_spec[] =
  {
    "implementation_id", "RobotControllerRTC",
    "type_name",         "RobotControllerRTC",
    "description",       "Robot Controller component",
    "version",           "1.0.0",
    "vendor",            "AIST",
    "category",          "Generic",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    ""
  };

RobotControllerRTC::RobotControllerRTC(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    m_angleIn("q", m_angle),
    m_torqueOut("u", m_torque)
{
}

RobotControllerRTC::~RobotControllerRTC()
{

}


RTC::ReturnCode_t RobotControllerRTC::onInitialize()
{
  addInPort("q", m_angleIn);
  addOutPort("u", m_torqueOut);

  return RTC::RTC_OK;
}

RTC::ReturnCode_t RobotControllerRTC::onActivated(RTC::UniqueId ec_id)
{
  if(!qseq){
    string filename = getNativePathString(
      boost::filesystem::path(shareDirectory())
      / "motion" / "RobotPattern.yaml");

    BodyMotion motion;

    if(!motion.loadStandardYAMLformat(filename)){
      cout << motion.seqMessage() << endl;
      return RTC::RTC_ERROR;
    }
    qseq = motion.jointPosSeq();
    if(qseq->numFrames() == 0){
      cout << "Empty motion data." << endl;
      return RTC::RTC_ERROR;
    }
    q0.resize(qseq->numParts());
    timeStep_ = qseq->getTimeStep();
  }

  m_torque.data.length(qseq->numParts());

  if(m_angleIn.isNew()){
    m_angleIn.read();
  }
  for(int i=0; i < qseq->numParts(); ++i){
    q0[i] = m_angle.data[i];
  }
  oldFrame = qseq->frame(0);
  currentFrame = 0;

  return RTC::RTC_OK;
}


RTC::ReturnCode_t RobotControllerRTC::onDeactivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}

RTC::ReturnCode_t RobotControllerRTC::onExecute(RTC::UniqueId ec_id)
{
  if(m_angleIn.isNew()){
    m_angleIn.read();
  }

  MultiValueSeq::Frame frame;

  if(currentFrame > qseq->numFrames()){
    frame = oldFrame;
  }else{
    frame = qseq->frame(currentFrame++);
  }

  for(int i=0; i < frame.size(); i++){
    double q_ref = frame[i];
    double q = m_angle.data[i];
    double dq_ref = (q_ref - oldFrame[i]) / timeStep_;
    double dq = (q - q0[i]) / timeStep_;
    m_torque.data[i] = (q_ref - q) * pgain[i]/100.0 + (dq_ref - dq) * dgain[i]/100.0;
    q0[i] = q;
  }
  oldFrame = frame;

  m_torqueOut.write();

  return RTC::RTC_OK;
}


extern "C"
{
  DLL_EXPORT void RobotControllerRTCInit(RTC::Manager* manager)
  {
    coil::Properties profile(robotcontrollerrtc_spec);
    manager->registerFactory(profile,
                             RTC::Create<RobotControllerRTC>,
                             RTC::Delete<RobotControllerRTC>);
  }
};

出力ポートに関する設定は、入力ポートの場合と関数名が異なるだけでよく似ています。

onActivated() のときの処理に注目しましょう。この関数はRTCが有効化された際に一度だけ呼ばれます。ここで、Choreonoidの共有ディレクトリからRobotPattern.yamlを読み出しています。これはロボットの全関節角度の軌道を記述した動作パターンファイルです。 motion.loadStandardYAMLformat() によりモーションデータに変換します。 onActivated()では初期値の設定も行っています。

onExecute()ではトルクの計算と出力の処理が追加されました。関節角度を読み込む部分のコードはこれまでと同じですが、 m_torque.data[i] に計算したトルクの値を代入しています。ここでは簡単なPD制御によりトルクの値を求めています。各関節毎のPgainとDgainはソースコードの先頭付近に固定値で定義されています。ロボットがうまく制御できない場合はこの値を調整する必要があります。 m_torque.data にセットした値は m_torqueOut.write() により実際のロボットの制御トルクとして出力されます。出力ポートは値をセットするだけなので入力ポートよりも簡単です。

CMakeLists.txtの編集¶

このコントローラはChoreonoidのライブラリが提供している機能を使用しているため、Choreonoidのライブラリをリンクする必要があります。そこで$HOME/workspace/RobotControllerRTC/src/CMakeLists.txtを次のように編集します。（編集済みのファイルはsamples/tutorials/rtc/RobotControllerRTC3/src/CMakeLists.txtに収録されています）

set(comp_srcs RobotControllerRTC.cpp )
set(standalone_srcs RobotControllerRTCComp.cpp)

if (DEFINED OPENRTM_INCLUDE_DIRS)
  string(REGEX REPLACE "-I" ";"
    OPENRTM_INCLUDE_DIRS "${OPENRTM_INCLUDE_DIRS}")
  string(REGEX REPLACE " ;" ";"
    OPENRTM_INCLUDE_DIRS "${OPENRTM_INCLUDE_DIRS}")
endif (DEFINED OPENRTM_INCLUDE_DIRS)

if (DEFINED OPENRTM_LIBRARY_DIRS)
  string(REGEX REPLACE "-L" ";"
    OPENRTM_LIBRARY_DIRS "${OPENRTM_LIBRARY_DIRS}")
  string(REGEX REPLACE " ;" ";"
    OPENRTM_LIBRARY_DIRS "${OPENRTM_LIBRARY_DIRS}")
endif (DEFINED OPENRTM_LIBRARY_DIRS)

if (DEFINED OPENRTM_LIBRARIES)
  string(REGEX REPLACE "-l" ";"
    OPENRTM_LIBRARIES "${OPENRTM_LIBRARIES}")
  string(REGEX REPLACE " ;" ";"
    OPENRTM_LIBRARIES "${OPENRTM_LIBRARIES}")
endif (DEFINED OPENRTM_LIBRARIES)

include_directories(${PROJECT_SOURCE_DIR}/include)
include_directories(${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME})
include_directories(${PROJECT_BINARY_DIR})
include_directories(${PROJECT_BINARY_DIR}/idl)
include_directories(${OPENRTM_INCLUDE_DIRS})
include_directories(${OMNIORB_INCLUDE_DIRS})
add_definitions(${OPENRTM_CFLAGS})
add_definitions(${OMNIORB_CFLAGS})

MAP_ADD_STR(comp_hdrs "../" comp_headers)

link_directories(${OPENRTM_LIBRARY_DIRS})
link_directories(${OMNIORB_LIBRARY_DIRS})

include(FindPkgConfig)
pkg_check_modules(CNOID choreonoid-body-plugin)
include_directories(${CNOID_INCLUDE_DIRS})
link_directories(${CNOID_LIBRARY_DIRS})

add_library(${PROJECT_NAME} ${LIB_TYPE} ${comp_srcs}
  ${comp_headers} ${ALL_IDL_SRCS})
set_target_properties(${PROJECT_NAME} PROPERTIES PREFIX "")
set_source_files_properties(${ALL_IDL_SRCS} PROPERTIES GENERATED 1)
if(NOT TARGET ALL_IDL_TGT)
  add_custom_target(ALL_IDL_TGT)
endif(NOT TARGET ALL_IDL_TGT)
add_dependencies(${PROJECT_NAME} ALL_IDL_TGT)
target_link_libraries(${PROJECT_NAME} ${OPENRTM_LIBRARIES} ${CNOID_LIBRARIES} boost_system boost_filesystem)

add_executable(${PROJECT_NAME}Comp ${standalone_srcs}
  ${comp_srcs} ${comp_headers} ${ALL_IDL_SRCS})
target_link_libraries(${PROJECT_NAME}Comp ${OPENRTM_LIBRARIES} ${CNOID_LIBRARIES} boost_system boost_filesystem)

install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}Comp
    EXPORT ${PROJECT_NAME}
    RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT component
    LIBRARY DESTINATION ${LIB_INSTALL_DIR} COMPONENT component
    ARCHIVE DESTINATION ${LIB_INSTALL_DIR} COMPONENT component)
install(EXPORT ${PROJECT_NAME}
    DESTINATION ${LIB_INSTALL_DIR}/${PROJECT_NAME}
    FILE ${PROJECT_NAME}Depends.cmake)

コントローラのビルド¶

buildディレクトリ内にて再度makeをし、前節と同じ場所にRTCをインストールします。

$ make
$ sudo cp -p src/RobotControllerRTC.so /usr/lib/choreonoid-1.5/rtc

Choreonoidの起動¶

前節で作成したプロジェクトファイル、sample2.cnoidを開きます。もしくは、samples/tutorials/cnoid/sample2.cnoidでも構いません。

$ choreonoid sample2.cnoid

ポーズ列の追加¶

まずアイテムビューで「JVRC」を選択します。次に、「メニュー」の「ファイル」「新規」より「ポーズ列」を選択し「SampleMotion」という名前で追加します。

次に、[表示]-[ビューの表示]-[ポーズロール]を選択します。次の画面が表示されるはずです。

基準の姿勢を作るため、アイテムビューで「JVRC」を選択し、ツールバーにある「選択ボディを初期姿勢に」のボタンを押します。

ポーズロールにおいて、1.0 を選択して「挿入」を押します。

今回は首を左右に振る動作パターンを作成します。以下の手順でポーズを作成してください。

ポーズロールにおいて 2.0 を選択し、関節スライダにおいて首関節のヨー軸「NECK_Y」を 70.0 にセットし、ポーズロールの「挿入」を押す。
ポーズロールにおいて 3.0 を選択し、関節スライダにおいて首関節のヨー軸「NECK_Y」を 0.0 にセットし、ポーズロールの「挿入」を押す。
ポーズロールにおいて 4.0 を選択し、関節スライダにおいて首関節のヨー軸「NECK_Y」を -70.0 にセットし、ポーズロールの「挿入」を押す。
ポーズロールにおいて 5.0 を選択し、関節スライダにおいて首関節のヨー軸「NECK_Y」を 0.0 にセットし、ポーズロールの「挿入」を押す。
ポーズロールにおいて 6.0 を選択し、関節スライダにおいて首関節のヨー軸「NECK_R」を -50.0 にセットし、ポーズロールの「挿入」を押す。

ポーズロールで作成したのはキーフレームと呼びます。これより、プログラムで使用するモーションを生成させます。ツールバーから「ボディモーションの生成」ボタンを押します。

モーションはツールバーのボタンで手動で生成しなくても、キーフレームの更新時に自動生成することができます。これを有効にするにはツールバーの「自動更新モード」のボタンをオンにしてください。

SampleMotion の子供に motion があるので、これを選択し[ファイル]-[名前を付けて選択アイテムを保存]します。

ファイル名はRobotPattern.yamlとし、適当なディレクトリ（例えば/tmp）に保存します。その後でコントローラが参照するChoreonoidの共有ディレクトリに次のコマンドで移動します。

$ sudo mv RobotPattern.yaml /usr/share/choreonoid-1.5/motion

コントローラの設定¶

Choreonoidには1体のロボットに複数のコントローラを割り当てる機能があります。今JVRCの子アイテムとしてBodyRTCとSampleMotionの2つがありますが、これら2つのうちどちらをシミュレーション時に使用するかを指定する必要があります。ここではBodyRTCを使いますのでBodyRTCの方にチェックを入れておきます。

シミュレーションを実行する¶

シミュレーションを実行すると今度はロボットが崩れ落ちることなく、先ほど作成した首振りのモーションを実行しているはずです。この時RTSystemEditorでRTCの接続を確認すると、下図のようになっており、フィードバックループができていることが確認できます。

注釈

別のシーンビューを生成して、同時にカメラの映像を表示してみましょう。

メインメニュー「表示」の「ビューの生成」から「シーン」を選択します。次のウィンドウが表示されるので OK をクリックします。

新しいシーンビュー「シーン2」が生成されます。「シーン2」タブをクリックして選択し、カメラの選択ボタンでロボットのカメラ「JVRC - rcamera」などに表示を切り替えます。

「シーン2」タブをドラッグして二つのシーンを同時に表示することもできます。

プロジェクトの保存¶

シミュレーションの実行が終わったら、プロジェクトを保存しておきましょう。[ファイル]-[名前を付けてプロジェクトを保存]を選択し、ファイル名をsample3.cnoidとして保存します。

サンプルファイルについて¶

このチュートリアルで作成されるプロジェクトファイルはcnoid/sample3.cnoidに、コントローラのソースコードはrtc/RobotControllerRTC3に収録されています。

ポート名:	u
データ型:	RTC::TimeDoubleSeq
変数名:	torque
表示位置:	RIGHT